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 0a2faa648ca..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, @@ -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); } 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; 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