From 01be3842c8390a92dba8711ac6e5d773ea35e0b0 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Mon, 17 Oct 2016 14:15:02 +0100 Subject: [PATCH] Added filter unit test and tidied filter code --- src/main/common/filter.c | 71 +----------- src/main/common/filter.h | 31 ++--- src/main/flight/pid.c | 10 +- src/main/sensors/gyro.c | 6 +- src/test/Makefile | 22 ++++ src/test/unit/common_filter_unittest.cc | 147 ++++++++++++++++++++++++ 6 files changed, 190 insertions(+), 97 deletions(-) create mode 100644 src/test/unit/common_filter_unittest.cc diff --git a/src/main/common/filter.c b/src/main/common/filter.c index 420418e0d3..98d82919e4 100644 --- a/src/main/common/filter.c +++ b/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]; } - -/* - * 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) +void firFilterDenoiseInit(firFilterDenoise_t *filter, uint8_t gyroSoftLpfHz, uint16_t targetLooptime) { - filter->buf = buf; - filter->bufLength = bufLength; - filter->coeffs = coeffs; - filter->coeffsLength = coeffsLength; - memset(filter->buf, 0, sizeof(int16_t) * filter->bufLength); + filter->targetCount = constrain(lrintf((1.0f / (0.000001f * (float)targetLooptime)) / gyroSoftLpfHz), 1, MAX_FIR_DENOISE_WINDOW_SIZE); } -/* - * FIR filter initialisation - * 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) +// prototype function for denoising of signal by dynamic moving average. Mainly for test purposes +float firFilterDenoiseUpdate(firFilterDenoise_t *filter, float input) { - 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; } - diff --git a/src/main/common/filter.h b/src/main/common/filter.h index 4db6999e1c..2b0e75a431 100644 --- a/src/main/common/filter.h +++ b/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]; -} firFilterState_t; + float state[MAX_FIR_DENOISE_WINDOW_SIZE]; +} 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 firFilterInt16Init2(firFilterInt16_t *filter, int16_t *buf, uint8_t bufLength, const float *coeffs, uint8_t coeffsLength); -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); +void firFilterDenoiseInit(firFilterDenoise_t *filter, uint8_t gyroSoftLpfHz, uint16_t targetLooptime); +float firFilterDenoiseUpdate(firFilterDenoise_t *filter, float input); diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index c73e04ddaa..a2c5bb4e7c 100644 --- a/src/main/flight/pid.c +++ b/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; diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 56b778e7a0..364502adeb 100644 --- a/src/main/sensors/gyro.c +++ b/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]); diff --git a/src/test/Makefile b/src/test/Makefile index bc98d05f2d..6581e7ae2b 100644 --- a/src/test/Makefile +++ b/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 \ diff --git a/src/test/unit/common_filter_unittest.cc b/src/test/unit/common_filter_unittest.cc new file mode 100644 index 0000000000..88d0c40ef6 --- /dev/null +++ b/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 . + */ + +#include +#include + +#include + +#include + +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)); +}