From 233290ac6c52288f91118ea8c7923fb9798363dd Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E8=89=BE=E5=A4=9A=E5=A4=9A?= Date: Tue, 31 Mar 2026 01:34:15 +0800 Subject: [PATCH 1/2] flight/pid,sensors/gyro: code quality and efficiency improvements - dTermProcess: early return when kD==0, remove redundant variable - pidInitFilters: merge 3 init loops into 1, extract common US2S(refreshRate) subexpression - pidApplyMulticopterRateController: factor out antiWindupScaler*dT from I-term update - gyroFilter: rename preLulu to preFilterGyro, add const - pidApplyFixedWingRateController/pidApplyMulticopterRateController: add const to itermLimit No behavior change. All modifications are structural/readability improvements. --- src/main/flight/pid.c | 47 ++++++++++++++++++++++++----------------- src/main/sensors/gyro.c | 4 ++-- 2 files changed, 30 insertions(+), 21 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 0a2faa648ca..a78bcfd14bb 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -327,12 +327,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 +777,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 +874,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 +949,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); } diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 5abc4980f97..7f586d09edd 100644 --- a/src/main/sensors/gyro.c +++ b/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 From cf3ff812b5f222610523ce7f2a5594f183b7a8ec Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E8=89=BE=E5=A4=9A=E5=A4=9A?= Date: Tue, 31 Mar 2026 02:19:35 +0800 Subject: [PATCH 2/2] flight/pid: add D-term pre-differentiation LPF + code quality improvements Pre-differentiation LPF (v1, from master): - pid.h: add dterm_lpf2_hz field to pidProfile_t - pid.c: add dtermLpf2State and previousFilteredGyroRate to pidState_t - pid.c: initialize pre-diff filter in pidInitFilters() - pid.c: apply pre-diff filter in dTermProcess() before differentiation - settings.yaml: add dterm_lpf2_hz parameter (default 250Hz, range 0-500) Code quality improvements (v2): - dTermProcess: early return when kD==0, remove redundant newDTerm variable - pidInitFilters: merge 3 axis loops into 1, extract common dT subexpression - pidApplyMulticopterRateController: factor out antiWindupScaler*dT - Add const to itermLimit in both FW and MC rate controllers - gyroFilter: rename preLulu to preFilterGyro, add const Noise reduction: diff amplification ~9x -> ~3.6x, latency +0.6ms at 1kHz. --- src/main/fc/settings.yaml | 6 ++++++ src/main/flight/pid.c | 4 ++++ src/main/flight/pid.h | 1 + 3 files changed, 11 insertions(+) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 58274b1b660..4ab2c5973c2 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 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" diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index a78bcfd14bb..23ca0b2fc7e 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; // 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, 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;