Full Filter Rework FIR replaced by BiQuad

src/main/common/filter.c

@@ -24,6 +24,8 @@
 #include "common/filter.h"
 #include "common/maths.h"
 
+#include "drivers/gyro_sync.h"
+
 
 // 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) {
@@ -38,49 +40,64 @@ float filterApplyPt1(float input, filterStatePt1_t *filter, uint8_t f_cut, float
     return filter->state;
 }
 
-static int8_t gyroFIRCoeff_500[FILTER_TAPS] = { 0, 18, 14, 16, 20, 22, 24, 25, 25, 24, 20, 18, 12, 18 };  // looptime=500;
+void setBiQuadCoefficients(int type, biquad_t *state) {
-static int8_t gyroFIRCoeff_1000[FILTER_TAPS] = { 0, 0, 0, 0, 0, 0, 0, 12, 23, 40, 51, 52, 40, 38 }; // looptime=1000; group delay 2.5ms; -0.5db = 32Hz ; -1db = 45Hz; -5db = 97Hz; -10db = 132Hz
 
-static int8_t deltaFIRCoeff_500[FILTER_TAPS] = {36, 12, 14, 14, 16, 16, 18, 18, 18, 16, 16, 14, 12, 36};
+    /* zero initial samples */
-static int8_t deltaFIRCoeff_1000[FILTER_TAPS] = {0, 0, 0, 0, 0, 18, 12, 28, 40, 44, 40, 32, 22, 20};
+    state->x1=0;
+    state->x2=0;
+    state->y1=0;
+    state->y2=0;
 
-int8_t * filterGetFIRCoefficientsTable(uint8_t filter_type, uint32_t targetLooptime)
+	/* set coefficients */
-{
+	switch(type) {
-	int8_t *filterCoeff;
+	    case(GYRO_FILTER):
-
-    switch(filter_type){
-        case(0):
-		    filterCoeff = NULL;
-            break;
-        case(1):
             if (targetLooptime == 500) {
-                filterCoeff = gyroFIRCoeff_500;
+                state->a0= 0.007820199;
-            } else {    // filter for 1kHz looptime
+                state->a1= 0.015640399;
-                filterCoeff = gyroFIRCoeff_1000;
+                state->a2= 0.007820199;
-            }
+                state->a3= -1.73472382;
+                state->a4= 0.766004619;
+            } else {
+                state->a0= 0.027859711;
+                state->a1= 0.055719422;
+                state->a2= 0.027859711;
+                state->a3= -1.47547752;
+                state->a4= 0.586916365;
+	        }
             break;
-        case(2):
+	    case(DELTA_FILTER):
             if (targetLooptime == 500) {
-                filterCoeff = deltaFIRCoeff_500;
+                state->a0= 0.003621679;
-            } else {    // filter for 1kHz looptime
+                state->a1= 0.007243357;
-                filterCoeff = deltaFIRCoeff_1000;
+                state->a2= 0.003621679;
+                state->a3= -1.82269350;
+                state->a4= 0.837180216;
+            } else {
+                state->a0= 0.013359181;
+                state->a1= 0.026718362;
+                state->a2= 0.013359181;
+                state->a3= -1.64745762;
+                state->a4= 0.700894342;
             }
-    }
+	}
-    return filterCoeff;
 }
 
-// Thanks to Qcopter & BorisB & DigitalEntity
+/* Computes a BiQuad filter on a sample */
-void filterApplyFIR(int16_t *data, int16_t state[FILTER_TAPS], int8_t coeff[FILTER_TAPS])
+float applyBiQuadFilter(float sample, biquad_t *state)
 {
-    int32_t FIRsum;
+    float result;
-    FIRsum = 0;
-    int i;
 
-    for (i = 0; i <= FILTER_TAPS-2; i++) {
+    /* compute result */
-        state[i] = state[i + 1];
+    result = state->a0 * sample + state->a1 * state->x1 + state->a2 * state->x2 -
-        FIRsum += state[i] * (int16_t)coeff[i];
+        state->a3 * state->y1 - state->a4 * state->y2;
-    }
+
-    state[FILTER_TAPS-1] = *data;
+    /* shift x1 to x2, sample to x1 */
-    FIRsum += state[FILTER_TAPS-1] * coeff[FILTER_TAPS-1];
+    state->x2 = state->x1;
-    *data = FIRsum / 256;
+    state->x1 = sample;
+
+    /* shift y1 to y2, result to y1 */
+    state->y2 = state->y1;
+    state->y1 = result;
+
+    return result;
 }

src/main/common/filter.h

@@ -23,6 +23,18 @@ 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;
+
+typedef enum {
+    GYRO_FILTER,
+    ACCELEROMETER_FILTER,
+    DELTA_FILTER
+} filterType_e;
+
 float filterApplyPt1(float input, filterStatePt1_t *filter, uint8_t f_cut, float dt);
-int8_t * filterGetFIRCoefficientsTable(uint8_t filter_level, uint32_t targetLooptime);
+float applyBiQuadFilter(float sample, biquad_t * b);
-void filterApplyFIR(int16_t *data, int16_t state[FILTER_TAPS], int8_t coeff[FILTER_TAPS]);
+void setBiQuadCoefficients(int type, biquad_t *state);

src/main/config/config.c

@@ -747,8 +747,7 @@ void activateConfig(void)
         &currentProfile->pidProfile
     );
 
-
+    useGyroConfig(&masterConfig.gyroConfig, currentProfile->pidProfile.gyro_soft_lpf);
-    useGyroConfig(&masterConfig.gyroConfig, filterGetFIRCoefficientsTable(currentProfile->pidProfile.gyro_soft_lpf, targetLooptime));
 
 #ifdef TELEMETRY
     telemetryUseConfig(&masterConfig.telemetryConfig);

src/main/flight/pid.c

@@ -112,8 +112,10 @@ void airModePlus(airModePlus_t *axisState, int axis, pidProfile_t *pidProfile) {
 const angle_index_t rcAliasToAngleIndexMap[] = { AI_ROLL, AI_PITCH };
 
 static airModePlus_t airModePlusAxisState[3];
-static int8_t * deltaFIRTable = 0L;
+//static int8_t * deltaFIRTable = 0L;
-static int16_t deltaFIRState[3][FILTER_TAPS];
+//static int16_t deltaFIRState[3][FILTER_TAPS];
+static bool deltaStateIsSet = false;
+static biquad_t deltaBiQuadState[3];
 
 static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig,
         uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig)
@@ -126,7 +128,10 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
     float horizonLevelStrength = 1;
     static float previousErrorGyroIf[3] = { 0.0f, 0.0f, 0.0f };
 
-    if (!deltaFIRTable) deltaFIRTable = filterGetFIRCoefficientsTable(2, targetLooptime);
+    if (!deltaStateIsSet) {
+        for (axis = 0; axis < 3; axis++) setBiQuadCoefficients(DELTA_FILTER, &deltaBiQuadState[axis]);
+        deltaStateIsSet = true;
+    }
 
     if (FLIGHT_MODE(HORIZON_MODE)) {
         // Figure out the raw stick positions
@@ -210,16 +215,12 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
         delta = RateError - lastError[axis];
         lastError[axis] = RateError;
 
+        delta = applyBiQuadFilter(delta, &deltaBiQuadState[axis]);
 
         // 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 *= (1.0f / dT);
 
-        /* Delta filtering is in integers, which should be fine */
-        int16_t deltaTemp = (int16_t) delta;
-        filterApplyFIR(&deltaTemp, deltaFIRState[axis], deltaFIRTable);
-        delta = (float) deltaTemp;
-
         DTerm = constrainf(delta * pidProfile->D_f[axis] * PIDweight[axis] / 100, -300.0f, 300.0f);
 
         // -----calculate total PID output
@@ -249,15 +250,17 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
     UNUSED(rxConfig);
 
     int axis;
-    int16_t delta;
+    int32_t PTerm, ITerm, DTerm, delta;
-    int32_t PTerm, ITerm, DTerm;
     static int32_t lastError[3] = { 0, 0, 0 };
     static int32_t previousErrorGyroI[3] = { 0, 0, 0 };
     int32_t AngleRateTmp, RateError, gyroRate;
 
     int8_t horizonLevelStrength = 100;
 
-    if (!deltaFIRTable) deltaFIRTable = filterGetFIRCoefficientsTable(2, targetLooptime);
+    if (!deltaStateIsSet) {
+        for (axis = 0; axis < 3; axis++) setBiQuadCoefficients(DELTA_FILTER, &deltaBiQuadState[axis]);
+        deltaStateIsSet = true;
+    }
 
     if (FLIGHT_MODE(HORIZON_MODE)) {
         // Figure out the raw stick positions
@@ -340,15 +343,15 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
         }
 
         //-----calculate D-term
-        delta = (int16_t) RateError - lastError[axis]; // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
+        delta = RateError - lastError[axis]; // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
         lastError[axis] = RateError;
 
+        delta = lrintf(applyBiQuadFilter((float) delta, &deltaBiQuadState[axis]));
+
         // 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 / ((uint16_t)targetLooptime >> 4))) >> 6;
 
-        filterApplyFIR(&delta, deltaFIRState[axis], deltaFIRTable);
-
         DTerm = (delta * 3 * pidProfile->D8[axis] * PIDweight[axis] / 100) >> 8;
 
         // -----calculate total PID output

src/main/sensors/gyro.c

@@ -38,17 +38,20 @@ 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 biquad_t gyroBiQuadState[3];
-static int16_t gyroFIRState[3][FILTER_TAPS];
+static bool useSoftFilter;
-
 
 gyro_t gyro;                      // gyro access functions
 sensor_align_e gyroAlign = 0;
 
-void useGyroConfig(gyroConfig_t *gyroConfigToUse, int8_t * filterTableToUse)
+void useGyroConfig(gyroConfig_t *gyroConfigToUse, uint8_t useFilter)
 {
+    int axis;
     gyroConfig = gyroConfigToUse;
-    gyroFIRTable = filterTableToUse;
+    if (useFilter) {
+        useSoftFilter = true;
+        for (axis = 0; axis < 3; axis++) setBiQuadCoefficients(GYRO_FILTER, &gyroBiQuadState[axis]);
+    }
 }
 
 void gyroSetCalibrationCycles(uint16_t calibrationCyclesRequired)
@@ -126,9 +129,10 @@ void gyroUpdate(void)
         return;
     }
 
-    if (gyroFIRTable) {
+    if (useSoftFilter) {
         int axis;
-        for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) filterApplyFIR(&gyroADC[axis], gyroFIRState[axis], gyroFIRTable);
+        //for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) filterApplyFIR(&gyroADC[axis], gyroFIRState[axis], gyroFIRTable);
+        for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) gyroADC[axis] = applyBiQuadFilter((float) gyroADC[axis], &gyroBiQuadState[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, uint8_t useFilter);
 void gyroSetCalibrationCycles(uint16_t calibrationCyclesRequired);
 void gyroUpdate(void);
 bool isGyroCalibrationComplete(void);