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

fix error in biquad coefficients calculation

Browse source 
improve biquad precision and performance by using direct form 2 transposed instead of direct form 1
keep float results for luxfloat pid controller, instead of casting twice
This commit is contained in:
rav-rav 2016-05-04 22:44:33 +02:00
parent e9963f454b
commit f62ec043cf
5 changed files with 24 additions and 26 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -38,6 +38,7 @@
uint16_t calibratingG = 0;
int32_t gyroADC[XYZ_AXIS_COUNT];
float gyroADCf[XYZ_AXIS_COUNT];
int32_t gyroZero[FLIGHT_DYNAMICS_INDEX_COUNT] = { 0, 0, 0 };
static gyroConfig_t *gyroConfig;
@ -155,7 +156,10 @@ void gyroUpdate(void)
if (!gyroFilterStateIsSet) initGyroFilterCoefficients(); /* initialise filter coefficients */
if (gyroFilterStateIsSet) {
for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) gyroADC[axis] = lrintf(applyBiQuadFilter((float) gyroADC[axis], &gyroFilterState[axis]));
for (axis = 0; axis < XYZ_AXIS_COUNT; axis++){
gyroADCf[axis] = applyBiQuadFilter((float) gyroADC[axis], &gyroFilterState[axis]);
gyroADC[axis] = lrintf(gyroADCf[axis]);
}
}
}