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

Fix gyro filter initialisation

Browse source
This commit is contained in:
Martin Budden 2016-11-23 22:27:49 +00:00
parent 181a12bb00
commit 594b9d8ad3
1 changed files with 21 additions and 12 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -44,10 +44,6 @@ float gyroADCf[XYZ_AXIS_COUNT];
static int32_t gyroZero[XYZ_AXIS_COUNT] = { 0, 0, 0 };
static const gyroConfig_t *gyroConfig;
static biquadFilter_t gyroFilterLPF[XYZ_AXIS_COUNT];
static biquadFilter_t gyroFilterNotch_1[XYZ_AXIS_COUNT], gyroFilterNotch_2[XYZ_AXIS_COUNT];
static pt1Filter_t gyroFilterPt1[XYZ_AXIS_COUNT];
static firFilterDenoise_t gyroDenoiseState[XYZ_AXIS_COUNT];
static uint8_t gyroSoftLpfType;
static uint16_t gyroSoftNotchHz1, gyroSoftNotchHz2;
static float gyroSoftNotchQ1, gyroSoftNotchQ2;
@ -55,8 +51,11 @@ static uint8_t gyroSoftLpfHz;
static uint16_t calibratingG = 0;
static filterApplyFnPtr softLpfFilterApplyFn;
static void *softLpfFilter[3];
static filterApplyFnPtr notchFilter1ApplyFn;
static void *notchFilter1[3];
static filterApplyFnPtr notchFilter2ApplyFn;
static void *notchFilter2[3];
void gyroUseConfig(const gyroConfig_t *gyroConfigToUse,
uint8_t gyro_soft_lpf_hz,
@ -77,6 +76,11 @@ void gyroUseConfig(const gyroConfig_t *gyroConfigToUse,
void gyroInit(void)
{
static biquadFilter_t gyroFilterLPF[XYZ_AXIS_COUNT];
static pt1Filter_t gyroFilterPt1[XYZ_AXIS_COUNT];
static firFilterDenoise_t gyroDenoiseState[XYZ_AXIS_COUNT];
static biquadFilter_t gyroFilterNotch_1[XYZ_AXIS_COUNT];
static biquadFilter_t gyroFilterNotch_2[XYZ_AXIS_COUNT];
softLpfFilterApplyFn = nullFilterApply;
notchFilter1ApplyFn = nullFilterApply;
@ -86,18 +90,21 @@ void gyroInit(void)
if (gyroSoftLpfType == FILTER_BIQUAD) {
softLpfFilterApplyFn = (filterApplyFnPtr)biquadFilterApply;
for (int axis = 0; axis < 3; axis++) {
biquadFilterInitLPF(&gyroFilterLPF[axis], gyroSoftLpfHz, gyro.targetLooptime);
softLpfFilter[axis] = &gyroFilterLPF[axis];
biquadFilterInitLPF(softLpfFilter[axis], gyroSoftLpfHz, gyro.targetLooptime);
}
} 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);
softLpfFilter[axis] = &gyroFilterPt1[axis];
pt1FilterInit(softLpfFilter[axis], gyroSoftLpfHz, gyroDt);
}
} else {
softLpfFilterApplyFn = (filterApplyFnPtr)firFilterDenoiseUpdate;
for (int axis = 0; axis < 3; axis++) {
firFilterDenoiseInit(&gyroDenoiseState[axis], gyroSoftLpfHz, gyro.targetLooptime);
softLpfFilter[axis] = &gyroDenoiseState[axis];
firFilterDenoiseInit(softLpfFilter[axis], gyroSoftLpfHz, gyro.targetLooptime);
}
}
}
@ -105,13 +112,15 @@ void gyroInit(void)
if (gyroSoftNotchHz1) {
notchFilter1ApplyFn = (filterApplyFnPtr)biquadFilterApply;
for (int axis = 0; axis < 3; axis++) {
biquadFilterInit(&gyroFilterNotch_1[axis], gyroSoftNotchHz1, gyro.targetLooptime, gyroSoftNotchQ1, FILTER_NOTCH);
notchFilter1[axis] = &gyroFilterNotch_1[axis];
biquadFilterInit(notchFilter1[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);
notchFilter2[axis] = &gyroFilterNotch_2[axis];
biquadFilterInit(notchFilter2[axis], gyroSoftNotchHz2, gyro.targetLooptime, gyroSoftNotchQ2, FILTER_NOTCH);
}
}
}
@ -208,14 +217,14 @@ void gyroUpdate(void)
if (debugMode == DEBUG_GYRO)
debug[axis] = gyroADC[axis];
gyroADCf[axis] = softLpfFilterApplyFn(&gyroDenoiseState[axis], (float) gyroADC[axis]);
gyroADCf[axis] = softLpfFilterApplyFn(softLpfFilter[axis], (float) gyroADC[axis]);
if (debugMode == DEBUG_NOTCH)
debug[axis] = lrintf(gyroADCf[axis]);
gyroADCf[axis] = notchFilter1ApplyFn(&gyroFilterNotch_1[axis], gyroADCf[axis]);
gyroADCf[axis] = notchFilter1ApplyFn(notchFilter1[axis], gyroADCf[axis]);
gyroADCf[axis] = notchFilter2ApplyFn(&gyroFilterNotch_2[axis], gyroADCf[axis]);
gyroADCf[axis] = notchFilter2ApplyFn(notchFilter2[axis], gyroADCf[axis]);
gyroADC[axis] = lrintf(gyroADCf[axis]);
}