PT1 low pass filter clean up + fixes

src/main/common/filter.c

@@ -42,64 +42,52 @@ float nullFilterApply4(void *filter, float input, float f_cut, float dt)
     return input;
 }
 
-// PT1 Low Pass filter
+/* PT1 Low Pass filter
+ * f_cut = cutoff frequency. Use pt1FilterSetTimeConstant to directly set RC time constant tau if required.
+ */
 
 static float FAST_CODE pt1ComputeRC(const float f_cut)
 {
     return 1.0f / (2.0f * M_PIf * f_cut);
 }
 
-// f_cut = cutoff frequency
-void pt1FilterInitRC(pt1Filter_t *filter, float tau, float dT)
+void pt1FilterInit(pt1Filter_t *filter, float f_cut, float dT)
 {
-    filter->state = 0.0f;
-    filter->RC = tau;
+    filter->RC = pt1ComputeRC(f_cut);
     filter->dT = dT;
     filter->alpha = filter->dT / (filter->RC + filter->dT);
+    filter->state = 0.0f;
 }
 
-void pt1FilterInit(pt1Filter_t *filter, float f_cut, float dT)
+float FAST_CODE NOINLINE pt1FilterApply(pt1Filter_t *filter, float input)  // use with pt1FilterInit if dT and f_cut are constants
 {
-    pt1FilterInitRC(filter, pt1ComputeRC(f_cut), dT);
+    return filter->state = filter->state + filter->alpha * (input - filter->state);
 }
 
-void pt1FilterSetTimeConstant(pt1Filter_t *filter, float tau) {
-    filter->RC = tau;
-}
-
-float pt1FilterGetLastOutput(pt1Filter_t *filter) {
-    return filter->state;
-}
-
-void pt1FilterUpdateCutoff(pt1Filter_t *filter, float f_cut)
+float FAST_CODE NOINLINE pt1FilterApply4(pt1Filter_t *filter, float input, float f_cut, float dT)
 {
-    filter->RC = pt1ComputeRC(f_cut);
+    // Pre calculate and store RC only if f_cut non zero
+    if (f_cut && !filter->RC) {
+        filter->RC = pt1ComputeRC(f_cut);
+    }
+
+    filter->dT = dT;    // cache latest dT for possible use in pt1FilterApply
     filter->alpha = filter->dT / (filter->RC + filter->dT);
-}
 
-float FAST_CODE NOINLINE pt1FilterApply(pt1Filter_t *filter, float input)
-{
-    filter->state = filter->state + filter->alpha * (input - filter->state);
-    return filter->state;
+    return filter->state = filter->state + filter->alpha * (input - filter->state);
 }
 
-float pt1FilterApply3(pt1Filter_t *filter, float input, float dT)
-{
-    filter->dT = dT;
-    filter->state = filter->state + dT / (filter->RC + dT) * (input - filter->state);
-    return filter->state;
+void pt1FilterSetTimeConstant(pt1Filter_t *filter, float tau) {
+    filter->RC = tau;
 }
 
-float FAST_CODE NOINLINE pt1FilterApply4(pt1Filter_t *filter, float input, float f_cut, float dT)
+void pt1FilterSetCutoff(pt1Filter_t *filter, float f_cut)
 {
-    // Pre calculate and store RC
-    if (!filter->RC) {
-        filter->RC = pt1ComputeRC(f_cut);
-    }
-
-    filter->dT = dT;    // cache latest dT for possible use in pt1FilterApply
+    filter->RC = pt1ComputeRC(f_cut);
     filter->alpha = filter->dT / (filter->RC + filter->dT);
-    filter->state = filter->state + filter->alpha * (input - filter->state);
+}
+
+float pt1FilterGetLastOutput(pt1Filter_t *filter) {
     return filter->state;
 }
 

src/main/common/filter.h

@@ -47,8 +47,8 @@ typedef struct biquadFilter_s {
     float x1, x2, y1, y2;
 } biquadFilter_t;
 
-typedef union { 
-    biquadFilter_t biquad; 
+typedef union {
+    biquadFilter_t biquad;
     pt1Filter_t pt1;
     pt2Filter_t pt2;
     pt3Filter_t pt3;
@@ -97,13 +97,11 @@ float nullFilterApply(void *filter, float input);
 float nullFilterApply4(void *filter, float input, float f_cut, float dt);
 
 void pt1FilterInit(pt1Filter_t *filter, float f_cut, float dT);
-void pt1FilterInitRC(pt1Filter_t *filter, float tau, float dT);
-void pt1FilterSetTimeConstant(pt1Filter_t *filter, float tau);
-void pt1FilterUpdateCutoff(pt1Filter_t *filter, float f_cut);
-float pt1FilterGetLastOutput(pt1Filter_t *filter);
 float pt1FilterApply(pt1Filter_t *filter, float input);
-float pt1FilterApply3(pt1Filter_t *filter, float input, float dT);
 float pt1FilterApply4(pt1Filter_t *filter, float input, float f_cut, float dt);
+void pt1FilterSetTimeConstant(pt1Filter_t *filter, float tau);
+void pt1FilterSetCutoff(pt1Filter_t *filter, float f_cut);
+float pt1FilterGetLastOutput(pt1Filter_t *filter);
 void pt1FilterReset(pt1Filter_t *filter, float input);
 
 /*

src/main/flight/pid.c

@@ -150,7 +150,7 @@ static EXTENDED_FASTRAM float antigravityAccelerator;
 #endif
 
 #define D_BOOST_GYRO_LPF_HZ 80    // Biquad lowpass input cutoff to peak D around propwash frequencies
-#define D_BOOST_LPF_HZ 7          // PT1 lowpass cutoff to smooth the boost effect
+#define D_BOOST_LPF_HZ 7.0f       // PT1 lowpass cutoff to smooth the boost effect
 
 #ifdef USE_D_BOOST
 static EXTENDED_FASTRAM float dBoostMin;
@@ -384,7 +384,8 @@ bool pidInitFilters(void)
 void pidResetTPAFilter(void)
 {
     if (usedPidControllerType == PID_TYPE_PIFF && currentControlProfile->throttle.fixedWingTauMs > 0) {
-        pt1FilterInitRC(&fixedWingTpaFilter, MS2S(currentControlProfile->throttle.fixedWingTauMs), US2S(TASK_PERIOD_HZ(TASK_AUX_RATE_HZ)));
+        pt1FilterInit(&fixedWingTpaFilter, 1.0f, HZ2S(TASK_AUX_RATE_HZ));
+        pt1FilterSetTimeConstant(&fixedWingTpaFilter, MS2S(currentControlProfile->throttle.fixedWingTauMs));
         pt1FilterReset(&fixedWingTpaFilter, getThrottleIdleValue());
     }
 }
@@ -549,7 +550,7 @@ void updatePIDCoefficients(void)
     for (int axis = 0; axis < 3; axis++) {
         pidState[axis].stickPosition = constrain(rxGetChannelValue(axis) - PWM_RANGE_MIDDLE, -500, 500) / 500.0f;
     }
-    
+
     float tpaFactor=1.0f;
     float iTermFactor=1.0f;  // Separate factor for I-term scaling
     if(usedPidControllerType == PID_TYPE_PIFF){ // Fixed wing TPA calculation
@@ -569,13 +570,13 @@ void updatePIDCoefficients(void)
     }
     tpaFactorprev = tpaFactor;
 
-    
+
     // If nothing changed - don't waste time recalculating coefficients
     if (!pidGainsUpdateRequired) {
         return;
     }
 
-    
+
     // PID coefficients can be update only with THROTTLE and TPA or inflight PID adjustments
     //TODO: Next step would be to update those only at THROTTLE or inflight adjustments change
     for (int axis = 0; axis < 3; axis++) {
@@ -1378,12 +1379,8 @@ void pidInit(void)
 
         pidState[axis].axis = axis;
         pidState[axis].pidSumLimit = getPidSumLimit(axis);
-        if (axis == FD_YAW) {
-            if (yawLpfHz) {
-                pidState[axis].ptermFilterApplyFn = (filterApply4FnPtr) pt1FilterApply4;
-            } else {
-                pidState[axis].ptermFilterApplyFn = (filterApply4FnPtr) nullFilterApply4;
-            }
+        if (axis == FD_YAW && yawLpfHz) {
+            pidState[axis].ptermFilterApplyFn = (filterApply4FnPtr) pt1FilterApply4;
         } else {
             pidState[axis].ptermFilterApplyFn = (filterApply4FnPtr) nullFilterApply4;
         }

src/main/flight/pid.h

@@ -33,7 +33,7 @@
 
 #define AXIS_ACCEL_MIN_LIMIT        50
 
-#define HEADING_HOLD_ERROR_LPF_FREQ 2
+#define HEADING_HOLD_ERROR_LPF_FREQ 2.0f
 
 /*
 FP-PID has been rescaled to match LuxFloat (and MWRewrite) from Cleanflight 1.13

src/main/flight/power_limits.c

@@ -86,8 +86,7 @@ void powerLimiterInit(void) {
     burstCurrentReserve = burstCurrentReserveMax = currentBurstOverContinuous * currentBatteryProfile->powerLimits.burstCurrentTime * 1e6;
     burstCurrentReserveFalldown = currentBurstOverContinuous * currentBatteryProfile->powerLimits.burstCurrentFalldownTime * 1e6;
 
-    pt1FilterInit(&currentThrAttnFilter, powerLimitsConfig()->attnFilterCutoff, 0);
-    pt1FilterInitRC(&currentThrLimitingBaseFilter, LIMITING_THR_FILTER_TCONST, 0);
+    pt1FilterSetTimeConstant(&currentThrLimitingBaseFilter, LIMITING_THR_FILTER_TCONST);
 
 #ifdef USE_ADC
     // Only enforce burst >= continuous if burst is enabled (non-zero)
@@ -102,8 +101,7 @@ void powerLimiterInit(void) {
     burstPowerReserve = burstPowerReserveMax = powerBurstOverContinuous * currentBatteryProfile->powerLimits.burstPowerTime * 1e6;
     burstPowerReserveFalldown = powerBurstOverContinuous * currentBatteryProfile->powerLimits.burstPowerFalldownTime * 1e6;
 
-    pt1FilterInit(&powerThrAttnFilter, powerLimitsConfig()->attnFilterCutoff, 0);
-    pt1FilterInitRC(&powerThrLimitingBaseFilter, LIMITING_THR_FILTER_TCONST, 0);
+    pt1FilterSetTimeConstant(&powerThrLimitingBaseFilter, LIMITING_THR_FILTER_TCONST);
 #endif
 }
 
@@ -150,6 +148,7 @@ void powerLimiterApply(int16_t *throttleCommand) {
     static timeUs_t lastCallTimestamp = 0;
     timeUs_t currentTimeUs = micros();
     timeDelta_t callTimeDelta = cmpTimeUs(currentTimeUs, lastCallTimestamp);
+    const float attnFilterCutoff = powerLimitsConfig()->attnFilterCutoff;
 
     int16_t throttleBase;
     int16_t currentThrottleCommand;
@@ -172,9 +171,9 @@ void powerLimiterApply(int16_t *throttleCommand) {
 
     float currentThrAttnProportional = MAX(0, overCurrent) * powerLimitsConfig()->piP * 1e-3f;
 
-    uint16_t currentThrAttn = lrintf(pt1FilterApply3(&currentThrAttnFilter, currentThrAttnProportional + currentThrAttnIntegrator, callTimeDelta * 1e-6f));
+    uint16_t currentThrAttn = lrintf(pt1FilterApply4(&currentThrAttnFilter, currentThrAttnProportional + currentThrAttnIntegrator, attnFilterCutoff, US2S(callTimeDelta)));
 
-    throttleBase = wasLimitingCurrent ? lrintf(pt1FilterApply3(&currentThrLimitingBaseFilter, *throttleCommand, callTimeDelta * 1e-6f)) : *throttleCommand;
+    throttleBase = wasLimitingCurrent ? lrintf(pt1FilterApply4(&currentThrLimitingBaseFilter, *throttleCommand, 0.0f, US2S(callTimeDelta))) : *throttleCommand;
     uint16_t currentThrAttned = MAX(PWM_RANGE_MIN, (int16_t)throttleBase - currentThrAttn);
 
     if (activeCurrentLimit && currentThrAttned < *throttleCommand) {
@@ -201,9 +200,9 @@ void powerLimiterApply(int16_t *throttleCommand) {
 
     float powerThrAttnProportional = MAX(0, overPower) * powerLimitsConfig()->piP / voltage * 1e-1f;
 
-    uint16_t powerThrAttn = lrintf(pt1FilterApply3(&powerThrAttnFilter, powerThrAttnProportional + powerThrAttnIntegrator, callTimeDelta * 1e-6f));
+    uint16_t powerThrAttn = lrintf(pt1FilterApply4(&powerThrAttnFilter, powerThrAttnProportional + powerThrAttnIntegrator, attnFilterCutoff, US2S(callTimeDelta)));
 
-    throttleBase = wasLimitingPower ? lrintf(pt1FilterApply3(&powerThrLimitingBaseFilter, *throttleCommand, callTimeDelta * 1e-6)) : *throttleCommand;
+    throttleBase = wasLimitingPower ? lrintf(pt1FilterApply4(&powerThrLimitingBaseFilter, *throttleCommand, 0.0f, US2S(callTimeDelta))) : *throttleCommand;
     uint16_t powerThrAttned = MAX(PWM_RANGE_MIN, (int16_t)throttleBase - powerThrAttn);
 
     if (activePowerLimit && powerThrAttned < *throttleCommand) {

src/main/flight/servos.c

@@ -206,7 +206,7 @@ int getServoCount(void)
 
 void loadCustomServoMixer(void)
 {
-    
+
     //move the rate filter to new servo rules
     int movefilterCount = 0;
     static servoMixerSwitch_t servoMixerSwitchHelper[MAX_SERVO_RULES_SWITCH_CARRY]; // helper to keep track of servoSpeedLimitFilter of servo rules
@@ -603,7 +603,7 @@ void processServoAutotrimMode(void)
     }
 }
 
-#define SERVO_AUTOTRIM_FILTER_CUTOFF    1       // LPF cutoff frequency
+#define SERVO_AUTOTRIM_FILTER_CUTOFF    1.0f     // LPF cutoff frequency
 #define SERVO_AUTOTRIM_CENTER_MIN       1300
 #define SERVO_AUTOTRIM_CENTER_MAX       1700
 #define SERVO_AUTOTRIM_UPDATE_SIZE      5

src/main/io/osd.c

@@ -125,7 +125,7 @@
 #define VIDEO_BUFFER_CHARS_HDZERO 900
 #define VIDEO_BUFFER_CHARS_DJIWTF 1320
 
-#define GFORCE_FILTER_TC 0.2
+#define GFORCE_FILTER_T_CUT_HZ 0.8f
 
 #define OSD_STATS_SINGLE_PAGE_MIN_ROWS 18
 #define IS_HI(X)  (rxGetChannelValue(X) > 1750)
@@ -1313,7 +1313,7 @@ uint16_t osdGetRemainingGlideTime(void) {
     const  timeMs_t curTimeMs = millis();
     static timeMs_t glideTimeUpdatedMs;
 
-    value = pt1FilterApply4(&glideTimeFilterState, isnormal(value) ? value : 0, 0.5, MS2S(curTimeMs - glideTimeUpdatedMs));
+    value = pt1FilterApply4(&glideTimeFilterState, isnormal(value) ? value : 0.0f, 0.5f, MS2S(curTimeMs - glideTimeUpdatedMs));
     glideTimeUpdatedMs = curTimeMs;
 
     if (value < 0) {
@@ -2071,7 +2071,7 @@ static bool osdDrawSingleElement(uint8_t item)
             const timeMs_t currentTimeMs = millis();
             static timeMs_t gsUpdatedTimeMs;
             float glideSlope = horizontalSpeed / sinkRate;
-            glideSlope = pt1FilterApply4(&gsFilterState, isnormal(glideSlope) ? glideSlope : 200, 0.5, MS2S(currentTimeMs - gsUpdatedTimeMs));
+            glideSlope = pt1FilterApply4(&gsFilterState, isnormal(glideSlope) ? glideSlope : 200.0f, 0.5f, MS2S(currentTimeMs - gsUpdatedTimeMs));
             gsUpdatedTimeMs = currentTimeMs;
 
             buff[0] = SYM_GLIDESLOPE;
@@ -3113,7 +3113,7 @@ static bool osdDrawSingleElement(uint8_t item)
             if (getEstimatedActualVelocity(Z) > 0) {
                 if (vEfficiencyTimeDelta >= EFFICIENCY_UPDATE_INTERVAL) {
                                                             // Centiamps (kept for osdFormatCentiNumber) / m/s - Will appear as A / m/s in OSD
-                    value = pt1FilterApply4(&veFilterState, (float)getAmperage() / (getEstimatedActualVelocity(Z) / 100.0f), 1, US2S(vEfficiencyTimeDelta));
+                    value = pt1FilterApply4(&veFilterState, (float)getAmperage() / (getEstimatedActualVelocity(Z) / 100.0f), 1.0f, US2S(vEfficiencyTimeDelta));
 
                     vEfficiencyUpdated = currentTimeUs;
                 } else {
@@ -3592,8 +3592,7 @@ static bool osdDrawSingleElement(uint8_t item)
 #endif
                 ) && gpsSol.groundSpeed > 0) {
                 if (efficiencyTimeDelta >= EFFICIENCY_UPDATE_INTERVAL) {
-                    value = pt1FilterApply4(&eFilterState, ((float)getAmperage() / gpsSol.groundSpeed) / 0.0036f,
-                        1, US2S(efficiencyTimeDelta));
+                    value = pt1FilterApply4(&eFilterState, ((float)getAmperage() / gpsSol.groundSpeed) / 0.0036f, 1.0f, US2S(efficiencyTimeDelta));
 
                     efficiencyUpdated = currentTimeUs;
                 } else {
@@ -3667,8 +3666,7 @@ static bool osdDrawSingleElement(uint8_t item)
 #endif
                 ) && gpsSol.groundSpeed > 0) {
                 if (efficiencyTimeDelta >= EFFICIENCY_UPDATE_INTERVAL) {
-                    value = pt1FilterApply4(&eFilterState, ((float)getPower() / gpsSol.groundSpeed) / 0.0036f,
-                        1, US2S(efficiencyTimeDelta));
+                    value = pt1FilterApply4(&eFilterState, ((float)getPower() / gpsSol.groundSpeed) / 0.0036f, 1.0f, US2S(efficiencyTimeDelta));
 
                     efficiencyUpdated = currentTimeUs;
                 } else {
@@ -5785,26 +5783,22 @@ static void osdShowArmed(void)
     }
 }
 
-static void osdFilterData(timeUs_t currentTimeUs) {
+static void osdFilterData(timeUs_t currentTimeUs)
+{
     static timeUs_t lastRefresh = 0;
     float refresh_dT = US2S(cmpTimeUs(currentTimeUs, lastRefresh));
 
     GForce = fast_fsqrtf(vectorNormSquared(&imuMeasuredAccelBF)) / GRAVITY_MSS;
-    for (uint8_t axis = 0; axis < XYZ_AXIS_COUNT; ++axis) GForceAxis[axis] = imuMeasuredAccelBF.v[axis] / GRAVITY_MSS;
+    for (uint8_t axis = 0; axis < XYZ_AXIS_COUNT; ++axis) {
+        GForceAxis[axis] = imuMeasuredAccelBF.v[axis] / GRAVITY_MSS;
+    }
 
     if (lastRefresh) {
-        GForce = pt1FilterApply3(&GForceFilter, GForce, refresh_dT);
-        for (uint8_t axis = 0; axis < XYZ_AXIS_COUNT; ++axis) pt1FilterApply3(GForceFilterAxis + axis, GForceAxis[axis], refresh_dT);
-    } else {
-        pt1FilterInitRC(&GForceFilter, GFORCE_FILTER_TC, 0);
-        pt1FilterReset(&GForceFilter, GForce);
-
+        GForce = pt1FilterApply4(&GForceFilter, GForce, GFORCE_FILTER_T_CUT_HZ, refresh_dT);
         for (uint8_t axis = 0; axis < XYZ_AXIS_COUNT; ++axis) {
-            pt1FilterInitRC(GForceFilterAxis + axis, GFORCE_FILTER_TC, 0);
-            pt1FilterReset(GForceFilterAxis + axis, GForceAxis[axis]);
+            GForceAxis[axis] = pt1FilterApply4(&GForceFilterAxis[axis], GForceAxis[axis], GFORCE_FILTER_T_CUT_HZ, refresh_dT);
         }
     }
-
     lastRefresh = currentTimeUs;
 }
 

src/main/io/osd_dji_hd.c

@@ -785,8 +785,7 @@ static void osdDJIEfficiencyMahPerKM(char *buff)
 #endif
         ) && gpsSol.groundSpeed > 0) {
         if (efficiencyTimeDelta >= EFFICIENCY_UPDATE_INTERVAL) {
-            value = pt1FilterApply4(&eFilterState, ((float)getAmperage() / gpsSol.groundSpeed) / 0.0036f,
-                1, US2S(efficiencyTimeDelta));
+            value = pt1FilterApply4(&eFilterState, ((float)getAmperage() / gpsSol.groundSpeed) / 0.0036f, 1.0f, US2S(efficiencyTimeDelta));
 
             efficiencyUpdated = currentTimeUs;
         } else {

src/main/navigation/navigation_pos_estimator.c

@@ -313,7 +313,7 @@ void updatePositionEstimator_BaroTopic(timeUs_t currentTimeUs)
             const float baroDtSec = US2S(baroDtUs);
             posEstimator.baro.updateDt = baroDtSec;
 
-            posEstimator.baro.alt = pt1FilterApply3(&posEstimator.baro.avgFilter, posEstimator.baro.alt, baroDtSec);
+            posEstimator.baro.alt = pt1FilterApply4(&posEstimator.baro.avgFilter, posEstimator.baro.alt, INAV_BARO_AVERAGE_HZ, baroDtSec);
 
             // baro altitude rate
             static float baroAltPrevious = 0;
@@ -970,9 +970,6 @@ void initializePositionEstimator(void)
         posEstimator.est.pos.v[axis] = 0;
         posEstimator.est.vel.v[axis] = 0;
     }
-
-    pt1FilterInit(&posEstimator.baro.avgFilter, INAV_BARO_AVERAGE_HZ, 0.0f);
-    pt1FilterInit(&posEstimator.surface.avgFilter, INAV_SURFACE_AVERAGE_HZ, 0.0f);
 }
 
 /**

src/main/navigation/navigation_pos_estimator_agl.c

@@ -80,7 +80,7 @@ void updatePositionEstimator_SurfaceTopic(timeUs_t currentTimeUs, float newSurfa
 
         // Update average sonar altitude if range is good
         if (surfaceMeasurementWithinRange) {
-            pt1FilterApply3(&posEstimator.surface.avgFilter, newSurfaceAlt, surfaceDt);
+            pt1FilterApply4(&posEstimator.surface.avgFilter, newSurfaceAlt, INAV_SURFACE_AVERAGE_HZ, surfaceDt);
         }
     }
 }

src/main/navigation/navigation_private.h

@@ -28,7 +28,7 @@
 #include "navigation/navigation.h"
 
 #define MIN_POSITION_UPDATE_RATE_HZ         5       // Minimum position update rate at which XYZ controllers would be applied
-#define NAV_THROTTLE_CUTOFF_FREQENCY_HZ     4       // low-pass filter on throttle output
+#define NAV_THROTTLE_CUTOFF_FREQENCY_HZ     4.0f    // low-pass filter on throttle output
 #define NAV_FW_CONTROL_MONITORING_RATE      2
 #define NAV_DTERM_CUT_HZ                    10.0f
 #define NAV_VEL_Z_DERIVATIVE_CUT_HZ         5.0f