Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-16 21:05:35 +03:00
Code Issues Wiki Activity

Added filter unit test and tidied filter code

Browse source
This commit is contained in:
Martin Budden 2016-10-17 14:15:02 +01:00
parent eef0d7a761
commit 01be3842c8
6 changed files with 190 additions and 97 deletions
Download patch file Download diff file Expand all files Collapse all files

71
src/main/common/filter.c
View file

@ -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;
}

31
src/main/common/filter.h
View file

@ -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);

10
src/main/flight/pid.c
View file

@ -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;

6
src/main/sensors/gyro.c
View file

@ -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]);

22
src/test/Makefile
View file

@ -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 \

147
src/test/unit/common_filter_unittest.cc Normal file
View file

@ -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));
}