iNavFlight · breadoven · May 19, 2026
diff --git a/src/main/common/filter.c b/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;
 }
 

diff --git a/src/main/common/filter.h b/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);
 
 /*

diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
@@ -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;
         }

diff --git a/src/main/flight/power_limits.c b/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) {

diff --git a/src/main/io/osd.c b/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.8
 
 #define OSD_STATS_SINGLE_PAGE_MIN_ROWS 18
 #define IS_HI(X)  (rxGetChannelValue(X) > 1750)
@@ -5741,26 +5741,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;
 }
 

diff --git a/src/main/navigation/navigation_pos_estimator.c b/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);
 }
 
 /**

diff --git a/src/main/navigation/navigation_pos_estimator_agl.c b/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);
         }
     }
 }

diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c
@@ -708,7 +708,7 @@ void sagCompensatedVBatUpdate(timeUs_t currentTime, timeUs_t timeDelta)
 
             if (impedanceFilterState.state) {
                 pt1FilterSetTimeConstant(&impedanceFilterState, impedanceSampleCount > IMPEDANCE_STABLE_SAMPLE_COUNT_THRESH ? 1.2 : 0.5);
-                pt1FilterApply3(&impedanceFilterState, impedanceSample, US2S(timeDelta));
+                pt1FilterApply4(&impedanceFilterState, impedanceSample, 0.0f, US2S(timeDelta));
             } else {
                 pt1FilterReset(&impedanceFilterState, impedanceSample);
             }
@@ -722,7 +722,7 @@ void sagCompensatedVBatUpdate(timeUs_t currentTime, timeUs_t timeDelta)
 
         uint16_t sagCompensatedVBatSample = MIN(batteryFullVoltage, vbat + (int32_t)powerSupplyImpedance * amperage / 1000);
         pt1FilterSetTimeConstant(&sagCompVBatFilterState, sagCompensatedVBatSample < pt1FilterGetLastOutput(&sagCompVBatFilterState) ? 40 : 500);
-        sagCompensatedVBat = lrintf(pt1FilterApply3(&sagCompVBatFilterState, sagCompensatedVBatSample, US2S(timeDelta)));
+        sagCompensatedVBat = lrintf(pt1FilterApply4(&sagCompVBatFilterState, sagCompensatedVBatSample, 0.0f, US2S(timeDelta)));
     }
 
     DEBUG_SET(DEBUG_SAG_COMP_VOLTAGE, 0, powerSupplyImpedance);

diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c
@@ -332,7 +332,7 @@ bool gyroInit(void)
 
     // initFn will initialize sampleRateIntervalUs to actual gyro sampling rate (if driver supports it). Calculate target looptime using that value
     gyro.targetLooptime = gyroDev[0].sampleRateIntervalUs;
- 
+
     gyroInitFilters();
 
 #ifdef USE_DYNAMIC_FILTERS
@@ -505,7 +505,7 @@ void FAST_CODE NOINLINE gyroFilter(void)
         }
 
         /**
-         * Secondary dynamic notch filter. 
+         * Secondary dynamic notch filter.
          * In some cases, noise amplitude is high enough not to be filtered by the primary filter.
          * This happens on the first frequency with the biggest aplitude
          */
@@ -534,7 +534,7 @@ void FAST_CODE NOINLINE gyroFilter(void)
             );
 
             secondaryDynamicGyroNotchFiltersUpdate(
-                &secondaryDynamicGyroNotchState, 
+                &secondaryDynamicGyroNotchState,
                 gyroAnalyseState.filterUpdateAxis,
                 gyroAnalyseState.centerFrequency[gyroAnalyseState.filterUpdateAxis]
             );
@@ -612,7 +612,7 @@ int16_t gyroRateDps(int axis)
 
 void gyroUpdateDynamicLpf(float cutoffFreq) {
     for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
-        pt1FilterUpdateCutoff(&gyroLpf2State[axis].pt1, cutoffFreq);
+        pt1FilterSetCutoff(&gyroLpf2State[axis].pt1, cutoffFreq);
     }
 }
 

diff --git a/src/main/sensors/pitotmeter.c b/src/main/sensors/pitotmeter.c
@@ -241,9 +241,6 @@ STATIC_PROTOTHREAD(pitotThread)
 
     // Init filter
     pitot.lastMeasurementUs = micros();
-    if (pitotmeterConfig()->pitot_lpf_milli_hz > 0) {
-        pt1FilterInit(&pitot.lpfState, pitotmeterConfig()->pitot_lpf_milli_hz / 1000.0f, 0.0f);
-    }
 
     while(1) {
 #ifdef USE_SIMULATOR
@@ -296,8 +293,9 @@ STATIC_PROTOTHREAD(pitotThread)
 
             // NOTE ::filter pressure - apply filter when NOT calibrating for zero !!!
             currentTimeUs = micros();
-            if (pitotmeterConfig()->pitot_lpf_milli_hz > 0) {
-                pitot.pressure = pt1FilterApply3(&pitot.lpfState, pitotPressureTmp, US2S(currentTimeUs - pitot.lastMeasurementUs));
+            const uint16_t pitot_lpf_milli_hz = pitotmeterConfig()->pitot_lpf_milli_hz;
+            if (pitot_lpf_milli_hz) {
+                pitot.pressure = pt1FilterApply4(&pitot.lpfState, pitotPressureTmp, pitot_lpf_milli_hz, US2S(currentTimeUs - pitot.lastMeasurementUs));
             } else {
                 pitot.pressure = pitotPressureTmp;
             }