Substitute pt1 filter of gyroADC for 7 Tap FIR filter

src/main/common/filter.c

@@ -11,6 +11,7 @@
 #include <math.h>
 
 #include "common/filter.h"
+#include "common/axis.h"
 
 
 // PT1 Low Pass filter (when no dT specified it will be calculated from the cycleTime)
@@ -25,3 +26,24 @@ float filterApplyPt1(float input, filterStatePt1_t *filter, uint8_t f_cut, float
 
     return filter->state;
 }
+
+// 7 Tap FIR filter as described here:
+// http://www.rcgroups.com/forums/showpost.php?p=32723398&postcount=2776
+void filterApply7TapFIR(int16_t data[]) {
+    int16_t FIRcoeff[7] = { 12, 23, 40, 51, 52, 40, 38 };
+    static int16_t gyro_delay[3][7] = { {0}, {0}, {0} };
+    int32_t FIRsum;
+    int axis, i;
+
+    // 7 tap FIR, <-20dB at >170Hz with looptime 1ms, groupdelay = 2.5ms
+    for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
+        FIRsum = 0;
+        for (i = 0; i <= 5; i++) {    
+            gyro_delay[axis][i] = gyro_delay[axis][i + 1];
+            FIRsum += gyro_delay[axis][i] * FIRcoeff[i];
+        }
+        gyro_delay[axis][6] = data[axis];
+        FIRsum += gyro_delay[axis][6] * FIRcoeff[6];
+        data[axis] = FIRsum / 256;
+    }
+}

src/main/common/filter.h

@@ -13,3 +13,5 @@ typedef struct filterStatePt1_s {
 } filterStatePt1_t;
 
 float filterApplyPt1(float input, filterStatePt1_t *filter, uint8_t f_cut, float dt);
+
+void filterApply7TapFIR(int16_t data[]);

src/main/config/config.c

@@ -171,6 +171,7 @@ static void resetPidProfile(pidProfile_t *pidProfile)
     pidProfile->D8[PIDVEL] = 1;
 
     pidProfile->yaw_p_limit = YAW_P_LIMIT_MAX;
+    pidProfile->gyro_fir_filter_enable = 0;
     pidProfile->gyro_cut_hz = 0;
     pidProfile->pterm_cut_hz = 60;
     pidProfile->dterm_cut_hz = 17;

src/main/flight/pid.c

@@ -111,8 +111,7 @@ 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;
 
@@ -210,17 +209,13 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
         // 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);
-        // add moving average here to reduce noise
-        deltaSum = delta1[axis] + delta2[axis] + delta;
-        delta2[axis] = delta1[axis];
-        delta1[axis] = delta;
 
         // Dterm low pass
         if (pidProfile->dterm_cut_hz) {
-            deltaSum = filterApplyPt1(deltaSum, &DTermState[axis], pidProfile->dterm_cut_hz, dT);
+            delta = filterApplyPt1(delta, &DTermState[axis], pidProfile->dterm_cut_hz, dT);
         }
 
-        DTerm = constrainf((deltaSum / 3.0f) * pidProfile->D_f[axis] * PIDweight[axis] / 100, -300.0f, 300.0f);
+        DTerm = constrainf((delta / 3.0f) * pidProfile->D_f[axis] * PIDweight[axis] / 100, -300.0f, 300.0f);
 
         // -----calculate total PID output
         axisPID[axis] = constrain(lrintf(PTerm + ITerm + DTerm), -1000, 1000);

src/main/flight/pid.h

@@ -65,6 +65,7 @@ typedef struct pidProfile_s {
     uint8_t dterm_cut_hz;                   // (default 17Hz, Range 1-50Hz) Used for PT1 element in PID1, PID2 and PID5
     uint8_t pterm_cut_hz;                   // Used for fitlering Pterm noise on noisy frames
     uint8_t gyro_cut_hz;                    // Used for soft gyro filtering
+    uint8_t gyro_fir_filter_enable;         // Switches PT1 filter to 7TapFIRFilter for gyro
 } pidProfile_t;
 
 #define DEGREES_TO_DECIDEGREES(angle) (angle * 10)

src/main/io/serial_cli.c

@@ -522,6 +522,7 @@ const clivalue_t valueTable[] = {
 	{ "gyro_cut_hz",                VAR_UINT8  | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gyro_cut_hz, 0, 200 },
 	{ "pterm_cut_hz",               VAR_UINT8  | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.pterm_cut_hz, 0, 200 },
 	{ "dterm_cut_hz",               VAR_UINT8  | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_cut_hz, 0, 200 },
+    { "gyro_fir_filter_enable",     VAR_UINT8  | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gyro_fir_filter_enable, 0, 1 },
 
 #ifdef BLACKBOX
     { "blackbox_rate_num",          VAR_UINT8  | MASTER_VALUE,  &masterConfig.blackbox_rate_num, 1, 32 },

src/main/mw.c

@@ -812,7 +812,9 @@ void loop(void)
 
         dT = (float)cycleTime * 0.000001f;
 
-        if (currentProfile->pidProfile.gyro_cut_hz) {
+        if (currentProfile->pidProfile.gyro_fir_filter_enable) {
+            filterApply7TapFIR(gyroADC);
+        else if (currentProfile->pidProfile.gyro_cut_hz) {
             filterGyro();
         }