Merge pull request #77 from iNavFlight/biquad-filter

Biquad filter (kudos @borisbstyle)

Makefile

@@ -232,7 +232,6 @@ COMMON_SRC = build_config.c \
 		   flight/imu.c \
 		   flight/hil.c \
 		   flight/mixer.c \
-		   flight/lowpass.c \
 		   drivers/bus_i2c_soft.c \
 		   drivers/serial.c \
 		   drivers/sound_beeper.c \

src/main/common/filter.c

@@ -24,16 +24,75 @@
 #include "common/filter.h"
 #include "common/maths.h"
 
-// PT1 Low Pass filter (when no dT specified it will be calculated from the cycleTime)
+#include "drivers/gyro_sync.h"
-float filterApplyPt1(float input, filterStatePt1_t *filter, uint8_t f_cut, float dT) {
 
+#define BIQUAD_BANDWIDTH 1.9f     /* bandwidth in octaves */
+
+/* sets up a biquad Filter */
+void filterInitBiQuad(uint8_t filterCutFreq, biquad_t *newState, int16_t samplingRate)
+{
+    float omega, sn, cs, alpha;
+    float a0, a1, a2, b0, b1, b2;
+
+    /* If sampling rate == 0 - use main loop target rate */
+    if (!samplingRate) {
+        samplingRate = 1000000 / targetLooptime;
+    }
+
+    /* setup variables */
+    omega = 2 * M_PIf * (float)filterCutFreq / (float)samplingRate;
+    sn = sin_approx(omega);
+    cs = cos_approx(omega);
+    alpha = sn * sin_approx(M_LN2f / 2 * BIQUAD_BANDWIDTH * omega / sn);
+
+    b0 = (1 - cs) / 2;
+    b1 = 1 - cs;
+    b2 = (1 - cs) / 2;
+    a0 = 1 + alpha;
+    a1 = -2 * cs;
+    a2 = 1 - alpha;
+
+    /* precompute the coefficients */
+    newState->a0 = b0 / a0;
+    newState->a1 = b1 / a0;
+    newState->a2 = b2 / a0;
+    newState->a3 = a1 / a0;
+    newState->a4 = a2 / a0;
+
+    /* zero initial samples */
+    newState->x1 = newState->x2 = 0;
+    newState->y1 = newState->y2 = 0;
+}
+
+/* Computes a biquad_t filter on a sample */
+float filterApplyBiQuad(float sample, biquad_t *state)
+{
+    float result;
+
+    /* compute result */
+    result = state->a0 * sample + state->a1 * state->x1 + state->a2 * state->x2 -
+        state->a3 * state->y1 - state->a4 * state->y2;
+
+    /* shift x1 to x2, sample to x1 */
+    state->x2 = state->x1;
+    state->x1 = sample;
+
+    /* shift y1 to y2, result to y1 */
+    state->y2 = state->y1;
+    state->y1 = result;
+
+    return result;
+}
+
+// PT1 Low Pass filter (when no dT specified it will be calculated from the cycleTime)
+float filterApplyPt1(float input, filterStatePt1_t *filter, uint8_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->state = filter->state + dT / (filter->RC + dT) * (input - filter->state);
-
     return filter->state;
 }
 
@@ -41,59 +100,3 @@ void filterResetPt1(filterStatePt1_t *filter, float input)
 {
     filter->state = input;
 }
-
-/**
- * Typical quadcopter motor noise frequency (at 50% throttle):
- *  450-sized, 920kv, 9.4x4.3 props, 3S : 4622rpm = 77Hz
- *  250-sized, 2300kv, 5x4.5 props, 4S : 14139rpm = 235Hz
- */
-static int8_t gyroFIRCoeff_1000[3][9] = { { 0, 0, 12, 23, 40, 51, 52, 40, 38 },    // looptime=1000; group delay 2.5ms; -0.5db = 32Hz ; -1db = 45Hz; -5db = 97Hz; -10db = 132Hz
-                                          { 19, 31, 43, 48, 41, 35, 23, 8, 8},     // looptime=1000; group delay 3ms;   -0.5db = 18Hz ; -1db = 33Hz; -5db = 81Hz; -10db = 113Hz
-                                          { 18, 12, 28, 40, 44, 40, 32, 22, 20} }; // looptime=1000; group delay 4ms;   -0.5db = 23Hz ; -1db = 35Hz; -5db = 75Hz; -10db = 103Hz
-static int8_t gyroFIRCoeff_2000[3][9] = { { 0, 0, 0, 6, 24, 58, 83, 65, 20 },      // looptime=2000, group delay 4ms;   -0.5db = 21Hz ; -1db = 31Hz; -5db = 71Hz; -10db = 99Hz
-                                          { 0, 0, 14, 21, 45, 59, 55, 39, 23},     // looptime=2000, group delay 5ms;   -0.5db = 20Hz ; -1db = 26Hz; -5db = 52Hz; -10db = 71Hz
-                                          { 14, 12, 26, 38, 45, 43, 34, 24, 20} }; // looptime=2000, group delay 7ms;   -0.5db = 11Hz ; -1db = 18Hz; -5db = 38Hz; -10db = 52Hz
-static int8_t gyroFIRCoeff_3000[3][9] = { { 0, 0, 0, 0, 4, 35, 87, 87, 43 },       // looptime=3000, group delay 4.5ms; -0.5db = 18Hz ; -1db = 26Hz; -5db = 57Hz; -10db = 78Hz
-                                          { 0, 0, 0, 14, 31, 62, 70, 52, 27},      // looptime=3000, group delay 6.5ms; -0.5db = 16Hz ; -1db = 21Hz; -5db = 42Hz; -10db = 57Hz
-                                          { 0, 6, 10, 28, 44, 54, 54, 38, 22} };   // looptime=3000, group delay 9ms;   -0.5db = 10Hz ; -1db = 13Hz; -5db = 32Hz; -10db = 45Hz
-
-int8_t * filterGetFIRCoefficientsTable(uint8_t filter_level, uint16_t targetLooptime)
-{
-    if (filter_level == 0) {
-        return NULL;
-    }
-
-    int firIndex = constrain(filter_level, 1, 3) - 1;
-
-    // For looptimes faster than 1499 (and looptime=0) use filter for 1kHz looptime
-    if (targetLooptime < 1500) {
-        return gyroFIRCoeff_1000[firIndex];
-    }
-    // 1500 ... 2499
-    else if (targetLooptime < 2500) {
-        return gyroFIRCoeff_2000[firIndex];
-    }
-    // > 2500
-    else {
-        return gyroFIRCoeff_3000[firIndex];
-    }
-}
-
-// 9 Tap FIR filter as described here:
-// Thanks to Qcopter & BorisB & DigitalEntity
-void filterApply9TapFIR(int16_t data[3], int16_t state[3][9], int8_t coeff[9])
-{
-    int32_t FIRsum;
-    int axis, i;
-
-    for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
-        FIRsum = 0;
-        for (i = 0; i <= 7; i++) {
-            state[axis][i] = state[axis][i + 1];
-            FIRsum += state[axis][i] * (int16_t)coeff[i];
-        }
-        state[axis][8] = data[axis];
-        FIRsum += state[axis][8] * coeff[8];
-        data[axis] = FIRsum / 256;
-    }
-}

src/main/common/filter.h

@@ -23,7 +23,14 @@ typedef struct filterStatePt1_s {
 	float constdT;
 } filterStatePt1_t;
 
+/* this holds the data required to update samples thru a filter */
+typedef struct biquad_s {
+    float a0, a1, a2, a3, a4;
+    float x1, x2, y1, y2;
+} biquad_t;
+
 float filterApplyPt1(float input, filterStatePt1_t *filter, uint8_t f_cut, float dt);
 void filterResetPt1(filterStatePt1_t *filter, float input);
-int8_t * filterGetFIRCoefficientsTable(uint8_t filter_level, uint16_t targetLooptime);
+
-void filterApply9TapFIR(int16_t data[3], int16_t state[3][9], int8_t coeff[9]);
+void filterInitBiQuad(uint8_t filterCutFreq, biquad_t *newState, int16_t samplingRate);
+float filterApplyBiQuad(float sample, biquad_t *state);

src/main/common/maths.h

@@ -27,6 +27,7 @@
 
 // Use floating point M_PI instead explicitly.
 #define M_PIf       3.14159265358979323846f
+#define M_LN2f      0.69314718055994530942f
 
 #define RAD    (M_PIf / 180.0f)
 

src/main/config/config.c

@@ -180,12 +180,11 @@ void resetPidProfile(pidProfile_t *pidProfile)
     pidProfile->I8[PIDVEL] = 5;     // NAV_VEL_Z_I * 100
     pidProfile->D8[PIDVEL] = 100;   // NAV_VEL_Z_D * 1000
 
-    pidProfile->acc_soft_lpf = 2;    // MEDIUM filtering by default
+    pidProfile->acc_soft_lpf_hz = 15;
-    pidProfile->gyro_soft_lpf = 1;   // LOW filtering by default
+    pidProfile->gyro_soft_lpf_hz = 60;
 
     pidProfile->yaw_p_limit = YAW_P_LIMIT_MAX;
-    pidProfile->yaw_pterm_cut_hz = 0;
+    pidProfile->dterm_lpf_hz = 30;
-    pidProfile->dterm_cut_hz = 30;
 
     pidProfile->P_f[ROLL] = 1.4f;     // new PID with preliminary defaults test carefully
     pidProfile->I_f[ROLL] = 0.3f;
@@ -764,7 +763,7 @@ void activateConfig(void)
         &currentProfile->pidProfile
     );
 
-    useGyroConfig(&masterConfig.gyroConfig, filterGetFIRCoefficientsTable(currentProfile->pidProfile.gyro_soft_lpf, masterConfig.looptime));
+    useGyroConfig(&masterConfig.gyroConfig, currentProfile->pidProfile.gyro_soft_lpf_hz);
 
 #ifdef TELEMETRY
     telemetryUseConfig(&masterConfig.telemetryConfig);
@@ -777,7 +776,7 @@ void activateConfig(void)
 
     setAccelerationZero(&masterConfig.accZero);
     setAccelerationGain(&masterConfig.accGain);
-    setAccelerationFilter(filterGetFIRCoefficientsTable(currentProfile->pidProfile.acc_soft_lpf, masterConfig.looptime));
+    setAccelerationFilter(currentProfile->pidProfile.acc_soft_lpf_hz);
 
     mixerUseConfigs(
 #ifdef USE_SERVOS

src/main/flight/imu.c

@@ -495,17 +495,14 @@ static void imuUpdateMeasuredRotationRate(void)
 }
 
 /* Calculate measured acceleration in body frame cm/s/s */
-#define ACCELERATION_LPF_HZ 10
+static void imuUpdateMeasuredAcceleration(void)
-static void imuUpdateMeasuredAcceleration(float dT)
 {
-    t_fp_vector measuredAccelerationBF;
-    static filterStatePt1_t accFilter[XYZ_AXIS_COUNT];
     int axis;
 
     /* Convert acceleration to cm/s/s */
     for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
-        measuredAccelerationBF.A[axis] = accADC[axis] * (GRAVITY_CMSS / acc_1G);
+        imuAccelInBodyFrame.A[axis] = accADC[axis] * (GRAVITY_CMSS / acc_1G);
-        imuMeasuredGravityBF.A[axis] = measuredAccelerationBF.A[axis];
+        imuMeasuredGravityBF.A[axis] = imuAccelInBodyFrame.A[axis];
     }
 
 #ifdef GPS
@@ -519,11 +516,6 @@ static void imuUpdateMeasuredAcceleration(float dT)
         imuMeasuredGravityBF.A[Z] += gpsSol.groundSpeed * imuMeasuredRotationBF.A[Y];
     }
 #endif
-
-    /* Apply LPF to acceleration readings and publish imuAccelInBodyFrame */
-    for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
-        imuAccelInBodyFrame.A[axis] = filterApplyPt1(measuredAccelerationBF.A[axis], &accFilter[axis], ACCELERATION_LPF_HZ, dT);
-    }
 }
 
 #ifdef HIL
@@ -574,16 +566,16 @@ void imuUpdateGyroAndAttitude(void)
 #ifdef HIL
         if (!hilActive) {
             imuUpdateMeasuredRotationRate();    // Calculate gyro rate in body frame in rad/s
-            imuUpdateMeasuredAcceleration(dT);  // Calculate accel in body frame in cm/s/s
+            imuUpdateMeasuredAcceleration();  // Calculate accel in body frame in cm/s/s
             imuCalculateEstimatedAttitude(dT);  // Update attitude estimate
         }
         else {
             imuHILUpdate();
-            imuUpdateMeasuredAcceleration(dT);
+            imuUpdateMeasuredAcceleration();
         }
 #else
             imuUpdateMeasuredRotationRate();    // Calculate gyro rate in body frame in rad/s
-            imuUpdateMeasuredAcceleration(dT);  // Calculate accel in body frame in cm/s/s
+            imuUpdateMeasuredAcceleration();  // Calculate accel in body frame in cm/s/s
             imuCalculateEstimatedAttitude(dT);  // Update attitude estimate
 #endif
     } else {

src/main/flight/lowpass.c

@@ -1,118 +0,0 @@
-/*
- * This file is part of Cleanflight.
- *
- * Cleanflight is free software: you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation, either version 3 of the License, or
- * (at your option) any later version.
- *
- * Cleanflight is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
- */
-
-#include <stdbool.h>
-#include <stdint.h>
-#include <stdlib.h>
-#include <math.h>
-
-#include "common/maths.h"
-#include "flight/lowpass.h"
-
-//#define DEBUG_LOWPASS
-
-void generateLowpassCoeffs2(int16_t freq, lowpass_t *filter)
-{
-    float fixedScaler;
-    int i;
-
-    // generates coefficients for a 2nd-order butterworth low-pass filter
-    float freqf = (float)freq*0.001f;
-    float omega = tan_approx((float)M_PI*freqf/2.0f);
-    float scaling = 1.0f / (omega*omega + 1.4142136f*omega + 1.0f);
-
-
-#if defined(UNIT_TEST) && defined(DEBUG_LOWPASS)
-    printf("lowpass cutoff: %f, omega: %f\n", freqf, omega);
-#endif
-   
-    filter->bf[0] = scaling * omega*omega;
-    filter->bf[1] = 2.0f * filter->bf[0];
-    filter->bf[2] = filter->bf[0];
-
-    filter->af[0] = 1.0f;
-    filter->af[1] = scaling * (2.0f * omega*omega - 2.0f);
-    filter->af[2] = scaling * (omega*omega - 1.4142136f * omega + 1.0f);
-
-    // Scale for fixed-point
-    filter->input_bias = 1500; // Typical servo range is 1500 +/- 500
-    filter->input_shift = 16;
-    filter->coeff_shift = 24; 
-    fixedScaler = (float)(1ULL << filter->coeff_shift);
-    for (i = 0; i < LOWPASS_NUM_COEF; i++) {
-        filter->a[i] = LPF_ROUND(filter->af[i] * fixedScaler);
-        filter->b[i] = LPF_ROUND(filter->bf[i] * fixedScaler);
-#if defined(UNIT_TEST) && defined(DEBUG_LOWPASS)
-        printf("(%d) bf: %f af: %f b: %ld a: %ld\n", i, 
-                filter->bf[i], filter->af[i], filter->b[i], filter->a[i]);
-#endif
-    }
-
-    filter->freq = freq;
-}
-
-int32_t lowpassFixed(lowpass_t *filter, int32_t in, int16_t freq)
-{
-    int16_t coefIdx;
-    int64_t out;
-    int32_t in_s;
-  
-    // Check to see if cutoff frequency changed
-    if (freq != filter->freq) {
-        filter->init = false;
-    }
-
-    // Initialize if needed
-    if (!filter->init) {
-        generateLowpassCoeffs2(freq, filter);
-        for (coefIdx = 0; coefIdx < LOWPASS_NUM_COEF; coefIdx++) {
-            filter->x[coefIdx] = (in - filter->input_bias) << filter->input_shift;
-            filter->y[coefIdx] = (in - filter->input_bias) << filter->input_shift;
-        }
-        filter->init = true;
-    }
-
-    // Unbias input and scale
-    in_s = (in - filter->input_bias) << filter->input_shift;
-
-    // Delays
-    for (coefIdx = LOWPASS_NUM_COEF-1; coefIdx > 0; coefIdx--) {
-        filter->x[coefIdx] = filter->x[coefIdx-1];
-        filter->y[coefIdx] = filter->y[coefIdx-1];
-    }
-    filter->x[0] = in_s;
-
-    // Accumulate result
-    out = filter->x[0] * filter->b[0];
-    for (coefIdx = 1; coefIdx < LOWPASS_NUM_COEF; coefIdx++) {
-        out -= filter->y[coefIdx] * filter->a[coefIdx];
-        out += filter->x[coefIdx] * filter->b[coefIdx];
-    }
-
-    // Scale output by coefficient shift
-    out >>= filter->coeff_shift;
-    filter->y[0] = (int32_t)out;
-
-    // Scale output by input shift and round
-    out = (out + (1 << (filter->input_shift-1))) >> filter->input_shift;
-    
-    // Reapply bias
-    out += filter->input_bias;
-
-    return (int32_t)out;
-}
-

src/main/flight/lowpass.h

@@ -1,41 +0,0 @@
-/*
- * This file is part of Cleanflight.
- *
- * Cleanflight is free software: you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation, either version 3 of the License, or
- * (at your option) any later version.
- *
- * Cleanflight is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
- */
-
-#pragma once
-
-
-#define LOWPASS_NUM_COEF 3
-#define LPF_ROUND(x) (x < 0 ? (x - 0.5f) : (x + 0.5f))
-
-typedef struct lowpass_s {
-    bool init;
-    int16_t freq;                           // Normalized freq in 1/1000ths
-    float bf[LOWPASS_NUM_COEF];
-    float af[LOWPASS_NUM_COEF];
-    int64_t b[LOWPASS_NUM_COEF];
-    int64_t a[LOWPASS_NUM_COEF];
-    int16_t coeff_shift;
-    int16_t input_shift;
-    int32_t input_bias;
-    float xf[LOWPASS_NUM_COEF];
-    float yf[LOWPASS_NUM_COEF];
-    int32_t x[LOWPASS_NUM_COEF];
-    int32_t y[LOWPASS_NUM_COEF];
-} lowpass_t;
-
-void generateLowpassCoeffs2(int16_t freq, lowpass_t *filter);
-int32_t lowpassFixed(lowpass_t *filter, int32_t in, int16_t freq);

src/main/flight/mixer.c

@@ -27,6 +27,7 @@
 
 #include "common/axis.h"
 #include "common/maths.h"
+#include "common/filter.h"
 
 #include "drivers/system.h"
 #include "drivers/pwm_output.h"
@@ -48,7 +49,6 @@
 #include "flight/failsafe.h"
 #include "flight/pid.h"
 #include "flight/imu.h"
-#include "flight/lowpass.h"
 #include "flight/navigation_rewrite.h"
 
 #include "config/runtime_config.h"
@@ -78,7 +78,8 @@ int16_t servo[MAX_SUPPORTED_SERVOS];
 static int useServo;
 STATIC_UNIT_TESTED uint8_t servoCount;
 static servoParam_t *servoConf;
-static lowpass_t lowpassFilters[MAX_SUPPORTED_SERVOS];
+static biquad_t servoFitlerState[MAX_SUPPORTED_SERVOS];
+static bool servoFilterIsSet;
 #endif
 
 static const motorMixer_t mixerQuadX[] = {
@@ -890,10 +891,18 @@ void filterServos(void)
 #endif
 
     if (mixerConfig->servo_lowpass_enable) {
-        for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) {
+        // Initialize servo lowpass filter (servos are calculated at looptime rate)
-            servo[servoIdx] = (int16_t)lowpassFixed(&lowpassFilters[servoIdx], servo[servoIdx], mixerConfig->servo_lowpass_freq);
+        if (!servoFilterIsSet) {
+            for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) {
+                filterInitBiQuad(mixerConfig->servo_lowpass_freq, &servoFitlerState[servoIdx], 0);
+            }
 
-            // Sanity check
+            servoFilterIsSet = true;
+        }
+
+        for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) {
+            // Apply servo lowpass filter and do sanity cheching
+            servo[servoIdx] = (int16_t) filterApplyBiQuad((float)servo[servoIdx], &servoFitlerState[servoIdx]);
             servo[servoIdx] = constrain(servo[servoIdx], servoConf[servoIdx].min, servoConf[servoIdx].max);
         }
     }

src/main/flight/pid.c

@@ -29,6 +29,7 @@
 
 #include "drivers/sensor.h"
 #include "drivers/accgyro.h"
+#include "drivers/gyro_sync.h"
 
 #include "sensors/sensors.h"
 #include "sensors/gyro.h"
@@ -83,7 +84,8 @@ void pidResetErrorGyro(void)
 
 const angle_index_t rcAliasToAngleIndexMap[] = { AI_ROLL, AI_PITCH };
 
-static filterStatePt1_t yawPTermState, DTermState[3];
+static biquad_t deltaBiQuadState[3];
+static bool deltaStateIsSet = false;
 
 static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig,
         uint16_t max_angle_inclination, rxConfig_t *rxConfig)
@@ -92,11 +94,16 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
     float ITerm,PTerm,DTerm;
     int32_t stickPosAil, stickPosEle, mostDeflectedPos;
     static float lastError[3];
-    static float delta1[3], delta2[3];
+    float delta;
-    float delta, deltaSum;
     int axis;
     float horizonLevelStrength = 1;
 
+    if (!deltaStateIsSet && pidProfile->dterm_lpf_hz) {
+        for (axis = 0; axis < 3; axis++)
+            filterInitBiQuad(pidProfile->dterm_lpf_hz, &deltaBiQuadState[axis], 0);
+        deltaStateIsSet = true;
+    }
+
     if (FLIGHT_MODE(HORIZON_MODE)) {
 
         // Figure out the raw stick positions
@@ -156,11 +163,6 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
         // -----calculate P component
         PTerm = RateError * pidProfile->P_f[axis] * PIDweight[axis] / 100;
 
-        // Yaw Pterm low pass
-        if (axis == YAW && pidProfile->yaw_pterm_cut_hz) {
-            PTerm = filterApplyPt1(PTerm, &yawPTermState, pidProfile->yaw_pterm_cut_hz, dT);
-        }
-
         // -----calculate I component.
         errorGyroIf[axis] = constrainf(errorGyroIf[axis] + RateError * dT * pidProfile->I_f[axis] * 10, -250.0f, 250.0f);
 
@@ -176,23 +178,11 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
         // would be scaled by different dt each time. Division by dT fixes that.
         delta *= (1.0f / dT);
 
-        // In case of soft gyro lpf combined with dterm_cut_hz we don't need the old 3 point averaging
+        if (deltaStateIsSet) {
-        if (pidProfile->gyro_soft_lpf && pidProfile->dterm_cut_hz) {
+            delta = filterApplyBiQuad(delta, &deltaBiQuadState[axis]);
-            deltaSum = delta;
-        } else {
-            // add moving average here to reduce noise
-            deltaSum = delta1[axis] + delta2[axis] + delta;
-            delta2[axis] = delta1[axis];
-            delta1[axis] = delta;
-            deltaSum /= 3.0f;
         }
 
-        // Dterm low pass
+        DTerm = constrainf(delta * pidProfile->D_f[axis] * PIDweight[axis] / 100, -300.0f, 300.0f);
-        if (pidProfile->dterm_cut_hz) {
-            deltaSum = filterApplyPt1(deltaSum, &DTermState[axis], pidProfile->dterm_cut_hz, dT);
-        }
-
-        DTerm = constrainf(deltaSum * pidProfile->D_f[axis] * PIDweight[axis] / 100, -300.0f, 300.0f);
 
         // -----calculate total PID output
         axisPID[axis] = constrain(lrintf(PTerm + ITerm + DTerm), -1000, 1000);
@@ -216,36 +206,30 @@ static void pidMultiWiiRewrite(pidProfile_t *pidProfile, controlRateConfig_t *co
 {
     UNUSED(rxConfig);
 
-    int32_t errorAngle;
     int axis;
-    int32_t delta, deltaSum;
+    int32_t delta;
-    static int32_t delta1[3], delta2[3];
     int32_t PTerm, ITerm, DTerm;
     static int32_t lastError[3] = { 0, 0, 0 };
-    int32_t AngleRateTmp, RateError;
+    int32_t AngleRateTmp, RateError, gyroRate;
 
     int8_t horizonLevelStrength = 100;
-    int32_t stickPosAil, stickPosEle, mostDeflectedPos;
+
+    if (!deltaStateIsSet && pidProfile->dterm_lpf_hz) {
+        for (axis = 0; axis < 3; axis++)
+            filterInitBiQuad(pidProfile->dterm_lpf_hz, &deltaBiQuadState[axis], 0);
+        deltaStateIsSet = true;
+    }
 
     if (FLIGHT_MODE(HORIZON_MODE)) {
-
         // Figure out the raw stick positions
-        stickPosAil = getRcStickDeflection(FD_ROLL, rxConfig->midrc);
+        const int32_t stickPosAil = ABS(getRcStickDeflection(FD_ROLL, rxConfig->midrc));
-        stickPosEle = getRcStickDeflection(FD_PITCH, rxConfig->midrc);
+        const int32_t stickPosEle = ABS(getRcStickDeflection(FD_PITCH, rxConfig->midrc));
-
+        const int32_t mostDeflectedPos = MAX(stickPosAil, stickPosEle);
-        if(ABS(stickPosAil) > ABS(stickPosEle)){
-            mostDeflectedPos = ABS(stickPosAil);
-        }
-        else {
-            mostDeflectedPos = ABS(stickPosEle);
-        }
-
         // Progressively turn off the horizon self level strength as the stick is banged over
         horizonLevelStrength = (500 - mostDeflectedPos) / 5;  // 100 at centre stick, 0 = max stick deflection
-
         // Using Level D as a Sensitivity for Horizon. 0 more level to 255 more rate. Default value of 100 seems to work fine.
         // For more rate mode increase D and slower flips and rolls will be possible
-       	horizonLevelStrength = constrain((10 * (horizonLevelStrength - 100) * (10 * pidProfile->D8[PIDLEVEL] / 80) / 100) + 100, 0, 100);
+        horizonLevelStrength = constrain((10 * (horizonLevelStrength - 100) * (10 * pidProfile->D8[PIDLEVEL] / 80) / 100) + 100, 0, 100);
     }
 
     // ----------PID controller----------
@@ -256,18 +240,21 @@ static void pidMultiWiiRewrite(pidProfile_t *pidProfile, controlRateConfig_t *co
         if (axis == FD_YAW) { // YAW is always gyro-controlled (MAG correction is applied to rcCommand)
             AngleRateTmp = (((int32_t)(rate + 27) * rcCommand[YAW]) >> 5);
         } else {
-            // calculate error and limit the angle to max configured inclination
+            AngleRateTmp = ((int32_t)(rate + 27) * rcCommand[axis]) >> 4;
-            errorAngle = constrain(2 * rcCommand[axis], -((int) max_angle_inclination),
-                    +max_angle_inclination) - attitude.raw[axis]; // 16 bits is ok here
 
-            if (!FLIGHT_MODE(ANGLE_MODE)) { //control is GYRO based (ACRO and HORIZON - direct sticks control is applied to rate PID
+            if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) {
-                AngleRateTmp = ((int32_t)(rate + 27) * rcCommand[axis]) >> 4;
+                // calculate error and limit the angle to max configured inclination
-                if (FLIGHT_MODE(HORIZON_MODE)) {
+                int32_t errorAngle = constrain(2 * rcCommand[axis], -((int) max_angle_inclination),
-                    // mix up angle error to desired AngleRateTmp to add a little auto-level feel. horizonLevelStrength is scaled to the stick input
+                        +max_angle_inclination) - attitude.raw[axis]; // 16 bits is ok here
-                	AngleRateTmp += (errorAngle * pidProfile->I8[PIDLEVEL] * horizonLevelStrength / 100) >> 4;
+
+                if (FLIGHT_MODE(ANGLE_MODE)) { //control is GYRO based (ACRO and HORIZON - direct sticks control is applied to rate PID
+                    // it's the ANGLE mode - control is angle based, so control loop is needed
+                    AngleRateTmp = (errorAngle * pidProfile->P8[PIDLEVEL]) >> 4;
+                } else {
+                    // HORIZON mode - mix up angle error to desired AngleRateTmp to add a little auto-level feel,
+                    // horizonLevelStrength is scaled to the stick input
+                    AngleRateTmp += (errorAngle * pidProfile->I8[PIDLEVEL] * horizonLevelStrength / 100) >> 4;
                 }
-            } else { // it's the ANGLE mode - control is angle based, so control loop is needed
-                AngleRateTmp = (errorAngle * pidProfile->P8[PIDLEVEL]) >> 4;
             }
         }
 
@@ -275,22 +262,18 @@ static void pidMultiWiiRewrite(pidProfile_t *pidProfile, controlRateConfig_t *co
         // Used in stand-alone mode for ACRO, controlled by higher level regulators in other modes
         // -----calculate scaled error.AngleRates
         // multiplication of rcCommand corresponds to changing the sticks scaling here
-        RateError = AngleRateTmp - (gyroADC[axis] / 4);
+        gyroRate = gyroADC[axis] / 4;
+        RateError = AngleRateTmp - gyroRate;
 
         // -----calculate P component
         PTerm = (RateError * pidProfile->P8[axis] * PIDweight[axis] / 100) >> 7;
 
-        // Yaw Pterm low pass
-        if (axis == YAW && pidProfile->yaw_pterm_cut_hz) {
-            PTerm = filterApplyPt1(PTerm, &yawPTermState, pidProfile->yaw_pterm_cut_hz, dT);
-        }
-
         // -----calculate I component
         // there should be no division before accumulating the error to integrator, because the precision would be reduced.
         // Precision is critical, as I prevents from long-time drift. Thus, 32 bits integrator is used.
         // Time correction (to avoid different I scaling for different builds based on average cycle time)
         // is normalized to cycle time = 2048.
-        errorGyroI[axis] = errorGyroI[axis] + ((RateError * cycleTime) >> 11) * pidProfile->I8[axis];
+        errorGyroI[axis] = errorGyroI[axis] + ((RateError * (uint16_t)cycleTime) >> 11) * pidProfile->I8[axis];
 
         // limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated.
         // I coefficient (I8) moved before integration to make limiting independent from PID settings
@@ -303,24 +286,14 @@ static void pidMultiWiiRewrite(pidProfile_t *pidProfile, controlRateConfig_t *co
 
         // Correct difference by cycle time. Cycle time is jittery (can be different 2 times), so calculated difference
         // would be scaled by different dt each time. Division by dT fixes that.
-        delta = (delta * ((uint16_t) 0xFFFF / (cycleTime >> 4))) >> 6;
+        delta = (delta * ((uint16_t) 0xFFFF / ((uint16_t)cycleTime >> 4))) >> 6;
 
-        // In case of soft gyro lpf combined with dterm_cut_hz we don't need the old 3 point averaging
+        if (deltaStateIsSet) {
-        if (pidProfile->gyro_soft_lpf && pidProfile->dterm_cut_hz) {
+            // Upscale x3 before filtering to avoid rounding errors
-            deltaSum = delta * 3;  // Scaling to approximately match the old D values
+            delta = lrintf(filterApplyBiQuad(3.0f * delta, &deltaBiQuadState[axis]));
-        } else {
-            // add moving average here to reduce noise
-            deltaSum = delta1[axis] + delta2[axis] + delta;
-            delta2[axis] = delta1[axis];
-            delta1[axis] = delta;
         }
 
-        // Dterm delta low pass
+        DTerm = (delta * pidProfile->D8[axis] * PIDweight[axis] / 100) >> 8;
-        if (pidProfile->dterm_cut_hz) {
-            deltaSum = filterApplyPt1(deltaSum, &DTermState[axis], pidProfile->dterm_cut_hz, dT);
-        }
-
-        DTerm = (deltaSum * pidProfile->D8[axis] * PIDweight[axis] / 100) >> 8;
 
         // -----calculate total PID output
         axisPID[axis] = PTerm + ITerm + DTerm;
@@ -341,6 +314,9 @@ static void pidMultiWiiRewrite(pidProfile_t *pidProfile, controlRateConfig_t *co
 
 void pidSetController(pidControllerType_e type)
 {
+    // Force filter re-init
+    deltaStateIsSet = false;
+
     switch (type) {
         default:
         case PID_CONTROLLER_MWREWRITE:

src/main/flight/pid.h

@@ -58,11 +58,10 @@ typedef struct pidProfile_s {
     uint8_t H_sensitivity;
 
     uint16_t yaw_p_limit;                   // set P term limit (fixed value was 300)
-    uint8_t dterm_cut_hz;                   // (default 17Hz, Range 1-50Hz) Used for PT1 element in PID1, PID2 and PID5
+    uint8_t dterm_lpf_hz;                   // (default 17Hz, Range 1-50Hz) Used for PT1 element in PID1, PID2 and PID5
-    uint8_t yaw_pterm_cut_hz;               // Used for filering Pterm noise on noisy frames
 
-    uint8_t gyro_soft_lpf;                  // Gyro FIR filtering
+    uint8_t gyro_soft_lpf_hz;               // Gyro FIR filtering
-    uint8_t acc_soft_lpf;                   // Set the Low Pass Filter factor for ACC. Reducing this value would reduce ACC noise (visible in GUI), but would increase ACC lag time. Zero = no filter
+    uint8_t acc_soft_lpf_hz;                // Set the Low Pass Filter factor for ACC. Reducing this value would reduce ACC noise (visible in GUI), but would increase ACC lag time. Zero = no filter
 
 #ifdef GTUNE
     uint8_t  gtune_lolimP[3];               // [0..200] Lower limit of P during G tune

src/main/io/serial_cli.c

@@ -374,10 +374,6 @@ static const char * const lookupTableGyroLpf[] = {
     "10HZ"
 };
 
-static const char * const lookupTableSoftFilter[] = {
-    "OFF", "LOW", "MEDIUM", "HIGH"
-};
-
 static const char * const lookupTableFailsafeProcedure[] = {
     "SET-THR", "DROP", "RTH"
 };
@@ -413,7 +409,6 @@ typedef enum {
     TABLE_GIMBAL_MODE,
     TABLE_PID_CONTROLLER,
     TABLE_SERIAL_RX,
-    TABLE_SOFT_FILTER,
     TABLE_GYRO_LPF,
     TABLE_FAILSAFE_PROCEDURE,
 #ifdef NAV
@@ -438,7 +433,6 @@ static const lookupTableEntry_t lookupTables[] = {
     { lookupTableGimbalMode, sizeof(lookupTableGimbalMode) / sizeof(char *) },
     { lookupTablePidController, sizeof(lookupTablePidController) / sizeof(char *) },
     { lookupTableSerialRX, sizeof(lookupTableSerialRX) / sizeof(char *) },
-    { lookupTableSoftFilter, sizeof(lookupTableSoftFilter) / sizeof(char *) },
     { lookupTableGyroLpf, sizeof(lookupTableGyroLpf) / sizeof(char *) },
     { lookupTableFailsafeProcedure, sizeof(lookupTableFailsafeProcedure) / sizeof(char *) },
 #ifdef NAV
@@ -732,10 +726,9 @@ const clivalue_t valueTable[] = {
     { "d_level",                    VAR_UINT8  | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.D8[PIDLEVEL], .config.minmax = { 0,  200 }, 0 },
 
     { "yaw_p_limit",                VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yaw_p_limit, .config.minmax = { YAW_P_LIMIT_MIN, YAW_P_LIMIT_MAX } },
-	{ "gyro_soft_lpf",              VAR_UINT8  | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.gyro_soft_lpf, .config.lookup = { TABLE_SOFT_FILTER }, 0 },
+	{ "gyro_soft_lpf_hz",           VAR_UINT8  | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gyro_soft_lpf_hz, .config.minmax = {0, 200 } },
-    { "acc_soft_lpf",               VAR_UINT8  | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.acc_soft_lpf, .config.lookup = { TABLE_SOFT_FILTER }, 0 },
+    { "acc_soft_lpf_hz",            VAR_UINT8  | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.acc_soft_lpf_hz, .config.minmax = {0, 200 } },
-    { "dterm_cut_hz",               VAR_UINT8  | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_cut_hz, .config.minmax = {0, 200 } },
+    { "dterm_lpf_hz",               VAR_UINT8  | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_lpf_hz, .config.minmax = {0, 200 } },
-    { "yaw_pterm_cut_hz",           VAR_UINT8  | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yaw_pterm_cut_hz, .config.minmax = {0, 200 } },
 
 #ifdef GTUNE
     { "gtune_loP_rll",              VAR_UINT8  | PROFILE_VALUE,  &masterConfig.profile[0].pidProfile.gtune_lolimP[FD_ROLL], .config.minmax = { 10,  200 }, 0 },

src/main/mw.c

@@ -113,8 +113,6 @@ extern uint32_t currentTime;
 extern uint8_t dynP8[3], dynI8[3], dynD8[3], PIDweight[3];
 
 static bool isRXDataNew;
-static filterStatePt1_t filteredCycleTimeState;
-uint16_t filteredCycleTime;
 
 typedef void (*pidControllerFuncPtr)(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig,
         uint16_t max_angle_inclination, rxConfig_t *rxConfig);            // pid controller function prototype
@@ -538,15 +536,27 @@ void processRx(void)
 
 }
 
-void filterRc(bool isRXDataNew){
+void filterRc(bool isRXDataNew)
+{
     static int16_t lastCommand[4] = { 0, 0, 0, 0 };
     static int16_t deltaRC[4] = { 0, 0, 0, 0 };
     static int16_t factor, rcInterpolationFactor;
+    static biquad_t filteredCycleTimeState;
+    static bool filterInitialised;
+    uint16_t filteredCycleTime;
     uint16_t rxRefreshRate;
 
     // Set RC refresh rate for sampling and channels to filter
     initRxRefreshRate(&rxRefreshRate);
 
+    // Calculate average cycle time (1Hz LPF on cycle time)
+    if (!filterInitialised) {
+        filterInitBiQuad(1, &filteredCycleTimeState, 0);
+        filterInitialised = true;
+    }
+
+    filteredCycleTime = filterApplyBiQuad((float) cycleTime, &filteredCycleTimeState);
+
     rcInterpolationFactor = rxRefreshRate / filteredCycleTime + 1;
 
     if (isRXDataNew) {
@@ -575,9 +585,6 @@ void taskMainPidLoop(void)
     cycleTime = getTaskDeltaTime(TASK_SELF);
     dT = (float)cycleTime * 0.000001f;
 
-    // Calculate average cycle time and average jitter
-    filteredCycleTime = filterApplyPt1(cycleTime, &filteredCycleTimeState, 1, dT);
-    
     imuUpdateAccelerometer();
     imuUpdateGyroAndAttitude();
 
@@ -711,8 +718,9 @@ void taskUpdateBattery(void)
 
     if (feature(FEATURE_VBAT)) {
         if (cmp32(currentTime, vbatLastServiced) >= VBATINTERVAL) {
+            uint32_t vbatTimeDelta = currentTime - vbatLastServiced;
             vbatLastServiced = currentTime;
-            updateBattery();
+            updateBattery(vbatTimeDelta);
         }
     }
 

src/main/sensors/acceleration.c

@@ -27,6 +27,7 @@
 
 #include "drivers/sensor.h"
 #include "drivers/accgyro.h"
+#include "drivers/gyro_sync.h"
 
 #include "sensors/battery.h"
 #include "sensors/sensors.h"
@@ -47,8 +48,10 @@ uint16_t calibratingA = 0;      // the calibration is done is the main loop. Cal
 
 static flightDynamicsTrims_t * accZero;
 static flightDynamicsTrims_t * accGain;
-static int8_t * accFIRTable = 0L;
+
-static int16_t accFIRState[3][9];
+static int8_t accLpfCutHz = 0;
+static biquad_t accFilterState[XYZ_AXIS_COUNT];
+static bool accFilterInitialised = false;
 
 void accSetCalibrationCycles(uint16_t calibrationCyclesRequired)
 {
@@ -173,12 +176,28 @@ void applyAccelerationZero(flightDynamicsTrims_t * accZero, flightDynamicsTrims_
 
 void updateAccelerationReadings(void)
 {
+    int axis;
+
     if (!acc.read(accADC)) {
         return;
     }
 
-    if (accFIRTable) {
+    if (accLpfCutHz) {
-        filterApply9TapFIR(accADC, accFIRState, accFIRTable);
+        if (!accFilterInitialised) {
+            if (targetLooptime) {  /* Initialisation needs to happen once sample rate is known */
+                for (axis = 0; axis < 3; axis++) {
+                    filterInitBiQuad(accLpfCutHz, &accFilterState[axis], 0);
+                }
+
+                accFilterInitialised = true;
+            }
+        }
+
+        if (accFilterInitialised) {
+            for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
+                accADC[axis] = lrintf(filterApplyBiQuad((float) accADC[axis], &accFilterState[axis]));
+            }
+        }
     }
 
     if (!isAccelerationCalibrationComplete()) {
@@ -200,7 +219,7 @@ void setAccelerationGain(flightDynamicsTrims_t * accGainToUse)
     accGain = accGainToUse;
 }
 
-void setAccelerationFilter(int8_t * filterTableToUse)
+void setAccelerationFilter(int8_t initialAccLpfCutHz)
 {
-    accFIRTable = filterTableToUse;
+    accLpfCutHz = initialAccLpfCutHz;
 }

src/main/sensors/acceleration.h

@@ -44,4 +44,4 @@ void accSetCalibrationCycles(uint16_t calibrationCyclesRequired);
 void updateAccelerationReadings(void);
 void setAccelerationZero(flightDynamicsTrims_t * accZeroToUse);
 void setAccelerationGain(flightDynamicsTrims_t * accGainToUse);
-void setAccelerationFilter(int8_t * filterTableToUse);
+void setAccelerationFilter(int8_t initialAccLpfCutHz);

src/main/sensors/battery.c

@@ -21,6 +21,7 @@
 #include "platform.h"
 
 #include "common/maths.h"
+#include "common/filter.h"
 
 #include "drivers/adc.h"
 #include "drivers/system.h"
@@ -33,11 +34,10 @@
 #include "rx/rx.h"
 
 #include "io/rc_controls.h"
-#include "flight/lowpass.h"
 #include "io/beeper.h"
 
 #define VBATT_PRESENT_THRESHOLD_MV    10
-#define VBATT_LPF_FREQ  10
+#define VBATT_LPF_FREQ  1
 
 // Battery monitoring stuff
 uint8_t batteryCellCount = 3;       // cell count
@@ -54,7 +54,6 @@ int32_t mAhDrawn = 0;               // milliampere hours drawn from the battery
 batteryConfig_t *batteryConfig;
 
 static batteryState_e batteryState;
-static lowpass_t lowpassFilter;
 
 uint16_t batteryAdcToVoltage(uint16_t src)
 {
@@ -63,24 +62,24 @@ uint16_t batteryAdcToVoltage(uint16_t src)
     return ((((uint32_t)src * batteryConfig->vbatscale * 33 + (0xFFF * 5)) / (0xFFF * batteryConfig->vbatresdivval))/batteryConfig->vbatresdivmultiplier);
 }
 
-static void updateBatteryVoltage(void)
+static void updateBatteryVoltage(uint32_t vbatTimeDelta)
 {
     uint16_t vbatSample;
-    uint16_t vbatFiltered;
+    static filterStatePt1_t vbatFilterState;
 
     // store the battery voltage with some other recent battery voltage readings
     vbatSample = vbatLatestADC = adcGetChannel(ADC_BATTERY);
-    vbatFiltered = (uint16_t)lowpassFixed(&lowpassFilter, vbatSample, VBATT_LPF_FREQ);
+    vbatSample = filterApplyPt1(vbatSample, &vbatFilterState, VBATT_LPF_FREQ, vbatTimeDelta * 1e-6f);
-    vbat = batteryAdcToVoltage(vbatFiltered);
+    vbat = batteryAdcToVoltage(vbatSample);
 }
 
 #define VBATTERY_STABLE_DELAY 40
 /* Batt Hysteresis of +/-100mV */
 #define VBATT_HYSTERESIS 1
 
-void updateBattery(void)
+void updateBattery(uint32_t vbatTimeDelta)
 {
-    updateBatteryVoltage();
+    updateBatteryVoltage(vbatTimeDelta);
     
     /* battery has just been connected*/
     if (batteryState == BATTERY_NOT_PRESENT && vbat > VBATT_PRESENT_THRESHOLD_MV)
@@ -92,7 +91,7 @@ void updateBattery(void)
         We only do this on the ground so don't care if we do block, not
         worse than original code anyway*/
         delay(VBATTERY_STABLE_DELAY);
-        updateBatteryVoltage();
+        updateBatteryVoltage(vbatTimeDelta);
 
         unsigned cells = (batteryAdcToVoltage(vbatLatestADC) / batteryConfig->vbatmaxcellvoltage) + 1;
         if (cells > 8) {

src/main/sensors/battery.h

@@ -68,7 +68,7 @@ extern int32_t mAhDrawn;
 uint16_t batteryAdcToVoltage(uint16_t src);
 batteryState_e getBatteryState(void);
 const  char * getBatteryStateString(void);
-void updateBattery(void);
+void updateBattery(uint32_t vbatTimeDelta);
 void batteryInit(batteryConfig_t *initialBatteryConfig);
 
 void updateCurrentMeter(int32_t lastUpdateAt, rxConfig_t *rxConfig, uint16_t deadband3d_throttle);

src/main/sensors/gyro.c

@@ -17,6 +17,7 @@
 
 #include <stdbool.h>
 #include <stdint.h>
+#include <math.h>
 
 #include "platform.h"
 
@@ -26,11 +27,13 @@
 
 #include "drivers/sensor.h"
 #include "drivers/accgyro.h"
-#include "sensors/sensors.h"
+#include "drivers/gyro_sync.h"
+
 #include "io/beeper.h"
 #include "io/statusindicator.h"
-#include "sensors/boardalignment.h"
 
+#include "sensors/sensors.h"
+#include "sensors/boardalignment.h"
 #include "sensors/gyro.h"
 
 uint16_t calibratingG = 0;
@@ -38,16 +41,18 @@ int16_t gyroADC[XYZ_AXIS_COUNT];
 int16_t gyroZero[FLIGHT_DYNAMICS_INDEX_COUNT] = { 0, 0, 0 };
 
 static gyroConfig_t *gyroConfig;
-static int8_t * gyroFIRTable = 0L;
+
-static int16_t gyroFIRState[3][9];
+static int8_t gyroLpfCutHz = 0;
+static biquad_t gyroFilterState[XYZ_AXIS_COUNT];
+static bool gyroFilterInitialised = false;
 
 gyro_t gyro;                      // gyro access functions
 sensor_align_e gyroAlign = 0;
 
-void useGyroConfig(gyroConfig_t *gyroConfigToUse, int8_t * filterTableToUse)
+void useGyroConfig(gyroConfig_t *gyroConfigToUse, int8_t initialGyroLpfCutHz)
 {
     gyroConfig = gyroConfigToUse;
-    gyroFIRTable = filterTableToUse;
+    gyroLpfCutHz = initialGyroLpfCutHz;
 }
 
 void gyroSetCalibrationCycles(uint16_t calibrationCyclesRequired)
@@ -120,13 +125,29 @@ static void applyGyroZero(void)
 
 void gyroUpdate(void)
 {
+    int8_t axis;
+
     // range: +/- 8192; +/- 2000 deg/sec
     if (!gyro.read(gyroADC)) {
         return;
     }
 
-    if (gyroFIRTable) {
+    if (gyroLpfCutHz) {
-        filterApply9TapFIR(gyroADC, gyroFIRState, gyroFIRTable);
+        if (!gyroFilterInitialised) {
+            if (targetLooptime) {  /* Initialisation needs to happen once sample rate is known */
+                for (axis = 0; axis < 3; axis++) {
+                    filterInitBiQuad(gyroLpfCutHz, &gyroFilterState[axis], 0);
+                }
+
+                gyroFilterInitialised = true;
+            }
+        }
+
+        if (gyroFilterInitialised) {
+            for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
+                gyroADC[axis] = lrintf(filterApplyBiQuad((float) gyroADC[axis], &gyroFilterState[axis]));
+            }
+        }
     }
 
     alignSensors(gyroADC, gyroADC, gyroAlign);

src/main/sensors/gyro.h

@@ -39,7 +39,7 @@ typedef struct gyroConfig_s {
     uint8_t gyroMovementCalibrationThreshold; // people keep forgetting that moving model while init results in wrong gyro offsets. and then they never reset gyro. so this is now on by default.
 } gyroConfig_t;
 
-void useGyroConfig(gyroConfig_t *gyroConfigToUse, int8_t * filterTableToUse);
+void useGyroConfig(gyroConfig_t *gyroConfigToUse, int8_t initialGyroLpfCutHz);
 void gyroSetCalibrationCycles(uint16_t calibrationCyclesRequired);
 void gyroUpdate(void);
 bool isGyroCalibrationComplete(void);