Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-19 06:15:16 +03:00
Code Issues Wiki Activity

Optimise biquad filter

Browse source
This commit is contained in:
Martin Budden 2017-07-29 21:34:15 +01:00
parent c750918e8b
commit bd568593bb
2 changed files with 3 additions and 9 deletions
Download patch file Download diff file Expand all files Collapse all files

11
src/main/common/filter.c
View file

@ -125,7 +125,6 @@ void biquadFilterInit(biquadFilter_t *filter, float filterFreq, uint32_t refresh
// zero initial samples // zero initial samples
filter->x1 = filter->x2 = 0; filter->x1 = filter->x2 = 0;
filter->y1 = filter->y2 = 0; filter->y1 = filter->y2 = 0;
filter->d1 = filter->d2 = 0;
} }
void biquadFilterUpdate(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType) void biquadFilterUpdate(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType)
@ -135,8 +134,6 @@ void biquadFilterUpdate(biquadFilter_t *filter, float filterFreq, uint32_t refre
float x2 = filter->x2; float x2 = filter->x2;
float y1 = filter->y1; float y1 = filter->y1;
float y2 = filter->y2; float y2 = filter->y2;
float d1 = filter->d1;
float d2 = filter->d2;
biquadFilterInit(filter, filterFreq, refreshRate, Q, filterType); biquadFilterInit(filter, filterFreq, refreshRate, Q, filterType);
@ -145,8 +142,6 @@ void biquadFilterUpdate(biquadFilter_t *filter, float filterFreq, uint32_t refre
filter->x2 = x2; filter->x2 = x2;
filter->y1 = y1; filter->y1 = y1;
filter->y2 = y2; filter->y2 = y2;
filter->d1 = d1;
filter->d2 = d2;
} }
/* Computes a biquadFilter_t filter on a sample (slightly less precise than df2 but works in dynamic mode) */ /* Computes a biquadFilter_t filter on a sample (slightly less precise than df2 but works in dynamic mode) */
@ -169,9 +164,9 @@ float biquadFilterApplyDF1(biquadFilter_t *filter, float input)
/* Computes a biquadFilter_t filter in direct form 2 on a sample (higher precision but can't handle changes in coefficients */ /* Computes a biquadFilter_t filter in direct form 2 on a sample (higher precision but can't handle changes in coefficients */
float biquadFilterApply(biquadFilter_t *filter, float input) float biquadFilterApply(biquadFilter_t *filter, float input)
{ {
const float result = filter->b0 * input + filter->d1; const float result = filter->b0 * input + filter->x1;
filter->d1 = filter->b1 * input - filter->a1 * result + filter->d2; filter->x1 = filter->b1 * input - filter->a1 * result + filter->x2;
filter->d2 = filter->b2 * input - filter->a2 * result; filter->x2 = filter->b2 * input - filter->a2 * result;
return result; return result;
} }

1
src/main/common/filter.h
View file

@ -36,7 +36,6 @@ typedef struct pt1Filter_s {
typedef struct biquadFilter_s { typedef struct biquadFilter_s {
float b0, b1, b2, a1, a2; float b0, b1, b2, a1, a2;
float x1, x2, y1, y2; float x1, x2, y1, y2;
float d1, d2;
} biquadFilter_t; } biquadFilter_t;
typedef struct firFilterDenoise_s{ typedef struct firFilterDenoise_s{