From 141d6ec30afa8234ba54ceb11fc97ff33d0325f7 Mon Sep 17 00:00:00 2001 From: Petr Ledvina Date: Wed, 14 Mar 2018 13:45:20 +0100 Subject: [PATCH] Higher-order gyro filter (#5257) * Implement nth order Butterworth Uses biquad sections * Purge RC+FIR2 * Add butterworth LPS as gyro filter Replaces RC+FIR * Make FKF code conditional * Add USE_FIR_FILTER_DENOISE Denoise is almost useless anyway ... --- src/main/common/filter.c | 76 ++++++++++++++++++++++++++------ src/main/common/filter.h | 11 +++-- src/main/flight/pid.c | 4 ++ src/main/interface/settings.c | 6 ++- src/main/sensors/gyro.c | 66 ++++++++++++++++++--------- src/main/sensors/gyro.h | 5 ++- src/main/target/OMNIBUS/target.h | 2 +- src/main/target/common_fc_pre.h | 2 +- 8 files changed, 129 insertions(+), 43 deletions(-) diff --git a/src/main/common/filter.c b/src/main/common/filter.c index 4700928961..3d80a48931 100644 --- a/src/main/common/filter.c +++ b/src/main/common/filter.c @@ -29,7 +29,7 @@ #define M_LN2_FLOAT 0.69314718055994530942f #define M_PI_FLOAT 3.14159265358979323846f #define BIQUAD_BANDWIDTH 1.9f /* bandwidth in octaves */ -#define BIQUAD_Q 1.0f / sqrtf(2.0f) /* quality factor - butterworth*/ +#define BIQUAD_Q 1.0f / sqrtf(2.0f) /* quality factor - 2nd order butterworth*/ // NULL filter @@ -94,6 +94,51 @@ void biquadFilterInitLPF(biquadFilter_t *filter, float filterFreq, uint32_t refr biquadFilterInit(filter, filterFreq, refreshRate, BIQUAD_Q, FILTER_LPF); } +// Q coefficients for Butterworth filter of given order. Implicit odd section coefficient of 0.5 +// coefficients should be in ascending order +// generated by http://www.earlevel.com/main/2016/09/29/cascading-filters/ +static const float butterworthLpfQ[] = { +#if BIQUAD_LPF_ORDER_MAX >= 2 + 0.70710678, // 2nd +#endif +#if BIQUAD_LPF_ORDER_MAX >= 3 + 1.0, // 3rd +#endif +#if BIQUAD_LPF_ORDER_MAX >= 4 + 0.54119610, 1.3065630, // 4th +#endif +#if BIQUAD_LPF_ORDER_MAX >= 5 + 0.61803399, 1.6180340, // 5th +#endif +#if BIQUAD_LPF_ORDER_MAX >= 6 + 0.51763809, 0.70710678, 1.9318517 // 6th +#endif +}; +#define BUTTERWORTH_QINDEX(o) (((o) - 1) * ((o) - 1) / 4) + +// make sure that we have correct number of coefficients +STATIC_ASSERT(BUTTERWORTH_QINDEX(BIQUAD_LPF_ORDER_MAX + 1) == ARRAYLEN(butterworthLpfQ), butterworthLpfQ_mismatch); + +// setup cascade of biquad sections for Butterworth LPF filter +// sections is pointer to array of biquad sections, it must be long enough +// return number of sections used +int biquadFilterLpfCascadeInit(biquadFilter_t *sections, int order, float filterFreq, uint32_t refreshRate) +{ + biquadFilter_t *section = sections; + // single-pole section first + if (order % 2 == 1) { + biquadFilterInit(section, filterFreq, refreshRate, 0.5, FILTER_LPF1); + section++; + } + const float *Qptr = butterworthLpfQ + BUTTERWORTH_QINDEX(order); // first coefficient for given order + // 2 poles per section + for (int i = order; i >= 2; i -= 2) { + biquadFilterInit(section, filterFreq, refreshRate, *Qptr, FILTER_LPF); + Qptr++; section++; + } + return section - sections; +} + void biquadFilterInit(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType) { // setup variables @@ -106,6 +151,8 @@ void biquadFilterInit(biquadFilter_t *filter, float filterFreq, uint32_t refresh switch (filterType) { case FILTER_LPF: + // 2nd order Butterworth (with Q=1/sqrt(2)) / Butterworth biquad section with Q + // described in http://www.ti.com/lit/an/slaa447/slaa447.pdf b0 = (1 - cs) * 0.5f; b1 = 1 - cs; b2 = (1 - cs) * 0.5f; @@ -113,6 +160,18 @@ void biquadFilterInit(biquadFilter_t *filter, float filterFreq, uint32_t refresh a1 = -2 * cs; a2 = 1 - alpha; break; + case FILTER_LPF1: { + // 1st order Butterworth, H(s) = 1 / (1 + s), + // transformed according to http://www.iowahills.com/A4IIRBilinearTransform.html + const float T = 2.0f * sn / (cs + 1.0f); // T = 2 * tan(omega / 2) + b0 = T; + b1 = T; + b2 = 0; + a0 = T + 2.0f; + a1 = T - 2.0f; + a2 = 0; + break; + } case FILTER_NOTCH: b0 = 1; b1 = -2 * cs; @@ -306,18 +365,7 @@ float firFilterDenoiseUpdate(firFilterDenoise_t *filter, float input) } } -// ledvinap's proposed RC+FIR2 Biquad-- 1st order IIR, RC filter k -void biquadRCFIR2FilterInit(biquadFilter_t *filter, uint16_t f_cut, float dT) -{ - float RC = 1.0f / ( 2.0f * M_PI_FLOAT * f_cut ); - float k = dT / (RC + dT); - filter->b0 = k / 2; - filter->b1 = k / 2; - filter->b2 = 0; - filter->a1 = -(1 - k); - filter->a2 = 0; -} - +#if defined(USE_GYRO_FAST_KALMAN) // Fast two-state Kalman void fastKalmanInit(fastKalman_t *filter, float q, float r, float p) { @@ -347,3 +395,5 @@ FAST_CODE float fastKalmanUpdate(fastKalman_t *filter, float input) return filter->x; } + +#endif diff --git a/src/main/common/filter.h b/src/main/common/filter.h index 8744f889bd..cf09aa6461 100644 --- a/src/main/common/filter.h +++ b/src/main/common/filter.h @@ -70,9 +70,10 @@ typedef enum { } filterType_e; typedef enum { - FILTER_LPF, + FILTER_LPF, // 2nd order Butterworth section FILTER_NOTCH, FILTER_BPF, + FILTER_LPF1, // 1st order Butterworth section } biquadFilterType_e; typedef struct firFilter_s { @@ -89,15 +90,17 @@ typedef float (*filterApplyFnPtr)(filter_t *filter, float input); float nullFilterApply(filter_t *filter, float input); +#define BIQUAD_LPF_ORDER_MAX 6 + void biquadFilterInitLPF(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate); void biquadFilterInit(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); +int biquadFilterLpfCascadeInit(biquadFilter_t *sections, int order, float filterFreq, uint32_t refreshRate); + float biquadFilterApplyDF1(biquadFilter_t *filter, float input); float biquadFilterApply(biquadFilter_t *filter, float input); float filterGetNotchQ(uint16_t centerFreq, uint16_t cutoff); -void biquadRCFIR2FilterInit(biquadFilter_t *filter, uint16_t f_cut, float dT); - void fastKalmanInit(fastKalman_t *filter, float q, float r, float p); float fastKalmanUpdate(fastKalman_t *filter, float input); @@ -120,5 +123,7 @@ float firFilterCalcPartialAverage(const firFilter_t *filter, uint8_t count); float firFilterCalcMovingAverage(const firFilter_t *filter); float firFilterLastInput(const firFilter_t *filter); +#if defined(USE_FIR_FILTER_DENOISE) void firFilterDenoiseInit(firFilterDenoise_t *filter, uint8_t gyroSoftLpfHz, uint16_t targetLooptime); float firFilterDenoiseUpdate(firFilterDenoise_t *filter, float input); +#endif diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 5533e3fc3f..6861461073 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -177,7 +177,9 @@ static FAST_RAM void *ptermYawFilter; typedef union dtermFilterLpf_u { pt1Filter_t pt1Filter[2]; biquadFilter_t biquadFilter[2]; +#if defined(USE_FIR_FILTER_DENOISE) firFilterDenoise_t denoisingFilter[2]; +#endif } dtermFilterLpf_t; void pidInitFilters(const pidProfile_t *pidProfile) @@ -239,6 +241,7 @@ void pidInitFilters(const pidProfile_t *pidProfile) biquadFilterInitLPF(dtermFilterLpf[axis], pidProfile->dterm_lpf_hz, targetPidLooptime); } break; +#if defined(USE_FIR_FILTER_DENOISE) case FILTER_FIR: dtermLpfApplyFn = (filterApplyFnPtr)firFilterDenoiseUpdate; for (int axis = FD_ROLL; axis <= FD_PITCH; axis++) { @@ -246,6 +249,7 @@ void pidInitFilters(const pidProfile_t *pidProfile) firFilterDenoiseInit(dtermFilterLpf[axis], pidProfile->dterm_lpf_hz, targetPidLooptime); } break; +#endif } } diff --git a/src/main/interface/settings.c b/src/main/interface/settings.c index 1ce76757c4..75649932b3 100644 --- a/src/main/interface/settings.c +++ b/src/main/interface/settings.c @@ -380,8 +380,10 @@ const clivalue_t valueTable[] = { { "gyro_filter_q", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 16000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_filter_q) }, { "gyro_filter_r", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 16000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_filter_r) }, { "gyro_filter_p", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 16000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_filter_p) }, -#elif defined(USE_GYRO_BIQUAD_RC_FIR2) - { "gyro_stage2_lowpass_hz", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 16000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_soft_lpf_hz_2) }, +#endif +#if defined(USE_GYRO_LPF2) + { "gyro_stage2_lowpass_hz", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 16000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_soft_lpf2_hz) }, + { "gyro_stage2_lowpass_order", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, GYRO_LPF2_ORDER_MAX}, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_soft_lpf2_order) }, #endif { "moron_threshold", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 200 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyroMovementCalibrationThreshold) }, { "gyro_offset_yaw", VAR_INT16 | MASTER_VALUE, .config.minmax = { -1000, 1000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_offset_yaw) }, diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index c75dc87914..3b1355df64 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -101,7 +101,9 @@ bool firstArmingCalibrationWasStarted = false; typedef union gyroSoftFilter_u { biquadFilter_t gyroFilterLpfState[XYZ_AXIS_COUNT]; pt1Filter_t gyroFilterPt1State[XYZ_AXIS_COUNT]; +#if defined(USE_FIR_FILTER_DENOISE) firFilterDenoise_t gyroDenoiseState[XYZ_AXIS_COUNT]; +#endif } gyroSoftLpfFilter_t; typedef struct gyroSensor_s { @@ -124,10 +126,11 @@ typedef struct gyroSensor_s { // gyro kalman filter filterApplyFnPtr fastKalmanApplyFn; fastKalman_t fastKalman[XYZ_AXIS_COUNT]; -#elif defined(USE_GYRO_BIQUAD_RC_FIR2) - // gyro biquad RC FIR2 filter - filterApplyFnPtr biquadRCFIR2ApplyFn; - biquadFilter_t biquadRCFIR2[XYZ_AXIS_COUNT]; +#endif +#if defined(USE_GYRO_LPF2) + // lowpass filter, cascaded biquad sections + int biquadLpf2Sections; + biquadFilter_t biquadLpf2[XYZ_AXIS_COUNT][(GYRO_LPF2_ORDER_MAX + 1) / 2]; // each section is of second order #endif } gyroSensor_t; @@ -143,8 +146,9 @@ STATIC_UNIT_TESTED gyroDev_t * const gyroDevPtr = &gyroSensor1.gyroDev; #if defined(USE_GYRO_FAST_KALMAN) static void gyroInitFilterKalman(gyroSensor_t *gyroSensor, uint16_t gyro_filter_q, uint16_t gyro_filter_r, uint16_t gyro_filter_p); -#elif defined (USE_GYRO_BIQUAD_RC_FIR2) -static void gyroInitFilterBiquadRCFIR2(gyroSensor_t *gyroSensor, uint16_t lpfHz); +#endif +#if defined (USE_GYRO_LPF2) +static void gyroInitFilterLpf2(gyroSensor_t *gyroSensor, int order, int lpfHz); #endif static void gyroInitSensorFilters(gyroSensor_t *gyroSensor); @@ -187,11 +191,12 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig, .gyro_soft_notch_hz_2 = 200, .gyro_soft_notch_cutoff_2 = 100, .checkOverflow = GYRO_OVERFLOW_CHECK_ALL_AXES, - .gyro_soft_lpf_hz_2 = 0, + .gyro_soft_lpf2_hz = 0, .gyro_filter_q = 0, .gyro_filter_r = 0, .gyro_filter_p = 0, .gyro_offset_yaw = 0, + .gyro_soft_lpf2_order = 1, ); @@ -577,11 +582,14 @@ void gyroInitFilterLpf(gyroSensor_t *gyroSensor, uint8_t lpfHz) } break; default: +#if defined(USE_FIR_FILTER_DENOISE) + // this should be case FILTER_FIR: gyroSensor->softLpfFilterApplyFn = (filterApplyFnPtr)firFilterDenoiseUpdate; for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { gyroSensor->softLpfFilterPtr[axis] = (filter_t *)&gyroSensor->softLpfFilter.gyroDenoiseState[axis]; firFilterDenoiseInit(&gyroSensor->softLpfFilter.gyroDenoiseState[axis], lpfHz, gyro.targetLooptime); } +#endif break; } } @@ -673,18 +681,26 @@ static void gyroInitFilterKalman(gyroSensor_t *gyroSensor, uint16_t gyro_filter_ } } } -#elif defined(USE_GYRO_BIQUAD_RC_FIR2) -static void gyroInitFilterBiquadRCFIR2(gyroSensor_t *gyroSensor, uint16_t lpfHz) +#endif + + +#if defined(USE_GYRO_LPF2) + +#if GYRO_LPF2_ORDER_MAX > BIQUAD_LPF_ORDER_MAX +# error "GYRO_LPF2_ORDER_MAX is larger than BIQUAD_LPF_ORDER_MAX" +#endif + +static void gyroInitFilterLpf2(gyroSensor_t *gyroSensor, int order, int lpfHz) { - gyroSensor->biquadRCFIR2ApplyFn = nullFilterApply; - const uint32_t gyroFrequencyNyquist = 1000000 / 2 / gyro.targetLooptime; - const float gyroDt = (float) gyro.targetLooptime * 0.000001f; - if (lpfHz && lpfHz <= gyroFrequencyNyquist) { // Initialisation needs to happen once samplingrate is known - gyroSensor->biquadRCFIR2ApplyFn = (filterApplyFnPtr)biquadFilterApply; + const int gyroFrequencyNyquist = 1000000 / 2 / gyro.targetLooptime; + int sections = 0; + if (lpfHz && lpfHz <= gyroFrequencyNyquist && order <= GYRO_LPF2_ORDER_MAX) { // Initialisation needs to happen once samplingrate is known for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { - biquadRCFIR2FilterInit(&gyroSensor->biquadRCFIR2[axis], lpfHz, gyroDt); + const int axisSections = biquadFilterLpfCascadeInit(gyroSensor->biquadLpf2[axis], order, lpfHz, gyro.targetLooptime); + sections = MAX(sections, axisSections); } } + gyroSensor->biquadLpf2Sections = sections; } #endif @@ -695,8 +711,9 @@ static void gyroInitSensorFilters(gyroSensor_t *gyroSensor) #endif #if defined(USE_GYRO_FAST_KALMAN) gyroInitFilterKalman(gyroSensor, gyroConfig()->gyro_filter_q, gyroConfig()->gyro_filter_r, gyroConfig()->gyro_filter_p); -#elif defined(USE_GYRO_BIQUAD_RC_FIR2) - gyroInitFilterBiquadRCFIR2(gyroSensor, gyroConfig()->gyro_soft_lpf_hz_2); +#endif +#if defined(USE_GYRO_LPF2) + gyroInitFilterLpf2(gyroSensor, gyroConfig()->gyro_soft_lpf2_order, gyroConfig()->gyro_soft_lpf2_hz); #endif gyroInitFilterLpf(gyroSensor, gyroConfig()->gyro_soft_lpf_hz); gyroInitFilterNotch1(gyroSensor, gyroConfig()->gyro_soft_notch_hz_1, gyroConfig()->gyro_soft_notch_cutoff_1); @@ -933,8 +950,11 @@ static FAST_CODE void gyroUpdateSensor(gyroSensor_t *gyroSensor, timeUs_t curren float gyroADCf = gyroSensor->gyroDev.gyroADC[axis] * gyroSensor->gyroDev.scale; #if defined(USE_GYRO_FAST_KALMAN) gyroADCf = gyroSensor->fastKalmanApplyFn((filter_t *)&gyroSensor->fastKalman[axis], gyroADCf); -#elif defined(USE_GYRO_BIQUAD_RC_FIR2) - gyroADCf = gyroSensor->biquadRCFIR2ApplyFn((filter_t *)&gyroSensor->biquadRCFIR2[axis], gyroADCf); +#endif +#if defined(USE_GYRO_LPF2) + for(int i = 0; i < gyroSensor->biquadLpf2Sections; i++) { + gyroADCf = biquadFilterApply(&gyroSensor->biquadLpf2[axis][i], gyroADCf); + } #endif #ifdef USE_GYRO_DATA_ANALYSE gyroADCf = gyroSensor->notchFilterDynApplyFn((filter_t *)&gyroSensor->notchFilterDyn[axis], gyroADCf); @@ -960,9 +980,11 @@ static FAST_CODE void gyroUpdateSensor(gyroSensor_t *gyroSensor, timeUs_t curren #if defined(USE_GYRO_FAST_KALMAN) // apply fast kalman gyroADCf = gyroSensor->fastKalmanApplyFn((filter_t *)&gyroSensor->fastKalman[axis], gyroADCf); -#elif defined(USE_GYRO_BIQUAD_RC_FIR2) - // apply biquad RC+FIR2 - gyroADCf = gyroSensor->biquadRCFIR2ApplyFn((filter_t *)&gyroSensor->biquadRCFIR2[axis], gyroADCf); +#endif +#if defined(USE_GYRO_LPF2) + for(int i = 0; i < gyroSensor->biquadLpf2Sections; i++) { + gyroADCf = biquadFilterApply(&gyroSensor->biquadLpf2[axis][i], gyroADCf); + } #endif #ifdef USE_GYRO_DATA_ANALYSE diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index 05d3ab4451..25baf1b0a7 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -69,7 +69,7 @@ typedef struct gyroConfig_s { bool gyro_high_fsr; bool gyro_use_32khz; uint8_t gyro_to_use; - uint16_t gyro_soft_lpf_hz_2; + uint16_t gyro_soft_lpf2_hz; uint16_t gyro_soft_notch_hz_1; uint16_t gyro_soft_notch_cutoff_1; uint16_t gyro_soft_notch_hz_2; @@ -79,8 +79,11 @@ typedef struct gyroConfig_s { uint16_t gyro_filter_r; uint16_t gyro_filter_p; int16_t gyro_offset_yaw; + uint8_t gyro_soft_lpf2_order; } gyroConfig_t; +#define GYRO_LPF2_ORDER_MAX 6 + PG_DECLARE(gyroConfig_t, gyroConfig); bool gyroInit(void); diff --git a/src/main/target/OMNIBUS/target.h b/src/main/target/OMNIBUS/target.h index 012c656559..37c97f40f5 100644 --- a/src/main/target/OMNIBUS/target.h +++ b/src/main/target/OMNIBUS/target.h @@ -20,7 +20,7 @@ // Removed to make the firmware fit into flash (in descending order of priority): #undef USE_DSHOT_DMAR // OMNIBUS (F3) does not benefit from burst Dshot #undef USE_GYRO_OVERFLOW_CHECK -#undef USE_GYRO_BIQUAD_RC_FIR2 +#undef USE_GYRO_LPF2 #undef USE_SERIALRX_XBUS #undef USE_TELEMETRY_LTM diff --git a/src/main/target/common_fc_pre.h b/src/main/target/common_fc_pre.h index ad1078704b..83ee843666 100644 --- a/src/main/target/common_fc_pre.h +++ b/src/main/target/common_fc_pre.h @@ -156,7 +156,7 @@ #define USE_VTX_CONTROL #define USE_VTX_SMARTAUDIO #define USE_VTX_TRAMP -#define USE_GYRO_BIQUAD_RC_FIR2 +#define USE_GYRO_LPF2 #ifdef USE_SERIALRX_SPEKTRUM #define USE_SPEKTRUM_BIND