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;
   }