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

@@ -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
+#if defined(USE_GYRO_FAST_KALMAN)
-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;
-}
-
 // 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

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

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
         }
     }
 

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)
+#endif
-    { "gyro_stage2_lowpass_hz",     VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 16000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_soft_lpf_hz_2) },
+#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) },

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)
+#endif
-    // gyro biquad RC FIR2 filter
+#if defined(USE_GYRO_LPF2)
-    filterApplyFnPtr biquadRCFIR2ApplyFn;
+    // lowpass filter, cascaded biquad sections
-    biquadFilter_t biquadRCFIR2[XYZ_AXIS_COUNT];
+    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)
+#endif
-static void gyroInitFilterBiquadRCFIR2(gyroSensor_t *gyroSensor, uint16_t lpfHz);
+#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)
+#endif
-static void gyroInitFilterBiquadRCFIR2(gyroSensor_t *gyroSensor, uint16_t lpfHz)
+
+
+#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 int gyroFrequencyNyquist = 1000000 / 2 / gyro.targetLooptime;
-    const uint32_t gyroFrequencyNyquist = 1000000 / 2 / gyro.targetLooptime;
+    int sections = 0;
-    const float gyroDt = (float) gyro.targetLooptime * 0.000001f;
+    if (lpfHz && lpfHz <= gyroFrequencyNyquist && order <= GYRO_LPF2_ORDER_MAX) {  // Initialisation needs to happen once samplingrate is known
-    if (lpfHz && lpfHz <= gyroFrequencyNyquist) {  // Initialisation needs to happen once samplingrate is known
-        gyroSensor->biquadRCFIR2ApplyFn = (filterApplyFnPtr)biquadFilterApply;
         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)
+#endif
-    gyroInitFilterBiquadRCFIR2(gyroSensor, gyroConfig()->gyro_soft_lpf_hz_2);
+#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)
+#endif
-            gyroADCf = gyroSensor->biquadRCFIR2ApplyFn((filter_t *)&gyroSensor->biquadRCFIR2[axis], gyroADCf);
+#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)
+#endif
-            // apply biquad RC+FIR2
+#if defined(USE_GYRO_LPF2)
-            gyroADCf = gyroSensor->biquadRCFIR2ApplyFn((filter_t *)&gyroSensor->biquadRCFIR2[axis], gyroADCf);
+            for(int i = 0; i < gyroSensor->biquadLpf2Sections; i++) {
+                gyroADCf = biquadFilterApply(&gyroSensor->biquadLpf2[axis][i], gyroADCf);
+            }
 #endif
 
 #ifdef USE_GYRO_DATA_ANALYSE

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);

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

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