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

Quick median filter for floats

Browse source
This commit is contained in:
Konstantin Sharlaimov (DigitalEntity) 2015-12-24 19:49:31 +10:00 • committed by borisbstyle
parent 6d679b72e1
commit e8917b7a28
2 changed files with 55 additions and 0 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -232,6 +232,8 @@ void rotateV(struct fp_vector *v, fp_angles_t *delta)
#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 QMF_COPY(p,v,n) { int32_t i; for (i=0; i<n; i++) p[i]=v[i]; }
#define QMF_SORTF(a,b) { if ((a)>(b)) QMF_SWAPF((a),(b)); }
#define QMF_SWAPF(a,b) { float temp=(a);(a)=(b);(b)=temp; }
int32_t quickMedianFilter3(int32_t * v)
{
@ -281,6 +283,54 @@ int32_t quickMedianFilter9(int32_t * v)
return p[4];
}
float quickMedianFilter3f(float * v)
{
float p[3];
QMF_COPY(p, v, 3);
QMF_SORTF(p[0], p[1]); QMF_SORTF(p[1], p[2]); QMF_SORTF(p[0], p[1]) ;
return p[1];
}
float quickMedianFilter5f(float * v)
{
float p[5];
QMF_COPY(p, v, 5);
QMF_SORTF(p[0], p[1]); QMF_SORTF(p[3], p[4]); QMF_SORTF(p[0], p[3]);
QMF_SORTF(p[1], p[4]); QMF_SORTF(p[1], p[2]); QMF_SORTF(p[2], p[3]);
QMF_SORTF(p[1], p[2]);
return p[2];
}
float quickMedianFilter7f(float * v)
{
float p[7];
QMF_COPY(p, v, 7);
QMF_SORTF(p[0], p[5]); QMF_SORTF(p[0], p[3]); QMF_SORTF(p[1], p[6]);
QMF_SORTF(p[2], p[4]); QMF_SORTF(p[0], p[1]); QMF_SORTF(p[3], p[5]);
QMF_SORTF(p[2], p[6]); QMF_SORTF(p[2], p[3]); QMF_SORTF(p[3], p[6]);
QMF_SORTF(p[4], p[5]); QMF_SORTF(p[1], p[4]); QMF_SORTF(p[1], p[3]);
QMF_SORTF(p[3], p[4]);
return p[3];
}
float quickMedianFilter9f(float * v)
{
float p[9];
QMF_COPY(p, v, 9);
QMF_SORTF(p[1], p[2]); QMF_SORTF(p[4], p[5]); QMF_SORTF(p[7], p[8]);
QMF_SORTF(p[0], p[1]); QMF_SORTF(p[3], p[4]); QMF_SORTF(p[6], p[7]);
QMF_SORTF(p[1], p[2]); QMF_SORTF(p[4], p[5]); QMF_SORTF(p[7], p[8]);
QMF_SORTF(p[0], p[3]); QMF_SORTF(p[5], p[8]); QMF_SORTF(p[4], p[7]);
QMF_SORTF(p[3], p[6]); QMF_SORTF(p[1], p[4]); QMF_SORTF(p[2], p[5]);
QMF_SORTF(p[4], p[7]); QMF_SORTF(p[4], p[2]); QMF_SORTF(p[6], p[4]);
QMF_SORTF(p[4], p[2]);
return p[4];
}
void arraySubInt32(int32_t *dest, int32_t *array1, int32_t *array2, int count)
{
for (int i = 0; i < count; i++) {