flight/pid,sensors/gyro: D-term pre-diff LPF + code quality improvements

src/main/fc/settings.yaml

@@ -2034,6 +2034,12 @@ groups:
         default_value: 110
         min: 0
         max: 500
+      - name: dterm_lpf2_hz
+        description: "Dterm pre-differentiation low pass filter cutoff frequency. Filters gyro before differentiation to reduce noise amplification. Set to 0 to disable. Default 250Hz adds ~0.6ms delay at 1kHz loop rate."
+        default_value: 250
+        field: dterm_lpf2_hz
+        min: 0
+        max: 500
       - name: dterm_lpf_type
         description: "Defines the type of stage 1 D-term LPF filter. Possible values: `PT1`, `BIQUAD`, `PT2`, `PT3`."
         default_value: "PT2"

src/main/flight/pid.c

@@ -97,6 +97,8 @@ typedef struct {
     rateLimitFilter_t axisAccelFilter;
     pt1Filter_t ptermLpfState;
     filter_t dtermLpfState;
+    pt1Filter_t dtermLpf2State;         // Pre-differentiation LPF (BF-style: filter before diff to reduce noise amplification)
+    float previousFilteredGyroRate;     // Stores filtered gyro for pre-diff architecture
 
     float stickPosition;
 
@@ -160,6 +162,7 @@ static EXTENDED_FASTRAM float dBoostMaxAtAlleceleration;
 static EXTENDED_FASTRAM uint8_t yawLpfHz;
 static EXTENDED_FASTRAM float motorItermWindupPoint;
 static EXTENDED_FASTRAM float antiWindupScaler;
+static EXTENDED_FASTRAM uint16_t dtermLpf2Hz;   // Pre-diff LPF cutoff, 0 = disabled
 #ifdef USE_ANTIGRAVITY
 static EXTENDED_FASTRAM float iTermAntigravityGain;
 #endif
@@ -262,6 +265,7 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
 
         .dterm_lpf_type = SETTING_DTERM_LPF_TYPE_DEFAULT,
         .dterm_lpf_hz = SETTING_DTERM_LPF_HZ_DEFAULT,
+        .dterm_lpf2_hz = 250,   // Pre-diff LPF default 250Hz: ~0.6ms delay at 1kHz
         .yaw_lpf_hz = SETTING_YAW_LPF_HZ_DEFAULT,
 
         .itermWindupPointPercent = SETTING_ITERM_WINDUP_DEFAULT,
@@ -327,12 +331,18 @@ bool pidInitFilters(void)
         return false;
     }
 
-    for (int axis = 0; axis < 3; ++ axis) {
-        initFilter(pidProfile()->dterm_lpf_type, &pidState[axis].dtermLpfState, pidProfile()->dterm_lpf_hz, refreshRate);
-    }
+    // Pre-differentiation LPF: filter gyro before diff to avoid amplifying high-freq noise.
+    // High cutoff (default 250Hz) keeps delay <0.6ms at 1kHz loop.
+    dtermLpf2Hz = pidProfile()->dterm_lpf2_hz;
+    const float dT = US2S(refreshRate);
 
-    for (int i = 0; i < XYZ_AXIS_COUNT; i++) {
-        pt1FilterInit(&windupLpf[i], pidProfile()->iterm_relax_cutoff, US2S(refreshRate));
+    for (int axis = 0; axis < 3; axis++) {
+        initFilter(pidProfile()->dterm_lpf_type, &pidState[axis].dtermLpfState, pidProfile()->dterm_lpf_hz, refreshRate);
+        if (dtermLpf2Hz > 0) {
+            pt1FilterInit(&pidState[axis].dtermLpf2State, dtermLpf2Hz, dT);
+            pidState[axis].previousFilteredGyroRate = 0.0f;
+        }
+        pt1FilterInit(&windupLpf[axis], pidProfile()->iterm_relax_cutoff, dT);
     }
 
 #ifdef USE_ANTIGRAVITY
@@ -771,20 +781,24 @@ static float applyDBoost(pidState_t *pidState, float dT) {
 #endif
 
 static float dTermProcess(pidState_t *pidState, float currentRateTarget, float dT, float dT_inv) {
-    // Calculate new D-term
-    float newDTerm = 0;
     if (pidState->kD == 0) {
-        // optimisation for when D is zero, often used by YAW axis
-        newDTerm = 0;
+        return 0;
+    }
+
+    float delta;
+    if (dtermLpf2Hz > 0) {
+        // BF-style: pre-filter gyro before differentiation.
+        // Diff amplifies noise; filtering first keeps D clean with minimal delay (PT1@250Hz = ~0.6ms at 1kHz).
+        const float filteredGyro = pt1FilterApply(&pidState->dtermLpf2State, pidState->gyroRate);
+        delta = pidState->previousFilteredGyroRate - filteredGyro;
+        pidState->previousFilteredGyroRate = filteredGyro;
     } else {
-        float delta = pidState->previousRateGyro - pidState->gyroRate;
+        delta = pidState->previousRateGyro - pidState->gyroRate;
+    }
 
-        delta = dTermLpfFilterApplyFn((filter_t *) &pidState->dtermLpfState, delta);
+    delta = dTermLpfFilterApplyFn((filter_t *) &pidState->dtermLpfState, delta);
 
-        // Calculate derivative
-        newDTerm =  delta * (pidState->kD * dT_inv) * applyDBoost(pidState, currentRateTarget, dT, dT_inv);
-    }
-    return(newDTerm);
+    return delta * (pidState->kD * dT_inv) * applyDBoost(pidState, currentRateTarget, dT, dT_inv);
 }
 
 static void applyItermLimiting(pidState_t *pidState) {
@@ -864,7 +878,7 @@ static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, float
     const uint16_t limit = getPidSumLimit(pidState->axis);
 
     if (pidProfile()->pidItermLimitPercent != 0){
-        float itermLimit = limit * pidProfile()->pidItermLimitPercent * 0.01f;
+        const float itermLimit = limit * pidProfile()->pidItermLimitPercent * 0.01f;
         pidState->errorGyroIf = constrainf(pidState->errorGyroIf, -itermLimit, +itermLimit);
     }
 
@@ -939,11 +953,10 @@ static void FAST_CODE NOINLINE pidApplyMulticopterRateController(pidState_t *pid
     itermErrorRate *= iTermAntigravityGain;
 #endif
 
-    pidState->errorGyroIf += (itermErrorRate * pidState->kI * antiWindupScaler * dT)
-                             + ((newOutputLimited - newOutput) * pidState->kT * antiWindupScaler * dT);
+    pidState->errorGyroIf += (itermErrorRate * pidState->kI + (newOutputLimited - newOutput) * pidState->kT) * antiWindupScaler * dT;
 
     if (pidProfile()->pidItermLimitPercent != 0){
-        float itermLimit = limit * pidProfile()->pidItermLimitPercent * 0.01f;
+        const float itermLimit = limit * pidProfile()->pidItermLimitPercent * 0.01f;
         pidState->errorGyroIf = constrainf(pidState->errorGyroIf, -itermLimit, +itermLimit);
     }
 

src/main/flight/pid.h

@@ -103,6 +103,7 @@ typedef struct pidProfile_s {
 
     uint8_t dterm_lpf_type;                 // Dterm LPF type: PT1, BIQUAD
     uint16_t dterm_lpf_hz;
+    uint16_t dterm_lpf2_hz;                 // Dterm second stage LPF (pre-differentiation, like Betaflight)
 
     uint8_t yaw_lpf_hz;
 

src/main/sensors/gyro.c

@@ -483,12 +483,12 @@ void FAST_CODE NOINLINE gyroFilter(void)
 
         // LULU gyro filter
         DEBUG_SET(DEBUG_LULU, axis, gyroADCf); //Pre LULU debug
-        float preLulu = gyroADCf;
+        const float preFilterGyro = gyroADCf;
         gyroADCf = gyroLuluApplyFn((filter_t *) &gyroLuluState[axis], gyroADCf);
         DEBUG_SET(DEBUG_LULU, axis + 3, gyroADCf); //Post LULU debug
 
         if (axis == ROLL) {
-            DEBUG_SET(DEBUG_LULU, 6, gyroADCf - preLulu); //LULU delta debug
+            DEBUG_SET(DEBUG_LULU, 6, gyroADCf - preFilterGyro); //LULU delta debug
         }
 
         // Gyro Main LPF