diff --git a/docs/development/Development.md b/docs/development/Development.md index 59063ad89cc..2629a9f6b0f 100755 --- a/docs/development/Development.md +++ b/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 ` 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 ` @@ -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. diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 3e353e9dfcd..8b879cff649 100644 --- a/src/main/fc/settings.yaml +++ b/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" diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 0a2faa648ca..e852598103c 100644 --- a/src/main/flight/pid.c +++ b/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); } diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index ff2e85031b7..dbcd0e3e26a 100644 --- a/src/main/flight/pid.h +++ b/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;