For discussion - updated filters to be more consistent and more efficient (#330)

* Made filters more consistent
* Added pT1 init function. Made FIR filters consistent.

src/main/common/filter.c

@@ -17,7 +17,7 @@
 
 #include <stdbool.h>
 #include <stdint.h>
-#include <stdlib.h>
+#include <string.h>
 #include <math.h>
 
 #include "common/axis.h"
@@ -27,9 +27,10 @@
 #include "drivers/gyro_sync.h"
 
 #define BIQUAD_Q    (1.0f / 1.41421356f)     /* quality factor - butterworth (1 / sqrt(2)) */
+#define M_PI_FLOAT  3.14159265358979323846f
 
 /* sets up a biquad Filter */
-void filterInitBiQuad(uint8_t filterCutFreq, biquad_t *newState, int16_t samplingRate)
+void biquadFilterInit(biquadFilter_t *newState, uint8_t filterCutFreq, int16_t samplingRate)
 {
     float omega, sn, cs, alpha;
     float a0, a1, a2, b0, b1, b2;
@@ -64,7 +65,7 @@ void filterInitBiQuad(uint8_t filterCutFreq, biquad_t *newState, int16_t samplin
 }
 
 /* Computes a biquad_t filter on a sample */
-float filterApplyBiQuad(float sample, biquad_t *state)
+float biquadFilterApply(biquadFilter_t *state, float sample)
 {
     float result;
 
@@ -75,27 +76,40 @@ float filterApplyBiQuad(float sample, biquad_t *state)
     return result;
 }
 
-// PT1 Low Pass filter (when no dT specified it will be calculated from the cycleTime)
+// PT1 Low Pass filter
-float filterApplyPt1(float input, filterStatePt1_t *filter, float f_cut, float dT)
+
+// f_cut = cutoff frequency
+void pt1FilterInit(pt1Filter_t *filter, uint8_t f_cut, float dT)
 {
-	// Pre calculate and store RC
+    filter->RC = 1.0f / ( 2.0f * M_PI_FLOAT * f_cut );
-	if (!filter->RC) {
+    filter->dT = dT;
-		filter->RC = 1.0f / ( 2.0f * (float)M_PI * f_cut );
+}
-	}
+
+float pt1FilterApply(pt1Filter_t *filter, float input)
+{
+    filter->state = filter->state + filter->dT / (filter->RC + filter->dT) * (input - filter->state);
+    return filter->state;
+}
+
+float pt1FilterApply4(pt1Filter_t *filter, float input, float f_cut, float dT)
+{
+    // Pre calculate and store RC
+    if (!filter->RC) {
+        filter->RC = 1.0f / ( 2.0f * (float)M_PI * f_cut );
+    }
 
     filter->state = filter->state + dT / (filter->RC + dT) * (input - filter->state);
     return filter->state;
 }
 
-// PT1 Low Pass filter (when no dT specified it will be calculated from the cycleTime)
 // f_cut = cutoff frequency
 // rate_limit = maximum rate of change of the output value in units per second
-float filterApplyPt1WithRateLimit(float input, filterStatePt1_t *filter, float f_cut, float rate_limit, float dT)
+float pt1FilterApplyWithRateLimit(pt1Filter_t *filter, float input, float f_cut, float rate_limit, float dT)
 {
-	// Pre calculate and store RC
+    // Pre calculate and store RC
-	if (!filter->RC) {
+    if (!filter->RC) {
-		filter->RC = 1.0f / ( 2.0f * (float)M_PI * f_cut );
+        filter->RC = 1.0f / ( 2.0f * (float)M_PI * f_cut );
-	}
+    }
 
     const float newState = filter->state + dT / (filter->RC + dT) * (input - filter->state);
     const float rateLimitPerSample = rate_limit * dT;
@@ -104,26 +118,37 @@ float filterApplyPt1WithRateLimit(float input, filterStatePt1_t *filter, float f
     return filter->state;
 }
 
-void filterResetPt1(filterStatePt1_t *filter, float input)
+void pt1FilterReset(pt1Filter_t *filter, float input)
 {
     filter->state = input;
 }
 
-void filterUpdateFIR(int filterLength, float *shiftBuf, float newSample)
+// FIR filter
+void firFilterInit2(firFilter_t *filter, float *buf, uint8_t bufLength, const float *coeffs, uint8_t coeffsLength)
 {
-    // Shift history buffer and push new sample
+    filter->buf = buf;
-    for (int i = filterLength - 1; i > 0; i--)
+    filter->bufLength = bufLength;
-        shiftBuf[i] = shiftBuf[i - 1];
+    filter->coeffs = coeffs;
-
+    filter->coeffsLength = coeffsLength;
-    shiftBuf[0] = newSample;
+    memset(filter->buf, 0, sizeof(float) * filter->bufLength);
 }
 
-float filterApplyFIR(int filterLength, const float *shiftBuf, const float *coeffBuf, float commonMultiplier)
+void firFilterInit(firFilter_t *filter, float *buf, uint8_t bufLength, const float *coeffs)
 {
-    float accum = 0;
+    firFilterInit2(filter, buf, bufLength, coeffs, bufLength);
-
+}
-    for (int i = 0; i < filterLength; i++)
+
-        accum += shiftBuf[i] * coeffBuf[i];
+void firFilterUpdate(firFilter_t *filter, float input)
-
+{
-    return accum * commonMultiplier;
+    memmove(&filter->buf[1], &filter->buf[0], (filter->bufLength-1) * sizeof(float));
+    filter->buf[0] = input;
+}
+
+float firFilterApply(firFilter_t *filter)
+{
+    float ret = 0.0f;
+    for (int ii = 0; ii < filter->coeffsLength; ++ii) {
+        ret += filter->coeffs[ii] * filter->buf[ii];
+    }
+    return ret;
 }

src/main/common/filter.h

@@ -17,24 +17,35 @@
 
 #pragma once
 
-typedef struct filterStatePt1_s {
+typedef struct pt1Filter_s {
-	float state;
+    float state;
-	float RC;
+    float RC;
-	float constdT;
+    float dT;
-} filterStatePt1_t;
+} pt1Filter_t;
 
 /* this holds the data required to update samples thru a filter */
-typedef struct biquad_s {
+typedef struct biquadFilter_s {
     float b0, b1, b2, a1, a2;
     float d1, d2;
-} biquad_t;
+} biquadFilter_t;
 
-float filterApplyPt1(float input, filterStatePt1_t *filter, float f_cut, float dt);
+typedef struct firFilter_s {
-float filterApplyPt1WithRateLimit(float input, filterStatePt1_t *filter, float f_cut, float rate_limit, float dT);
+    float *buf;
-void filterResetPt1(filterStatePt1_t *filter, float input);
+    const float *coeffs;
+    uint8_t bufLength;
+    uint8_t coeffsLength;
+} firFilter_t;
 
-void filterInitBiQuad(uint8_t filterCutFreq, biquad_t *newState, int16_t samplingRate);
+float pt1FilterApply(pt1Filter_t *filter, float input);
-float filterApplyBiQuad(float sample, biquad_t *state);
+float pt1FilterApply4(pt1Filter_t *filter, float input, float f_cut, float dt);
+float pt1FilterApplyWithRateLimit(pt1Filter_t *filter, float input, float f_cut, float rate_limit, float dT);
+void pt1FilterReset(pt1Filter_t *filter, float input);
+
+void biquadFilterInit(biquadFilter_t *filter, uint8_t filterCutFreq, int16_t samplingRate);
+float biquadFilterApply(biquadFilter_t *filter, float sample);
+
+void firFilterInit(firFilter_t *filter, float *buf, uint8_t bufLength, const float *coeffs);
+void firFilterInit2(firFilter_t *filter, float *buf, uint8_t bufLength, const float *coeffs, uint8_t coeffsLength);
+void firFilterUpdate(firFilter_t *filter, float input);
+float firFilterApply(firFilter_t *filter);
 
-void filterUpdateFIR(int filterLength, float *shiftBuf, float newSample);
-float filterApplyFIR(int filterLength, const float *shiftBuf, const float *coeffBuf, float commonMultiplier);

src/main/config/config.c

@@ -789,6 +789,8 @@ void activateConfig(void)
 
     imuConfigure(&imuRuntimeConfig, &currentProfile->pidProfile);
 
+    pidInit();
+
 #ifdef NAV
     navigationUseConfig(&masterConfig.navConfig);
     navigationUsePIDs(&currentProfile->pidProfile);

src/main/flight/mixer.c

@@ -84,7 +84,7 @@ static uint8_t minServoIndex;
 static uint8_t maxServoIndex;
 
 static servoParam_t *servoConf;
-static biquad_t servoFitlerState[MAX_SUPPORTED_SERVOS];
+static biquadFilter_t servoFitlerState[MAX_SUPPORTED_SERVOS];
 static bool servoFilterIsSet;
 #endif
 
@@ -889,7 +889,7 @@ void filterServos(void)
         // Initialize servo lowpass filter (servos are calculated at looptime rate)
         if (!servoFilterIsSet) {
             for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) {
-                filterInitBiQuad(mixerConfig->servo_lowpass_freq, &servoFitlerState[servoIdx], 0);
+                biquadFilterInit(&servoFitlerState[servoIdx], mixerConfig->servo_lowpass_freq, 0);
             }
 
             servoFilterIsSet = true;
@@ -897,7 +897,7 @@ void filterServos(void)
 
         for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) {
             // Apply servo lowpass filter and do sanity cheching
-            servo[servoIdx] = (int16_t) filterApplyBiQuad((float)servo[servoIdx], &servoFitlerState[servoIdx]);
+            servo[servoIdx] = (int16_t) biquadFilterApply(&servoFitlerState[servoIdx], (float)servo[servoIdx]);
         }
     }
 

src/main/flight/navigation_rewrite.c

@@ -1236,7 +1236,7 @@ float navPidApply2(float setpoint, float measurement, float dt, pidController_t
         pid->last_input = measurement;
     }
 
-    newDerivative = pid->param.kD * filterApplyPt1(newDerivative, &pid->dterm_filter_state, NAV_DTERM_CUT_HZ, dt);
+    newDerivative = pid->param.kD * pt1FilterApply4(&pid->dterm_filter_state, newDerivative, NAV_DTERM_CUT_HZ, dt);
 
     /* Pre-calculate output and limit it if actuator is saturating */
     float outVal = newProportional + pid->integrator + newDerivative;

src/main/flight/navigation_rewrite_fixedwing.c

@@ -96,7 +96,7 @@ bool adjustFixedWingAltitudeFromRCInput(void)
 // Position to velocity controller for Z axis
 static void updateAltitudeVelocityAndPitchController_FW(uint32_t deltaMicros)
 {
-    static filterStatePt1_t velzFilterState;
+    static pt1Filter_t velzFilterState;
 
     // On a fixed wing we might not have a reliable climb rate source (if no BARO available), so we can't apply PID controller to
     // velocity error. We use PID controller on altitude error and calculate desired pitch angle from desired climb rate and forward velocity
@@ -109,7 +109,7 @@ static void updateAltitudeVelocityAndPitchController_FW(uint32_t deltaMicros)
     float maxVelocityDive = -forwardVelocity * sin_approx(DEGREES_TO_RADIANS(posControl.navConfig->fw_max_dive_angle));
 
     posControl.desiredState.vel.V.Z = navPidApply2(posControl.desiredState.pos.V.Z, posControl.actualState.pos.V.Z, US2S(deltaMicros), &posControl.pids.fw_alt, maxVelocityDive, maxVelocityClimb, false);
-    posControl.desiredState.vel.V.Z = filterApplyPt1(posControl.desiredState.vel.V.Z, &velzFilterState, NAV_FW_VEL_CUTOFF_FREQENCY_HZ, US2S(deltaMicros));
+    posControl.desiredState.vel.V.Z = pt1FilterApply4(&velzFilterState, posControl.desiredState.vel.V.Z, NAV_FW_VEL_CUTOFF_FREQENCY_HZ, US2S(deltaMicros));
 
     // Calculate climb angle ( >0 - climb, <0 - dive)
     int16_t climbAngleDeciDeg = RADIANS_TO_DECIDEGREES(atan2_approx(posControl.desiredState.vel.V.Z, forwardVelocity));
@@ -177,7 +177,7 @@ bool adjustFixedWingHeadingFromRCInput(void)
  * XY-position controller for multicopter aircraft
  *-----------------------------------------------------------*/
 static t_fp_vector virtualDesiredPosition;
-static filterStatePt1_t fwPosControllerCorrectionFilterState;
+static pt1Filter_t fwPosControllerCorrectionFilterState;
 
 void resetFixedWingPositionController(void)
 {
@@ -189,7 +189,7 @@ void resetFixedWingPositionController(void)
     posControl.rcAdjustment[ROLL] = 0;
     isRollAdjustmentValid = false;
 
-    filterResetPt1(&fwPosControllerCorrectionFilterState, 0.0f);
+    pt1FilterReset(&fwPosControllerCorrectionFilterState, 0.0f);
 }
 
 static void calculateVirtualPositionTarget_FW(float trackingPeriod)
@@ -279,7 +279,7 @@ static void updatePositionHeadingController_FW(uint32_t deltaMicros)
                                         true);
 
     // Apply low-pass filter to prevent rapid correction
-    rollAdjustment = filterApplyPt1(rollAdjustment, &fwPosControllerCorrectionFilterState, NAV_FW_ROLL_CUTOFF_FREQUENCY_HZ, US2S(deltaMicros));
+    rollAdjustment = pt1FilterApply4(&fwPosControllerCorrectionFilterState, rollAdjustment, NAV_FW_ROLL_CUTOFF_FREQUENCY_HZ, US2S(deltaMicros));
 
     // Convert rollAdjustment to decidegrees (rcAdjustment holds decidegrees)
     posControl.rcAdjustment[ROLL] = CENTIDEGREES_TO_DECIDEGREES(rollAdjustment);

src/main/flight/navigation_rewrite_multicopter.c

@@ -56,7 +56,7 @@
  *-----------------------------------------------------------*/
 static int16_t rcCommandAdjustedThrottle;
 static int16_t altHoldThrottleRCZero = 1500;
-static filterStatePt1_t altholdThrottleFilterState;
+static pt1Filter_t altholdThrottleFilterState;
 static bool prepareForTakeoffOnReset = false;
 
 /* Calculate global altitude setpoint based on surface setpoint */
@@ -107,7 +107,7 @@ static void updateAltitudeThrottleController_MC(uint32_t deltaMicros)
 
     posControl.rcAdjustment[THROTTLE] = navPidApply2(posControl.desiredState.vel.V.Z, posControl.actualState.vel.V.Z, US2S(deltaMicros), &posControl.pids.vel[Z], thrAdjustmentMin, thrAdjustmentMax, false);
 
-    posControl.rcAdjustment[THROTTLE] = filterApplyPt1(posControl.rcAdjustment[THROTTLE], &altholdThrottleFilterState, NAV_THROTTLE_CUTOFF_FREQENCY_HZ, US2S(deltaMicros));
+    posControl.rcAdjustment[THROTTLE] = pt1FilterApply4(&altholdThrottleFilterState, posControl.rcAdjustment[THROTTLE], NAV_THROTTLE_CUTOFF_FREQENCY_HZ, US2S(deltaMicros));
     posControl.rcAdjustment[THROTTLE] = constrain(posControl.rcAdjustment[THROTTLE], thrAdjustmentMin, thrAdjustmentMax);
 }
 
@@ -174,7 +174,7 @@ void resetMulticopterAltitudeController(void)
 {
     navPidReset(&posControl.pids.vel[Z]);
     navPidReset(&posControl.pids.surface);
-    filterResetPt1(&altholdThrottleFilterState, 0.0f);
+    pt1FilterReset(&altholdThrottleFilterState, 0.0f);
     posControl.desiredState.vel.V.Z = posControl.actualState.vel.V.Z;   // Gradually transition from current climb
     posControl.rcAdjustment[THROTTLE] = 0;
 
@@ -247,7 +247,7 @@ bool adjustMulticopterHeadingFromRCInput(void)
 /*-----------------------------------------------------------
  * XY-position controller for multicopter aircraft
  *-----------------------------------------------------------*/
-static filterStatePt1_t mcPosControllerAccFilterStateX, mcPosControllerAccFilterStateY;
+static pt1Filter_t mcPosControllerAccFilterStateX, mcPosControllerAccFilterStateY;
 static float lastAccelTargetX = 0.0f, lastAccelTargetY = 0.0f;
 
 void resetMulticopterPositionController(void)
@@ -256,8 +256,8 @@ void resetMulticopterPositionController(void)
     for (axis = 0; axis < 2; axis++) {
         navPidReset(&posControl.pids.vel[axis]);
         posControl.rcAdjustment[axis] = 0;
-        filterResetPt1(&mcPosControllerAccFilterStateX, 0.0f);
+        pt1FilterReset(&mcPosControllerAccFilterStateX, 0.0f);
-        filterResetPt1(&mcPosControllerAccFilterStateY, 0.0f);
+        pt1FilterReset(&mcPosControllerAccFilterStateY, 0.0f);
         lastAccelTargetX = 0.0f;
         lastAccelTargetY = 0.0f;
     }
@@ -394,8 +394,8 @@ static void updatePositionAccelController_MC(uint32_t deltaMicros, float maxAcce
     lastAccelTargetY = newAccelY;
 
     // Apply LPF to jerk limited acceleration target
-    float accelN = filterApplyPt1(newAccelX, &mcPosControllerAccFilterStateX, NAV_ACCEL_CUTOFF_FREQUENCY_HZ, US2S(deltaMicros));
+    float accelN = pt1FilterApply4(&mcPosControllerAccFilterStateX, newAccelX, NAV_ACCEL_CUTOFF_FREQUENCY_HZ, US2S(deltaMicros));
-    float accelE = filterApplyPt1(newAccelY, &mcPosControllerAccFilterStateY, NAV_ACCEL_CUTOFF_FREQUENCY_HZ, US2S(deltaMicros));
+    float accelE = pt1FilterApply4(&mcPosControllerAccFilterStateY, newAccelY, NAV_ACCEL_CUTOFF_FREQUENCY_HZ, US2S(deltaMicros));
 
     // Rotate acceleration target into forward-right frame (aircraft)
     float accelForward = accelN * posControl.actualState.cosYaw + accelE * posControl.actualState.sinYaw;

src/main/flight/navigation_rewrite_private.h

@@ -93,7 +93,7 @@ typedef struct {
 
 typedef struct {
     pidControllerParam_t param;
-    filterStatePt1_t dterm_filter_state;  // last derivative for low-pass filter
+    pt1Filter_t dterm_filter_state;  // last derivative for low-pass filter
     float integrator;       // integrator value
     float last_input;       // last input for derivative
 } pidController_t;

src/main/flight/pid.c

@@ -59,8 +59,9 @@ typedef struct {
     float rateTarget;
 
     // Buffer for derivative calculation
-#define DTERM_BUF_COUNT 5
+#define PID_GYRO_RATE_BUF_LENGTH 5
-    float dTermBuf[DTERM_BUF_COUNT];
+    float gyroRateBuf[PID_GYRO_RATE_BUF_LENGTH];
+    firFilter_t gyroRateFilter;
 
     // Rate integrator
     float errorGyroIf;
@@ -70,11 +71,11 @@ typedef struct {
     float axisLockAccum;
 
     // Used for ANGLE filtering
-    filterStatePt1_t angleFilterState;
+    pt1Filter_t angleFilterState;
 
     // Rate filtering
-    filterStatePt1_t ptermLpfState;
+    pt1Filter_t ptermLpfState;
-    filterStatePt1_t deltaLpfState;
+    pt1Filter_t deltaLpfState;
 } pidState_t;
 
 extern uint8_t motorCount;
@@ -93,6 +94,17 @@ int32_t axisPID_P[FLIGHT_DYNAMICS_INDEX_COUNT], axisPID_I[FLIGHT_DYNAMICS_INDEX_
 
 static pidState_t pidState[FLIGHT_DYNAMICS_INDEX_COUNT];
 
+void pidInit(void)
+{
+    // Calculate derivative using 5-point noise-robust differentiators without time delay (one-sided or forward filters)
+    // by Pavel Holoborodko, see http://www.holoborodko.com/pavel/numerical-methods/numerical-derivative/smooth-low-noise-differentiators/
+    // h[0] = 5/8, h[-1] = 1/4, h[-2] = -1, h[-3] = -1/4, h[-4] = 3/8
+    static const float dtermCoeffs[PID_GYRO_RATE_BUF_LENGTH] = {5.0f/8, 2.0f/8, -8.0f/8, -2.0f/8, 3.0f/8};
+    for (int axis = 0; axis < 3; ++ axis) {
+        firFilterInit(&pidState[axis].gyroRateFilter, pidState[axis].gyroRateBuf, PID_GYRO_RATE_BUF_LENGTH, dtermCoeffs);
+    }
+}
+
 void pidResetErrorAccumulators(void)
 {
     // Reset R/P/Y integrator
@@ -245,7 +257,7 @@ static void pidLevel(const pidProfile_t *pidProfile, pidState_t *pidState, fligh
     //     response to rapid attitude changes and smoothing out self-leveling reaction
     if (pidProfile->I8[PIDLEVEL]) {
         // I8[PIDLEVEL] is filter cutoff frequency (Hz). Practical values of filtering frequency is 5-10 Hz
-        pidState->rateTarget = filterApplyPt1(pidState->rateTarget, &pidState->angleFilterState, pidProfile->I8[PIDLEVEL], dT);
+        pidState->rateTarget = pt1FilterApply4(&pidState->angleFilterState, pidState->rateTarget, pidProfile->I8[PIDLEVEL], dT);
     }
 }
 
@@ -262,7 +274,7 @@ static void pidApplyRateController(const pidProfile_t *pidProfile, pidState_t *p
 
     // Additional P-term LPF on YAW axis
     if (axis == FD_YAW && pidProfile->yaw_lpf_hz) {
-        newPTerm = filterApplyPt1(newPTerm, &pidState->ptermLpfState, pidProfile->yaw_lpf_hz, dT);
+        newPTerm = pt1FilterApply4(&pidState->ptermLpfState, newPTerm, pidProfile->yaw_lpf_hz, dT);
     }
 
     // Calculate new D-term
@@ -271,16 +283,12 @@ static void pidApplyRateController(const pidProfile_t *pidProfile, pidState_t *p
         // optimisation for when D8 is zero, often used by YAW axis
         newDTerm = 0;
     } else {
-        // Calculate derivative using 5-point noise-robust differentiators without time delay (one-sided or forward filters)
+        firFilterUpdate(&pidState->gyroRateFilter, pidState->gyroRate);
-        // by Pavel Holoborodko, see http://www.holoborodko.com/pavel/numerical-methods/numerical-derivative/smooth-low-noise-differentiators/
+        newDTerm = firFilterApply(&pidState->gyroRateFilter) * (-pidState->kD / dT);
-        // h[0] = 5/8, h[-1] = 1/4, h[-2] = -1, h[-3] = -1/4, h[-4] = 3/8
-        static const float dtermCoeffs[DTERM_BUF_COUNT] = {5.0f, 2.0f, -8.0f, -2.0f, 3.0f};
-        filterUpdateFIR(DTERM_BUF_COUNT, pidState->dTermBuf, pidState->gyroRate);
-        newDTerm = filterApplyFIR(DTERM_BUF_COUNT, pidState->dTermBuf, dtermCoeffs, -pidState->kD / (8 * dT));
 
         // Apply additional lowpass
         if (pidProfile->dterm_lpf_hz) {
-            newDTerm = filterApplyPt1(newDTerm, &pidState->deltaLpfState, pidProfile->dterm_lpf_hz, dT);
+            newDTerm = pt1FilterApply4(&pidState->deltaLpfState, newDTerm, pidProfile->dterm_lpf_hz, dT);
         }
     }
 
@@ -355,7 +363,7 @@ uint8_t getMagHoldState()
 float pidMagHold(const pidProfile_t *pidProfile)
 {
 
-    static filterStatePt1_t magHoldRateFilter;
+    static pt1Filter_t magHoldRateFilter;
     float magHoldRate;
 
     int16_t error = DECIDEGREES_TO_DEGREES(attitude.values.yaw) - magHoldTargetHeading;
@@ -403,7 +411,7 @@ float pidMagHold(const pidProfile_t *pidProfile)
 
     magHoldRate = error * pidProfile->P8[PIDMAG] / 30;
     magHoldRate = constrainf(magHoldRate, -pidProfile->mag_hold_rate_limit, pidProfile->mag_hold_rate_limit);
-    magHoldRate = filterApplyPt1(magHoldRate, &magHoldRateFilter, MAG_HOLD_ERROR_LPF_FREQ, dT);
+    magHoldRate = pt1FilterApply4(&magHoldRateFilter, magHoldRate, MAG_HOLD_ERROR_LPF_FREQ, dT);
 
     return magHoldRate;
 }

src/main/flight/pid.h

@@ -69,6 +69,7 @@ typedef struct pidProfile_s {
 extern int16_t axisPID[];
 extern int32_t axisPID_P[], axisPID_I[], axisPID_D[], axisPID_Setpoint[];
 
+void pidInit(void);
 void pidResetErrorAccumulators(void);
 void updatePIDCoefficients(const pidProfile_t *pidProfile, const controlRateConfig_t *controlRateConfig, const rxConfig_t *rxConfig);
 

src/main/mw.c

@@ -492,7 +492,7 @@ void filterRc(bool isRXDataNew)
     static int16_t lastCommand[4] = { 0, 0, 0, 0 };
     static int16_t deltaRC[4] = { 0, 0, 0, 0 };
     static int16_t factor, rcInterpolationFactor;
-    static biquad_t filteredCycleTimeState;
+    static biquadFilter_t filteredCycleTimeState;
     static bool filterInitialised;
     uint16_t filteredCycleTime;
     uint16_t rxRefreshRate;
@@ -502,11 +502,11 @@ void filterRc(bool isRXDataNew)
 
     // Calculate average cycle time (1Hz LPF on cycle time)
     if (!filterInitialised) {
-        filterInitBiQuad(1, &filteredCycleTimeState, 0);
+        biquadFilterInit(&filteredCycleTimeState, 1, 0);
         filterInitialised = true;
     }
 
-    filteredCycleTime = filterApplyBiQuad((float) cycleTime, &filteredCycleTimeState);
+    filteredCycleTime = biquadFilterApply(&filteredCycleTimeState, (float) cycleTime);
 
     rcInterpolationFactor = rxRefreshRate / filteredCycleTime + 1;
 

src/main/sensors/acceleration.c

@@ -49,7 +49,7 @@ static flightDynamicsTrims_t * accZero;
 static flightDynamicsTrims_t * accGain;
 
 static int8_t accLpfCutHz = 0;
-static biquad_t accFilterState[XYZ_AXIS_COUNT];
+static biquadFilter_t accFilterState[XYZ_AXIS_COUNT];
 static bool accFilterInitialised = false;
 
 void accSetCalibrationCycles(uint16_t calibrationCyclesRequired)
@@ -184,7 +184,7 @@ void updateAccelerationReadings(void)
         if (!accFilterInitialised) {
             if (targetLooptime) {  /* Initialisation needs to happen once sample rate is known */
                 for (int axis = 0; axis < 3; axis++) {
-                    filterInitBiQuad(accLpfCutHz, &accFilterState[axis], 0);
+                    biquadFilterInit(&accFilterState[axis], accLpfCutHz, 0);
                 }
 
                 accFilterInitialised = true;
@@ -193,7 +193,7 @@ void updateAccelerationReadings(void)
 
         if (accFilterInitialised) {
             for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
-                accADC[axis] = lrintf(filterApplyBiQuad((float) accADC[axis], &accFilterState[axis]));
+                accADC[axis] = lrintf(biquadFilterApply(&accFilterState[axis], (float) accADC[axis]));
             }
         }
     }

src/main/sensors/battery.c

@@ -65,11 +65,11 @@ uint16_t batteryAdcToVoltage(uint16_t src)
 static void updateBatteryVoltage(uint32_t vbatTimeDelta)
 {
     uint16_t vbatSample;
-    static filterStatePt1_t vbatFilterState;
+    static pt1Filter_t vbatFilterState;
 
     // store the battery voltage with some other recent battery voltage readings
     vbatSample = vbatLatestADC = adcGetChannel(ADC_BATTERY);
-    vbatSample = filterApplyPt1(vbatSample, &vbatFilterState, VBATT_LPF_FREQ, vbatTimeDelta * 1e-6f);
+    vbatSample = pt1FilterApply4(&vbatFilterState, vbatSample, VBATT_LPF_FREQ, vbatTimeDelta * 1e-6f);
     vbat = batteryAdcToVoltage(vbatSample);
 }
 

src/main/sensors/gyro.c

@@ -48,7 +48,7 @@ static int16_t gyroADCRaw[XYZ_AXIS_COUNT];
 static int32_t gyroZero[FLIGHT_DYNAMICS_INDEX_COUNT] = { 0, 0, 0 };
 
 static int8_t gyroLpfCutHz = 0;
-static biquad_t gyroFilterState[XYZ_AXIS_COUNT];
+static biquadFilter_t gyroFilterState[XYZ_AXIS_COUNT];
 static bool gyroFilterInitialised = false;
 
 void useGyroConfig(gyroConfig_t *gyroConfigToUse, int8_t initialGyroLpfCutHz)
@@ -137,7 +137,7 @@ void gyroUpdate(void)
         if (!gyroFilterInitialised) {
             if (targetLooptime) {  /* Initialisation needs to happen once sample rate is known */
                 for (int axis = 0; axis < 3; axis++) {
-                    filterInitBiQuad(gyroLpfCutHz, &gyroFilterState[axis], 0);
+                    biquadFilterInit(&gyroFilterState[axis], gyroLpfCutHz, 0);
                 }
 
                 gyroFilterInitialised = true;
@@ -146,7 +146,7 @@ void gyroUpdate(void)
 
         if (gyroFilterInitialised) {
             for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
-                gyroADC[axis] = lrintf(filterApplyBiQuad((float) gyroADC[axis], &gyroFilterState[axis]));
+                gyroADC[axis] = lrintf(biquadFilterApply(&gyroFilterState[axis], (float) gyroADC[axis]));
             }
         }
     }