Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-17 13:25:30 +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];
}
// 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];
}