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

Merge pull request #4894 from martinbudden/bfa_gyro_filter_tidy

Browse source 
Tidied gyro filter code
This commit is contained in:
Martin Budden 2018-01-05 10:35:50 +00:00 • committed by GitHub
commit 172c1e370b
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
5 changed files with 36 additions and 42 deletions
Download patch file Download diff file Expand all files Collapse all files

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

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

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

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

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

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

18
src/main/sensors/gyroanalyse.c
View file

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

1
src/main/sensors/gyroanalyse.h
View file

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