From daf9ba7ddef6da91d52123b53801f01b60344500 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Wed, 14 Sep 2016 17:20:55 +0100 Subject: [PATCH] Added notch filter from betaflight (#592) --- src/main/common/filter.c | 122 +++++++++++++++++++------------- src/main/common/filter.h | 19 ++++- src/main/fc/mw.c | 2 +- src/main/flight/mixer.c | 2 +- src/main/sensors/acceleration.c | 4 +- src/main/sensors/gyro.c | 2 +- 6 files changed, 94 insertions(+), 57 deletions(-) diff --git a/src/main/common/filter.c b/src/main/common/filter.c index 2ce69286e..b689b25ef 100644 --- a/src/main/common/filter.c +++ b/src/main/common/filter.c @@ -20,59 +20,18 @@ #include #include -#include "common/axis.h" #include "common/filter.h" #include "common/maths.h" -#include "drivers/gyro_sync.h" - -#define BIQUAD_Q (1.0f / 1.41421356f) /* quality factor - butterworth (1 / sqrt(2)) */ -#define M_PI_FLOAT 3.14159265358979323846f - -void biquadFilterInit(biquadFilter_t *filter, uint32_t filterCutFreq, uint32_t refreshRate) -{ - // setup variables - const float sampleRate = 1.0f / ((float)refreshRate * 0.000001f); - const float omega = 2 * M_PIf * ((float)filterCutFreq) / sampleRate; - const float sn = sin_approx(omega); - const float cs = cos_approx(omega); - const float alpha = sn / (2 * BIQUAD_Q); - - float a0, a1, a2, b0, b1, b2; - - b0 = (1 - cs) / 2; - b1 = 1 - cs; - b2 = (1 - cs) / 2; - a0 = 1 + alpha; - a1 = -2 * cs; - a2 = 1 - alpha; - - // precompute the coefficients - filter->b0 = b0 / a0; - filter->b1 = b1 / a0; - filter->b2 = b2 / a0; - filter->a1 = a1 / a0; - filter->a2 = a2 / a0; - - // zero initial samples - filter->d1 = filter->d2 = 1; -} - -/* Computes a biquad_t filter on a sample */ -float biquadFilterApply(biquadFilter_t *filter, float input) -{ - const float result = filter->b0 * input + filter->d1; - filter->d1 = filter->b1 * input - filter->a1 * result + filter->d2; - filter->d2 = filter->b2 * input - filter->a2 * result; - return result; -} +#define BIQUAD_BANDWIDTH 1.9f /* bandwidth in octaves */ +#define BIQUAD_Q 1.0f / sqrtf(2.0f) /* quality factor - butterworth*/ // PT1 Low Pass filter // f_cut = cutoff frequency void pt1FilterInit(pt1Filter_t *filter, uint8_t f_cut, float dT) { - filter->RC = 1.0f / ( 2.0f * M_PI_FLOAT * f_cut ); + filter->RC = 1.0f / ( 2.0f * M_PIf * f_cut ); filter->dT = dT; } @@ -82,14 +41,15 @@ float pt1FilterApply(pt1Filter_t *filter, float input) return filter->state; } -float pt1FilterApply4(pt1Filter_t *filter, float input, float f_cut, float dT) +float pt1FilterApply4(pt1Filter_t *filter, float input, uint16_t f_cut, float dT) { // Pre calculate and store RC if (!filter->RC) { - filter->RC = 1.0f / ( 2.0f * (float)M_PI * f_cut ); + filter->RC = 1.0f / ( 2.0f * M_PIf * f_cut ); + filter->dT = dT; } - filter->state = filter->state + dT / (filter->RC + dT) * (input - filter->state); + filter->state = filter->state + filter->dT / (filter->RC + filter->dT) * (input - filter->state); return filter->state; } @@ -117,7 +77,67 @@ float rateLimitFilterApply4(rateLimitFilter_t *filter, float input, float rate_l return filter->state; } -// FIR filter +float filterGetNotchQ(uint16_t centerFreq, uint16_t cutoff) +{ + const float octaves = log2f((float)centerFreq / (float)cutoff) * 2; + return sqrtf(powf(2, octaves)) / (powf(2, octaves) - 1); +} + +// sets up a biquad Filter +void biquadFilterInitLPF(biquadFilter_t *filter, uint16_t filterFreq, uint32_t refreshRate) +{ + biquadFilterInit(filter, filterFreq, refreshRate, BIQUAD_Q, FILTER_LPF); +} + +void biquadFilterInit(biquadFilter_t *filter, uint16_t filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType) +{ + // setup variables + const float sampleRate = 1.0f / ((float)refreshRate * 0.000001f); + const float omega = 2.0f * M_PIf * ((float)filterFreq) / sampleRate; + const float sn = sin_approx(omega); + const float cs = cos_approx(omega); + const float alpha = sn / (2 * Q); + + float b0, b1, b2; + switch (filterType) { + case FILTER_LPF: + b0 = (1 - cs) / 2; + b1 = 1 - cs; + b2 = (1 - cs) / 2; + break; + case FILTER_NOTCH: + b0 = 1; + b1 = -2 * cs; + b2 = 1; + break; + } + const float a0 = 1 + alpha; + const float a1 = -2 * cs; + const float a2 = 1 - alpha; + + // precompute the coefficients + filter->b0 = b0 / a0; + filter->b1 = b1 / a0; + filter->b2 = b2 / a0; + filter->a1 = a1 / a0; + filter->a2 = a2 / a0; + + // zero initial samples + filter->d1 = filter->d2 = 0; +} + +// Computes a biquad_t filter on a sample +float biquadFilterApply(biquadFilter_t *filter, float input) +{ + const float result = filter->b0 * input + filter->d1; + filter->d1 = filter->b1 * input - filter->a1 * result + filter->d2; + filter->d2 = filter->b2 * input - filter->a2 * result; + return result; +} + +/* + * FIR filter + */ void firFilterInit2(firFilter_t *filter, float *buf, uint8_t bufLength, const float *coeffs, uint8_t coeffsLength) { filter->buf = buf; @@ -127,6 +147,10 @@ void firFilterInit2(firFilter_t *filter, float *buf, uint8_t bufLength, const fl memset(filter->buf, 0, sizeof(float) * filter->bufLength); } +/* + * FIR filter initialisation + * If FIR filter is just used for averaging, coeffs can be set to NULL + */ void firFilterInit(firFilter_t *filter, float *buf, uint8_t bufLength, const float *coeffs) { firFilterInit2(filter, buf, bufLength, coeffs, bufLength); @@ -138,7 +162,7 @@ void firFilterUpdate(firFilter_t *filter, float input) filter->buf[0] = input; } -float firFilterApply(firFilter_t *filter) +float firFilterApply(const firFilter_t *filter) { float ret = 0.0f; for (int ii = 0; ii < filter->coeffsLength; ++ii) { diff --git a/src/main/common/filter.h b/src/main/common/filter.h index 089dae570..66b4a5a31 100644 --- a/src/main/common/filter.h +++ b/src/main/common/filter.h @@ -33,6 +33,17 @@ typedef struct biquadFilter_s { float d1, d2; } biquadFilter_t; +typedef enum { + FILTER_PT1 = 0, + FILTER_BIQUAD, + FILTER_FIR, +} filterType_e; + +typedef enum { + FILTER_LPF, + FILTER_NOTCH +} biquadFilterType_e; + typedef struct firFilter_s { float *buf; const float *coeffs; @@ -41,17 +52,19 @@ typedef struct firFilter_s { } firFilter_t; float pt1FilterApply(pt1Filter_t *filter, float input); -float pt1FilterApply4(pt1Filter_t *filter, float input, float f_cut, float dt); +float pt1FilterApply4(pt1Filter_t *filter, float input, uint16_t f_cut, float dt); void pt1FilterReset(pt1Filter_t *filter, float input); void rateLimitFilterInit(rateLimitFilter_t *filter); float rateLimitFilterApply4(rateLimitFilter_t *filter, float input, float rate_limit, float dT); -void biquadFilterInit(biquadFilter_t *filter, uint32_t filterCutFreq, uint32_t refreshRate); +void biquadFilterInitLPF(biquadFilter_t *filter, uint16_t filterFreq, uint32_t refreshRate); +void biquadFilterInit(biquadFilter_t *filter, uint16_t filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType); float biquadFilterApply(biquadFilter_t *filter, float sample); +float filterGetNotchQ(uint16_t centerFreq, uint16_t cutoff); void firFilterInit(firFilter_t *filter, float *buf, uint8_t bufLength, const float *coeffs); void firFilterInit2(firFilter_t *filter, float *buf, uint8_t bufLength, const float *coeffs, uint8_t coeffsLength); void firFilterUpdate(firFilter_t *filter, float input); -float firFilterApply(firFilter_t *filter); +float firFilterApply(const firFilter_t *filter); diff --git a/src/main/fc/mw.c b/src/main/fc/mw.c index 273fa9a8a..b1feee4d8 100755 --- a/src/main/fc/mw.c +++ b/src/main/fc/mw.c @@ -499,7 +499,7 @@ void filterRc(bool isRXDataNew) // Calculate average cycle time (1Hz LPF on cycle time) if (!filterInitialised) { - biquadFilterInit(&filteredCycleTimeState, 1, gyro.targetLooptime); + biquadFilterInitLPF(&filteredCycleTimeState, 1, gyro.targetLooptime); filterInitialised = true; } diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index da507af81..964896d5e 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -921,7 +921,7 @@ void filterServos(void) // Initialize servo lowpass filter (servos are calculated at looptime rate) if (!servoFilterIsSet) { for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) { - biquadFilterInit(&servoFitlerState[servoIdx], mixerConfig->servo_lowpass_freq, gyro.targetLooptime); + biquadFilterInitLPF(&servoFitlerState[servoIdx], mixerConfig->servo_lowpass_freq, gyro.targetLooptime); } servoFilterIsSet = true; diff --git a/src/main/sensors/acceleration.c b/src/main/sensors/acceleration.c index b0a4e7bcb..956e2ebb3 100644 --- a/src/main/sensors/acceleration.c +++ b/src/main/sensors/acceleration.c @@ -58,7 +58,7 @@ void accInit(uint32_t targetLooptime) accTargetLooptime = targetLooptime; if (accLpfCutHz) { for (int axis = 0; axis < 3; axis++) { - biquadFilterInit(&accFilter[axis], accLpfCutHz, accTargetLooptime); + biquadFilterInitLPF(&accFilter[axis], accLpfCutHz, accTargetLooptime); } } } @@ -223,7 +223,7 @@ void setAccelerationFilter(uint8_t initialAccLpfCutHz) accLpfCutHz = initialAccLpfCutHz; if (accTargetLooptime) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { - biquadFilterInit(&accFilter[axis], accLpfCutHz, accTargetLooptime); + biquadFilterInitLPF(&accFilter[axis], accLpfCutHz, accTargetLooptime); } } } diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 9806d846e..e2dadee16 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -59,7 +59,7 @@ void gyroInit(void) { if (gyroSoftLpfHz && gyro.targetLooptime) { // Initialisation needs to happen once samplingrate is known for (int axis = 0; axis < 3; axis++) { - biquadFilterInit(&gyroFilterLPF[axis], gyroSoftLpfHz, gyro.targetLooptime); + biquadFilterInitLPF(&gyroFilterLPF[axis], gyroSoftLpfHz, gyro.targetLooptime); } } }