Fixed array out of bounds issue - not all set.
This commit is contained in:
parent
00c0ca7c71
commit
d779daad2d
|
@ -161,13 +161,13 @@ void gyroUpdate(void)
|
|||
if (gyroLpfCutFreq) {
|
||||
if (!gyroFilterStateIsSet) initGyroFilterCoefficients(); /* initialise filter coefficients */
|
||||
|
||||
if (gyroFilterStateIsSet) {
|
||||
for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
|
||||
if (gyroFilterStateIsSet) {
|
||||
gyroADCf[axis] = applyBiQuadFilter((float) gyroADC[axis], &gyroFilterState[axis]);
|
||||
gyroADC[axis] = lrintf(gyroADCf[axis]);
|
||||
}
|
||||
} else {
|
||||
gyroADCf[axis] = gyroADC[axis]; // Otherwise float pid controller will not have gyro input when filter disabled
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
Loading…
Reference in New Issue