From 32f686f4a0b145d44b3668be8987045728a79a3e Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Fri, 14 Feb 2025 13:05:35 +0100 Subject: [PATCH] Updates to Iterm Lock for better readability --- docs/Settings.md | 4 ++-- src/main/fc/settings.yaml | 4 ++-- src/main/flight/pid.c | 34 +++++++++++++++++----------------- 3 files changed, 21 insertions(+), 21 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index cfe369c70a..a357b5f1e5 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -1354,7 +1354,7 @@ Defines error rate (in percents of max rate) when Iterm Lock is engaged when sti ### fw_iterm_lock_rate_threshold -Defines rate percentage when full P I and D attenuation should happen. 100 disables Iterm Lock for P and D term +Defines the steepness of the attenuation curve. Higher values result in flatter attenuation. Lower values force full attenuation with lower stick deflection | Default | Min | Max | | --- | --- | --- | @@ -1364,7 +1364,7 @@ Defines rate percentage when full P I and D attenuation should happen. 100 disab ### fw_iterm_lock_time_max_ms -Defines max time in milliseconds for how long ITerm Lock will shut down Iterm after sticks are release +Defines max time in milliseconds for how long ITerm Lock will depress Iterm after sticks are release | Default | Min | Max | | --- | --- | --- | diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index a82b9e1bb0..a4df643623 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -2324,13 +2324,13 @@ groups: min: 0 max: 20 - name: fw_iterm_lock_time_max_ms - description: Defines max time in milliseconds for how long ITerm Lock will shut down Iterm after sticks are release + description: Defines max time in milliseconds for how long ITerm Lock will depress Iterm after sticks are release default_value: 500 field: fwItermLockTimeMaxMs min: 100 max: 1000 - name: fw_iterm_lock_rate_threshold - description: Defines rate percentage when full P I and D attenuation should happen. 100 disables Iterm Lock for P and D term + description: Defines the steepness of the attenuation curve. Higher values result in flatter attenuation. Lower values force full attenuation with lower stick deflection field: fwItermLockRateLimit default_value: 40 min: 10 diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index ae8eb57fb3..99af4427df 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -750,22 +750,23 @@ static void nullRateController(pidState_t *pidState, float dT, float dT_inv) { UNUSED(dT_inv); } -static void fwRateAttenuation(pidState_t *pidState, const float rateTarget, const float rateError) { + +/** + * ITerm Lock is a mechanism that minimizes the effect of bounceback after a rapid stick input has ended + * It is based on the idea, that during a high stick input stabilization (P, I and D) can be damped as craft's + * behavior is driven by FF term. + * On top of that, after stick is relased, it also locks Iterm to prevent it from accumulating error and unloading it after + */ +static void iTermLockApply(pidState_t *pidState, const float rateTarget, const float rateError) { const float maxRate = currentControlRateProfile->stabilized.rates[pidState->axis] * 10.0f; + //Compute damping factor based on rate target and max rate scaled by fw_iterm_lock_rate_threshold const float dampingFactor = attenuation(rateTarget, maxRate * pidProfile()->fwItermLockRateLimit / 100.0f); - /* - * Iterm damping is applied (down to 0) when: - * abs(error) > 10% rate and sticks were moved in the last 500ms (hard stop at this mark) - - * itermAttenuation = MIN(curve(setpoint), (abs(error) > 10%) && (sticks were deflected in 500ms) ? 0 : 1) - */ - - //If error is greater than 10% or max rate + //Check if error rate is above threshold. With default values, this is above 10% of max rate const bool errorThresholdReached = fabsf(rateError) > maxRate * pidProfile()->fwItermLockEngageThreshold / 100.0f; - //If stick (setpoint) was moved above threshold in the last 500ms + //When abs of rate target is above 20% of max rate, we start tracking time if (fabsf(rateTarget) > maxRate * 0.2f) { pidState->attenuation.targetOverThresholdTimeMs = millis(); } @@ -775,17 +776,16 @@ static void fwRateAttenuation(pidState_t *pidState, const float rateTarget, cons pidState->attenuation.targetOverThresholdTimeMs = 0; } + /** + * Iterm attenuation is a lower value of: + * - dampingFactor + * - for 500ms (fw_iterm_lock_time_max_ms) force 0 if error is above threshold + */ pidState->attenuation.aI = MIN(dampingFactor, (errorThresholdReached && (millis() - pidState->attenuation.targetOverThresholdTimeMs) < pidProfile()->fwItermLockTimeMaxMs) ? 0.0f : 1.0f); //P & D damping factors are always the same and based on current damping factor pidState->attenuation.aP = dampingFactor; pidState->attenuation.aD = dampingFactor; - - if (pidState->axis == FD_ROLL) { - DEBUG_SET(DEBUG_ALWAYS, 0, pidState->attenuation.aP * 1000); - DEBUG_SET(DEBUG_ALWAYS, 1, pidState->attenuation.aI * 1000); - DEBUG_SET(DEBUG_ALWAYS, 2, pidState->attenuation.aD * 1000); - } } static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, float dT, float dT_inv) @@ -794,7 +794,7 @@ static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, float const float rateError = rateTarget - pidState->gyroRate; - fwRateAttenuation(pidState, rateTarget, rateError); + iTermLockApply(pidState, rateTarget, rateError); const float newPTerm = pTermProcess(pidState, rateError, dT) * pidState->attenuation.aP; const float newDTerm = dTermProcess(pidState, rateTarget, dT, dT_inv) * pidState->attenuation.aD;