From e30823bfec8c8d7d89c30349fd18e7577855e35d Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Sun, 20 Nov 2016 17:04:36 +0000 Subject: [PATCH 1/4] Use function pointers to simplify gyro filter calls --- src/main/common/filter.c | 17 ++++++++++ src/main/common/filter.h | 4 +++ src/main/sensors/gyro.c | 69 +++++++++++++++++++++++----------------- 3 files changed, 61 insertions(+), 29 deletions(-) diff --git a/src/main/common/filter.c b/src/main/common/filter.c index 6fdf6e5c59..72b5652914 100644 --- a/src/main/common/filter.c +++ b/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(const 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 items. */ diff --git a/src/main/common/filter.h b/src/main/common/filter.h index 2b0e75a431..a925f68864 100644 --- a/src/main/common/filter.h +++ b/src/main/common/filter.h @@ -62,6 +62,9 @@ typedef struct firFilter_s { uint8_t coeffsLength; } firFilter_t; +typedef float (*filterApplyFnPtr)(const void *filter, float input); + +float nullFilterApply(const 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); diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 5ed678e103..0a2667d90d 100644 --- a/src/main/sensors/gyro.c +++ b/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++) { - if (gyroSoftLpfType == FILTER_BIQUAD) + + softLpfFilterApplyFn = nullFilterApply; + 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 + } + } else if (gyroSoftLpfType == FILTER_PT1) { + 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) - debug[axis] = gyroADC[axis]; + if (debugMode == DEBUG_GYRO) + debug[axis] = gyroADC[axis]; - if (gyroSoftLpfType == FILTER_BIQUAD) - 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]); + gyroADCf[axis] = softLpfFilterApplyFn(&gyroDenoiseState[axis], (float) gyroADC[axis]); - if (debugMode == DEBUG_NOTCH) - debug[axis] = lrintf(gyroADCf[axis]); + if (debugMode == DEBUG_NOTCH) + debug[axis] = lrintf(gyroADCf[axis]); - if (gyroSoftNotchHz1) - gyroADCf[axis] = biquadFilterApply(&gyroFilterNotch_1[axis], gyroADCf[axis]); + gyroADCf[axis] = notchFilter1ApplyFn(&gyroFilterNotch_1[axis], gyroADCf[axis]); - if (gyroSoftNotchHz2) - gyroADCf[axis] = biquadFilterApply(&gyroFilterNotch_2[axis], gyroADCf[axis]); + gyroADCf[axis] = notchFilter2ApplyFn(&gyroFilterNotch_2[axis], gyroADCf[axis]); - gyroADC[axis] = lrintf(gyroADCf[axis]); - } - } else { - for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) - gyroADCf[axis] = gyroADC[axis]; + gyroADC[axis] = lrintf(gyroADCf[axis]); } } From bf683bcdc6065fec708f32f4fe8a9712a9af9fb8 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Sun, 20 Nov 2016 17:19:15 +0000 Subject: [PATCH 2/4] Updated function signatures --- src/main/common/filter.c | 2 +- src/main/common/filter.h | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/common/filter.c b/src/main/common/filter.c index 72b5652914..5b580a14e1 100644 --- a/src/main/common/filter.c +++ b/src/main/common/filter.c @@ -33,7 +33,7 @@ // NULL filter -float nullFilterApply(const void *filter, float input) +float nullFilterApply(void *filter, float input) { UNUSED(filter); return input; diff --git a/src/main/common/filter.h b/src/main/common/filter.h index a925f68864..0121c71b1d 100644 --- a/src/main/common/filter.h +++ b/src/main/common/filter.h @@ -62,9 +62,9 @@ typedef struct firFilter_s { uint8_t coeffsLength; } firFilter_t; -typedef float (*filterApplyFnPtr)(const void *filter, float input); +typedef float (*filterApplyFnPtr)(void *filter, float input); -float nullFilterApply(const 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); From ae356d485b01230f3e89d5e8e1474e5a7e1290d6 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Sun, 20 Nov 2016 17:26:29 +0000 Subject: [PATCH 3/4] Fixed UNUSED #define --- src/main/common/utils.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/common/utils.h b/src/main/common/utils.h index 23088d076d..2e27c930c2 100644 --- a/src/main/common/utils.h +++ b/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)])) From d76d65f19bd787dbaaee5243787649fd2c5b2090 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Sun, 20 Nov 2016 17:44:59 +0000 Subject: [PATCH 4/4] Further UNUSED #define fix --- src/main/drivers/io.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/drivers/io.c b/src/main/drivers/io.c index d60e3af649..0390a0add2 100644 --- a/src/main/drivers/io.c +++ b/src/main/drivers/io.c @@ -15,12 +15,12 @@ * along with Cleanflight. If not, see . */ - #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