diff --git a/src/main/common/filter.c b/src/main/common/filter.c index f86bca3cf3..9bfbc83f81 100644 --- a/src/main/common/filter.c +++ b/src/main/common/filter.c @@ -53,12 +53,12 @@ biquad_t *BiQuadNewLpf(uint8_t filterCutFreq) float samplingRate; samplingRate = 1 / (targetLooptime * 0.000001f); - biquad_t *b; + biquad_t *newState; float omega, sn, cs, alpha; float a0, a1, a2, b0, b1, b2; - b = malloc(sizeof(biquad_t)); - if (b == NULL) + newState = malloc(sizeof(biquad_t)); + if (newState == NULL) return NULL; /* setup variables */ @@ -75,17 +75,17 @@ biquad_t *BiQuadNewLpf(uint8_t filterCutFreq) a2 = 1 - alpha; /* precompute the coefficients */ - b->a0 = b0 /a0; - b->a1 = b1 /a0; - b->a2 = b2 /a0; - b->a3 = a1 /a0; - b->a4 = a2 /a0; + newState->a0 = b0 /a0; + newState->a1 = b1 /a0; + newState->a2 = b2 /a0; + newState->a3 = a1 /a0; + newState->a4 = a2 /a0; /* zero initial samples */ - b->x1 = b->x2 = 0; - b->y1 = b->y2 = 0; + newState->x1 = newState->x2 = 0; + newState->y1 = newState->y2 = 0; - return b; + return newState; } /* Computes a biquad_t filter on a sample */ diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index af1c6b3f7c..beeee5818f 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -113,7 +113,7 @@ void airModePlus(airModePlus_t *axisState, int axis, pidProfile_t *pidProfile) { const angle_index_t rcAliasToAngleIndexMap[] = { AI_ROLL, AI_PITCH }; static airModePlus_t airModePlusAxisState[3]; -static biquad_t deltaBiQuadState[3]; +static biquad_t *deltaBiQuadState[3]; static bool deltaStateIsSet; static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, @@ -128,7 +128,7 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa static float previousErrorGyroIf[3] = { 0.0f, 0.0f, 0.0f }; if (!deltaStateIsSet && pidProfile->dterm_lpf_hz) { - for (axis = 0; axis < 3; axis++) deltaBiQuadState[axis] = *(biquad_t *)BiQuadNewLpf(pidProfile->dterm_lpf_hz); + for (axis = 0; axis < 3; axis++) deltaBiQuadState[axis] = (biquad_t *)BiQuadNewLpf(pidProfile->dterm_lpf_hz); deltaStateIsSet = true; } @@ -215,7 +215,7 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa lastError[axis] = RateError; if (deltaStateIsSet) { - delta = applyBiQuadFilter(delta, &deltaBiQuadState[axis]); + delta = applyBiQuadFilter(delta, deltaBiQuadState[axis]); } // Correct difference by cycle time. Cycle time is jittery (can be different 2 times), so calculated difference @@ -259,7 +259,7 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat int8_t horizonLevelStrength = 100; if (!deltaStateIsSet && pidProfile->dterm_lpf_hz) { - for (axis = 0; axis < 3; axis++) deltaBiQuadState[axis] = *(biquad_t *)BiQuadNewLpf(pidProfile->dterm_lpf_hz); + for (axis = 0; axis < 3; axis++) deltaBiQuadState[axis] = (biquad_t *)BiQuadNewLpf(pidProfile->dterm_lpf_hz); deltaStateIsSet = true; } @@ -348,7 +348,7 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat lastError[axis] = RateError; if (deltaStateIsSet) { - delta = lrintf(applyBiQuadFilter((float) delta, &deltaBiQuadState[axis])); + delta = lrintf(applyBiQuadFilter((float) delta, deltaBiQuadState[axis])); } // Correct difference by cycle time. Cycle time is jittery (can be different 2 times), so calculated difference diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 0ad9823b29..9dd0d7552f 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -41,7 +41,7 @@ int16_t gyroADC[XYZ_AXIS_COUNT]; int16_t gyroZero[FLIGHT_DYNAMICS_INDEX_COUNT] = { 0, 0, 0 }; static gyroConfig_t *gyroConfig; -static biquad_t gyroBiQuadState[3]; +static biquad_t *gyroBiQuadState[3]; static bool gyroFilterStateIsSet; static uint8_t gyroLpfCutFreq; int axis; @@ -57,7 +57,7 @@ void useGyroConfig(gyroConfig_t *gyroConfigToUse, uint8_t *gyro_lpf_hz) void initGyroFilterCoefficients(void) { if (gyroLpfCutFreq && targetLooptime) { /* Initialisation needs to happen once samplingrate is known */ - for (axis = 0; axis < 3; axis++) gyroBiQuadState[axis] = *(biquad_t *)BiQuadNewLpf(gyroLpfCutFreq); + for (axis = 0; axis < 3; axis++) gyroBiQuadState[axis] = (biquad_t *)BiQuadNewLpf(gyroLpfCutFreq); gyroFilterStateIsSet = true; } } @@ -143,7 +143,7 @@ void gyroUpdate(void) if (!gyroFilterStateIsSet) { initGyroFilterCoefficients(); } else { - for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) gyroADC[axis] = lrintf(applyBiQuadFilter((float) gyroADC[axis], &gyroBiQuadState[axis])); + for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) gyroADC[axis] = lrintf(applyBiQuadFilter((float) gyroADC[axis], gyroBiQuadState[axis])); } }