flight/pid: add D-term pre-differentiation LPF (dterm_lpf2_hz)

docs/development/Development.md

@@ -101,7 +101,7 @@ The main flow for a contributing is as follows:
 1. Login to github, go to the INAV repository and press `fork`.
 2. Then using the command line/terminal on your computer: `git clone <url to YOUR fork>`
 3. `cd inav`
-4. `git checkout master`
+4. `git checkout maintenance-10.x`
 5. `git checkout -b my-new-code`
 6. Make changes
 7. `git add <files that have changed>`
@@ -114,13 +114,13 @@ The primary thing to remember is that separate pull requests should be created f
 
 **Important:** Most contributions should target a maintenance branch, not `master`. See the branching section below for guidance on choosing the correct target branch.
 
-Later, you can get the changes from the INAV repo into your `master` branch by adding INAV as a git remote and merging from it as follows:
+Later, you can get the changes from the INAV repo into your version branch by adding INAV as a git remote and merging from it as follows:
 
 1. `git remote add upstream https://github.com/iNavFlight/inav.git`
-2. `git checkout master`
+2. `git checkout maintenance-10.x`
 3. `git fetch upstream`
-4. `git merge upstream/master`
-5. `git push origin master` is an optional step that will update your fork on github
+4. `git merge upstream/maintenance-10.x`
+5. `git push origin` is an optional step that will update your fork on github
 
 
 You can also perform the git commands using the git client inside Eclipse.  Refer to the Eclipse git manual.

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 LPF cutoff (Hz). Filters gyro BEFORE differentiation to reduce noise amplification (Betaflight-style). Higher = less delay, more noise. 0 = disabled. Recommended: 250 for 5in, 200 for 7in."
+        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;
 #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,
         .yaw_lpf_hz = SETTING_YAW_LPF_HZ_DEFAULT,
 
         .itermWindupPointPercent = SETTING_ITERM_WINDUP_DEFAULT,
@@ -331,6 +335,14 @@ bool pidInitFilters(void)
         initFilter(pidProfile()->dterm_lpf_type, &pidState[axis].dtermLpfState, pidProfile()->dterm_lpf_hz, refreshRate);
     }
 
+    dtermLpf2Hz = pidProfile()->dterm_lpf2_hz;
+    if (dtermLpf2Hz > 0) {
+        for (int axis = 0; axis < 3; axis++) {
+            pt1FilterInit(&pidState[axis].dtermLpf2State, dtermLpf2Hz, US2S(refreshRate));
+            pidState[axis].previousFilteredGyroRate = 0.0f;
+        }
+    }
+
     for (int i = 0; i < XYZ_AXIS_COUNT; i++) {
         pt1FilterInit(&windupLpf[i], pidProfile()->iterm_relax_cutoff, US2S(refreshRate));
     }
@@ -771,18 +783,25 @@ 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;
     } else {
-        float delta = pidState->previousRateGyro - pidState->gyroRate;
+        float delta;
+        if (dtermLpf2Hz > 0) {
+            // Pre-filter gyro before differentiation (Betaflight-style).
+            // Differentiation amplifies noise by f_loop/f_cutoff (~9x at 1kHz);
+            // filtering first reduces this to ~3.6x with only +0.6ms latency at 250Hz.
+            const float filteredGyro = pt1FilterApply(&pidState->dtermLpf2State, pidState->gyroRate);
+            delta = pidState->previousFilteredGyroRate - filteredGyro;
+            pidState->previousFilteredGyroRate = filteredGyro;
+        } else {
+            delta = pidState->previousRateGyro - pidState->gyroRate;
+        }
 
         delta = dTermLpfFilterApplyFn((filter_t *) &pidState->dtermLpfState, delta);
 
-        // Calculate derivative
-        newDTerm =  delta * (pidState->kD * dT_inv) * applyDBoost(pidState, currentRateTarget, dT, dT_inv);
+        newDTerm = delta * (pidState->kD * dT_inv) * applyDBoost(pidState, currentRateTarget, dT, dT_inv);
     }
     return(newDTerm);
 }

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;