Merge pull request #1625 from martinbudden/bf_gyro_filter_calls
Use function pointers to simplify gyro filter calls
This commit is contained in:
commit
12e7d3ad8d
|
@ -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(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 <count> items.
|
||||
*/
|
||||
|
|
|
@ -62,6 +62,9 @@ typedef struct firFilter_s {
|
|||
uint8_t coeffsLength;
|
||||
} firFilter_t;
|
||||
|
||||
typedef float (*filterApplyFnPtr)(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);
|
||||
|
@ -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);
|
||||
|
|
|
@ -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)]))
|
||||
|
|
|
@ -15,12 +15,12 @@
|
|||
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#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
|
||||
|
|
|
@ -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]);
|
||||
}
|
||||
}
|
||||
|
|
Loading…
Reference in New Issue