D filter rewrite to FIR

src/main/common/filter.c

@@ -38,37 +38,50 @@ float filterApplyPt1(float input, filterStatePt1_t *filter, uint8_t f_cut, float
     return filter->state;
 }
 
-static int8_t gyroFIRCoeff_500[FILTER_TAPS] = { 18, 14, 16, 20, 22, 24, 25, 25, 24, 20, 18, 12, 18 };  // looptime=500;
+static int8_t gyroFIRCoeff_500[FILTER_TAPS] = { 0, 18, 14, 16, 20, 22, 24, 25, 25, 24, 20, 18, 12, 18 };  // looptime=500;
-static int8_t gyroFIRCoeff_1000[FILTER_TAPS] = { 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 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
 
-int8_t * filterGetFIRCoefficientsTable(uint8_t filter_level, uint32_t targetLooptime)
+
+static int8_t deltaFIRCoeff_500[FILTER_TAPS] = {0, 0, 0, 0, 0, 18, 12, 28, 40, 44, 40, 32, 22, 20};
+static int8_t deltaFIRCoeff_1000[FILTER_TAPS] = {36, 12, 14, 14, 16, 16, 18, 18, 18, 16, 16, 14, 12, 36};
+
+int8_t * filterGetFIRCoefficientsTable(uint8_t filter_type, uint32_t targetLooptime)
 {
-    if (filter_level == 0) {
+	int8_t *filterCoeff;
-        return NULL;
-    }
 
-    // filter for 2kHz looptime
+    switch(filter_type){
-    if (targetLooptime == 500) {
+        case(0):
-        return gyroFIRCoeff_500;
+		    filterCoeff = NULL;
-    } else {    // filter for 1kHz looptime
+            break;
-        return gyroFIRCoeff_1000;
+        case(1):
+            if (targetLooptime == 500) {
+                filterCoeff = gyroFIRCoeff_500;
+            } else {    // filter for 1kHz looptime
+                filterCoeff = gyroFIRCoeff_1000;
+            }
+            break;
+        case(2):
+            if (targetLooptime == 500) {
+                filterCoeff = deltaFIRCoeff_500;
+            } else {    // filter for 1kHz looptime
+                filterCoeff = deltaFIRCoeff_1000;
+            }
     }
+    return filterCoeff;
 }
 
 // Thanks to Qcopter & BorisB & DigitalEntity
-void filterApplyFIR(int16_t data[3], int16_t state[3][FILTER_TAPS], int8_t coeff[FILTER_TAPS])
+void filterApplyFIR(int16_t *data, int16_t state[FILTER_TAPS], int8_t coeff[FILTER_TAPS])
 {
     int32_t FIRsum;
-    int axis, i;
+    FIRsum = 0;
+    int i;
 
-    for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
+    for (i = 0; i <= FILTER_TAPS-2; i++) {
-        FIRsum = 0;
+        state[i] = state[i + 1];
-        for (i = 0; i <= FILTER_TAPS-2; i++) {
+        FIRsum += state[i] * (int16_t)coeff[i];
-            state[axis][i] = state[axis][i + 1];
-            FIRsum += state[axis][i] * (int16_t)coeff[i];
-        }
-        state[axis][FILTER_TAPS-1] = data[axis];
-        FIRsum += state[axis][FILTER_TAPS-1] * coeff[FILTER_TAPS-1];
-        data[axis] = FIRsum / 256;
     }
+    state[FILTER_TAPS-1] = *data;
+    FIRsum += state[FILTER_TAPS-1] * coeff[FILTER_TAPS-1];
+    *data = FIRsum / 256;
 }

src/main/common/filter.h

@@ -15,7 +15,7 @@
  * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
  */
 
-#define FILTER_TAPS 13
+#define FILTER_TAPS 14
 
 typedef struct filterStatePt1_s {
 	float state;
@@ -25,4 +25,4 @@ typedef struct filterStatePt1_s {
 
 float filterApplyPt1(float input, filterStatePt1_t *filter, uint8_t f_cut, float dt);
 int8_t * filterGetFIRCoefficientsTable(uint8_t filter_level, uint32_t targetLooptime);
-void filterApplyFIR(int16_t data[3], int16_t state[3][FILTER_TAPS], int8_t coeff[FILTER_TAPS]);
+void filterApplyFIR(int16_t *data, int16_t state[FILTER_TAPS], int8_t coeff[FILTER_TAPS]);

src/main/flight/pid.c

@@ -64,8 +64,6 @@ uint8_t dynP8[3], dynI8[3], dynD8[3], PIDweight[3];
 static int32_t errorGyroI[3] = { 0, 0, 0 };
 static float errorGyroIf[3] = { 0.0f, 0.0f, 0.0f };
 
-static uint8_t deltaTotalSamples = 0;
-
 static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig,
         uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig);
 
@@ -85,18 +83,6 @@ void pidResetErrorGyro(void)
     errorGyroIf[YAW] = 0.0f;
 }
 
-void setPidDeltaSamples(uint8_t filter) {
-    if (!filter) {
-        if (targetLooptime < 1000) {
-            deltaTotalSamples = 8;
-        } else {
-           deltaTotalSamples = 4;
-        }
-    } else {
-        deltaTotalSamples = 1;
-    }
-}
-
 void airModePlus(airModePlus_t *axisState, int axis, pidProfile_t *pidProfile, float referenceTerm) {
     float rcCommandReflection = (float)rcCommand[axis] / 500.0f;
 
@@ -137,23 +123,22 @@ void airModePlus(airModePlus_t *axisState, int axis, pidProfile_t *pidProfile, f
 
 const angle_index_t rcAliasToAngleIndexMap[] = { AI_ROLL, AI_PITCH };
 
-static filterStatePt1_t DTermState[3];
 static airModePlus_t airModePlusAxisState[3];
+static int8_t * deltaFIRTable = 0L;
+static int16_t deltaFIRState[3][FILTER_TAPS];
 
 static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig,
         uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig)
 {
     float RateError, AngleRate, gyroRate;
     float ITerm,PTerm,DTerm;
-    static float lastGyroRate[3];
     static float lastError[3];
-    static float previousDelta[3][8];
+    float delta;
-    float delta, deltaSum;
+    int axis;
-    int axis, deltaCount;
     float horizonLevelStrength = 1;
     static float previousErrorGyroIf[3] = { 0.0f, 0.0f, 0.0f };
 
-    if (!deltaTotalSamples) setPidDeltaSamples(pidProfile->dterm_cut_hz);
+    if (!deltaFIRTable) deltaFIRTable = filterGetFIRCoefficientsTable(2, targetLooptime);
 
     if (FLIGHT_MODE(HORIZON_MODE)) {
         // Figure out the raw stick positions
@@ -234,33 +219,20 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
         ITerm = errorGyroIf[axis];
 
         //-----calculate D-term
-        if (pidProfile->delta_from_gyro_error || axis == YAW) {
+        delta = RateError - lastError[axis];
-            delta = -(gyroRate - lastGyroRate[axis]);  // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
+        lastError[axis] = RateError;
-            lastGyroRate[axis] = gyroRate;
+
-        } else {
-            delta = RateError - lastError[axis];
-            lastError[axis] = RateError;
-        }
 
         // 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);
 
-        deltaSum = 0;
+        /* Delta filtering is in integers, which should be fine */
-        if (pidProfile->dterm_cut_hz) {
+        int16_t deltaTemp = (int16_t) delta;
-            // Dterm low pass
+        filterApplyFIR(&deltaTemp, deltaFIRState[axis], deltaFIRTable);
-            deltaSum = filterApplyPt1(delta, &DTermState[axis], pidProfile->dterm_cut_hz, dT);
+        delta = (float) deltaTemp;
-        }
 
-        // Apply moving average
+        DTerm = constrainf(delta * pidProfile->D_f[axis] * PIDweight[axis] / 100, -300.0f, 300.0f);
-        if (deltaTotalSamples > 1) {
-            for (deltaCount = deltaTotalSamples-1; deltaCount > 0; deltaCount--) previousDelta[axis][deltaCount] = previousDelta[axis][deltaCount-1];
-            previousDelta[axis][0] = delta;
-            for (deltaCount = 0; deltaCount < deltaTotalSamples; deltaCount++) deltaSum += previousDelta[axis][deltaCount];
-            deltaSum = (deltaSum / deltaTotalSamples);
-        }
-
-        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);
@@ -288,18 +260,16 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
 {
     UNUSED(rxConfig);
 
-    int axis, deltaCount;
+    int axis;
-    int32_t delta, deltaSum;
+    int16_t delta;
-    static int32_t previousDelta[3][8];
     int32_t PTerm, ITerm, DTerm;
     static int32_t lastError[3] = { 0, 0, 0 };
-    static int32_t lastGyroRate[3] = { 0, 0, 0 };
     static int32_t previousErrorGyroI[3] = { 0, 0, 0 };
     int32_t AngleRateTmp, RateError, gyroRate;
 
     int8_t horizonLevelStrength = 100;
 
-    if (!deltaTotalSamples) setPidDeltaSamples(pidProfile->dterm_cut_hz);
+    if (!deltaFIRTable) deltaFIRTable = filterGetFIRCoefficientsTable(2, targetLooptime);
 
     if (FLIGHT_MODE(HORIZON_MODE)) {
         // Figure out the raw stick positions
@@ -383,32 +353,16 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
         }
 
         //-----calculate D-term
-        if (pidProfile->delta_from_gyro_error || axis == YAW) {   // quick and dirty solution for testing
+        delta = (int16_t) RateError - lastError[axis]; // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
-            delta = -(gyroRate - lastGyroRate[axis]);  // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
+        lastError[axis] = RateError;
-            lastGyroRate[axis] = gyroRate;
-        } else {
-            delta = RateError - lastError[axis]; // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
-            lastError[axis] = RateError;
-        }
 
         // 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;
 
-        // Apply moving average
+        filterApplyFIR(&delta, deltaFIRState[axis], deltaFIRTable);
-        deltaSum = 0;
-        if (pidProfile->dterm_cut_hz) {
-            // Dterm low pass
-            deltaSum = filterApplyPt1(delta, &DTermState[axis], pidProfile->dterm_cut_hz, dT);
-        }
-        if (deltaTotalSamples > 1) {
-            for (deltaCount = deltaTotalSamples-1; deltaCount > 0; deltaCount--) previousDelta[axis][deltaCount] = previousDelta[axis][deltaCount-1];
-            previousDelta[axis][0] = delta;
-            for (deltaCount = 0; deltaCount < deltaTotalSamples; deltaCount++) deltaSum += previousDelta[axis][deltaCount];
-        }
-        deltaSum = (deltaSum / deltaTotalSamples) * 3; // get old scaling by multiplying with 3
 
-        DTerm = (deltaSum * pidProfile->D8[axis] * PIDweight[axis] / 100) >> 8;
+        DTerm = (delta * 2 * pidProfile->D8[axis] * PIDweight[axis] / 100) >> 8;
 
         // -----calculate total PID output
         axisPID[axis] = PTerm + ITerm + DTerm;

src/main/io/serial_cli.c

@@ -656,8 +656,6 @@ const clivalue_t valueTable[] = {
     { "d_vel",                      VAR_UINT8  | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.D8[PIDVEL], .config.minmax = { 0,  200 } },
 
     { "gyro_soft_lpf",              VAR_UINT8  | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.gyro_soft_lpf, .config.lookup = { TABLE_OFF_ON } },
-    { "dterm_cut_hz",               VAR_UINT8  | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_cut_hz, .config.minmax = {0, 200 } },
-    { "delta_from_gyro_error",      VAR_UINT8  | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.delta_from_gyro_error, .config.minmax = {0, 1} },
     { "insane_acrobility_factor",   VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.airModeInsaneAcrobilityFactor, .config.minmax = {0, 30 } },
 
 #ifdef BLACKBOX

src/main/sensors/gyro.c

@@ -127,7 +127,8 @@ void gyroUpdate(void)
     }
 
     if (gyroFIRTable) {
-        filterApplyFIR(gyroADC, gyroFIRState, gyroFIRTable);
+        int axis;
+        for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) filterApplyFIR(&gyroADC[axis], gyroFIRState[axis], gyroFIRTable);
     }
 
     alignSensors(gyroADC, gyroADC, gyroAlign);