Merge pull request #1625 from martinbudden/bf_gyro_filter_calls

Use function pointers to simplify gyro filter calls

src/main/common/filter.c

@@ -22,6 +22,7 @@
 
 #include "common/filter.h"
 #include "common/maths.h"
+#include "common/utils.h"
 
 #define M_LN2_FLOAT 0.69314718055994530942f
 #define M_PI_FLOAT  3.14159265358979323846f
@@ -29,6 +30,16 @@
 #define BIQUAD_BANDWIDTH 1.9f     /* bandwidth in octaves */
 #define BIQUAD_Q 1.0f / sqrtf(2.0f)     /* quality factor - butterworth*/
 
+
+// NULL filter
+
+float nullFilterApply(void *filter, float input)
+{
+    UNUSED(filter);
+    return input;
+}
+
+
 // PT1 Low Pass filter
 
 void pt1FilterInit(pt1Filter_t *filter, uint8_t f_cut, float dT)
@@ -178,6 +189,12 @@ float firFilterApply(const firFilter_t *filter)
     return ret;
 }
 
+float firFilterUpdateAndApply(firFilter_t *filter, float input)
+{
+    firFilterUpdate(filter, input);
+    return firFilterApply(filter);
+}
+
 /*
  * Returns average of the last <count> items.
  */

src/main/common/filter.h

@@ -62,6 +62,9 @@ typedef struct firFilter_s {
     uint8_t coeffsLength;
 } firFilter_t;
 
+typedef float (*filterApplyFnPtr)(void *filter, float input);
+
+float nullFilterApply(void *filter, float input);
 
 void biquadFilterInitLPF(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate);
 void biquadFilterInit(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType);
@@ -77,6 +80,7 @@ void firFilterInit2(firFilter_t *filter, float *buf, uint8_t bufLength, const fl
 void firFilterUpdate(firFilter_t *filter, float input);
 void firFilterUpdateAverage(firFilter_t *filter, float input);
 float firFilterApply(const firFilter_t *filter);
+float firFilterUpdateAndApply(firFilter_t *filter, float input);
 float firFilterCalcPartialAverage(const firFilter_t *filter, uint8_t count);
 float firFilterCalcMovingAverage(const firFilter_t *filter);
 float firFilterLastInput(const firFilter_t *filter);

src/main/common/utils.h

@@ -32,7 +32,7 @@
 #define EXPAND_I(x) x
 #define EXPAND(x) EXPAND_I(x)
 
-#if !defined(USE_HAL_DRIVER)
+#if !defined(UNUSED)
 #define UNUSED(x) (void)(x)
 #endif
 #define BUILD_BUG_ON(condition) ((void)sizeof(char[1 - 2*!!(condition)]))

src/main/drivers/io.c

@@ -15,12 +15,12 @@
  * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
  */
 
- #include "common/utils.h"
-
 #include "io.h"
 #include "io_impl.h"
 #include "rcc.h"
 
+#include "common/utils.h"
+
 #include "target.h"
 
 // io ports defs are stored in array by index now

src/main/sensors/gyro.c

@@ -53,7 +53,10 @@ static uint16_t gyroSoftNotchHz1, gyroSoftNotchHz2;
 static float gyroSoftNotchQ1, gyroSoftNotchQ2;
 static uint8_t gyroSoftLpfHz;
 static uint16_t calibratingG = 0;
-static float gyroDt;
+
+static filterApplyFnPtr softLpfFilterApplyFn;
+static filterApplyFnPtr notchFilter1ApplyFn;
+static filterApplyFnPtr notchFilter2ApplyFn;
 
 void gyroUseConfig(const gyroConfig_t *gyroConfigToUse,
                    uint8_t gyro_soft_lpf_hz,
@@ -74,20 +77,40 @@ void gyroUseConfig(const gyroConfig_t *gyroConfigToUse,
 
 void gyroInit(void)
 {
-    if (gyroSoftLpfHz && gyro.targetLooptime) {  // Initialisation needs to happen once samplingrate is known
+
-        for (int axis = 0; axis < 3; axis++) {
+    softLpfFilterApplyFn = nullFilterApply;
-            if (gyroSoftLpfType == FILTER_BIQUAD)
+    notchFilter1ApplyFn = nullFilterApply;
+    notchFilter2ApplyFn = nullFilterApply;
+
+    if (gyroSoftLpfHz) {  // Initialisation needs to happen once samplingrate is known
+        if (gyroSoftLpfType == FILTER_BIQUAD) {
+            softLpfFilterApplyFn = (filterApplyFnPtr)biquadFilterApply;
+            for (int axis = 0; axis < 3; axis++) {
                 biquadFilterInitLPF(&gyroFilterLPF[axis], gyroSoftLpfHz, gyro.targetLooptime);
-            else if (gyroSoftLpfType == FILTER_PT1)
+            }
-                gyroDt = (float) gyro.targetLooptime * 0.000001f;
+        } else if (gyroSoftLpfType == FILTER_PT1) {
-            else
+            softLpfFilterApplyFn = (filterApplyFnPtr)pt1FilterApply;
+            const float gyroDt = (float) gyro.targetLooptime * 0.000001f;
+            for (int axis = 0; axis < 3; axis++) {
+                pt1FilterInit(&gyroFilterPt1[axis], gyroSoftLpfHz, gyroDt);
+            }
+        } else {
+            softLpfFilterApplyFn = (filterApplyFnPtr)firFilterDenoiseUpdate;
+            for (int axis = 0; axis < 3; axis++) {
                 firFilterDenoiseInit(&gyroDenoiseState[axis], gyroSoftLpfHz, gyro.targetLooptime);
+            }
         }
     }
 
-    if ((gyroSoftNotchHz1 || gyroSoftNotchHz2) && gyro.targetLooptime) {
+    if (gyroSoftNotchHz1) {
+        notchFilter1ApplyFn = (filterApplyFnPtr)biquadFilterApply;
         for (int axis = 0; axis < 3; axis++) {
             biquadFilterInit(&gyroFilterNotch_1[axis], gyroSoftNotchHz1, gyro.targetLooptime, gyroSoftNotchQ1, FILTER_NOTCH);
+        }
+    }
+    if (gyroSoftNotchHz1) {
+        notchFilter2ApplyFn = (filterApplyFnPtr)biquadFilterApply;
+        for (int axis = 0; axis < 3; axis++) {
             biquadFilterInit(&gyroFilterNotch_2[axis], gyroSoftNotchHz2, gyro.targetLooptime, gyroSoftNotchQ2, FILTER_NOTCH);
         }
     }
@@ -180,32 +203,20 @@ void gyroUpdate(void)
     gyroADC[Y] -= gyroZero[Y];
     gyroADC[Z] -= gyroZero[Z];
 
-    if (gyroSoftLpfHz) {
+    for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
-        for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
 
-            if (debugMode == DEBUG_GYRO)
+        if (debugMode == DEBUG_GYRO)
-                debug[axis] = gyroADC[axis];
+            debug[axis] = gyroADC[axis];
 
-            if (gyroSoftLpfType == FILTER_BIQUAD)
+        gyroADCf[axis] = softLpfFilterApplyFn(&gyroDenoiseState[axis], (float) gyroADC[axis]);
-                gyroADCf[axis] = biquadFilterApply(&gyroFilterLPF[axis], (float) gyroADC[axis]);
-            else if (gyroSoftLpfType == FILTER_PT1)
-                gyroADCf[axis] = pt1FilterApply4(&gyroFilterPt1[axis], (float) gyroADC[axis], gyroSoftLpfHz, gyroDt);
-            else
-                gyroADCf[axis] = firFilterDenoiseUpdate(&gyroDenoiseState[axis], (float) gyroADC[axis]);
 
-            if (debugMode == DEBUG_NOTCH)
+        if (debugMode == DEBUG_NOTCH)
-                debug[axis] = lrintf(gyroADCf[axis]);
+            debug[axis] = lrintf(gyroADCf[axis]);
 
-            if (gyroSoftNotchHz1)
+        gyroADCf[axis] = notchFilter1ApplyFn(&gyroFilterNotch_1[axis], gyroADCf[axis]);
-                gyroADCf[axis] = biquadFilterApply(&gyroFilterNotch_1[axis], gyroADCf[axis]);
 
-            if (gyroSoftNotchHz2)
+        gyroADCf[axis] = notchFilter2ApplyFn(&gyroFilterNotch_2[axis], gyroADCf[axis]);
-                gyroADCf[axis] = biquadFilterApply(&gyroFilterNotch_2[axis], gyroADCf[axis]);
 
-            gyroADC[axis] = lrintf(gyroADCf[axis]);
+        gyroADC[axis] = lrintf(gyroADCf[axis]);
-        }
-    } else {
-        for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++)
-            gyroADCf[axis] = gyroADC[axis];
     }
 }