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

Added filter unit test and tidied filter code

Browse source
This commit is contained in:
Martin Budden 2016-10-17 14:15:02 +01:00
parent eef0d7a761
commit 01be3842c8
6 changed files with 190 additions and 97 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -49,7 +49,7 @@ 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 firFilterState_t gyroDenoiseState[XYZ_AXIS_COUNT];
static firFilterDenoise_t gyroDenoiseState[XYZ_AXIS_COUNT];
static uint8_t gyroSoftLpfType;
static uint16_t gyroSoftNotchHz1, gyroSoftNotchHz2;
static float gyroSoftNotchQ1, gyroSoftNotchQ2;
@ -83,7 +83,7 @@ void gyroInit(void)
else if (gyroSoftLpfType == FILTER_PT1)
gyroDt = (float) gyro.targetLooptime * 0.000001f;
else
initFirFilter(&gyroDenoiseState[axis], gyroSoftLpfHz, gyro.targetLooptime);
firFilterDenoiseInit(&gyroDenoiseState[axis], gyroSoftLpfHz, gyro.targetLooptime);
}
}
@ -198,7 +198,7 @@ void gyroUpdate(void)
else if (gyroSoftLpfType == FILTER_PT1)
gyroADCf[axis] = pt1FilterApply4(&gyroFilterPt1[axis], (float) gyroADC[axis], gyroSoftLpfHz, gyroDt);
else
gyroADCf[axis] = firFilterUpdate(&gyroDenoiseState[axis], (float) gyroADC[axis]);
gyroADCf[axis] = firFilterDenoiseUpdate(&gyroDenoiseState[axis], (float) gyroADC[axis]);
if (debugMode == DEBUG_NOTCH)
debug[axis] = lrintf(gyroADCf[axis]);