Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-12 19:10:27 +03:00
Code Issues Wiki Activity

Updates to Iterm Lock for better readability

Browse source
This commit is contained in:
Pawel Spychalski (DzikuVx) 2025-02-14 13:05:35 +01:00 • committed by Ray Morris
parent bb1e9ca1f3
commit 32f686f4a0
3 changed files with 21 additions and 21 deletions
Download patch file Download diff file Expand all files Collapse all files

4
docs/Settings.md
View file

@ -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 |
| --- | --- | --- |

4
src/main/fc/settings.yaml
View file

@ -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

34
src/main/flight/pid.c
View file

@ -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;