Added filter unit test and tidied filter code

src/main/common/filter.c

@@ -140,7 +140,7 @@ void firFilterInit(firFilter_t *filter, float *buf, uint8_t bufLength, const flo
     firFilterInit2(filter, buf, bufLength, coeffs, bufLength);
 }
 
-void filterFirUpdate(firFilter_t *filter, float input)
+void firFilterUpdate(firFilter_t *filter, float input)
 {
     filter->buf[filter->index++] = input; // index is at the first empty buffer positon
     if (filter->index >= filter->bufLength) {
@@ -207,74 +207,14 @@ float firFilterLastInput(const firFilter_t *filter)
     return filter->buf[index];
 }
 
-
+void firFilterDenoiseInit(firFilterDenoise_t *filter, uint8_t gyroSoftLpfHz, uint16_t targetLooptime)
-/*
- *  int16_t based FIR filter
- *  Can be directly updated from devices that produce 16-bit data, eg gyros and accelerometers
- */
-void firFilterInt16Init2(firFilterInt16_t *filter, int16_t *buf, uint8_t bufLength, const float *coeffs, uint8_t coeffsLength)
 {
-    filter->buf = buf;
+    filter->targetCount = constrain(lrintf((1.0f / (0.000001f * (float)targetLooptime)) / gyroSoftLpfHz), 1, MAX_FIR_DENOISE_WINDOW_SIZE);
-    filter->bufLength = bufLength;
-    filter->coeffs = coeffs;
-    filter->coeffsLength = coeffsLength;
-    memset(filter->buf, 0, sizeof(int16_t) * filter->bufLength);
 }
 
-/*
+// prototype function for denoising of signal by dynamic moving average. Mainly for test purposes
- * FIR filter initialisation
+float firFilterDenoiseUpdate(firFilterDenoise_t *filter, float input)
- * If FIR filter is just used for averaging, coeffs can be set to NULL
- */
-void firFilterInt16Init(firFilterInt16_t *filter, int16_t *buf, uint8_t bufLength, const float *coeffs)
 {
-    firFilterInt16Init2(filter, buf, bufLength, coeffs, bufLength);
-}
-
-void firFilterInt16Update(firFilterInt16_t *filter, int16_t input)
-{
-    memmove(&filter->buf[1], &filter->buf[0], (filter->bufLength-1) * sizeof(input));
-    filter->buf[0] = input;
-}
-
-float firFilterInt16Apply(const firFilterInt16_t *filter)
-{
-    float ret = 0.0f;
-    for (int ii = 0; ii < filter->coeffsLength; ++ii) {
-        ret += filter->coeffs[ii] * filter->buf[ii];
-    }
-    return ret;
-}
-
-float firFilterInt16CalcPartialAverage(const firFilterInt16_t *filter, uint8_t count)
-{
-    float ret = 0;
-    for (int ii = 0; ii < count; ++ii) {
-        ret += filter->buf[ii];
-    }
-    return ret / count;
-}
-
-float firFilterInt16CalcAverage(const firFilterInt16_t *filter)
-{
-    return firFilterInt16CalcPartialAverage(filter, filter->coeffsLength);
-}
-
-int16_t firFilterInt16LastInput(const firFilterInt16_t *filter)
-{
-    return filter->buf[0];
-}
-
-int16_t firFilterInt16Get(const firFilter_t *filter, int index)
-{
-    return filter->buf[index];
-}
-
-void initFirFilter(firFilterState_t *filter, uint8_t gyroSoftLpfHz, uint16_t targetLooptime) {
-    filter->targetCount = constrain(lrintf((1.0f / (0.000001f * (float)targetLooptime)) / gyroSoftLpfHz), 1, MAX_FIR_WINDOW_SIZE);
-}
-
-/* prototype function for denoising of signal by dynamic moving average. Mainly for test purposes */
-float firFilterUpdate(firFilterState_t *filter, float input) {
     filter->state[filter->index] = input;
     filter->movingSum += filter->state[filter->index++];
     if (filter->index == filter->targetCount)
@@ -287,4 +227,3 @@ float firFilterUpdate(firFilterState_t *filter, float input) {
         return filter->movingSum / ++filter->filledCount + 1;
 }
 
-

src/main/common/filter.h

@@ -16,9 +16,9 @@
  */
 
 #ifdef STM32F10X
-#define MAX_FIR_WINDOW_SIZE 60
+#define MAX_FIR_DENOISE_WINDOW_SIZE 60
 #else
-#define MAX_FIR_WINDOW_SIZE 120
+#define MAX_FIR_DENOISE_WINDOW_SIZE 120
 #endif
 
 typedef struct pt1Filter_s {
@@ -33,13 +33,13 @@ typedef struct biquadFilter_s {
     float d1, d2;
 } biquadFilter_t;
 
-typedef struct firFilterState_s {
+typedef struct firFilterDenoise_s{
     int filledCount;
     int targetCount;
     int index;
     float movingSum;
-    float state[MAX_FIR_WINDOW_SIZE];
+    float state[MAX_FIR_DENOISE_WINDOW_SIZE];
-} firFilterState_t;
+} firFilterDenoise_t;
 
 typedef enum {
     FILTER_PT1 = 0,
@@ -62,13 +62,6 @@ typedef struct firFilter_s {
     uint8_t coeffsLength;
 } firFilter_t;
 
-typedef struct firFilterInt16_s {
-    int16_t *buf;
-    const float *coeffs;
-    uint8_t bufLength;
-    uint8_t coeffsLength;
-} firFilterInt16_t;
-
 
 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);
@@ -81,21 +74,13 @@ float pt1FilterApply4(pt1Filter_t *filter, float input, uint8_t f_cut, float dT)
 
 void firFilterInit(firFilter_t *filter, float *buf, uint8_t bufLength, const float *coeffs);
 void firFilterInit2(firFilter_t *filter, float *buf, uint8_t bufLength, const float *coeffs, uint8_t coeffsLength);
-void filterFirUpdate(firFilter_t *filter, float input);
+void firFilterUpdate(firFilter_t *filter, float input);
 void firFilterUpdateAverage(firFilter_t *filter, float input);
 float firFilterApply(const firFilter_t *filter);
 float firFilterCalcPartialAverage(const firFilter_t *filter, uint8_t count);
 float firFilterCalcMovingAverage(const firFilter_t *filter);
 float firFilterLastInput(const firFilter_t *filter);
 
-void firFilterInt16Init(firFilterInt16_t *filter, int16_t *buf, uint8_t bufLength, const float *coeffs);
+void firFilterDenoiseInit(firFilterDenoise_t *filter, uint8_t gyroSoftLpfHz, uint16_t targetLooptime);
-void firFilterInt16Init2(firFilterInt16_t *filter, int16_t *buf, uint8_t bufLength, const float *coeffs, uint8_t coeffsLength);
+float firFilterDenoiseUpdate(firFilterDenoise_t *filter, float input);
-void firFilterInt16Update(firFilterInt16_t *filter, int16_t input);
-float firFilterInt16Apply(const firFilterInt16_t *filter);
-float firFilterInt16CalcPartialAverage(const firFilterInt16_t *filter, uint8_t count);
-float firFilterInt16CalcAverage(const firFilterInt16_t *filter);
-int16_t firFilterInt16LastInput(const firFilterInt16_t *filter);
-int16_t firFilterInt16Get(const firFilter_t *filter, int index);
-void initFirFilter(firFilterState_t *filter, uint8_t gyroSoftLpfHz, uint16_t targetLooptime);
-float firFilterUpdate(firFilterState_t *filter, float input);
 

src/main/flight/pid.c

@@ -102,9 +102,9 @@ biquadFilter_t dtermFilterLpf[3];
 biquadFilter_t dtermFilterNotch[3];
 bool dtermNotchInitialised;
 bool dtermBiquadLpfInitialised;
-firFilterState_t dtermDenoisingState[3];
+firFilterDenoise_t dtermDenoisingState[3];
 
-void initFilters(const pidProfile_t *pidProfile) {
+static void pidInitFilters(const pidProfile_t *pidProfile) {
     int axis;
     static uint8_t lowpassFilterType;
 
@@ -120,7 +120,7 @@ void initFilters(const pidProfile_t *pidProfile) {
         }
 
         if (pidProfile->dterm_filter_type == FILTER_FIR) {
-            for (axis = 0; axis < 3; axis++) initFirFilter(&dtermDenoisingState[axis], pidProfile->dterm_lpf_hz, targetPidLooptime);
+            for (axis = 0; axis < 3; axis++) firFilterDenoiseInit(&dtermDenoisingState[axis], pidProfile->dterm_lpf_hz, targetPidLooptime);
         }
         lowpassFilterType = pidProfile->dterm_filter_type;
     }
@@ -141,7 +141,7 @@ void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclinatio
 
     float tpaFactor = PIDweight[0] / 100.0f; // tpa is now float
 
-    initFilters(pidProfile);
+    pidInitFilters(pidProfile);
 
     if (FLIGHT_MODE(HORIZON_MODE)) {
         // Figure out the raw stick positions
@@ -284,7 +284,7 @@ void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclinatio
                 else if (pidProfile->dterm_filter_type == FILTER_PT1)
                     delta = pt1FilterApply4(&deltaFilter[axis], delta, pidProfile->dterm_lpf_hz, getdT());
                 else
-                    delta = firFilterUpdate(&dtermDenoisingState[axis], delta);
+                    delta = firFilterDenoiseUpdate(&dtermDenoisingState[axis], delta);
             }
 
             DTerm = Kd[axis] * delta * tpaFactor;

src/main/sensors/gyro.c

@@ -49,7 +49,7 @@ static const gyroConfig_t *gyroConfig;
 static biquadFilter_t gyroFilterLPF[XYZ_AXIS_COUNT];
 static biquadFilter_t gyroFilterNotch_1[XYZ_AXIS_COUNT], gyroFilterNotch_2[XYZ_AXIS_COUNT];
 static pt1Filter_t gyroFilterPt1[XYZ_AXIS_COUNT];
-static firFilterState_t gyroDenoiseState[XYZ_AXIS_COUNT];
+static firFilterDenoise_t gyroDenoiseState[XYZ_AXIS_COUNT];
 static uint8_t gyroSoftLpfType;
 static uint16_t gyroSoftNotchHz1, gyroSoftNotchHz2;
 static float gyroSoftNotchQ1, gyroSoftNotchQ2;
@@ -83,7 +83,7 @@ void gyroInit(void)
             else if (gyroSoftLpfType == FILTER_PT1)
                 gyroDt = (float) gyro.targetLooptime * 0.000001f;
             else
-                initFirFilter(&gyroDenoiseState[axis], gyroSoftLpfHz, gyro.targetLooptime);
+                firFilterDenoiseInit(&gyroDenoiseState[axis], gyroSoftLpfHz, gyro.targetLooptime);
         }
     }
 
@@ -198,7 +198,7 @@ void gyroUpdate(void)
             else if (gyroSoftLpfType == FILTER_PT1)
                 gyroADCf[axis] = pt1FilterApply4(&gyroFilterPt1[axis], (float) gyroADC[axis], gyroSoftLpfHz, gyroDt);
             else
-                gyroADCf[axis] = firFilterUpdate(&gyroDenoiseState[axis], (float) gyroADC[axis]);
+                gyroADCf[axis] = firFilterDenoiseUpdate(&gyroDenoiseState[axis], (float) gyroADC[axis]);
 
             if (debugMode == DEBUG_NOTCH)
                 debug[axis] = lrintf(gyroADCf[axis]);

src/test/Makefile

@@ -110,6 +110,28 @@ $(OBJECT_DIR)/common/maths.o : \
 	@mkdir -p $(dir $@)
 	$(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/common/maths.c -o $@
 
+$(OBJECT_DIR)/common/filter.o : \
+	$(USER_DIR)/common/filter.c \
+	$(USER_DIR)/common/filter.h \
+	$(GTEST_HEADERS)
+
+	@mkdir -p $(dir $@)
+	$(CC) $(C_FLAGS) $(TEST_CFLAGS) -c $(USER_DIR)/common/filter.c -o $@
+
+$(OBJECT_DIR)/common_filter_unittest.o : \
+	$(TEST_DIR)/common_filter_unittest.cc \
+	$(GTEST_HEADERS)
+
+	@mkdir -p $(dir $@)
+	$(CXX) $(CXX_FLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/common_filter_unittest.cc -o $@
+
+$(OBJECT_DIR)/common_filter_unittest : \
+	$(OBJECT_DIR)/common_filter_unittest.o \
+	$(OBJECT_DIR)/common/filter.o \
+	$(OBJECT_DIR)/gtest_main.a
+
+	$(CXX) $(CXX_FLAGS) $^ -o $(OBJECT_DIR)/$@
+
 
 $(OBJECT_DIR)/drivers/io.o : \
 	$(USER_DIR)/drivers/io.c \

src/test/unit/common_filter_unittest.cc

@@ -0,0 +1,147 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <stdint.h>
+#include <stdbool.h>
+
+#include <limits.h>
+
+#include <math.h>
+
+extern "C" {
+    #include "common/filter.h"
+}
+
+#include "unittest_macros.h"
+#include "gtest/gtest.h"
+
+TEST(FilterUnittest, TestFirFilterInit)
+{
+#define BUFLEN 4
+    float buf[BUFLEN];
+    firFilter_t filter;
+
+
+    firFilterInit(&filter, buf, BUFLEN, NULL);
+
+    EXPECT_EQ(buf, filter.buf);
+    EXPECT_EQ(0, filter.coeffs);
+    EXPECT_EQ(0, filter.movingSum);
+    EXPECT_EQ(0, filter.index);
+    EXPECT_EQ(0, filter.count);
+    EXPECT_EQ(BUFLEN, filter.bufLength);
+    EXPECT_EQ(BUFLEN, filter.coeffsLength);
+}
+
+TEST(FilterUnittest, TestFirFilterUpdateAverage)
+{
+#define BUFLEN 4
+    float buf[BUFLEN];
+    const float coeffs[BUFLEN] = {1.0f, 1.0f, 1.0f, 1.0f};
+    firFilter_t filter;
+
+    firFilterInit(&filter, buf, BUFLEN, coeffs);
+
+    firFilterUpdateAverage(&filter, 2.0f);
+    EXPECT_FLOAT_EQ(2.0f, filter.buf[0]);
+    EXPECT_FLOAT_EQ(2.0f, filter.movingSum);
+    EXPECT_EQ(1, filter.index);
+    EXPECT_EQ(1, filter.count);
+    EXPECT_EQ(2.0f, firFilterCalcMovingAverage(&filter));
+    EXPECT_FLOAT_EQ(2.0f, firFilterCalcPartialAverage(&filter, 1));
+    EXPECT_FLOAT_EQ(2.0f, firFilterApply(&filter));
+
+    firFilterUpdateAverage(&filter, 3.0f);
+    EXPECT_FLOAT_EQ(3.0f, filter.buf[1]);
+    EXPECT_FLOAT_EQ(5.0f, filter.movingSum);
+    EXPECT_EQ(2, filter.index);
+    EXPECT_EQ(2, filter.count);
+    EXPECT_EQ(2.5f, firFilterCalcMovingAverage(&filter));
+    EXPECT_FLOAT_EQ(2.5f, firFilterCalcPartialAverage(&filter, 2));
+    EXPECT_FLOAT_EQ(5.0f, firFilterApply(&filter));
+
+    firFilterUpdateAverage(&filter, 4.0f);
+    EXPECT_FLOAT_EQ(4.0f, filter.buf[2]);
+    EXPECT_FLOAT_EQ(9.0f, filter.movingSum);
+    EXPECT_EQ(3, filter.index);
+    EXPECT_EQ(3, filter.count);
+    EXPECT_EQ(3.0f, firFilterCalcMovingAverage(&filter));
+    EXPECT_FLOAT_EQ(3.0f, firFilterCalcPartialAverage(&filter, 3));
+    EXPECT_FLOAT_EQ(9.0f, firFilterApply(&filter));
+
+    firFilterUpdateAverage(&filter, 5.0f);
+    EXPECT_FLOAT_EQ(5.0f, filter.buf[3]);
+    EXPECT_FLOAT_EQ(14.0f, filter.movingSum);
+    EXPECT_EQ(0, filter.index);
+    EXPECT_EQ(4, filter.count);
+    EXPECT_EQ(3.5f, firFilterCalcMovingAverage(&filter));
+    EXPECT_FLOAT_EQ(3.5f, firFilterCalcPartialAverage(&filter, 4));
+    EXPECT_FLOAT_EQ(14.0f, firFilterApply(&filter));
+
+    firFilterUpdateAverage(&filter, 6.0f);
+    EXPECT_FLOAT_EQ(6.0f, filter.buf[0]);
+    EXPECT_FLOAT_EQ(18.0f, filter.movingSum);
+    EXPECT_EQ(1, filter.index);
+    EXPECT_EQ(BUFLEN, filter.count);
+    EXPECT_EQ(4.5f, firFilterCalcMovingAverage(&filter));
+    EXPECT_FLOAT_EQ(4.5f, firFilterCalcPartialAverage(&filter, BUFLEN));
+    EXPECT_FLOAT_EQ(18.0f, firFilterApply(&filter));
+
+    firFilterUpdateAverage(&filter, 7.0f);
+    EXPECT_FLOAT_EQ(7.0f, filter.buf[1]);
+    EXPECT_FLOAT_EQ(22.0f, filter.movingSum);
+    EXPECT_EQ(2, filter.index);
+    EXPECT_EQ(BUFLEN, filter.count);
+    EXPECT_EQ(5.5f, firFilterCalcMovingAverage(&filter));
+    EXPECT_FLOAT_EQ(5.5f, firFilterCalcPartialAverage(&filter, BUFLEN));
+    EXPECT_FLOAT_EQ(22.0f, firFilterApply(&filter));
+}
+
+TEST(FilterUnittest, TestFirFilterApply)
+{
+#define BUFLEN 4
+    float buf[BUFLEN];
+    firFilter_t filter;
+    const float coeffs[BUFLEN] = {26.0f, 27.0f, 28.0f, 29.0f};
+
+    float expected = 0.0f;
+    firFilterInit(&filter, buf, BUFLEN, coeffs);
+
+    firFilterUpdate(&filter, 2.0f);
+    expected = 2.0f * 26.0f;
+    EXPECT_FLOAT_EQ(expected, firFilterApply(&filter));
+
+    firFilterUpdate(&filter, 3.0f);
+    expected = 3.0f * 26.0f + 2.0 * 27.0;
+    EXPECT_FLOAT_EQ(expected, firFilterApply(&filter));
+
+    firFilterUpdate(&filter, 4.0f);
+    expected = 4.0f * 26.0f + 3.0 * 27.0 + 2.0 * 28.0;
+    EXPECT_FLOAT_EQ(expected, firFilterApply(&filter));
+
+    firFilterUpdate(&filter, 5.0f);
+    expected = 5.0f * 26.0f + 4.0 * 27.0 + 3.0 * 28.0 + 2.0f * 29.0f;
+    EXPECT_FLOAT_EQ(expected, firFilterApply(&filter));
+
+    firFilterUpdate(&filter, 6.0f);
+    expected = 6.0f * 26.0f + 5.0 * 27.0 + 4.0 * 28.0 + 3.0f * 29.0f;
+    EXPECT_FLOAT_EQ(expected, firFilterApply(&filter));
+
+    firFilterUpdate(&filter, 7.0f);
+    expected = 7.0f * 26.0f + 6.0 * 27.0 + 5.0 * 28.0 + 4.0f * 29.0f;
+    EXPECT_FLOAT_EQ(expected, firFilterApply(&filter));
+}