From 8f0d7de7910fec89872e7d9358cbfca3d342d3e6 Mon Sep 17 00:00:00 2001
From: borisbstyle <dreambb1982@gmail.com>
Date: Thu, 10 Dec 2015 21:37:57 +0100
Subject: [PATCH] Simplify filters

---
 src/main/common/filter.c | 34 +++++++++++-----------------------
 src/main/common/filter.h |  3 +--
 src/main/config/config.c |  5 ++---
 src/main/sensors/gyro.c  | 11 +++++------
 src/main/sensors/gyro.h  |  2 +-
 5 files changed, 20 insertions(+), 35 deletions(-)

diff --git a/src/main/common/filter.c b/src/main/common/filter.c
index f37196710d..9f6373f49f 100644
--- a/src/main/common/filter.c
+++ b/src/main/common/filter.c
@@ -10,10 +10,8 @@
 #include <stdlib.h>
 #include <math.h>
 
-#include "common/axis.h"
 #include "common/filter.h"
 #include "common/axis.h"
-#include "common/maths.h"
 
 
 // PT1 Low Pass filter (when no dT specified it will be calculated from the cycleTime)
@@ -29,33 +27,23 @@ float filterApplyPt1(float input, filterStatePt1_t *filter, uint8_t f_cut, float
     return filter->state;
 }
 
-/**
- * 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[7] = { 12, 23, 40, 51, 52, 40, 38 };   // 1khz; group delay 2.5ms; -0.5db = 32Hz ; -1db = 45Hz; -5db = 97Hz; -10db = 132hz
-
-int8_t * filterGetFIRCoefficientsTable(void)
-{
-    return gyroFIRCoeff_1000;
-}
-
-// 9 Tap FIR filter as described here:
-// Thanks to Qcopter & BorisB & DigitalEntity
-void filterApplyFIR(int16_t data[3], int16_t state[3][7], int8_t coeff[7])
-{
+// 7 Tap FIR filter as described here:
+// Thanks to Qcopter
+void filterApply7TapFIR(int16_t data[]) {
+    int16_t FIRcoeff[7] = { 12, 23, 40, 51, 52, 40, 38 }; // TODO - More coefficients needed. Now fixed to 1khz
+    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 <= 7; i++) {
-            state[axis][i] = state[axis][i + 1];
-            FIRsum += state[axis][i] * (int16_t)coeff[i];
+        for (i = 0; i <= 5; i++) {
+            gyro_delay[axis][i] = gyro_delay[axis][i + 1];
+            FIRsum += gyro_delay[axis][i] * FIRcoeff[i];
         }
-        state[axis][8] = data[axis];
-        FIRsum += state[axis][8] * coeff[8];
+        gyro_delay[axis][6] = data[axis];
+        FIRsum += gyro_delay[axis][6] * FIRcoeff[6];
         data[axis] = FIRsum / 256;
     }
 }
diff --git a/src/main/common/filter.h b/src/main/common/filter.h
index 1406ec55c5..39afd96ee5 100644
--- a/src/main/common/filter.h
+++ b/src/main/common/filter.h
@@ -13,5 +13,4 @@ typedef struct filterStatePt1_s {
 } filterStatePt1_t;
 
 float filterApplyPt1(float input, filterStatePt1_t *filter, uint8_t f_cut, float dt);
-int8_t * filterGetFIRCoefficientsTable(void);
-void filterApplyFIR(int16_t data[3], int16_t state[3][7], int8_t coeff[7]);
+void filterApplyFIR(int16_t data[3]);
diff --git a/src/main/config/config.c b/src/main/config/config.c
index c0b6f337a2..28dcaa965f 100755
--- a/src/main/config/config.c
+++ b/src/main/config/config.c
@@ -694,9 +694,8 @@ void activateConfig(void)
         &currentProfile->pidProfile
     );
 
-    if (currentProfile->pidProfile.gyro_soft_lpf) {
-        useGyroConfig(&masterConfig.gyroConfig, filterGetFIRCoefficientsTable()); // Leave this for more coefficients in the future
-    }
+
+    useGyroConfig(&masterConfig.gyroConfig, currentProfile->pidProfile.gyro_soft_lpf); // Leave this for more coefficients in the future
 
 #ifdef TELEMETRY
     telemetryUseConfig(&masterConfig.telemetryConfig);
diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c
index e6ee7f0162..22a5efc945 100644
--- a/src/main/sensors/gyro.c
+++ b/src/main/sensors/gyro.c
@@ -38,16 +38,15 @@ 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][7];
+static uint8_t gyroFIRFilter;
 
 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 filter)
 {
     gyroConfig = gyroConfigToUse;
-    gyroFIRTable = filterTableToUse;
+    gyroFIRFilter = filter;
 }
 
 void gyroSetCalibrationCycles(uint16_t calibrationCyclesRequired)
@@ -125,8 +124,8 @@ void gyroUpdate(void)
         return;
     }
 
-    if (gyroFIRTable) {
-        filterApplyFIR(gyroADC, gyroFIRState, gyroFIRTable);
+    if (gyroFIRFilter) {
+        filterApplyFIR(gyroADC);
     }
 
     alignSensors(gyroADC, gyroADC, gyroAlign);
diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h
index 6fa5f5e40d..6d0b6e7e84 100644
--- a/src/main/sensors/gyro.h
+++ b/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 filter);
 void gyroSetCalibrationCycles(uint16_t calibrationCyclesRequired);
 void gyroUpdate(void);
 bool isGyroCalibrationComplete(void);