From 3125ae252baa1e6fb4513e3a26b26d25731e937d Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Sat, 30 Dec 2017 00:12:49 +0000 Subject: [PATCH] Tidied gyro filter code --- src/main/common/filter.c | 2 +- src/main/common/filter.h | 7 +++-- src/main/sensors/gyro.c | 50 ++++++++++++++++++++-------------- src/main/sensors/gyroanalyse.c | 18 ------------ src/main/sensors/gyroanalyse.h | 1 - 5 files changed, 36 insertions(+), 42 deletions(-) diff --git a/src/main/common/filter.c b/src/main/common/filter.c index 303ba0138e..45cf02a9d4 100644 --- a/src/main/common/filter.c +++ b/src/main/common/filter.c @@ -33,7 +33,7 @@ // NULL filter -FAST_CODE float nullFilterApply(void *filter, float input) +FAST_CODE float nullFilterApply(filter_t *filter, float input) { UNUSED(filter); return input; diff --git a/src/main/common/filter.h b/src/main/common/filter.h index 2f77bc6d7f..25df99ba04 100644 --- a/src/main/common/filter.h +++ b/src/main/common/filter.h @@ -25,6 +25,9 @@ #define MAX_FIR_DENOISE_WINDOW_SIZE 120 #endif +struct filter_s; +typedef struct filter_s filter_t; + typedef struct pt1Filter_s { float state; float k; @@ -73,9 +76,9 @@ typedef struct firFilter_s { uint8_t coeffsLength; } firFilter_t; -typedef float (*filterApplyFnPtr)(void *filter, float input); +typedef float (*filterApplyFnPtr)(filter_t *filter, float input); -float nullFilterApply(void *filter, float input); +float nullFilterApply(filter_t *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); diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 94de90c2b9..9000790e42 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -29,6 +29,8 @@ #include "common/maths.h" #include "common/filter.h" +#include "config/feature.h" + #include "pg/pg.h" #include "pg/pg_ids.h" @@ -54,6 +56,7 @@ #include "drivers/bus_spi.h" #include "drivers/io.h" +#include "fc/config.h" #include "fc/runtime_config.h" #include "io/beeper.h" @@ -105,7 +108,7 @@ typedef struct gyroSensor_s { // gyro soft filter filterApplyFnPtr softLpfFilterApplyFn; gyroSoftLpfFilter_t softLpfFilter; - void *softLpfFilterPtr[XYZ_AXIS_COUNT]; + filter_t *softLpfFilterPtr[XYZ_AXIS_COUNT]; // notch filters filterApplyFnPtr notchFilter1ApplyFn; biquadFilter_t notchFilter1[XYZ_AXIS_COUNT]; @@ -442,23 +445,23 @@ void gyroInitFilterLpf(gyroSensor_t *gyroSensor, uint8_t lpfHz) switch (gyroConfig()->gyro_soft_lpf_type) { case FILTER_BIQUAD: gyroSensor->softLpfFilterApplyFn = (filterApplyFnPtr)biquadFilterApply; - for (int axis = 0; axis < 3; axis++) { - gyroSensor->softLpfFilterPtr[axis] = &gyroSensor->softLpfFilter.gyroFilterLpfState[axis]; + for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { + gyroSensor->softLpfFilterPtr[axis] = (filter_t *)&gyroSensor->softLpfFilter.gyroFilterLpfState[axis]; biquadFilterInitLPF(&gyroSensor->softLpfFilter.gyroFilterLpfState[axis], lpfHz, gyro.targetLooptime); } break; case FILTER_PT1: gyroSensor->softLpfFilterApplyFn = (filterApplyFnPtr)pt1FilterApply; const float gyroDt = (float) gyro.targetLooptime * 0.000001f; - for (int axis = 0; axis < 3; axis++) { - gyroSensor->softLpfFilterPtr[axis] = &gyroSensor->softLpfFilter.gyroFilterPt1State[axis]; + for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { + gyroSensor->softLpfFilterPtr[axis] = (filter_t *)&gyroSensor->softLpfFilter.gyroFilterPt1State[axis]; pt1FilterInit(&gyroSensor->softLpfFilter.gyroFilterPt1State[axis], lpfHz, gyroDt); } break; default: gyroSensor->softLpfFilterApplyFn = (filterApplyFnPtr)firFilterDenoiseUpdate; - for (int axis = 0; axis < 3; axis++) { - gyroSensor->softLpfFilterPtr[axis] = &gyroSensor->softLpfFilter.gyroDenoiseState[axis]; + for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { + gyroSensor->softLpfFilterPtr[axis] = (filter_t *)&gyroSensor->softLpfFilter.gyroDenoiseState[axis]; firFilterDenoiseInit(&gyroSensor->softLpfFilter.gyroDenoiseState[axis], lpfHz, gyro.targetLooptime); } break; @@ -498,7 +501,7 @@ static void gyroInitFilterNotch1(gyroSensor_t *gyroSensor, uint16_t notchHz, uin if (notchHz != 0 && notchCutoffHz != 0) { gyroSensor->notchFilter1ApplyFn = (filterApplyFnPtr)biquadFilterApply; const float notchQ = filterGetNotchQ(notchHz, notchCutoffHz); - for (int axis = 0; axis < 3; axis++) { + for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { biquadFilterInit(&gyroSensor->notchFilter1[axis], notchHz, gyro.targetLooptime, notchQ, FILTER_NOTCH); } } @@ -513,13 +516,18 @@ static void gyroInitFilterNotch2(gyroSensor_t *gyroSensor, uint16_t notchHz, uin if (notchHz != 0 && notchCutoffHz != 0) { gyroSensor->notchFilter2ApplyFn = (filterApplyFnPtr)biquadFilterApply; const float notchQ = filterGetNotchQ(notchHz, notchCutoffHz); - for (int axis = 0; axis < 3; axis++) { + for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { biquadFilterInit(&gyroSensor->notchFilter2[axis], notchHz, gyro.targetLooptime, notchQ, FILTER_NOTCH); } } } #ifdef USE_GYRO_DATA_ANALYSE +static bool isDynamicFilterActive(void) +{ + return feature(FEATURE_DYNAMIC_FILTER); +} + static void gyroInitFilterDynamicNotch(gyroSensor_t *gyroSensor) { gyroSensor->notchFilterDynApplyFn = nullFilterApply; @@ -527,7 +535,7 @@ static void gyroInitFilterDynamicNotch(gyroSensor_t *gyroSensor) if (isDynamicFilterActive()) { gyroSensor->notchFilterDynApplyFn = (filterApplyFnPtr)biquadFilterApplyDF1; // must be this function, not DF2 const float notchQ = filterGetNotchQ(400, 390); //just any init value - for (int axis = 0; axis < 3; axis++) { + for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { biquadFilterInit(&gyroSensor->notchFilterDyn[axis], 400, gyro.targetLooptime, notchQ, FILTER_NOTCH); } } @@ -725,7 +733,9 @@ static FAST_CODE void gyroUpdateSensor(gyroSensor_t *gyroSensor, timeUs_t curren } #ifdef USE_GYRO_DATA_ANALYSE - gyroDataAnalyse(&gyroSensor->gyroDev, gyroSensor->notchFilterDyn); + if (isDynamicFilterActive()) { + gyroDataAnalyse(&gyroSensor->gyroDev, gyroSensor->notchFilterDyn); + } #endif const timeDelta_t sampleDeltaUs = currentTimeUs - accumulationLastTimeSampledUs; @@ -740,10 +750,10 @@ static FAST_CODE void gyroUpdateSensor(gyroSensor_t *gyroSensor, timeUs_t curren // NOTE: this branch optimized for when there is no gyro debugging, ensure it is kept in step with non-optimized branch float gyroADCf = (float)gyroSensor->gyroDev.gyroADC[axis] * gyroSensor->gyroDev.scale; #ifdef USE_GYRO_DATA_ANALYSE - gyroADCf = gyroSensor->notchFilterDynApplyFn(&gyroSensor->notchFilterDyn[axis], gyroADCf); + gyroADCf = gyroSensor->notchFilterDynApplyFn((filter_t *)&gyroSensor->notchFilterDyn[axis], gyroADCf); #endif - gyroADCf = gyroSensor->notchFilter1ApplyFn(&gyroSensor->notchFilter1[axis], gyroADCf); - gyroADCf = gyroSensor->notchFilter2ApplyFn(&gyroSensor->notchFilter2[axis], gyroADCf); + gyroADCf = gyroSensor->notchFilter1ApplyFn((filter_t *)&gyroSensor->notchFilter1[axis], gyroADCf); + gyroADCf = gyroSensor->notchFilter2ApplyFn((filter_t *)&gyroSensor->notchFilter2[axis], gyroADCf); gyroADCf = gyroSensor->softLpfFilterApplyFn(gyroSensor->softLpfFilterPtr[axis], gyroADCf); gyro.gyroADCf[axis] = gyroADCf; if (!gyroSensor->overflowDetected) { @@ -761,23 +771,23 @@ static FAST_CODE void gyroUpdateSensor(gyroSensor_t *gyroSensor, timeUs_t curren DEBUG_SET(DEBUG_GYRO_NOTCH, axis, lrintf(gyroADCf)); #ifdef USE_GYRO_DATA_ANALYSE - // Apply Dynamic Notch filtering + // apply dynamic notch filter if (isDynamicFilterActive()) { if (axis == 0) { DEBUG_SET(DEBUG_FFT, 0, lrintf(gyroADCf)); // store raw data } - gyroADCf = gyroSensor->notchFilterDynApplyFn(&gyroSensor->notchFilterDyn[axis], gyroADCf); + gyroADCf = gyroSensor->notchFilterDynApplyFn((filter_t *)&gyroSensor->notchFilterDyn[axis], gyroADCf); if (axis == 0) { DEBUG_SET(DEBUG_FFT, 1, lrintf(gyroADCf)); // store data after dynamic notch } } #endif - // Apply Static Notch filtering - gyroADCf = gyroSensor->notchFilter1ApplyFn(&gyroSensor->notchFilter1[axis], gyroADCf); - gyroADCf = gyroSensor->notchFilter2ApplyFn(&gyroSensor->notchFilter2[axis], gyroADCf); + // apply static notch filters + gyroADCf = gyroSensor->notchFilter1ApplyFn((filter_t *)&gyroSensor->notchFilter1[axis], gyroADCf); + gyroADCf = gyroSensor->notchFilter2ApplyFn((filter_t *)&gyroSensor->notchFilter2[axis], gyroADCf); - // Apply LPF + // apply LPF DEBUG_SET(DEBUG_GYRO, axis, lrintf(gyroADCf)); gyroADCf = gyroSensor->softLpfFilterApplyFn(gyroSensor->softLpfFilterPtr[axis], gyroADCf); diff --git a/src/main/sensors/gyroanalyse.c b/src/main/sensors/gyroanalyse.c index 528961815a..7c022f5dd0 100644 --- a/src/main/sensors/gyroanalyse.c +++ b/src/main/sensors/gyroanalyse.c @@ -29,21 +29,12 @@ #include "common/time.h" #include "common/utils.h" -#include "config/feature.h" -#include "pg/pg.h" -#include "pg/pg_ids.h" - #include "drivers/accgyro/accgyro.h" #include "drivers/time.h" -#include "fc/config.h" -#include "fc/rc_controls.h" - #include "sensors/gyro.h" #include "sensors/gyroanalyse.h" -#include "common/filter.h" - // The FFT splits the frequency domain into an number of bins // A sampling frequency of 1000 and max frequency of 500 at a window size of 32 gives 16 frequency bins each with a width 31.25Hz // Eg [0,31), [31,62), [62, 93) etc @@ -136,20 +127,11 @@ const gyroFftData_t *gyroFftData(int axis) return &fftResult[axis]; } -bool isDynamicFilterActive(void) -{ - return feature(FEATURE_DYNAMIC_FILTER); -} - /* * Collect gyro data, to be analysed in gyroDataAnalyseUpdate function */ void gyroDataAnalyse(const gyroDev_t *gyroDev, biquadFilter_t *notchFilterDyn) { - if (!isDynamicFilterActive()) { - return; - } - // if gyro sampling is > 1kHz, accumulate multiple samples for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { fftAcc[axis] += gyroDev->gyroADC[axis]; diff --git a/src/main/sensors/gyroanalyse.h b/src/main/sensors/gyroanalyse.h index a4a64cd431..8de41b307f 100644 --- a/src/main/sensors/gyroanalyse.h +++ b/src/main/sensors/gyroanalyse.h @@ -31,4 +31,3 @@ const gyroFftData_t *gyroFftData(int axis); struct gyroDev_s; void gyroDataAnalyse(const struct gyroDev_s *gyroDev, biquadFilter_t *notchFilterDyn); void gyroDataAnalyseUpdate(biquadFilter_t *notchFilterDyn); -bool isDynamicFilterActive(void);