From d779daad2d86b8ac7545e4675b5178aa6cbbe4f5 Mon Sep 17 00:00:00 2001 From: blckmn Date: Sun, 26 Jun 2016 08:59:32 +1000 Subject: [PATCH] Fixed array out of bounds issue - not all set. --- src/main/sensors/gyro.c | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 8fe83d244..bce60a1fc 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -56,7 +56,7 @@ void useGyroConfig(gyroConfig_t *gyroConfigToUse, float gyro_lpf_hz) } void initGyroFilterCoefficients(void) { - int axis; + int axis; if (gyroLpfCutFreq && targetLooptime) { /* Initialisation needs to happen once samplingrate is known */ for (axis = 0; axis < 3; axis++) BiQuadNewLpf(gyroLpfCutFreq, &gyroFilterState[axis], targetLooptime); gyroFilterStateIsSet = true; @@ -137,8 +137,8 @@ static void applyGyroZero(void) void gyroUpdate(void) { - int16_t gyroADCRaw[XYZ_AXIS_COUNT]; - int axis; + int16_t gyroADCRaw[XYZ_AXIS_COUNT]; + int axis; // range: +/- 8192; +/- 2000 deg/sec if (!gyro.read(gyroADCRaw)) { @@ -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++){ + 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 } - } else { - gyroADCf[axis] = gyroADC[axis]; // Otherwise float pid controller will not have gyro input when filter disabled } } }