Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-18 05:45:31 +03:00
Code Issues Wiki Activity

Added median filter implementation. Added 3-point median filter to generic barometer code.

Browse source
This commit is contained in:
digitalentity 2015-04-03 20:53:54 +10:00 • committed by Dominic Clifton
parent d964e2d3a3
commit 6e041c57dd
3 changed files with 84 additions and 1 deletions
Download patch file Download diff file Expand all files Collapse all files

54
src/main/common/maths.c
View file

@ -141,3 +141,57 @@ void rotateV(struct fp_vector *v, fp_angles_t *delta)
v->Z = v_tmp.X * mat[0][2] + v_tmp.Y * mat[1][2] + v_tmp.Z * mat[2][2]; v->Z = v_tmp.X * mat[0][2] + v_tmp.Y * mat[1][2] + v_tmp.Z * mat[2][2];
} }
// Quick median filter implementation
// (c) N. Devillard - 1998
// http://ndevilla.free.fr/median/median.pdf
#define QMF_SORT(a,b) { if ((a)>(b)) QMF_SWAP((a),(b)); }
#define QMF_SWAP(a,b) { int32_t temp=(a);(a)=(b);(b)=temp; }
#define QMP_COPY(p,v,n) { int32_t i; for (i=0; i<n; i++) p[i]=v[i]; }
int32_t quickMedianFilter3(int32_t * v)
{
int32_t p[3];
QMP_COPY(p, v, 3);
QMF_SORT(p[0], p[1]); QMF_SORT(p[1], p[2]); QMF_SORT(p[0], p[1]) ;
return p[1];
}
int32_t quickMedianFilter5(int32_t * v)
{
int32_t p[5];
QMP_COPY(p, v, 5);
QMF_SORT(p[0], p[1]); QMF_SORT(p[3], p[4]); QMF_SORT(p[0], p[3]);
QMF_SORT(p[1], p[4]); QMF_SORT(p[1], p[2]); QMF_SORT(p[2], p[3]);
QMF_SORT(p[1], p[2]);
return p[2];
}
int32_t quickMedianFilter7(int32_t * v)
{
int32_t p[7];
QMP_COPY(p, v, 7);
QMF_SORT(p[0], p[5]); QMF_SORT(p[0], p[3]); QMF_SORT(p[1], p[6]);
QMF_SORT(p[2], p[4]); QMF_SORT(p[0], p[1]); QMF_SORT(p[3], p[5]);
QMF_SORT(p[2], p[6]); QMF_SORT(p[2], p[3]); QMF_SORT(p[3], p[6]);
QMF_SORT(p[4], p[5]); QMF_SORT(p[1], p[4]); QMF_SORT(p[1], p[3]);
QMF_SORT(p[3], p[4]);
return p[3];
}
int32_t quickMedianFilter9(int32_t * v)
{
int32_t p[9];
QMP_COPY(p, v, 9);
QMF_SORT(p[1], p[2]); QMF_SORT(p[4], p[5]); QMF_SORT(p[7], p[8]);
QMF_SORT(p[0], p[1]); QMF_SORT(p[3], p[4]); QMF_SORT(p[6], p[7]);
QMF_SORT(p[1], p[2]); QMF_SORT(p[4], p[5]); QMF_SORT(p[7], p[8]);
QMF_SORT(p[0], p[3]); QMF_SORT(p[5], p[8]); QMF_SORT(p[4], p[7]);
QMF_SORT(p[3], p[6]); QMF_SORT(p[1], p[4]); QMF_SORT(p[2], p[5]);
QMF_SORT(p[4], p[7]); QMF_SORT(p[4], p[2]); QMF_SORT(p[6], p[4]);
QMF_SORT(p[4], p[2]);
return p[4];
}

5
src/main/common/maths.h
View file

@ -77,3 +77,8 @@ int scaleRange(int x, int srcMin, int srcMax, int destMin, int destMax);
void normalizeV(struct fp_vector *src, struct fp_vector *dest); void normalizeV(struct fp_vector *src, struct fp_vector *dest);
void rotateV(struct fp_vector *v, fp_angles_t *delta); void rotateV(struct fp_vector *v, fp_angles_t *delta);
int32_t quickMedianFilter3(int32_t * v);
int32_t quickMedianFilter5(int32_t * v);
int32_t quickMedianFilter7(int32_t * v);
int32_t quickMedianFilter9(int32_t * v);

26
src/main/sensors/barometer.c
View file

@ -60,6 +60,30 @@ void baroSetCalibrationCycles(uint16_t calibrationCyclesRequired)
static bool baroReady = false; static bool baroReady = false;
#define PRESSURE_SAMPLES_MEDIAN 3
static int32_t applyBarometerMedianFilter(int32_t newPressureReading)
{
static int32_t barometerFilterSamples[PRESSURE_SAMPLES_MEDIAN];
static int currentFilterSampleIndex = 0;
static bool medianFilterReady = false;
int nextSampleIndex;
nextSampleIndex = (currentFilterSampleIndex + 1);
if (nextSampleIndex == PRESSURE_SAMPLES_MEDIAN) {
nextSampleIndex = 0;
medianFilterReady = true;
}
barometerFilterSamples[currentFilterSampleIndex] = newPressureReading;
currentFilterSampleIndex = nextSampleIndex;
if (medianFilterReady)
return quickMedianFilter3(barometerFilterSamples);
else
return newPressureReading;
}
#define PRESSURE_SAMPLE_COUNT (barometerConfig->baro_sample_count - 1) #define PRESSURE_SAMPLE_COUNT (barometerConfig->baro_sample_count - 1)
static uint32_t recalculateBarometerTotal(uint8_t baroSampleCount, uint32_t pressureTotal, int32_t newPressureReading) static uint32_t recalculateBarometerTotal(uint8_t baroSampleCount, uint32_t pressureTotal, int32_t newPressureReading)
@ -74,7 +98,7 @@ static uint32_t recalculateBarometerTotal(uint8_t baroSampleCount, uint32_t pres
nextSampleIndex = 0; nextSampleIndex = 0;
baroReady = true; baroReady = true;
} }
barometerSamples[currentSampleIndex] = newPressureReading; barometerSamples[currentSampleIndex] = applyBarometerMedianFilter(newPressureReading);
// recalculate pressure total // recalculate pressure total
// Note, the pressure total is made up of baroSampleCount - 1 samples - See PRESSURE_SAMPLE_COUNT // Note, the pressure total is made up of baroSampleCount - 1 samples - See PRESSURE_SAMPLE_COUNT