I2C driver using bit bang

Sathyanarayana Hadadi March 24, 20137 comments Coded in C
typedef struct 
{
  unsigned int PIN0:1;
  unsigned int PIN1:1;
  unsigned int PIN2:1;
  unsigned int PIN3:1;
  unsigned int PIN4:1;
  unsigned int PIN5:1;
  unsigned int PIN6:1;
  unsigned int PIN7:1;
} PORT;

/* TODO: Example address shown, but the proper address */
#define PORT0 *(volatile PORT *)0x1234

/* Define the port used for I2C data and clk as shown above to access them pin wise */
#define I2C_DATA PORT0.PIN0
#define I2C_CLK  PORT0.PIN1

#define HIGH 1
#define LOW  0

/* I2C Start - bit bang */
void I2C_START(void)
{
    /* I2C Start condition, data line goes low when clock is high */
    I2C_DATA = HIGH;
    I2C_CLK = HIGH;
    I2C_DATA = LOW;
    I2C_CLK = LOW;
}

/* I2C Stop - bit bang */
void I2C_STOP (void)
{
    /* I2C Stop condition, clock goes high when data is low */
    I2C_CLK = LOW;
    I2C_DATA = LOW;
    I2C_CLK = HIGH;
    I2C_DATA = HIGH;
}

/* I2C Write - bit bang */
void I2C_WRITE(unsigned char data)
{
	unsigned char outBits;
	unsigned char inBit;
	
 	/* 8 bits */
	for(outBits = 0; outBits < 8; outBits++) 
	{
	    if(data & 0x80)
		    I2C_DATA = 1;
		else
		    I2C_DATA = 0;
      	data  <<= 1;
		/* Generate clock for 8 data bits */
		SCLK = HIGH;
		SCLK = LOW;					
	}
	
	/* Generate clock for ACK */
	I2C_CLK = HIGH;
        /* Wait for clock to go high, clock stretching */
        while(I2C_CLK);
        /* Clock high, valid ACK */
	inBit = I2C_DATA;
	I2C_CLK = LOW;					
}

unsigned char I2C_READ (void)
{
	unsigned char inData, inBits;

	inData = 0x00;
	/* 8 bits */
	for(inBits = 0; inBits < 8; inBits++)
	{
		inData <<= 1;
		I2C_CLK = HIGH;
      	inData |= I2C_DATA;
		I2C_CLK = LOW;					
	}

   return inData;
}

/* Examble for writing to I2C Slave */
void writeI2CSlave (unsigned char data)	
{
    /* Start */
  	I2C_START();
	/* Slave address */
   	I2C_WRITE(0xAA)
	/* Slave control byte */
   	I2C_WRITE(0xBB);
	/* Slave data */
   	I2C_WRITE(data);
	/* Stop */
   	I2C_STOP();
}

/* Examble for reading from I2C Slave */
unsigned char readI2CSlave(unsigned char data)
{
   	unsigned char inData;

	/* Start */
  	I2C_START(); 
	/* Slave address */
   	I2C_WRITE(0xAA);
	/* Slave control byte */
   	I2C_WRITE(data);
	/* Stop */
   	I2C_STOP();
	
	/* Start */
   	I2C_START();
	/* Slave address + read */
   	I2C_WRITE(0xAA | 1);
	/* Read */
	inData = I2C_READ();

   	return inData;                 
}

Simple serial and timer ISR state machine

March 24, 2013 Coded in C for the Atmel AT89
/*
    k164_js.c
    
    Purpose:    New firmware for the k164 dtmf decoder board and
                the AT89C2051-24PC The source code was compiled with sdcc.

    URLs:
        http://www.digikey.com/product-detail/en/AT89C2051-24PU/AT89C2051-24PU-ND/1118880
        http://www.electronics123.com/kits-and-modules/Telephone-Call-Logger-Kit-16k.html
        http://www.kitsrus.com/pdf/k164.pdf

    Compile:    sdcc k164_js.c ; packihx k164_js.ihx > k164_js.hex
    Simulate:   s51 k164_js.hex

    Copyright (C) 2009 Nu Tech Software Solutions, Inc.

    Permission is hereby granted, free of charge, to any person obtaining a copy
    of this software and associated documentation files (the "Software"), to deal
    in the Software without restriction, including without limitation the rights
    to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
    copies of the Software, and to permit persons to whom the Software is
    furnished to do so.

    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
    IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
    AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
    LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
    OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
    THE SOFTWARE.

    AUTHOR: Sean Mathews <coder at f34r.com> 1/27/2009

*/

#include <at89x051.h>

#define STATUS_LED  P3_5
#define HOOK_LED    P3_4
#define LOOP_STATUS P3_3
#define STD_STATUS  P3_7
#define MODE_SWITCH P1_5

/* UART parameters                                            */
#define CpuClk        20275200              // 20.2752 MHz clock chip on the k164 board
#define Baudrate      9600                  // UART - 9600,N,8,1 baud used by current firmware
#define Timer1ReloadValue (256-(2*CpuClk/32/12/Baudrate))

#define F34R_MODE   0

char szVERSION[] = "V1.0";

/*
To determine the value that must be placed in TH1 to generate a given baud rate, we may use the following equation (assuming PCON.7 is clear).
      TH1 = 256 - ((Crystal / 384) / Baud) 
If PCON.7 is set then the baud rate is effectively doubled, thus the equation becomes:
      TH1 = 256 - ((Crystal / 192) / Baud)      

  make this next macro work and we wont hvae to hard code the values ... 
 */
#define InterruptRate 10000    // how oftin to hit our interrupt per second
#define Timer0H           0xBE //(char)((0xFF00 & (65536 - (InterruptRate / 12 * 1000))) >> 8)
#define Timer0L           0x00 //(char)(0x00FF & (65536 - (InterruptRate / 12 * 1000)))

/* prototypes                                                 */
void hw_init();
char getchar( void );
void myputchar( char c );
void doevents();
void myputs(char *);
void itoa(int value, char* string, int radix);
void uitoa(unsigned int value, char* string, int radix);
void send_version(void);
void send_hello(void);
void send_help(void);

#define UNKNOWN  0x01
#define OFFHOOK  0x02
#define ONHOOK   0x03
#define VERSION  0x04
#define EGGS     0x05
#define RESET    0x06
#define SEND_HELP 0x07

char hook_state;
char input_state;

int notdone=1;

#define ON       0x02
#define OFF      0x03
char std_state;

static char state_machine_active=0;

/* plug all of the other interrupt vectors                    */
#ifdef SDCC
void mydummyISR (void) interrupt 12 _naked {
}
#endif

/* Serial interrupt to track incoming key strokes             */
void serial_isr(void) interrupt 4 {
        if (RI != 0)
        {
          RI = 0;
          
          if(SBUF == '?')
            hook_state = UNKNOWN;
          if(SBUF == 'V' || SBUF == 'v')
            input_state = VERSION;
          if(SBUF == 'R' || SBUF == 'r')
            input_state = RESET;
          if(SBUF == '!')
            input_state = EGGS;
          if(SBUF == 'H' || SBUF == 'h')
            input_state = SEND_HELP;
          
        }
        return;
}

/*-------------------------------------------------------------------------
 integer to string conversion

 Written by:   Bela Torok, 1999 in the public domain
               bela.torok@kssg.ch
 usage:

 uitoa(unsigned int value, char* string, int radix)
 itoa(int value, char* string, int radix)

 value  ->  Number to be converted
 string ->  Result
 radix  ->  Base of value (e.g.: 2 for binary, 10 for decimal, 16 for hex)
---------------------------------------------------------------------------*/

#define NUMBER_OF_DIGITS 16   /* space for NUMBER_OF_DIGITS + '\0' */

void uitoa(unsigned int value, char* string, int radix)
{
unsigned char index, i;

  index = NUMBER_OF_DIGITS;
  i = 0;

  do {
    string[--index] = '0' + (value % radix);
    if ( string[index] > '9') string[index] += 'A' - ':';   /* continue with A, B,.. */
    value /= radix;
  } while (value != 0);

  do {
    string[i++] = string[index++];
  } while ( index < NUMBER_OF_DIGITS );

  string[i] = 0; /* string terminator */
}

void itoa(int value, char* string, int radix)
{
  if (value < 0 && radix == 10) {
    *string++ = '-';
    uitoa(-value, string, radix);
  }
  else {
    uitoa(value, string, radix);
  }
}

/* setup UART                                                 */
void hw_init() {

        LOOP_STATUS = 1; //set our loop status pin to an input
        STD_STATUS  = 1; //set our std status pin to an input
        MODE_SWITCH = 1; //set the "ECHO" switch input on the K164 board to input 

        EA = 0; // disable all interrupts
                  
        PCON |= 0x80;  // SMOD = 1 double speed clock for our baud rate interrupt

        TH1 = TL1 = Timer1ReloadValue;   // timer 1 mode 1 reload value 9600 baud as calculated in our macro

        TMOD &= 0x0f;    /* Set timer 1 */ 
        TMOD |= 0x20;    /* Set timer 1 as Gate=0 Timer, mode 2 */ 

        TR1 = 1;        // turn on serial timer Timer 1

        SCON = 0x40;    // init port as 8-bit UART with variable baudrate 
        SCON |= 0x10;   // Enabling serial reception
//      SCON |= 0x02;   // Setting TI bit 
        ES = 1;         // Enable Serial Interrupt */

        /* Timer 0 setup */
        TMOD &= 0xf0;    /* Set timer 0 */ 
        TMOD |= 0x01;    /* Set timer 0 16 bit timer  */ 

        /* configure generic timer 0 reset value */        
        TH0 = Timer0H;
        TL0 = Timer0L;  // reload with 35711 for 1Hz
    
        TR0 = 1;        // turn on timer 0
        ET0 = 1;        // Enable timer 0 interrupt 
        

        RI  = 0;
        TI  = 1;

        EA  = 1; // enable all interrupts

}

/* setup FIRMWARE                                              */
void fw_init() {

        /* initialize our state machine to ON HOOK */
        hook_state = UNKNOWN;
        input_state = UNKNOWN;
        std_state = UNKNOWN;

        /* Turn off our LED's we just started */
        HOOK_LED = 0;
        STATUS_LED = 0;

}

/* read a character from UART                                 */
char getchar( void ) { 
        while(!RI);
        RI = 0;
        return(SBUF);
}

/* send a character to UART port                              */
void myputchar( char c ) { 
        while(!TI);
        TI =0;
        SBUF = c;
}

void myputs(char *sz) {
   while(*sz) myputchar(*sz++);
} 

/* Timer 0 interrupt the state machines main interrupt */
void timer_isr(void) interrupt 1 {
        static int suppressfirst=1;
        static int x=0;
        static int counter=0;
        char buffer[17];
        /* configure generic timer 0 reset value */
        TH0 = Timer0H;
        TL0 = Timer0L;
        
        /* every 1 second do our event routine */        
        if(x++>50) {
                x=0;
                doevents();
        }

        /* we need to control this or we will be trying to send out serial data from two threads */
        if(state_machine_active) {

                if( input_state == VERSION ) {
                        send_version();
                        input_state = UNKNOWN;
                }
                if( input_state == SEND_HELP ) {
                        send_help();
                        input_state = UNKNOWN;
                }
                if( input_state == EGGS ) {
                        myputs("! Jack Edin 1961-2012 rip - Logic Unlimited !\r\n");
                        myputs("! Sean Mathews - NuTech.com   !\r\n");
                        input_state = UNKNOWN;
                }
                if( input_state == RESET ) {
                        notdone=0;
                        input_state = UNKNOWN;
                }

                /* check state of the hook line it seems to be inverted */
                if(!LOOP_STATUS) {
                        HOOK_LED = 1; /* ON  NPN Transistor base*/
                          if( hook_state != OFFHOOK ) {
                                counter++;
                                if(counter>10) { // 100ms
                                        hook_state = OFFHOOK;
                                        if(!suppressfirst) {
                                           myputs("OFFHOOK\r\n");
                                        } else {
                                           suppressfirst=0;
                                        }
                                }
                          }

                } else {
                        HOOK_LED = 0; /* OFF NPN Transistor base*/
                        counter=0;
                        if( hook_state != ONHOOK ) {
                                hook_state = ONHOOK;
                                if(!suppressfirst) {
                                   myputs("ONHOOK\r\n");                                   
                                } else {
                                   suppressfirst=0;
                                }
                        }
                }

                /* check state of the STD pin on the MT8870CE chip      */
                if(STD_STATUS) {
                        if( std_state != ON ) {
                                std_state = ON;
                                if(MODE_SWITCH==F34R_MODE) {
                                        myputs("TONE ");
                                }

                                switch(P1 & 0x0f) {
                                        case  10:
                                                buffer[0]='0';
                                                buffer[1]=0;
                                                break;
                                        case  11:
                                                buffer[0]='*';
                                                buffer[1]=0;
                                                break;
                                        case  12:
                                                buffer[0]='#';
                                                buffer[1]=0;
                                                break;
                                        default:
                                                itoa(P1 & 0x0f,buffer,10);
                                                break;
                                }
                                myputs(buffer);
                                if(MODE_SWITCH==F34R_MODE) {
                                        myputs("\r\n");
                                }
                        }
                } else {
                        if( std_state != OFF ) {
                                std_state = OFF;
                        }
                }
        }
} 

/* Event routine for periodic processing                      */
void doevents() {
  static char flipflop=0;
  /* one second event handler. Future use...*/

 
 
 /* flash the status led every 1 second */
 if(MODE_SWITCH!=F34R_MODE) {   
 
   STATUS_LED = !STATUS_LED;   
   
 } else {
 
        flipflop = !flipflop;
 
        if(flipflop) 
           STATUS_LED = !STATUS_LED;   
 }
                 
}

/* MAIN                                                       */
void main(void) {
        notdone=1;
        /* first setup our states and any other startup code so 
          when our hardware calls our routines they are ready */
        fw_init();

        /* ok now setup our hardware and start the interrupts */
        hw_init();

        /* tell the world we are up and running */
        send_hello();

        /* let the state machine go */
        state_machine_active=1;

        /* ... */
        while (notdone) { }

        // disable all interrupts
        EA = 0; 
        // jump to 0
        ((void (code *)(void)) 0) ();
}

void send_hello() {
        myputs("\r\n! K164mh Telephone DTMF Decoder ");
        myputs(szVERSION);
        myputs(" written for my good friend Jack Edin 1961-2012 rip!\r\n");
}

void send_version() {
        myputs(szVERSION);
        myputs("\r\n");
}

void send_help() {
 myputs("\r\n! Every line that starts with a ! is considered informational\r\n!and is not part of any call logging.\r\n");
 myputs("! The state messages are ONHOOK [%1], OFFHOOK, TONE %1\r\n");
 myputs("! The tones can also be on the ONHOOK line if the device is in inbound calls mode\r\n");
 myputs("! K164mh commands: \r\n!    ? = Information\r\n!    V = Version\r\n!    R = Reset\r\n!    H = This info\r\n");        
}

PID Control

March 23, 20131 comment Coded in C
// PID Control program by Bill Keenan, Melbourne

// Throttle PID loop stuff
#define Umin -1022
#define Umax 1022

#define Kp  0.5	//
#define Ki 0.005 //
#define Kd 1.25 	//
float U0 = 0;
float integ;  	// integration part of PID-loop must be saved 
float ep;		// previous error used for differential error (e - ep) 
 

/*****************************************************************************/
float prop, junk, der;      //  
//	junk, U0, Umax, Umin, Kp, Ki, err. 
int throttle_pidloop(int realpos) // PID-loop calc 
                          // integ - integral part of controller 
                          // ep - preceding control error 
                          // realpos - throttle position feedback 
// Besides this function uses global variables such as:
//                    Kp,Ki,Kd,U0 - constants of controller;
//                    accel_use - setpoint
//                    Umin,Umax - range of the output control signal 
{
int output;
float error;		// actual control error 

error = (float)(accel_use - realpos); // calculation of actual error 

der = ((error - ep) * Kd);
prop = error * Kp;  // calculation of proportional part 
if(prop > 1022)
	{
	prop = 1022;
	}
if(prop < -1022)
	{
	prop = -1022;
	}
junk = error * Ki;	// calculation of integral part 
integ = integ + junk;	 

if ((int)integ > Umax) 
	{
	integ = (float)Umax;
	}
else if ((int)integ < Umin) 
	{
	integ = (float)Umin;
	}

output = (int)(prop + integ + der); 

if (output > Umax) 
	{
	output = Umax;          
	}
 else if (output < Umin) 
 	{
	output = Umin;
	}
ep = error;                          		// new error 
return(output);        // return the object controlling magnitude 
}

Fast non-754 floating point library for Freescale 56F800E series

March 23, 2013 Coded in ASM for the Freescale DSP56F8xx
#ifndef ALREADY_READ_FFLOATV2A_H
#define ALREADY_READ_FFLOATV2A_H

/*******************************************************************************
                          FFloat Number Definitions
*******************************************************************************/

	#define MINUSONEFF	0x00008000	//FFloat number -1
	#define ZEROFF		0xFF800000	//FFloat number 0
	#define ONEFF		0x00014000	//FFloat number 1
	#define TWOFF		0x00024000	//FFloat number 2
	#define THREEFF		0x00026000	//FFloat number 3
	#define FOURFF		0x00034000	//FFloat number 4
	#define FIVEFF		0x00035000	//FFloat number 5
	#define SIXFF		0x00036000	//FFloat number 6
	#define SEVENFF		0x00037000	//FFloat number 7
	#define EIGHTFF		0x00044000	//FFloat number 8
	#define NINEFF		0x00044800	//FFloat number 9
	#define TENFF		0x00045000	//FFloat number 10
	#define ELEVENFF	0x00045800	//FFloat number 11
	#define TWELVEFF	0x00046000	//FFloat number 12
	
	#define NOMINALBATVOLTAGE 0x00044800

/*******************************************************************************
                          FFloat Data Type Definition
*******************************************************************************/
typedef	unsigned char bool;
typedef long unsigned int ffloat;

/*******************************************************************************
                           FFloat FUNCTION PROTOTYPES
*******************************************************************************/

asm ffloat FFabs(register ffloat ffnum);
asm ffloat FFneg(register ffloat ffnum);
asm ffloat S16int2FFloat(register short int inum);
asm ffloat S32int2FFloat(register long int inum);
asm ffloat U32int2FFloat(register long unsigned int unum);
asm ffloat FFadd(register ffloat ffnum1,register ffloat ffnum2);
asm ffloat FFdiv(register ffloat ffnum1,register ffloat ffnum2);
asm short int FFloatTrunc2S16int(register ffloat ffnum);
asm short int FFloatRnd2S16int(register ffloat ffnum);
asm ffloat FFmult(register ffloat ffnum1,register ffloat ffnum2);
asm ffloat FFsub(register ffloat ffnum1,register ffloat ffnum2);
asm ffloat IEEE2FFloat(register float fnum);
float FFloat2IEEE(ffloat ffnum);
asm bool FFgt(register ffloat ffnum1, register ffloat ffnum2);
asm bool FFlt(register ffloat ffnum1, register ffloat ffnum2);
asm bool FFgte(register ffloat a, register ffloat b);
asm bool FFlte(register ffloat a, register ffloat b);
asm bool FFgtz(register ffloat ffnum);
asm bool FFltz(register ffloat ffnum);
asm bool FFeqz(register ffloat ffnum);
ffloat FFatan(ffloat xin);
ffloat FFsin(ffloat xin);
ffloat FFcos(ffloat xin);

#endif

**********************************************************
Function code begins below
**********************************************************

#include "FFloatV2A.h"

ffloat FFatan(ffloat xin)
{
    int k,klo,khi;
    ffloat xdiff0, xdiff1;
    ffloat x=xin;
static ffloat xlo = 0x0005b000;
static ffloat xhi = 0x00055000;
static ffloat ya[151] = {0x00019eaa, 0x00019eb5, 0x00019ec0, 0x00019ecc, 0x00019ed8, 0x00019ee4, 0x00019ef1, 0x00019efe, 0x00019f0c, 0x00019f19, 0x00019f28, 0x00019f36, 0x00019f46, 0x00019f55, 0x00019f66, 0x00019f76, 0x00019f88, 0x00019f99, 0x00019fac, 0x00019fbf, 0x00019fd3, 0x00019fe8, 0x00019ffd, 0x0001a013, 0x0001a02a, 0x0001a042, 0x0001a05b, 0x0001a075, 0x0001a090, 0x0001a0ac, 0x0001a0ca, 0x0001a0e9, 0x0001a109, 0x0001a12b, 0x0001a14e, 0x0001a173, 0x0001a19a, 0x0001a1c3, 0x0001a1ee, 0x0001a21c, 0x0001a24c, 0x0001a27f, 0x0001a2b5, 0x0001a2ef, 0x0001a32c, 0x0001a36d, 0x0001a3b3, 0x0001a3fd, 0x0001a44d, 0x0001a4a2, 0x0001a4ff, 0x0001a563, 0x0001a5d0, 0x0001a646, 0x0001a6c7, 0x0001a754, 0x0001a7f0, 0x0001a89d, 0x0001a95d, 0x0001aa33, 0x0001ab25, 0x0001ac37, 0x0001ad71, 0x0001aeda, 0x0001b07f, 0x0001b26e, 0x0001b4bc, 0x0001b785, 0x0001baf1, 0x0001bf38, 0x0000894e, 0x00009757, 0x0000a9a2, 0xffff8292, 0xffffbd49, 0xff800000, 0xffff42b6, 0xffff7d6d, 0x0000565d, 0x000068a8, 0x000076b1, 0x000140c7, 0x0001450e, 0x0001487a, 0x00014b43, 0x00014d91, 0x00014f80, 0x00015125, 0x0001528e, 0x000153c8, 0x000154da, 0x000155cc, 0x000156a2, 0x00015762, 0x0001580f, 0x000158ab, 0x00015938, 0x000159b9, 0x00015a2f, 0x00015a9c, 0x00015b00, 0x00015b5d, 0x00015bb2, 0x00015c02, 0x00015c4c, 0x00015c92, 0x00015cd3, 0x00015d10, 0x00015d4a, 0x00015d80, 0x00015db3, 0x00015de3, 0x00015e11, 0x00015e3c, 0x00015e65, 0x00015e8c, 0x00015eb1, 0x00015ed4, 0x00015ef6, 0x00015f16, 0x00015f35, 0x00015f53, 0x00015f6f, 0x00015f8a, 0x00015fa4, 0x00015fbd, 0x00015fd5, 0x00015fec, 0x00016002, 0x00016017, 0x0001602c, 0x00016040, 0x00016053, 0x00016066, 0x00016077, 0x00016089, 0x00016099, 0x000160aa, 0x000160b9, 0x000160c9, 0x000160d7, 0x000160e6, 0x000160f3, 0x00016101, 0x0001610e, 0x0001611b, 0x00016127, 0x00016133, 0x0001613f, 0x0001614a, 0x00016155};
static ffloat y2a[151] = {0xff800000, 0xfff443e4, 0xfff446b6, 0xfff449b0, 0xfff44cd5, 0xfff45029, 0xfff453af, 0xfff4576a, 0xfff45b5f, 0xfff45f92, 0xfff46408, 0xfff468c6, 0xfff46dd1, 0xfff47331, 0xfff478ec, 0xfff47f0a, 0xfff542c9, 0xfff54648, 0xfff54a06, 0xfff54e0a, 0xfff55259, 0xfff556fa, 0xfff55bf6, 0xfff56156, 0xfff56722, 0xfff56d66, 0xfff5742f, 0xfff57b8a, 0xfff641c3, 0xfff6461c, 0xfff64ad8, 0xfff65004, 0xfff655ac, 0xfff65be0, 0xfff662b0, 0xfff66a30, 0xfff67278, 0xfff67ba1, 0xfff742e5, 0xfff7488b, 0xfff74ed9, 0xfff755e6, 0xfff75dd0, 0xfff766ba, 0xfff770cc, 0xfff77c39, 0xfff8449e, 0xfff84c0f, 0xfff8549c, 0xfff85e7b, 0xfff869ef, 0xfff8774e, 0xfff9437f, 0xfff94cc5, 0xfff957cc, 0xfff96504, 0xfff974fc, 0xfffa4439, 0xfffa5032, 0xfffa5f16, 0xfffa71cd, 0xfffb44d0, 0xfffb542e, 0xfffb684a, 0xfffc4182, 0xfffc538f, 0xfffc6c5c, 0xfffd4779, 0xfffd5fe2, 0xfffe4133, 0xfffe5918, 0xfffe77b6, 0xffff4b62, 0xffff503a, 0xfffe707d, 0xff800000, 0xfffe8f82, 0xffffafc5, 0xffffb49d, 0xfffe8849, 0xfffea6e7, 0xfffebecc, 0xfffda01d, 0xfffdb886, 0xfffc93a3, 0xfffcac70, 0xfffcbe7d, 0xfffb97b5, 0xfffbabd1, 0xfffbbb2f, 0xfffa8e32, 0xfffaa0e9, 0xfffaafcd, 0xfffabbc6, 0xfff98b03, 0xfff99afb, 0xfff9a833, 0xfff9b33a, 0xfff9bc80, 0xfff888b1, 0xfff89610, 0xfff8a184, 0xfff8ab63, 0xfff8b3f0, 0xfff8bb61, 0xfff783c6, 0xfff78f33, 0xfff79945, 0xfff7a22f, 0xfff7aa19, 0xfff7b126, 0xfff7b774, 0xfff7bd1a, 0xfff6845e, 0xfff68d87, 0xfff695cf, 0xfff69d4f, 0xfff6a41f, 0xfff6aa53, 0xfff6affb, 0xfff6b527, 0xfff6b9e3, 0xfff6be3c, 0xfff58475, 0xfff58bd0, 0xfff59299, 0xfff598dd, 0xfff59ea9, 0xfff5a409, 0xfff5a905, 0xfff5ada6, 0xfff5b1f5, 0xfff5b5f9, 0xfff5b9b7, 0xfff5bd36, 0xfff480f5, 0xfff48713, 0xfff48cce, 0xfff4922e, 0xfff49739, 0xfff49bf7, 0xfff4a06d, 0xfff4a4a0, 0xfff4a895, 0xfff4ac50, 0xfff4afd6, 0xfff4b32a, 0xfff4b64f, 0xfff4b949, 0xfff4bc1b, 0xfff4bc1b};
static int numpoints = 151;
static ffloat h = 0xffff4444;
static ffloat hinv = 0x00027800;
    klo = FFloatTrunc2S16int(FFmult(FFsub(x,xlo),hinv));
    khi=klo+1;
if(FFlt(x,xlo)){
    return(ya[0]);
}else if(FFgt(x,xhi)){
    return(ya[numpoints-1]);
}
    xdiff0 = FFsub(x, FFadd(xlo, FFmult(h,S16int2FFloat(klo))));
    xdiff1 = FFsub(xdiff0, h);
    return ( FFadd(ya[klo], FFadd(FFmult(FFmult(FFsub(ya[khi],ya[klo]), hinv), xdiff0), FFmult(FFmult(y2a[khi], xdiff0), xdiff1))) );
}

ffloat FFcos(ffloat xin)
{
    int k,klo,khi;
    ffloat xdiff0, xdiff1;
    ffloat x=xin;
static ffloat xlo = 0x00029b78;
static ffloat xhi = 0x00026487;
static ffloat ya[31] = {0x00008000, 0x000082cc, 0x00008b10, 0x00009872, 0x0000aa59, 0xffff8000, 0xffffb0e4, 0xfffd94f6, 0xfffd6b09, 0xffff4f1b, 0x00004000, 0x000055a6, 0x0000678d, 0x000074ef, 0x00007d33, 0x00014000, 0x00007d33, 0x000074ef, 0x0000678d, 0x000055a6, 0x00004000, 0xffff4f1b, 0xfffd6b09, 0xfffd94f6, 0xffffb0e4, 0xffff8000, 0x0000aa59, 0x00009872, 0x00008b10, 0x000082cc, 0x00008000};
static ffloat y2a[31] = {0xff800000, 0xffff7cbe, 0xffff7481, 0xffff672d, 0xffff5556, 0xfffe7f88, 0xfffe4ed1, 0xfffc6aa5, 0xfffc955a, 0xfffeb12e, 0xfffe8077, 0xffffaaa9, 0xffff98d2, 0xffff8b7e, 0xffff8341, 0xffff8077, 0xffff8341, 0xffff8b7e, 0xffff98d2, 0xffffaaa9, 0xfffe8077, 0xfffeb12e, 0xfffc955a, 0xfffc6aa5, 0xfffe4ed1, 0xfffe7f88, 0xffff5556, 0xffff672d, 0xffff7481, 0xffff7cbe, 0xffff7cbe};
static int numpoints = 31;
static ffloat h = 0xfffe6b3b;
static ffloat hinv = 0x00034c64;
static ffloat pi2=0x00036487;
static ffloat pi2inv=0xfffe517c;
if(FFlt(xin,xlo)){
    x=FFadd(
     xin,
     FFmult(
     S16int2FFloat(
     FFloatTrunc2S16int(
            FFmult(
            FFsub(xhi,xin),
            pi2inv
           )
           )
    ),
     pi2
    )
    );
}else if(FFgt(xin,xhi)){
    x=FFsub(
     xin,
     FFmult(
     S16int2FFloat(
     FFloatTrunc2S16int(
            FFmult(
            FFsub(xin,xlo),
            pi2inv
           )
           )
    ),
     pi2
    )
    );
}
    klo = FFloatTrunc2S16int(FFmult(FFsub(x,xlo),hinv));
    khi=klo+1;
    xdiff0 = FFsub(x, FFadd(xlo, FFmult(h,S16int2FFloat(klo))));
    xdiff1 = FFsub(xdiff0, h);
    return ( FFadd(ya[klo], FFadd(FFmult(FFmult(FFsub(ya[khi],ya[klo]), hinv), xdiff0), FFmult(FFmult(y2a[khi], xdiff0), xdiff1))) );
}

//Return the negative of ffnum
asm ffloat FFneg(register ffloat ffnum) 
{
	move.w	A1,Y0		//store ffnum exp in Y0
	move.w	A0,A		//A holds mantissa of ffnum
	neg		A			//full 36-bit negate
	asr		A			//shift right to prevent overflow of clb
	jeq		Zero		//Don't normalize if zero
	
	//ffnum != 0
	clb 	A,X0		//Count sign bits
	asll.l 	X0,A		//Normalize
		
	sub 	X0,Y0		//Adjust exponent
	inc.w	Y0			//Return to normal scale	
		
	clb 	Y0,X0		//check number of sign bits in exponent
	cmp.w	#8,X0		//If less than 8 (exp > 8 bits),
	jlt		Exp_Err		//jump to exponent exception handler
		
Continue:	
	rtsd				//delayed return from subroutine
	move.w 	A1,A0		//Move mantissa of sum to lower word of ffnum1 (return value)
	move.w 	Y0,A1		//Move exponent to upper word of ffnum1 (return value)
	sxt.l	A			//Sign-extend A to 36 bits
	//end of main neg function
		
Zero: 
	rtsd					//Delayed return from subroutine - will execute next three words
	move.w	#$FF80,A		//Set exp of sum to minimum
	clr.w	A0				//Set mantissa of sum to 0
	//end of zero handler			
		
Exp_Err:
	cmp.w	#$007F,Y0		
	jle		Underflow		//If not overflow, go to underflow check	
	tst.w	A1				//Positive or negative overflow?
	jlt		NegO			//If negative, go to negative handler
	move.w	#$007F,A		//Max out exponent
	rtsd					//Delayed return from subroutine - will execute next three words
	move.w	#$7FFF,A0		//Max out mantissa
	nop						//Delay slot filler
	//end
			
NegO:
	move.w	#$007F,A		//Max out exponent
	rtsd					//Delayed return from subroutine - will execute next three cycles
	move.w	#$8000,A0		//Most negative mantissa	
	nop						//Delay slot filler
	//end
			
Underflow:
	cmp.w	#$FF80,Y0		//Check for underflow
	jge		Continue		//Not an error
	tst.w	A1				//Positive or negative underflow?
	jlt		NegU			//If negative, go to negative handler
	move.w	#$FF80,A		//Minimum exponent
	rtsd
	move.w	#$4000,A0		//Minimum normalized positive mantissa
	nop						//Filler for third delay slot
	//end
			
NegU:
	move.w	#$FF80,A		//Minimum exponent
	rtsd					//Delayed return from subroutine - will execute next three words
	move.w	#$BFFF,A0		//Minimum (abs) normalized negative mantissa
	nop						//filler for third delay slot
	//end of E_Err	
}

//Return the absolute value of ffnum
asm ffloat FFabs(register ffloat ffnum) 
{
	move.w	A1,Y0		//store ffnum exp in Y0
	move.w	A0,A		//A holds mantissa of ffnum
	abs		A			//full-width absolute value
	asr		A			//shift right to prevent overflow of clb
	jeq		Zero		//Don't normalize if zero
	
	//ffnum != 0
	clb 	A,X0		//Count sign bits
	asll.l 	X0,A		//Normalize
		
	sub 	X0,Y0		//Adjust exponent
	inc.w	Y0			//Return to normal scale	
		
	clb 	Y0,X0		//check number of sign bits in exponent
	cmp.w	#8,X0		//If less than 8 (exp > 8 bits),
	jlt		Exp_Err		//jump to exponent exception handler
		
Continue:	
	rtsd				//delayed return from subroutine
	move.w 	A,A0		//Move mantissa of sum to lower word of ffnum1 (return value)
	move.w 	Y0,A1		//Move exponent to upper word of ffnum1 (return value)
	sxt.l	A			//Sign-extend A to 36 bits
	//end of main abs function
		
Zero: 
	rtsd					//Delayed return from subroutine - will execute next three words
	move.w	#$FF80,A		//Set exp of sum to minimum
	clr.w	A0				//Set mantissa of sum to 0
	//end of zero handler			
		
Exp_Err:
	cmp.w	#$007F,Y0		
	jle		Underflow		//If not overflow, go to underflow check	
	tst.w	A1				//Positive or negative overflow?
	jlt		NegO			//If negative, go to negative handler
	move.w	#$007F,A		//Max out exponent
	rtsd					//Delayed return from subroutine - will execute next three words
	move.w	#$7FFF,A0		//Max out mantissa
	nop						//Delay slot filler
	//end
			
NegO:
	move.w	#$007F,A		//Max out exponent
	rtsd					//Delayed return from subroutine - will execute next three cycles
	move.w	#$8000,A0		//Most negative mantissa
	nop						//Delay slot filler
	//end
			
Underflow:
	cmp.w	#$FF80,Y0		//Check for underflow
	jge		Continue		//Not an error
	tst.w	A1				//Positive or negative underflow?
	jlt		NegU			//If negative, go to negative handler
	move.w	#$FF80,A		//Minimum exponent
	rtsd
	move.w	#$4000,A0		//Minimum normalized positive mantissa
	nop						//Filler for third delay slot
	//end
			
NegU:
	move.w	#$FF80,A		//Minimum exponent
	rtsd					//Delayed return from subroutine - will execute next three words
	move.w	#$BFFF,A0		//Minimum (abs) normalized negative mantissa
	nop						//filler for third delay slot
	//end of E_Err	
}

//convert an int16 to an ffloat value
asm ffloat S16int2FFloat(register short int inum) 
{
	
	tst.w	Y0
	jeq		Zero
	
	//inum != 0
	clb		Y0,X0	
	asll.l	X0,Y0	//normalize inum
	neg		X0		//set exponent
	rtsd
	add.w	#15,X0
	move.w	X0,A	//exponent
	move.w	Y0,A0	//mantissa

//FFloat zero = 0xFF800000
Zero:
	rtsd	
	move.w	#$FF80,A
	clr.w	A0
}

asm ffloat FFadd(register ffloat ffnum1,register ffloat ffnum2)

{	
		move.w	A0,X0			//Store ffnum1 mantissa temporarily in X0
		move.w	B0,Y0			//Store ffnum2 mantissa temporarily in Y0
		
		move.w 	A1,Y1		 	//Put ffnum1 exponent (exp1) in Y1
		sub		B,Y1			//Y1 = exp1 - exp2
		
		
		//Setup: Larger ffnum exponent goes in Y0; mantissa to be shifted goes in B1;
		//mantissa to stay the same goes in A1; abs exp difference goes in Y1
		
		tlt		B,A				//Move ffnum2 (mantissa and exp) to A (not shifted) if Y1 neg
		tlt		X0,B			//Move ffnum1 mantissa to B1 for shifting if Y1 neg
		tge		Y0,B			//Move ffnum2 mantissa to B1 for shifting if Y1 not negative
		
		abs		Y1				//positive shift values
		
		cmp.w 	#15,Y1			//More than 15-bit shift (ASRAC only works to 15 bits)?
		jgt 	Neglect			//If yes, an input ffnum will go to zero if shifted	
		
		move.w	A1,Y0			//Move larger exp to Y0 for shifting
		move.w	A0,A			//Move mantissa A0 to A1 for adding
			
		asrac	B1,Y1,A			//Extend B1 to 36 bits, shift right by Y1, and add to A
		asr 	A				//Shift right to prevent overflow of CLB (next)
		
		clb 	A,X0			//Count sign bits
		asll.l 	X0,A			//Normalize
		
		tst.w	A1				//Check if relevant part of result is zero
		jeq		Zero			//Result is zero
		
		sub 	X0,Y0			//Adjust exponent of exp1
		inc.w	Y0				//Return to normal scale	
		
		clb 	Y0,X0			//check number of sign bits in exponent
		cmp.w	#8,X0			//If less than 8 (exp > 8 bits),
		jlt		Exp_Err			//jump to exponent exception handler
		
	Continue:	
		rnd		A				//round to 16 bits in A1
		rtsd					//delayed return from subroutine
		move.w 	A,A0			//Move mantissa of sum to lower word of ffnum1 (return value)
		move.w 	Y0,A1			//Move exponent to upper word of ffnum1 (return value)
		sxt.l	A				//Sign-extend A to 36 bits
		//end of main add function
		
	Zero: 
		rtsd					//Delayed return from subroutine - will execute next three words
		move.w	#$FF80,A		//Set exp of sum to minimum
		clr.w	A0				//Set mantissa of sum to 0
		//end of zero handler			
		
	Exp_Err:
		cmp.w	#$007F,Y0		
		jle		Underflow		//If not overflow, go to underflow check	
		tst.w	A1				//Positive or negative overflow?
		jlt		NegO			//If negative, go to negative handler
		move.w	#$007F,A		//Max out exponent
		rtsd					//Delayed return from subroutine - will execute next three words
		move.w	#$7FFF,A0		//Max out mantissa
		nop						//Delay slot filler
		//end
			
	NegO:
		move.w	#$007F,A		//Max out exponent
		rtsd					//Delayed return from subroutine - will execute next three cycles
		move.w	#$8000,A0		//Most negative mantissa
		nop						//Delay slot filler
		//end
			
	Underflow:
		cmp.w	#$FF80,Y0		//Check for underflow
		jge		Continue		//Not an error
		tst.w	A1				//Positive or negative underflow?
		jlt		NegU			//If negative, go to negative handler
		move.w	#$FF80,A		//Minimum exponent
		rtsd
		move.w	#$4000,A0		//Minimum normalized positive mantissa
		nop						//Filler for third delay slot
		//end
			
	NegU:
		move.w	#$FF80,A		//Minimum exponent
		rtsd					//Delayed return from subroutine - will execute next three words
		move.w	#$BFFF,A0		//Minimum (abs) normalized negative mantissa
		nop						//filler for third delay slot
		//end of E_Err	
		
	Neglect:
		rts						//The input with the larger exp becomes the output
}

asm ffloat FFdiv(register ffloat ffnum1, register ffloat ffnum2)
{
	move.w	A1,X0		//Move exponent of ffnum1 to X0
	move.w	B1,Y0		//Move exponent of ffnum2 to Y0
	move.w	A0,Y1		//Move mantissa of ffnum1 to Y1 for sign check
	move.w	A0,A		//Move mantissa of ffnum1 to A1
	move.w	B0,B		//Move mantissa of ffnum2 to B1
	eor.w	B,Y1		//Calculate sign of final result
						//(sign bit of result will be 1=negative if inputs signs differ)
	abs		A
	abs		B
	jeq		DivZero		//ffnum2 cannot be zero
	
L1:
	cmp		A,B			//Check result of B - A
	bgt		L2			//Ready to divide
	brad	L1			//Recheck (delayed branch)
	asr		A			//Reduce ffnum1 mantissa by factor of 2
	inc.w	X0			//Increase ffnum1 exponent by one
	//end
	
L2:	
	//Division of Positive Fractional Data (A1:A0 / B1)
	BFCLR 	#$0001,SR 	//Clear carry bit: required for 1st DIV instruction
	//REP #16
	DIV 	B1,A		//Form positive quotient in A0
	DIV 	B1,A		//Form positive quotient in A0
	DIV 	B1,A		//Form positive quotient in A0
	DIV 	B1,A		//Form positive quotient in A0
	DIV 	B1,A		//Form positive quotient in A0
	DIV 	B1,A		//Form positive quotient in A0
	DIV 	B1,A		//Form positive quotient in A0
	DIV 	B1,A		//Form positive quotient in A0
	DIV 	B1,A		//Form positive quotient in A0
	DIV 	B1,A		//Form positive quotient in A0
	DIV 	B1,A		//Form positive quotient in A0
	DIV 	B1,A		//Form positive quotient in A0
	DIV 	B1,A		//Form positive quotient in A0
	DIV 	B1,A		//Form positive quotient in A0
	DIV 	B1,A		//Form positive quotient in A0
	DIV 	B1,A		//Form positive quotient in A0
	
	move.w	A0,A		//Move A0 to A1

	tst.w	Y1			//Check sign  needed for final result
	BGE		L3			//Branch if final sign is non-neg
	NEG		A			//Negate mantissa if result is neg
	
L3:
	clb 	A,Y1			//Count sign bits
	asll.l 	Y1,A			//Normalize
		
	tst		A				//Check if relevant part of result is zero
	jeq		Zero			//Result is zero
		
	sub 	Y0,X0			//Adjust exponent of exp1
	sub		Y1,X0
			
	clb 	X0,Y0			//check size of exponent word
	cmp.w	#8,Y0	
	jlt		Exp_Err
		
Continue:
	RTSD
	MOVE.W	A,A0
	MOVE.W	X0,A1
	sxt.l	A			//Sign-extend A to 36 bits
	//END

DivZero:
	//Call error handler here
	MOVE.W	#$007F,A		//Needs work here
	RTSD
	MOVE.W	#$7FFF,A0	
	NOP
	//END
	
Zero:
	RTSD
	MOVE.W	#$FF80,A
	CLR.W	A0
	//END
	
Exp_Err:
	cmp.w	#$007F,X0		
	jle		Underflow		//If not overflow, go to underflow check	
	tst.w	A1				//Positive or negative overflow?
	jlt		NegO			//If negative, go to negative handler
	move.w	#$007F,A		//Max out exponent
	rtsd					//Delayed return from subroutine - will execute next three words
	move.w	#$7FFF,A0		//Max out mantissa
	nop
	//end
		
NegO:
	move.w	#$007F,A		//Max out exponent
	rtsd					//Delayed return from subroutine - will execute next three words
	move.w	#$8000,A0		//Most negative mantissa
	nop						//filler for third delay slot
	//end
		
Underflow:
	cmp.w	#$FF80,X0		//Check for underflow
	jge		Continue		//Not an error
	tst.w	A1				//Positive or negative underflow?
	jlt		NegU			//If negative, go to negative handler
	move.w	#$FF80,A		//Minimum exponent
	rtsd					//Delayed return from subroutine - will execute next three words
	move.w	#$4000,A0		//Minimum normalized positive mantissa
	nop						//Filler for third delay slot
	//end
		
NegU:
	move.w	#$FF80,A		//Minimum exponent
	rtsd					//Delayed return from subroutine - will execute next three words
	move.w	#$BFFF,A0		//Minimum (abs) normalized negative mantissa
	nop						//filler for third delay slot
	//end of E_Err	
}

asm short int FFloatRnd2S16int(register ffloat ffnum)
{
	move.w	A1,Y0
	move.w	A0,A
	
	//Scale so that exponent = 15; converts mantissa to integer scale
	//Check if resulting mantissa is in range -32768 to 32767 (16 bit signed int)
	sub.w	#15,Y0
	jgt		Over	//Number is outside range -32768 to 32767
	cmp.w	#-17,Y0
	jlt		Zero	//Number is small and rounds to zero
	rtsd
	asll.l	Y0,A	//Scale to exponent = 15 (one word, two cycles)
	rnd		A		//Convergent rounding (round down boundary case if even)
	move.w	A1,Y0
	//end
	
Zero:
	rtsd
	clr.w	Y0		//Result is zero
	nop
	nop
	//end
	
Over:
	tst		A
	blt		Neg			//branch to Neg: if number is below -32768
	rtsd
	move.w	#$7FFF,Y0	//Set to most positive 16-bit value
	nop					//Filler for third delay slot
	//end
	
Neg:
	rtsd
	move.w	#$8000,Y0	//Set to most negative 16-bit value
	nop					//Filler for third delay slot
	//end	
}

asm short int FFloatTrunc2S16int(register ffloat ffnum)
{
	move.w	A1,Y0
	move.w	A0,A
	
	//Scale so that exponent = 15; converts mantissa to integer scale
	//Check if resulting mantissa is in range -32768 to 32767 (16 bit signed int)
	sub.w	#15,Y0
	jgt		Over	//Number is outside range -32768 to 32767
	cmp.w	#-17,Y0
	jlt		Zero	//Number is small and rounds to zero
	rtsd
	asll.l	Y0,A	//Scale to exponent = 15 (one word, two cycles)
	move.w	A1,Y0
	nop				//Filler for third delay slot
	//end
	
Zero:
	rtsd
	clr.w	Y0		//Result is zero
	nop
	nop
	//end
	
Over:
	tst		A
	blt		Neg			//branch to Neg: if number is below -32768
	rtsd
	move.w	#$7FFF,Y0	//Set to most positive 16-bit value
	nop					//Filler for third delay slot
	//end
	
Neg:
	rtsd
	move.w	#$8000,Y0	//Set to most negative 16-bit value
	nop					//Filler for third delay slot
	//end	
}

//convert an unsigned int32 to an ffloat value
asm ffloat U32int2FFloat(register long unsigned int unum) 
{
	tst.l	A
	jeq		Zero			//unum = 0
	jlt		LongUnsigned	//If 2^31 <= unum <= 2^32-1, unum will
							//be a negative number

	//unum <= 2^31 - 1
	clb		A,X0
	asll.l	X0,A	//normalize unum
	neg		X0		//set exponent
	add.w	#31,X0
	rtsd
	move.w	A1,A0	//mantissa	
	move.w	X0,A1	//exponent
	sxt.l	A		//sign-extend A to 36 bits
	

//FFloat zero = 0xFF800000
Zero:
	rtsd
	move.w	#$FF80,A
	clr.w	A0
	
	
//If unum is between 2^31 and 2^32-1
LongUnsigned:
	lsr.w	A		//divide mantissa by 2
	move.w	A1,A0	//move mantissa to its right place
	
	//divide the mantissa by two and increase the exponent by 1
	//this will correct the sign of A while keeping the absolute
	//value of a the same
	rtsd
	move.w	#32,A1	//exponent will always be 32 for this case
	sxt.l	A		//sign-extend A to 36 bits
}

//convert an int32 to an ffloat value
asm ffloat S32int2FFloat(register long int inum) 
{
	//inum = 0
	tst.l	A
	jeq		Zero
	
	//inum != 0
	clb		A,X0	
	asll.l	X0,A	//normalize inum
	neg		X0		//set exponent
	add.w	#31,X0
	rtsd
	move.w	A1,A0	//mantissa
	move.w	X0,A1	//exponent
	sxt.l	A		//sign-extend A to 36 bits

//FFloat zero = 0xFF800000
Zero:
	rtsd
	move.w	#$FF80,A
	clr.w	A0
}

//typedef long unsigned int ffloat;

asm ffloat FFmult(register ffloat ffnum1, register ffloat ffnum2)
{
		move.w 	B1,Y1		//This is to save exp2, use B for mult, and prepare for exp add
		move.w 	A0,X0		//Can't multiply A0,B0 directly
		move.w 	B0,Y0
		mpyr 	X0,Y0,B		//Multiply with round; result unlikely to differ from mpy, since truncated later
		asr 	B			//Shift right, so CLB can give correct count
		clb 	B,X0		//Count sign bits for normalization
		asll.l 	X0,B		//Normalize
		tst.w	B1			//Check if relevant part of result is zero
		jeq		Zero		//Go to zero handler
		add 	A,Y1		//add A1 to Y1
		sub 	X0,Y1		//Update exponent after normalization
		inc.w	Y1			//Return to normal scale		
		clb 	Y1,Y0		//count sign bits in exponent word
		cmp.w 	#8,Y0		//If <8 (exp > 8 bits),
		jlt		Exp_Err		//jump to exponent exception handler
		
Continue:		
		rtsd				//return with 3-cyle delay
		move.w 	Y1,A		//Put exp in return register
		rnd		B			//Round to 16 bits in B1
		move.w 	B1,A0		//Move mantissa to A0
		//end of mult routine
		
Zero: 
		rtsd				//return with 3-cyle delay
		move.w	#$FF80,A	//Set exp of sum to minimum
		clr.w	A0			//Set mantissa of sum to 0
		//end of zero handler			
		
Exp_Err:
		cmp.w	#$007F,Y1	//Check for overflow	
		jle		Underflow	//If not overflow, go to underflow check	
		tst.w	B1			//Positive or negative overflow?
		jlt		NegO		//If negative, go to negative handler
		move.w	#$7FFF,A0	//Max out mantissa
		rtsd				//Delayed return - will execute next three words

		nop					//Filler for third delay slot
		//end
			
NegO:
		move.w	#$007F,A	//Max out exponent
		rtsd				//Delayed return - will execute next three words
		move.w	#$8000,A0	//Most negative mantissa
		nop					//Filler for third delay slot
		//end
			
Underflow:
		cmp.w	#$FF80,Y1	//Check for underflow
		jge		Continue	//Not an error - continue normal code
		tst.w	B1			//Positive or negative overflow?
		jlt		NegU		//If negative, go to negative handler
		move.w	#$FF80,A	//Minimum exponent
		rtsd				//Delayed return - will execute next three words
		move.w	#$4000,A0	//Minimum normalized positive mantissa
		nop					//Filler for third delay slot
		//end
			
NegU:
		move.w	#$FF80,A	//Minimum exponent
		rtsd				//Delayed return - will execute next three words
		move.w	#$BFFF,A0	//Minimum (abs) normalized negative mantissa
		nop					//Filler for third delay slot
		//end of Exp_Err	

}

asm ffloat FFsub(register ffloat ffnum1,register ffloat ffnum2)

{	
		move.w	A0,X0			//Store ffnum1 mantissa temporarily in X0
		move.w	B1,Y1			//Store ffnum2 mantissa temporarily in Y1
		
		move.w	B0,B			//Prepare to negate B
		asr		B				//Prevent overflow
		inc.w	Y1				//Adjust exponent
		neg		B				//Negate
		clb		B,Y0			//Count leading bits
		asll.l	Y0,B			//rescale
		sub		Y0,Y1			//adjust exponent
		move.w	B1,Y0
		move.w	Y1,B
		move.w	Y0,B0
		
		move.w 	A1,Y1		 	//Put ffnum1 exponent (exp1) in Y1
		sub		B,Y1			//Y1 = exp1 - exp2
		
		
		//Setup: Larger ffnum exponent goes in Y0; mantissa to be shifted goes in B1;
		//mantissa to stay the same goes in A1; abs exp difference goes in Y1
		
		tlt		B,A				//Move ffnum2 (mantissa and exp) to A (not shifted) if Y1 neg
		tlt		X0,B			//Move ffnum1 mantissa to B1 for shifting if Y1 neg
		tge		Y0,B			//Move ffnum2 mantissa to B1 for shifting if Y1 not negative
		
		abs		Y1				//positive shift values
		
		cmp.w 	#15,Y1			//More than 15-bit shift (ASRAC only works to 15 bits)?
		jgt 	Neglect			//If yes, an input ffnum will go to zero if shifted	
		
		move.w	A1,Y0			//Move larger exp to Y0 for shifting
		move.w	A0,A			//Move mantissa A0 to A1 for adding
			
		asrac	B1,Y1,A			//Extend B1 to 36 bits, shift right by Y1, and add to A
		asr 	A				//Shift right to prevent overflow of CLB (next)
		
		clb 	A,X0			//Count sign bits
		asll.l 	X0,A			//Normalize
		
		tst.w	A1				//Check if relevant part of result is zero
		jeq		Zero			//Result is zero
		
		sub 	X0,Y0			//Adjust exponent of exp1
		inc.w	Y0				//Return to normal scale	
		
		clb 	Y0,X0			//check size of exponent word
		cmp.w	#8,X0	
		jlt		Exp_Err
		
	Continue:
		rnd		A				//Round to 16 bits	
		rtsd					//delayed return from subroutine
		move.w 	A,A0			//Move mantissa of sum to lower word of ffnum1 (return value)
		move.w 	Y0,A1			//Move exponent to upper word of ffnum1 (return value)
		sxt.l	A				//Sign-extend A to 36 bits
		//end of main add function
		
	Zero: 
		rtsd					//Delayed return from subroutine - will execute next three inst.
		move.w	#$FF80,A		//Set exp of sum to minimum
		clr.w	A0				//Set mantissa of sum to 0
		//end of zero handler			
		
	Exp_Err:
		cmp.w	#$007F,Y0		
		jle		Underflow		//If not overflow, go to underflow check	
		tst.w	A1				//Positive or negative overflow?
		jlt		NegO			//If negative, go to negative handler
		move.w	#$007F,A		//Max out exponent
		rtsd					//Delayed return from subroutine - will execute next three words
		move.w	#$7FFF,A0		//Max out mantissa
		nop						//filler for third delay slot
		//end	
	NegO:
		move.w	#$007F,A		//Max out exponent
		rtsd					//Delayed return from subroutine - will execute next three words
		move.w	#$8000,A0		//Most negative mantissa
		nop						//filler for third delay slot
		//end	
	Underflow:
		cmp.w	#$FF80,Y0		//Check for underflow
		jge		Continue		//Not an error
		tst.w	A1				//Positive or negative underflow?
		jlt		NegU			//If negative, go to negative handler
		move.w	#$FF80,A		//Minimum exponent
		rtsd					//Delayed return from subroutine - will execute next three inst.
		move.w	#$4000,A0		//Minimum normalized positive mantissa
		nop						//Filler for third delay slot
		//end	
	NegU:
		move.w	#$FF80,A		//Minimum exponent
		rtsd					//Delayed return from subroutine - will execute next three inst.
		move.w	#$BFFF,A0		//Minimum (abs) normalized negative mantissa
		nop						//filler for third delay slot
		//end of E_Err	
		
	Neglect:
		rts						//The input with the larger exp becomes the output
}

asm ffloat IEEE2FFloat(register float fnum)
{
	bftstl	#$7F80,A1
	jcs		Zero			//For IEEE, zero is indicated by zero exp.
	
	move.w	A1,Y0
	bfclr	#$FF00,A1
	sxt.l	A				//Sign-extend A to 36 bits
	bfset	#$0080,A1
	brclr	#$8000,Y0,L1	//Branch if sign bit is positive
	neg		A				//Negate mantissa if sign bit is negative
L1:
	clb		A,X0			//Normalize mantissa
	asll.l	X0,A
	
	bfclr	#$807F,Y0
	lsrr.w	#7,Y0
	sub.w	#119,Y0
	sub		X0,Y0			//FFloat exponent is ready
	clb		Y0,X0			//Check for overflow/underflow
	cmp.w	#8,X0
	jlt		Exp_Err
Continue:
	rnd		A
	rtsd
	move.w	A,A0
	move.w	Y0,A1
	sxt.l	A				//Sign-extend A to 36 bits
	//end

	
Zero:
	RTSD
	MOVE.W	#$FF80,A
	CLR.W	A0
	//END
	
Exp_Err:
	cmp.w	#$007F,Y0		
	jle		Underflow		//If not overflow, go to underflow check	
	tst.w	A1				//Positive or negative overflow?
	jlt		NegO			//If negative, go to negative handler
	move.w	#$007F,A		//Max out exponent
	rtsd					//Delayed return from subroutine - will execute next three words
	move.w	#$7FFF,A0		//Max out mantissa
	nop						//filler for third delay slot
	//end	
NegO:
	move.w	#$007F,A		//Max out exponent
	rtsd					//Delayed return from subroutine - will execute next three words
	move.w	#$8000,A0		//Most negative mantissa
	nop						//filler for third delay slot
	//end	
Underflow:
	cmp.w	#$FF80,Y0		//Check for underflow
	jge		Continue		//Not an error
	tst.w	A1				//Positive or negative underflow?
	jlt		NegU
	move.w	#$FF80,A		//Minimum exponent
	rtsd					//Delayed return from subroutine - will execute next three words
	move.w	#$4000,A0		//Minimum normalized positive mantissa
	nop						//Filler for third delay slot
	//end	
NegU:
	move.w	#$FF80,A		//Minimum exponent
	rtsd					//Delayed return from subroutine - will execute next three words
	move.w	#$BFFF,A0		//Minimum (abs) normalized negative mantissa
	nop						//filler for third delay slot
	//end of E_Err
	
}

//A not very good C function. Ok for testing other functions in simulation.
//Converts an FFloat number to an IEEE 754-compatible single precision floating point number.

//typedef long unsigned int ffloat;

float FFloat2IEEE(ffloat ffnum)
{
	float fout = 0;
	long int iexp = 0;
	long unsigned int tempout = 0, sign = 0, mantissa = 0, exp = 0;
	void *VoidPointer;
	float *FloatPointer;
	long unsigned int *LintPointer;
		
	if (ffnum&0xFFFF)	//ffnum is not zero
	{
		mantissa = ffnum & 0x0000FFFF;
		
		exp = ffnum&0xFFFF0000;
		iexp = (long int)exp;
		
		iexp += 0x007F0000;		//Bias exponent positive by 127
	
		if (iexp < 0x00010000)		//Limit exponent size to allowed IEEE range
		{
			iexp = 0x00010000;
		}
		else if (iexp > 0x00FE0000)
		{
			iexp = 0x00FE0000;
		}
	
	
	
		if (mantissa&0x00008000)		//ffnum is negative
		{
			sign = 0x80000000;
			mantissa ^= 0x0000FFFF;	//Negate
			mantissa++;
		}
		
		while (!(mantissa&0x8000))		//normalize
		{
			mantissa <<= 1;
			iexp -= 0x00010000;
		}
	

	if (iexp < 0x00010000)		//Limit exponent size to allowed IEEE range
		{
			iexp = 0x00010000;
		}
		else if (iexp > 0x00FE0000)
		{
			iexp = 0x00FE0000;
		}
		
		exp = (long unsigned int)iexp;

		exp <<= 7;				//Shift exponent to correct position
	
		mantissa <<= 8;			//Shift to correct IEEE position
		mantissa &= 0x007FFFFF; //Clear leading one
	
		tempout = sign | exp | mantissa;
	
		
	}
	else exp = 0x00000000;			//zero

	VoidPointer = &(tempout);		//obtain pointer to unsigned long int tempout
	FloatPointer = VoidPointer;		//convert to float
	fout = *FloatPointer;
	return(fout);			
	
}

//return true if ffnum1>ffnum2, false otherwise
asm bool FFgt(register ffloat ffnum1, register ffloat ffnum2) 
{
	//First compare signs of numbers
	tst.w	A0
	blt		CheckSignANeg
	
	//a is nonnegative
	tst.w	B0
	//Both numbers are nonnegative - nonnegative exponents case
	bge		CasePNumExp
	//If b is negative, a>b
	rtsd
	move.w	#1,Y0
	nop
	nop

//a is negative
CheckSignANeg:
	tst.w	B0
	//Both numbers are negative - negative exponents case
	blt		CaseNNumExp
	//If b is nonnegative, a<b
	rtsd
	move.w	#0,Y0
	nop
	nop

//If a and b are positive, go here
//larger exponent = larger #
CasePNumExp:
	//move exponent data to X0 and Y0 registers for comparison
	move.w	A1,X0
	move.w	B1,Y0
	cmp.w	X0,Y0
	blt		aGTb		//if(expB<expA) then a>b
	bgt		aNotGTb		//if(expB>expA) then !(a>b)

	//If exponents are equal, check mantissas
	move.w	A0,X0
	move.w	B0,Y0	
	cmp.w	X0,Y0
	blt		aGTb		//if(mantissaB<mantissaA) then a>b
	rtsd
	move.w	#0,Y0
	nop
	nop
	
//If a and b are negative, go here
//larger exponent = smaller #
CaseNNumExp:
	//move exponent data to X0 and Y0 registers for comparison
	move.w	A1,X0
	move.w	B1,Y0
	cmp.w	X0,Y0
	bgt		aGTb		//if(expB>expA) then a>b
	blt		aNotGTb		//if(expB<expA) then !(a>b)
	
	//If exponents are equal, check mantissas
	move.w	A0,X0
	move.w	B0,Y0	
	cmp.w	X0,Y0
	blt		aGTb		//if(mantissaB<mantissaA) then a>b
	rtsd
	move.w	#0,Y0
	nop
	nop
	
//if a>b, go here
aGTb:
	rtsd
	move.w	#1,Y0
	nop
	nop

//if a<=b, go here
aNotGTb:
	rtsd
	move.w	#0,Y0
	nop
	nop
}

//return true if ffnum>0, false otherwise
asm bool FFgtz(register ffloat ffnum) 
{
	//Test ffnum mantissa
	tst.w	A0
	bgt		Positive
	
	//ffnum <= 0
	rtsd			//delayed return
	clr.w	Y0		//return value 0
	nop				//first filler instruction
	nop				//second filler instruction
	//end

Positive:
	//ffnum > 0
	rtsd			//delayed return
	move.w	#1,Y0	//return value 1
	nop				//first filler instruction
	nop				//second filler instruction
	//end
}

//return true if ffnum<0, false otherwise
asm bool FFltz(register ffloat ffnum) 
{
	//Test ffnum mantissa
	tst.w	A0
	blt		Negative
	
	//ffnum >= 0
	rtsd			//delayed return
	clr.w	Y0		//return value 0
	nop				//first filler instruction
	nop				//second filler instruction
	//end

Negative:
	//ffnum < 0
	rtsd			//delayed return
	move.w	#1,Y0	//return value 1
	nop				//first filler instruction
	nop				//second filler instruction
	//end
}

//return true if ffnum=0, false otherwise
asm bool FFeqz(register ffloat ffnum) 
{
	//Test ffnum mantissa
	tst.w	A0
	beq		Zero
	
	//ffnum != 0
	rtsd			//delayed return
	clr.w	Y0		//return value 0
	nop				//first filler instruction
	nop				//second filler instruction
	//end

Zero:
	//ffnum < 0
	rtsd			//delayed return
	move.w	#1,Y0	//return value 1
	nop				//first filler instruction
	nop				//second filler instruction
	//end
}

//return true if ffnum1<ffnum2, false otherwise
asm bool FFlt(register ffloat ffnum1, register ffloat ffnum2) 
{
	//First compare signs of numbers
	tst.w	A0
	blt		CheckSignANeg
	
	//a is nonnegative
	tst.w	B0
	//Both numbers are nonnegative - nonnegative exponents case
	bge		CasePNumExp
	//If b is negative, !(a<b)
	rtsd
	move.w	#0,Y0
	nop
	nop

//a is negative
CheckSignANeg:
	tst.w	B0
	//Both numbers are negative - negative exponents case
	blt		CaseNNumExp
	//If b is nonnegative, a<b
	rtsd
	move.w	#1,Y0
	nop
	nop

//If a and b are positive, go here
//larger exponent = larger #
CasePNumExp:
	//move exponent data to X0 and Y0 registers for comparison
	move.w	A1,X0
	move.w	B1,Y0
	cmp.w	X0,Y0
	bgt		aLTb		//if(expB>expA) then a<b
	blt		aNotLTb		//if(expB<expA) then !(a<b)

	//If exponents are equal, check mantissas
	move.w	A0,X0
	move.w	B0,Y0	
	cmp.w	X0,Y0
	bgt		aLTb		//if(mantissaB>mantissaA) then a<b
	rtsd
	move.w	#0,Y0
	nop
	nop
	
//If a and b are negative, go here
//larger exponent = smaller #
CaseNNumExp:
	//move exponent data to X0 and Y0 registers for comparison
	move.w	A1,X0
	move.w	B1,Y0
	cmp.w	X0,Y0
	blt		aLTb		//if(expB<expA) then a<b
	bgt		aNotLTb		//if(expB>expA) then !(a<b)
	
	//If exponents are equal, check mantissas
	move.w	A0,X0
	move.w	B0,Y0	
	cmp.w	X0,Y0
	bgt		aLTb		//if(mantissaB>mantissaA) then a<b
	rtsd
	move.w	#0,Y0
	nop
	nop
	
//if a<b, go here
aLTb:
	rtsd
	move.w	#1,Y0
	nop
	nop

//if a>=b, go here
aNotLTb:
	rtsd
	move.w	#0,Y0
	nop
	nop
}

//return true if a>=b, false otherwise
asm bool FFgte(register ffloat a, register ffloat b) 
{	
	//First compare signs of numbers
	tst.w	A0
	blt		CheckSignANeg
	
	//a is nonnegative
	tst.w	B0
	//Both numbers are nonnegative - nonnegative exponents case
	bge		CasePNumExp
	//If b is negative, a>=b
	rtsd
	move.w	#1,Y0
	nop
	nop

//a is negative
CheckSignANeg:
	tst.w	B0
	//Both numbers are negative - negative exponents case
	blt		CaseNNumExp
	//If b is nonnegative, a<b
	rtsd
	move.w	#0,Y0
	nop
	nop

//If a and b are positive, go here
//larger exponent = larger #
CasePNumExp:
	//move exponent data to X0 and Y0 registers for comparison
	move.w	A1,X0
	move.w	B1,Y0
	cmp.w	X0,Y0
	blt		aGTEb		//if(expB<expA) then a>=b
	bgt		aNotGTEb	//if(expB>expA) then !(a>=b)

	//If exponents are equal, check mantissas
	move.w	A0,X0
	move.w	B0,Y0	
	cmp.w	X0,Y0
	ble		aGTEb		//if(mantissaB<=mantissaA) then a>=b
	rtsd
	move.w	#0,Y0
	nop
	nop	
	
//If a and b are negative, go here
//larger exponent = smaller #
CaseNNumExp:
	//move exponent data to X0 and Y0 registers for comparison
	move.w	A1,X0
	move.w	B1,Y0
	cmp.w	X0,Y0
	bgt		aGTEb		//if(expB>expA) then a>b
	blt		aNotGTEb	//if(expB<expA) then !(a>b)
	
	//If exponents are equal, check mantissas
	move.w	A0,X0
	move.w	B0,Y0	
	cmp.w	X0,Y0
	ble		aGTEb		//if(mantissaB<=mantissaA) then a>=b
	rtsd
	move.w	#0,Y0
	nop
	nop
	
//if a>=b, go here
aGTEb:
	rtsd
	move.w	#1,Y0
	nop
	nop

//if a<b, go here
aNotGTEb:
	rtsd
	move.w	#0,Y0
	nop
	nop
}

//return true if a<=b, false otherwise
asm bool FFlte(register ffloat a, register ffloat b) 
{	
	//First compare signs of numbers
	tst.w	A0
	blt		CheckSignANeg
	
	//a is nonnegative
	tst.w	B0
	//Both numbers are nonnegative - nonnegative exponents case
	bge		CasePNumExp
	//If b is negative, !(a<=b)
	rtsd
	move.w	#0,Y0
	nop
	nop

//a is negative
CheckSignANeg:
	tst.w	B0
	//Both numbers are negative - negative exponents case
	blt		CaseNNumExp
	//If b is nonnegative, a<b
	rtsd
	move.w	#1,Y0
	nop
	nop

//If a and b are positive, go here
//larger exponent = larger #
CasePNumExp:
	//move exponent data to X0 and Y0 registers for comparison
	move.w	A1,X0
	move.w	B1,Y0
	cmp.w	X0,Y0
	bgt		aLTEb		//if(expB>expA) then a<=b
	blt		aNotLTEb	//if(expB>expA) then !(a<=b)

	//If exponents are equal, check mantissas
	move.w	A0,X0
	move.w	B0,Y0	
	cmp.w	X0,Y0
	bge		aLTEb		//if(mantissaB>=mantissaA) then a>=b
	rtsd
	move.w	#0,Y0
	nop
	nop	
	
//If a and b are negative, go here
//larger exponent = smaller #
CaseNNumExp:
	//move exponent data to X0 and Y0 registers for comparison
	move.w	A1,X0
	move.w	B1,Y0
	cmp.w	X0,Y0
	blt		aLTEb		//if(expB<expA) then a<=b
	bgt		aNotLTEb	//if(expB>expA) then !(a<=b)
	
	//If exponents are equal, check mantissas
	move.w	A0,X0
	move.w	B0,Y0	
	cmp.w	X0,Y0
	bge		aLTEb		//if(mantissaB>=mantissaA) then a>=b
	rtsd
	move.w	#0,Y0
	nop
	nop
	
//if a<=b, go here
aLTEb:
	rtsd
	move.w	#1,Y0
	nop
	nop

//if a>b, go here
aNotLTEb:
	rtsd
	move.w	#0,Y0
	nop
	nop
}

ffloat FFsin(ffloat xin)
{
    int k,klo,khi;
    ffloat xdiff0, xdiff1;
    ffloat x=xin;
static ffloat xlo = 0x00029b78;
static ffloat xhi = 0x00026487;
static ffloat ya[31] = {0xffccb968, 0xfffe958c, 0xffff97e0, 0x0000b4c3, 0x0000a0e0, 0x00009126, 0x00008643, 0x000080b3, 0x000080b3, 0x00008643, 0x00009126, 0x0000a0e0, 0x0000b4c3, 0xffff97e0, 0xfffe958c, 0xff800000, 0xfffe6a73, 0xffff681f, 0x00004b3c, 0x00005f1f, 0x00006ed9, 0x000079bc, 0x00007f4c, 0x00007f4c, 0x000079bc, 0x00006ed9, 0x00005f1f, 0x00004b3c, 0xffff681f, 0xfffe6a73, 0xffcc4698};
static ffloat y2a[31] = {0xff800000, 0xfffd6a0f, 0xfffe67be, 0xffff4af6, 0xffff5ec6, 0xffff6e72, 0xffff794a, 0xffff7ed5, 0xffff7ed5, 0xffff794a, 0xffff6e72, 0xffff5ec6, 0xffff4af6, 0xfffe67be, 0xfffd6a0f, 0xff800000, 0xfffd95f0, 0xfffe9841, 0xffffb509, 0xffffa139, 0xffff918d, 0xffff86b5, 0xffff812a, 0xffff812a, 0xffff86b5, 0xffff918d, 0xffffa139, 0xffffb509, 0xfffe9841, 0xfffd95f0, 0xfffd95f0};
static int numpoints = 31;
static ffloat h = 0xfffe6b3b;
static ffloat hinv = 0x00034c64;
static ffloat pi2=0x00036487;
static ffloat pi2inv=0xfffe517c;
if(FFlt(xin,xlo)){
    x=FFadd(
     xin,
     FFmult(
     S16int2FFloat(
     FFloatTrunc2S16int(
            FFmult(
            FFsub(xhi,xin),
            pi2inv
           )
           )
    ),
     pi2
    )
    );
}else if(FFgt(xin,xhi)){
    x=FFsub(
     xin,
     FFmult(
     S16int2FFloat(
     FFloatTrunc2S16int(
            FFmult(
            FFsub(xin,xlo),
            pi2inv
           )
           )
    ),
     pi2
    )
    );
}
    klo = FFloatTrunc2S16int(FFmult(FFsub(x,xlo),hinv));
    khi=klo+1;
    xdiff0 = FFsub(x, FFadd(xlo, FFmult(h,S16int2FFloat(klo))));
    xdiff1 = FFsub(xdiff0, h);
    return ( FFadd(ya[klo], FFadd(FFmult(FFmult(FFsub(ya[khi],ya[klo]), hinv), xdiff0), FFmult(FFmult(y2a[khi], xdiff0), xdiff1))) );
}

GPIO library

Niels Tjørnhøj-Thomsen March 23, 2013 Coded in C for the NXP LPC17xx series
//---------------------------------------------------------
// start of header file (gpio.h)
//---------------------------------------------------------

#if !defined(_GPIO_H_)
#define _GPIO_H_

typedef unsigned long dword;

typedef struct tGPIOpinTag
{
  dword dwReg;
  dword dwClr;
  dword dwSet;
  int iBit;
  dword dwFunction;
} tGPIOpin;

#define REG32                volatile dword *

//
// bit patterns for alternate pin functions.
//
#define PINFUNC_GPIO         ( 0x00000000 )
#define PINFUNC_ALT1         ( 0x55555555 )
#define PINFUNC_ALT2         ( 0xAAAAAAAA )
#define PINFUNC_ALT3         ( 0xFFFFFFFF )

//
// Active high and low works by swapping the SET and CLR register offsets round
//
#define GPIO_ACTIVE_L        FIO_SET_OFS, FIO_CLR_OFS
#define GPIO_ACTIVE_H        FIO_CLR_OFS, FIO_SET_OFS

//
// Start addresses of GPIO blocks of registers
//
#define LPC_GPIO_BASE        ( 0x2009C000UL )
#define LPC_GPIO0_BASE       ( LPC_GPIO_BASE + 0x00000 )
#define LPC_GPIO1_BASE       ( LPC_GPIO_BASE + 0x00020 )
#define LPC_GPIO2_BASE       ( LPC_GPIO_BASE + 0x00040 )
#define LPC_GPIO3_BASE       ( LPC_GPIO_BASE + 0x00060 )
#define LPC_GPIO4_BASE       ( LPC_GPIO_BASE + 0x00080 )

//
// Offsets to the direction register and PIN register
//
#define FIO_SET_OFS          ( 0x00000018 )
#define FIO_CLR_OFS          ( 0x0000001C )
#define FIO_DIR_OFS          ( 0x00000000 )
#define FIO_PIN_OFS          ( 0x00000014 )
#define FIO_MASK_OFS         ( 0x00000010 )

// Parameters for vGPIODDsetDirection
#define   PIN_IN             ( FALSE )
#define   PIN_OUT            ( TRUE )

//
// Macro for pin definition structure
//
#ifdef _INSIDE_GPIODD_
//
// this version is for use inside the gpio module
//
#define PINFUNC_DEF( name, port, bit, act, func )   \
  const tGPIOpin t##name = {                        \
    LPC_GPIO##port##_BASE,                          \
    GPIO_ACTIVE_##act,                              \
    bit,                                            \
    PINFUNC_##func                                  \
  };                                                \
  const tGPIOpin * const name = &t##name;
#else
//
// and this is how external modules see the pin definition
//
#define PINFUNC_DEF( name, port, bit, act, func )   \
  extern const tGPIOpin * const name;
#endif

//
// include the hardware pin allocations from another file
// (see example below)
//
#include "gpio_pindefs.h"

extern void vGPIODDsetPinFunction( const tGPIOpin * const psPin );
extern void vGPIODDsetPinDirection( const tGPIOpin * const psPin, 
                                    const bool boOutput );
extern void vGPIODDconfigurePin( const tGPIOpin * const psPin, 
                                 const bool boOutput );
extern void vGPIODDsetActive( const tGPIOpin * const psPin );
extern void vGPIODDsetInactive( const tGPIOpin * const psPin );
extern bool boGPIODDgetPin( const tGPIOpin * const psPin );

#endif // _GPIO_H_

//---------------------------------------------------------
// end of header file (gpio.h)
//---------------------------------------------------------

//---------------------------------------------------------
// start of example gpio_pindefs.h
//---------------------------------------------------------
 
#if !defined(_GPIO_PINDEFS_H_)
#define _GPIO_PINDEFS_H_

// LEDs
PINFUNC_DEF( GPIO_LED_GREEN,          1, 25, L, GPIO )

// USB interface
PINFUNC_DEF( GPIO_USB_VBUS,           1, 30, H, ALT1 )
PINFUNC_DEF( GPIO_USB_CONNECT,        2,  9, L, ALT1 )

// Serial Ports
PINFUNC_DEF( UART0_RX,                0,  3, H, ALT1 )
PINFUNC_DEF( UART0_TX,                0,  2, H, ALT1 )

PINFUNC_DEF( UART1_RX,                0, 16, H, ALT1 )
PINFUNC_DEF( UART1_TX,                0, 15, H, ALT1 )

// SPI port
PINFUNC_DEF( SPI_MOSI,                0,  9, H, ALT2 )
PINFUNC_DEF( SPI_MISO,                0,  8, H, ALT2 )
PINFUNC_DEF( SPI_SCK,                 0,  7, H, ALT2 )
PINFUNC_DEF( SPI_SSEL,                0,  6, L, ALT2 )

#endif // _GPIO_PINDEFS_H_

//---------------------------------------------------------
// end of example gpio_pindefs.h
//---------------------------------------------------------

//---------------------------------------------------------
// start of gpio.c
//---------------------------------------------------------

#define   _INSIDE_GPIODD_             ( 1 )
#include "gpio.h"

/* --------------------------------------------------------
   vGPIODDsetPinFunction
   ........................................................
   Description  : Set the pin connect block from a pin 
                  definition 

   Params : psPin - pin definition

   Returns : Nothing
   ----------------------------------------------------- */

void vGPIODDsetPinFunction( const tGPIOpin * const psPin )
{
  if ( psPin )
  {
    // Each PINSELXX register contains the settings 
    // for 16 port pins.  Each PINSEL register is 4 bytes 
    // above the previous one.  Base addresses for ports 
    // are 32 bytes apart.
    dword dwPinSel = LPC_PINCON_BASE + 
                     ((psPin->dwReg - LPC_GPIO0_BASE) / 4) + 
                     ((psPin->iBit / 16) * 4);
    dword dwMask = ( 0x00000003 << ( 2 * ( psPin->iBit % 16 ) ) );
    REG32 pdwPinSel = (REG32)dwPinSel;

    *pdwPinSel = ( *pdwPinSel & ~dwMask ) | 
                 ( psPin->dwFunction & dwMask );
  }
}

/* --------------------------------------------------------
   vGPIODDsetPinDirection
   ........................................................
   Description  : Sets the pin direction from a pin 
                  definition

   Params : psPin - pin definition
            boOutput - set to TRUE to turn the pin into an 
                       output

   Returns : Nothing
   ----------------------------------------------------- */

void vGPIODDsetPinDirection( const tGPIOpin * const psPin, 
                             const bool boOutput )
{
  if ( psPin )
  {
    if( boOutput )
    {
      (*((REG32) (psPin->dwReg + FIO_DIR_OFS))) |= ( 1 << psPin->iBit );
    }
    else
    {
      (*((REG32) (psPin->dwReg + FIO_DIR_OFS))) &= ~( 1 << psPin->iBit );
    }
  }
}

/* --------------------------------------------------------
   vGPIODDconfigurePin
   ........................................................
   Description  : Combination function to configure and set 
                  direction of GPIO pin

   Params : psPin - pin definition
            boOutput - set to TRUE to turn the pin into an 
                       output

   Returns : Nothing
   ----------------------------------------------------- */

void vGPIODDconfigurePin( const tGPIOpin * const psPin, 
                          const bool boOutput )
{
  if ( psPin )
  {
    vGPIODDsetInactive( psPin );
    vGPIODDsetPinFunction( psPin );
    vGPIODDsetPinDirection( psPin, boOutput );
  }
}

/* --------------------------------------------------------
   vGPIODDsetActive
   ........................................................
   Description  : Sets a pin to its active state

   Params : psPin - pin definition

   Returns : Nothing
   ----------------------------------------------------- */

void vGPIODDsetActive( const tGPIOpin * const psPin )
{
  if ( psPin )
  {
    // use the Set register to set a single bit
    (*((REG32) (psPin->dwReg + psPin->dwSet))) = ( 1 << psPin->iBit );
  }
}

/* --------------------------------------------------------
   vGPIODDsetInactive
   ........................................................
   Description  : Sets a pin to its inactive state

   Params : psPin - pin definition

   Returns : Nothing
   ----------------------------------------------------- */

void vGPIODDsetInactive( const tGPIOpin * const psPin )
{
  if ( psPin )
  {
    // use the Clr register to clear a single bit
    (*((REG32) (psPin->dwReg + psPin->dwClr))) = ( 1 << psPin->iBit );
  }
}

/* --------------------------------------------------------
   boGPIODDgetPin
   ........................................................
   Description  : Gets the current state of a pin

   Params : psPin - pin definition

   Returns : TRUE if the pin is in its active state, 
             FALSE otherwise.
   ----------------------------------------------------- */

bool boGPIODDgetPin( const tGPIOpin * const psPin )
{
  if ( psPin )
  {
    dword dwPins = *((REG32) (psPin->dwReg + FIO_PIN_OFS));

    if ( psPin->dwSet > psPin->dwClr )
    {
      dwPins = ~dwPins;
    }

    return ((dwPins & ( 1 << psPin->iBit )) != 0 );
  }
  else
  {
    return FALSE;
  }
}

//---------------------------------------------------------
// end of gpio.c
//---------------------------------------------------------

MSP430 SPI-Master IO without interrupt

March 23, 2013 Coded in C++ for the TI MSP430
/**
 * @file SPI.h
 * Control's for the SPI
*/

#ifndef FILE_SPI_H
#define FILE_SPI_H

class SPI
{
public:

	/**
	 * @brief   Init for SPI
	 * @details
	 * @param   void
	 * @return  void
	 */
	static void _Init(void);

	/**
	 * @brief   Send data to Display
	 * @details
	 * @param   unsigned char msg - Message send to Display
	 * @return  void
	 */
	static void LCD_DataOut(unsigned char msg);

	/**
	 * @brief   Send data to Display
	 * @details Display mode must be set
	 * @param   char* msg - Message send to Display
	 * @param   int Length - Length of message send to Display
	 * @return  void
	 */
	static void LCD_DataOut(char *msg, int Length);

	/**
	 * @brief   Send command to Display
	 * @details
	 * @param   unsigned char msg - Message send to Display
	 * @return  void
	 */
	static void LCD_CommOut(unsigned char msg);

private:
	typedef enum {ModeIdle, ModeLCD, ModeMem} SPI_Modes;
	static SPI_Modes SPI_Mode;
	/**
	 * It configures SPI in a mode to communicate to the selected device
	 * @param Mode This defines the mode to configure.
	 */
	static void ConfigSPI(SPI_Modes Mode);

	/**
	 * It performs data exchange via SPI for a given number of inputs and outputs
	 * @param TX	pointer to transmit buffer or 0. if 0 then a dummy byte is sent (0x81)
	 * @param RX	pointer to receive buffer or 0
	 * @param IOcount total number of receive and transmit bytes
	 */
	static void DoSPI_IO(char* TX, char* RX, int IOcount);

};

#############################################

/**
 * @file SPI.c
 * Control's for the SPI
*/

#include "SPI.h"
#include "msp430.h"

#define ArrayLength(array) (sizeof(array)/sizeof(array[0]))
/// Macro to set a bit y in variable x
#define SETB(x,y)       (x |= (1 << y))
/// Macro to reset a bit y in variable x
#define CLRB(x,y)       (x &= ~(1 << y))

	SPI::SPI_Modes SPI::SPI_Mode;

	/**
	 * @brief   Init for SPI
	 * @details
	 * @param   void
	 * @return  void
	 */
	void SPI::_Init(void)
	{
		SPI_Mode = ModeIdle;
	}

	/**
	 * @brief   Send data to Display
	 * @details Display mode must be set
	 * @param   unsigned char msg - Message send to Display
	 * @return  void
	 */
	void SPI::LCD_DataOut(unsigned char msg) //Data Output Serial Interface
	{
		ConfigSPI(ModeLCD);
		CLRB(P3OUT,4);			//Chip Select = Active, CS = 0
		SETB(P2OUT,7);				//A0 = Data, A0 = 1
	    /* Software delay for selection line to settle */
	    __delay_cycles(25);

		while (UCBUSY & UCB0STAT);	// Wait until SPI is no longer busy
		UCB0TXBUF = msg;				// Transmit Message
		while (UCBUSY & UCB0STAT);  // Wait until SPI is no longer busy

		SETB(P3OUT,4);				//after 1 byte, Chip Select = inactive, CS =1
	}

	/**
	 * @brief   Send data to Display
	 * @details Display mode must be set
	 * @param   char* msg - Message send to Display
	 * @param   int Length - Length of message send to Display
	 * @return  void
	 */
	void SPI::LCD_DataOut(char *msg, int Length) //Data Output Serial Interface
	{
		ConfigSPI(ModeLCD);
		CLRB(P3OUT,4);			//Chip Select = Active, CS = 0
		SETB(P2OUT,7);				//A0 = Data, A0 = 1
	    /* Software delay for selection line to settle */
	    __delay_cycles(25);
		DoSPI_IO(msg, 0, Length);
		SETB(P3OUT,4);				//after 1 byte, Chip Select = inactive, CS =1
	}

	/**
	 * @brief   Send command to Display
	 * @details Display mode must be set
	 * @param   unsigned char msg - Message send to Display
	 * @return  void
	 */
	void SPI::LCD_CommOut(unsigned char msg) //Command Output Serial Interface
	{
		ConfigSPI(ModeLCD);
		CLRB(P3OUT,4);				//Chip Select = Active, CS = 0
		CLRB(P2OUT,7);				//A0 = Command, A0 = 0
	    /* Software delay for selection line to settle */
	    __delay_cycles(25);

		while (UCBUSY & UCB0STAT);	// Wait until SPI is no longer busy
		UCB0TXBUF = msg;				// Transmit Message
		while (UCBUSY & UCB0STAT);	// Wait until SPI is no longer busy

		SETB(P3OUT,4);				//after 1 byte, Chip Select = inactive, CS = 1

	}
	/**
	 * It configures SPI in a mode to communicate to the selected device
	 * @param Mode This defines the mode to configure.
	 */
	void SPI::ConfigSPI(SPI_Modes Mode)
	{
		if(SPI_Mode == Mode)
			return;
		switch(Mode)
		{
		case ModeLCD:
			/*******************************************************
			 * 			USCI - SPI configuration for LCD				   *
			 *******************************************************/

		    UCB0CTL1 = UCSWRST;                      	// **Reset USCI state machine**
			// UCMST = Master Mode Selected
			// UCCKPH = Data is captured on the first UCLK edge and changed on the following edge.
			// ~UCCKPL = The inactive state is low.
			// UCMSB = MSB first
			// UCSYNC = Synchronous mode
			// 8 bit data, 3 pin SPI
			UCB0CTL0 = UCMST+UCCKPH+UCMSB+UCSYNC;

			UCB0CTL1 |= UCSSEL_2;                     // SMCLK clock source
		    UCB0BR0 |= 0x03;                          // SPI Clk = SMCLK / 3 = 1 MHz
		    UCB0BR1 = 0;
		    UCB0CTL1 &= ~UCSWRST;                     // **Initialize USCI state machine**
		    break;

		case ModeMem:
			/*******************************************************
			 * 			USCI - SPI configuration for Memory				   *
			 *******************************************************/

		    UCB0CTL1 = UCSWRST;                      	// **Reset USCI state machine**
			// UCMST = Master Mode Selected
			// UCCKPH = Data is captured on the first UCLK edge and changed on the following edge.
			// ~UCCKPL = The inactive state is low.
			// UCMSB = MSB first
			// UCSYNC = Synchronous mode
			// 8 bit data, 3 pin SPI
			UCB0CTL0 = UCMST+UCCKPH+UCMSB+UCSYNC;

			UCB0CTL1 |= UCSSEL_2;                     // SMCLK clock source
		    UCB0BR0 |= 0x01;                          // SPI Clk = SMCLK / 1 = 3 MHz
		    UCB0BR1 = 0;
		    UCB0CTL1 &= ~UCSWRST;                     // **Initialize USCI state machine**
		    break;

		case ModeIdle:
		default:
			Mode = ModeIdle;
			break;
		}
		SPI_Mode = Mode;
	}

	/**
	 * It performs data exchange via SPI for a given number of inputs and outputs
	 * @param TX	pointer to transmit buffer or 0. if 0 then a dummy byte is sent (0x81)
	 * @param RX	pointer to receive buffer or 0
	 * @param IOcount total number of receive and transmit bytes
	 */
	void SPI::DoSPI_IO(char* TX, char* RX, int IOcount)
	{
		volatile int dummy = 0x81;
		if((TX == 0) && (RX == 0)) return;

		while(IOcount > 0)
		{
			if(UCB0IFG & UCTXIFG)
			{
				if(TX == 0)
					UCB0TXBUF = dummy;	// dummy write
				else
				{
					UCB0TXBUF = *TX++;
					IOcount--;
				}
			}
			if(UCB0IFG & UCRXIFG)
			{
				if(RX == 0)
					dummy = UCB0RXBUF;	// dummy read
				else
				{
					*RX++ = UCB0RXBUF;
					IOcount--;
				}
			}
		}
		while (UCBUSY & UCB0STAT);	// Wait until SPI is no longer busy
		dummy = UCB0RXBUF;	// dummy read to empty any remaining data in RX buffer
	}

high resolution frequency counter implementation

Fabien Le Mentec March 23, 2013 Coded in C for the atmega328p
/* high resolution frequency counter implementation
*/

/* timer2 interrupt handler. timer1 is an extended
32 bits register (16 bits hard + 16 softs)
incremented once per:
1 / (fcpu / prescal) <=> prescal / fcpu
thus, it will overflow at:
2^16 * prescal / fcpu
on tim2 overflow, the interrupt handler is called
and stores the tim1 current value in tim1_cur_counter.
thus, the tim1 value integrated over the whole
tim1 period is:
(tim1_ovf_counter * 2^16) + tim1_cur_counter.
tim2_is_ovf is set to notify the application.
*/

static volatile uint8_t tim2_ovf_counter;
static volatile uint8_t tim2_is_ovf;
static volatile uint16_t tim1_cur_counter;

ISR(TIMER2_OVF_vect)
{
  if ((tim2_ovf_counter--) == 0)
  {
    /* disable tim1 before reading */
    TCCR1B = 0;
    tim1_cur_counter = TCNT1;

    /* disable tim2 */
    TCCR2B = 0;

    tim2_is_ovf = 1;
  }
}

/* timer2 interrupt handler. timer2 is a 8 bits counter
incremented by the input signal rising edges. since
8 bits are not enough to integrate, an auxiliary
register (tim2_ovf_counter) is updated on overflow.
tim2_ovf_counter is an 8 bits register, and will
overflow without any notice past 0xff.
*/

static volatile uint8_t tim1_ovf_counter;

ISR(TIMER1_OVF_vect)
{
  ++tim1_ovf_counter;
}

static void hfc_start(void)
{
  /* resolution: 1.907349 hz per tick */
  /* fmax: 500 khz */
  /* acquisition time: 0.524288 seconds */

  /* disable interrupts */
  TIMSK1 = 0;
  TIMSK2 = 0;

  /* reset stuff */
  tim1_ovf_counter = 0;
  tim1_cur_counter = 0;
  tim2_is_ovf = 0;

  /* 0x100 overflows make 16 bits */
  tim2_ovf_counter = 0xff;

  /* configure tim2
normal operation
prescaler 128
enable interrupt on overflow
*/
  TCNT2 = 0;
  TIMSK2 = 1 << 0;
  TCCR2A = 0;
  TCCR2B = 0;

  /* configure tim1
t1 pin (pd5) rising edge as external clock
*/
  DDRD &= ~(1 << 5);
  TCNT1 = 0;
  TIMSK1 = 1 << 0;
  TCCR1A = 0;
  TCCR1B = 0;

  /* start tim1, tim2 */
  TCCR1B = 7 << 0;
  TCCR2B = 5 << 0;
}

static uint8_t hfc_poll(void)
{
  return tim2_is_ovf;
}

static uint32_t hfc_wait(void)
{
  /* busy wait for tim1 to overflow. returns the resulting
16 bits counter, to be multiplied by the frequency
resolution (refer to hfc_start) to get the actual
frequency.
*/

  /* force inline, do not use hfc_poll */
  while (tim2_is_ovf == 0) ;

  return ((uint32_t)tim1_ovf_counter << 16) | (uint32_t)tim1_cur_counter;
}

static inline uint32_t hfc_start_wait(void)
{
  hfc_start();
  return hfc_wait();
}

static inline double hfc_to_hz(uint32_t counter)
{
  return 1.907349 * (double)counter;
}

Playing sound in DAC using simple 8051 MCU

March 23, 20134 comments Coded in C for the SiliconLabs 8051
/*System reads wav files by SPI from external memory . Samples read are sent to external DAC by other SPI to be played. It's nedeed ISR reception (not included) of SPI to manage info of this file. At the beginnig, this timing is not important, but after all info of wav file is read, timming is fitted to sampling time of wav to play.

This Timer ISR manages read samples by SPI_Memory and write in SPI_DAC in 8051 MCU. Because this MCU has only one SPI,the info transmited to DAC is managed by simple GPIOs */
void Timer2_ISR (void) interrupt 5
{     
   char desp=0;
   char temp=0;
        
   SFRPAGE_save = SFRPAGE;

   //! buffer to store 1 or 0 in bit content audio to convert	
   if(TF2H == 1)
   {
		/*When system starts, only reads from memory, to get wav header (sampletime, length, and because
		there are more than one wav file in memory,  the starting address of wav, In "configurating" the 
		timer period is not sampleTime*/
		if (configurating==TRUE)
		{
			SPI_Read_Memory(); /*it's nedeed ISR to manage recepcion*/
		}
		else
		{	
			/*pread counts samples of wav file reading*/
			if (pread<(audioLength+1))
			{	
				/*wavSampleRead is the last sample stored by SPI, taken in SPI ISR*/
				PCA0CPH0=wavSampleRead;
				desp=4;
				MOSI_SPI_SW=0;
				
				index++;
				if (index==256)
				{
					index=0;
				}
				CS_DAC=0; /*Chip Select of DAC. This SPI is controlled by GPIOs*/
				while (desp!=0)
				{
					CLK_SPI_SW=0;
					CLK_SPI_SW=1;
					desp--;
				}
					
				desp=0;
				while (desp!=8)
				{
					CLK_SPI_SW=0;
					
					if ((wavSampleRead&0x80)!=0)
					{
						MOSI_SPI_SW=1;
					}	
					else
					{
						MOSI_SPI_SW=0;
					}
					CLK_SPI_SW=1;
					desp++;
					wavSampleRead=wavSampleRead<<1;
				}
				CLK_SPI_SW=0;
				MOSI_SPI_SW=0;
				CLK_SPI_SW=1;
				CLK_SPI_SW=0;
				CLK_SPI_SW=1;					
				CLK_SPI_SW=0;
				CLK_SPI_SW=1;
				CLK_SPI_SW=0;
				CLK_SPI_SW=1;
				CLK_SPI_SW=0;
				CS_DAC=1;
				MOSI_SPI_SW=0;	
			
				/* starts new transmision on SPI to read new sample from memory*/
				SFRPAGE = ACTIVE_PAGE;
				SPI_transferData = NO_OK;
				SPI0DAT = TRANSFER_MEMORY_COMMAND;
				SFRPAGE = SFRPAGE_save;
				
				buffer_Index=0;
				pread++;
			}
			else 
			{  
				/*in configurating mode is stored a buffer with some samples. This buffer
				it's used to allow "clac" noises to reproduce the audioagain  (becuase the audio is 
				playing periodically. The audios are siren sounds, and the last sample must linked whit 
				the first sample, having a compled wave form)*/
				PCA0CPH0=buffer[buffer_Index];
				temp=buffer[buffer_Index];

				buffer_Index++;
				
				if (buffer_Index==1)
				{
					looping=1;
				}
				else if (buffer_Index==BUFFER_SIZE)
				{
					pread=BUFFER_SIZE+1;
				}	
					desp=4;
					MOSI_SPI_SW=0;
				
					CS_DAC=0;
					while (desp!=0)
					{
						CLK_SPI_SW=0;
						CLK_SPI_SW=1;
						desp--;
					}
					desp=0;
					while (desp!=8)
					{
						CLK_SPI_SW=0;
						if ((temp&0x80)!=0)
						{
							MOSI_SPI_SW=1;
						}	
						else
						{
							MOSI_SPI_SW=0;
						}
						//CLK_SPI_SW=0;
						CLK_SPI_SW=1;
						desp++;
						temp=temp<<1;
					}
					desp=4;
					while (desp!=0)
					{
						CLK_SPI_SW=0;
						MOSI_SPI_SW=0;
						//CLK_SPI_SW=0;
						CLK_SPI_SW=1;
						desp--;
					}
					CLK_SPI_SW=0;
					CS_DAC=1;
					MOSI_SPI_SW=0;
					CS_DAC=1;
				}
				if (pread==BUFFER_SIZE+1)
				{			
					/*First sample to begin the cicle*/
					SFRPAGE = ACTIVE_PAGE;
					SPI_transferData = NO_OK;
					SPI0DAT = TRANSFER_MEMORY_COMMAND;
					SFRPAGE = SFRPAGE_save;
				}
			}

		}
		TF2H = 0;
   }
}

Fast lookup plus interpolation computation of non-linear functions

March 23, 20131 comment Coded in C
// Fast integer arithmetic lookup+interpolation method for converting 
// barometric pressure readings to altitude readings.

// Each Lookup Table (LUT) entry is the altitude in centimeters above
// sea level, corresponding to an implicit pressure value, 
// calculated as [PA_INIT - 1024*LUTindex] in Pascals.
// The region of interest is approximately 460metres below sea level,
// to 10000 metres above sea level.

typedef signed long s32;
 
#define PZLUT_ENTRIES   80
#define PA_INIT         106956L
#define PA_DELTA        1024L

#define Z_LIMIT_LO   -99999L
#define Z_LIMIT_HI   99999L

const s32 gPZTbl[PZLUT_ENTRIES] = {
-45853,
-37662,
-29407,
-21087,
-12700,
-4245,
4279,
12874,
21540,
30279,
...  // values removed for brevity
959708,
984147,
1009345
};

// Calculates the altitude in centimeters above sea level, given the barometric
// sensor pressure reading in pascals. The nearest lower LUT index is computed.
// The altitude is then linearly interpolated from the corresponding altitude
// values at the lower and next higher LUT index. Computation is optimized by
// ensuring the difference between LUT entries are spaced by a power of 2, in
// this case 2^10 (1024), so no integer division is required.
// Returns the error values Z_LIMIT_LO or Z_LIMIT_HI if
// the pressure data exceeds the LUT index limits.

s32 sns_Pa2Cm(s32 pa)  {
   	s32 inx,pa1,z1,z2,z;
    
   	if (pa > PA_INIT) {  
      	z = Z_LIMIT_LO;  
      	}
   	else {
      	inx = (PA_INIT - pa)>>10;      
      	if (inx >= PZLUT_ENTRIES-1) {
         	z = Z_LIMIT_HI;
         	}
      	else {
         	pa1 = PA_INIT - (inx<<10);
         	z1 = gPZTbl[inx];
         	z2 = gPZTbl[inx+1];
         	z = z1 + (((pa1-pa)*(z2-z1))>>10);
         	}
      	}
   	return z;
   	}

Linear regression of samples in a circular buffer

March 23, 20131 comment Coded in C
// Linear regression of samples in a circular sample
// buffer. Uses only integer arithmetic, optimized for
// computation on 16bit microcontroller  with hardware 
// multiplier.  The linear regression computation is
// simplified considerably by subtracting out the rolling
// average of the buffer samples.
// This computation assumes the samples arrive at 
// regular intervals, and this sampling rate is known.
//   Usage : 
//   1. call lr_Init() to initialize gnLRDenominator,
//      gnNumSamples and gnSampleIndex
//   2. get first sample value and initialize gZBuffer 
//      with this value 
//   3. for each subsequent incoming sample 
//   	gZBuffer[gnSampleIndex] = lr_GetNewZSample();
//	gZAverage = lr_CalculateAverage(gZBuffer,gnNumSamples);
//	gSlope = lr_CalculateSlope(gZBuffer, gnNumSamples, gnSampleIndex, gZAverage);
//      gnSampleIndex++;
//	if (gnSampleIndex >= gnNumSamples) gnSampleIndex = 0;
//

typedef signed long    s32;

#define MAX_Z_SAMPLES   80
#define SENSOR_SAMPLES_PER_SEC	26L
#define MAX_SLOPE				2000L

#define CLAMP(x,min,max)       {if ((x) <= (min)) (x) = (min); else if ((x) >= (max)) (x) = (max);}

s32 gnLRDenominator;
int gnSampleIndex, gnNumSamples;
s32 gZBuffer[MAX_Z_SAMPLES];
s32 gZAverage;
s32 gSlope;

void lr_Init(int numSamples) {
	s32 zSample, sumT, sumT2;
	int inx;
	sumT = -(numSamples * (numSamples-1L))/2L;
	sumT2 = (numSamples * (numSamples-1L)*(2L*numSamples-1L))/6L;
	gnLRDenominator = (numSamples*sumT2) - (sumT*sumT);
	gnSampleIndex = 0;
	gnNumSamples = numSamples;
	zSample = lr_GetNewZSample(); // get a sample from the sensor
	inx = gnNumSamples;
	while (inx--) gZBuffer[inx] = zSample;  // fill the ZBuffer with first sample value
	}
 
 
s32 lr_CalculateAverage(s32* pZBuffer, int numSamples ) {
   int inx;
   s32 accumulator, average;
   inx = numSamples;
   accumulator = 0;
   while (inx--)  {
	  accumulator += pZBuffer[inx];
      }
   accumulator = (accumulator >= 0 ? accumulator +numSamples/2 : accumulator - numSamples/2); 
   average = accumulator/numSamples;  // rounded up average
   return average; 
   }

/// Linear regression of samples in buffer to calculate slope.

s32 lr_CalculateSlope(s32* pZBuffer, int numSamples, int currentSampleIndex, int zAverage)   {
   int inx,tRelative;
   s32 z, sumZT,slope;
   
   sumZT = 0;
   inx = numSamples;
   while (inx--)  {
      z = pZBuffer[inx] - zAverage;   // subtract out the average value to simplify the arithmetic
      tRelative = inx - currentSampleIndex; // time origin is the current sample in window
      if (tRelative > 0) {
         tRelative -= numSamples;
         }
      sumZT += ((s32)tRelative*z);
      }

   slope = (sumZT*(s32)(SENSOR_SAMPLES_PER_SEC*numSamples))/gnLRDenominator;
   CLAMP(slope,-MAX_SLOPE,MAX_SLOPE);
   return slope;
   }