mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-16 21:05:35 +03:00
Tidied gyro filter code
This commit is contained in:
Martin Budden
parent
5a448ea9b0
commit
3125ae252b
5 changed files with 36 additions and 42 deletions
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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
|
||||
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);
|
||||
|
||||
|
|
|
|||
|
|
@ -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];
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue