Code Snippets Submitted by wingnut
Linear regression of samples in a circular buffer
// 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;
}Fast lookup plus interpolation computation of non-linear functions
// 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;
}Fast lookup plus interpolation computation of non-linear functions
// 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
// 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;
}
