Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-25 17:25:20 +03:00
Code Issues Wiki Activity

Tidied gyro filter code

Browse source
This commit is contained in:
Martin Budden 2017-12-30 00:12:49 +00:00
parent 5a448ea9b0
commit 3125ae252b
5 changed files with 36 additions and 42 deletions
Download patch file Download diff file Expand all files Collapse all files

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