From 536c5c3958f8623d02cf3c46ae9956b005a381b6 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Sun, 19 May 2024 20:24:18 +0200
Subject: [PATCH] Make Iterm Lock configurable

---
 src/main/fc/settings.yaml | 12 ++++++++++++
 src/main/flight/pid.c     |  9 ++++++---
 src/main/flight/pid.h     |  5 +++++
 3 files changed, 23 insertions(+), 3 deletions(-)

diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index 307f6d9cb..3abe94e12 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -2227,6 +2227,18 @@ groups:
         field: fixedWingLevelTrimGain
         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
+        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
+        field: fwItermLockRateLimit
+        default_value: 40
+        min: 10
+        max: 100
 
   - name: PG_PID_AUTOTUNE_CONFIG
     type: pidAutotuneConfig_t
diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
index 479d8e8d3..711b3a0af 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -178,7 +178,7 @@ static EXTENDED_FASTRAM bool angleHoldIsLevel = false;
 static EXTENDED_FASTRAM float fixedWingLevelTrim;
 static EXTENDED_FASTRAM pidController_t fixedWingLevelTrimController;
 
-PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 8);
+PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 9);
 
 PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
         .bank_mc = {
@@ -312,6 +312,8 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
         .smithPredictorDelay = SETTING_SMITH_PREDICTOR_DELAY_DEFAULT,
         .smithPredictorFilterHz = SETTING_SMITH_PREDICTOR_LPF_HZ_DEFAULT,
 #endif
+        .fwItermLockTimeMaxMs = SETTING_FW_ITERM_LOCK_TIME_MAX_MS_DEFAULT,
+        .fwItermLockRateLimit = SETTING_FW_ITERM_LOCK_RATE_THRESHOLD_DEFAULT,
 );
 
 bool pidInitFilters(void)
@@ -748,7 +750,8 @@ static void nullRateController(pidState_t *pidState, float dT, float dT_inv) {
 
 static void fwRateAttenuation(pidState_t *pidState, const float rateTarget, const float rateError) {
     const float maxRate = currentControlRateProfile->stabilized.rates[pidState->axis] * 10.0f;
-    const float dampingFactor = attenuation(rateTarget, maxRate / 2.5f);
+
+    const float dampingFactor = attenuation(rateTarget, maxRate * pidProfile()->fwItermLockRateLimit / 100.0f);
 
     /*
      * Iterm damping is applied (down to 0) when:
@@ -770,7 +773,7 @@ static void fwRateAttenuation(pidState_t *pidState, const float rateTarget, cons
         pidState->attenuation.targetOverThresholdTimeMs = 0;
     }
 
-    pidState->attenuation.aI = MIN(dampingFactor, (errorThresholdReached && (millis() - pidState->attenuation.targetOverThresholdTimeMs) < 500) ? 0.0f : 1.0f);
+    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;
diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h
index c8df7231e..93e713b61 100644
--- a/src/main/flight/pid.h
+++ b/src/main/flight/pid.h
@@ -155,6 +155,11 @@ typedef struct pidProfile_s {
     float smithPredictorDelay;
     uint16_t smithPredictorFilterHz;
 #endif
+
+
+    uint16_t fwItermLockTimeMaxMs;
+    uint8_t fwItermLockRateLimit;
+
 } pidProfile_t;
 
 typedef struct pidAutotuneConfig_s {
-- 
2.11.4.GIT