From 731f0608c46e2f218585d4226b8516456f40959c Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Sun, 7 Apr 2024 14:22:30 +0200
Subject: [PATCH 01/14] Run damping of FW with stick movements

---
 src/main/flight/pid.c | 9 ++++++---
 1 file changed, 6 insertions(+), 3 deletions(-)

diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
index 3da150e39dc..d6c8fe81f39 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -758,16 +758,19 @@ static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, fligh
 {
     const float rateTarget = getFlightAxisRateOverride(axis, pidState->rateTarget);
 
+    const float maxRate = currentControlRateProfile->stabilized.rates[axis] * 10.0f;
+    const float dampingFactor = bellCurve(MIN(rateTarget, maxRate), maxRate);
+
     const float rateError = rateTarget - pidState->gyroRate;
-    const float newPTerm = pTermProcess(pidState, rateError, dT);
-    const float newDTerm = dTermProcess(pidState, rateTarget, dT, dT_inv);
+    const float newPTerm = pTermProcess(pidState, rateError, dT) * dampingFactor;
+    const float newDTerm = dTermProcess(pidState, rateTarget, dT, dT_inv) * dampingFactor;
     const float newFFTerm = rateTarget * pidState->kFF;
 
     /*
      * Integral should be updated only if axis Iterm is not frozen
      */
     if (!pidState->itermFreezeActive) {
-        pidState->errorGyroIf += rateError * pidState->kI * dT;
+        pidState->errorGyroIf += rateError * pidState->kI * dT * dampingFactor;
     }
 
     applyItermLimiting(pidState);

From 1f723837e96e37ea05e0aba99b983d91c1d87152 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Sun, 7 Apr 2024 20:28:45 +0200
Subject: [PATCH 02/14] Improve attenuation

---
 src/main/common/maths.c | 18 +++++++++++++++++-
 src/main/common/maths.h |  2 ++
 src/main/flight/pid.c   |  4 +++-
 3 files changed, 22 insertions(+), 2 deletions(-)

diff --git a/src/main/common/maths.c b/src/main/common/maths.c
index 1bc61ae4c36..eed7b863bae 100644
--- a/src/main/common/maths.c
+++ b/src/main/common/maths.c
@@ -516,9 +516,25 @@ bool sensorCalibrationSolveForScale(sensorCalibrationState_t * state, float resu
     return sensorCalibrationValidateResult(result);
 }
 
+float gaussian(const float x, const float mu, const float sigma) {
+    return exp(-pow(x - mu, 2) / (2 * pow(sigma, 2)));
+}
+
 float bellCurve(const float x, const float curveWidth)
 {
-    return powf(M_Ef, -sq(x) / (2.0f * sq(curveWidth)));
+    return gaussian(x, 0.0f, curveWidth);
+}
+
+/**
+ * @brief Calculate the attenuation of a value using a Gaussian function.
+ * Retuns 1 for input 0 and ~0 for input width.
+ * @param input The input value.
+ * @param width The width of the Gaussian function.
+ * @return The attenuation of the input value.
+*/
+float attenuation(const float input, const float width) {
+    const float sigma = width / 2.35482f; // Approximately width / sqrt(2 * ln(2))
+    return gaussian(input, 0.0f, sigma);
 }
 
 float fast_fsqrtf(const float value) {
diff --git a/src/main/common/maths.h b/src/main/common/maths.h
index f3f2fd62746..ce96b7064fd 100644
--- a/src/main/common/maths.h
+++ b/src/main/common/maths.h
@@ -195,6 +195,8 @@ float acos_approx(float x);
 void arraySubInt32(int32_t *dest, int32_t *array1, int32_t *array2, int count);
 
 float bellCurve(const float x, const float curveWidth);
+float attenuation(const float input, const float width);
+float gaussian(const float x, const float mu, const float sigma);
 float fast_fsqrtf(const float value);
 float calc_length_pythagorean_2D(const float firstElement, const float secondElement);
 float calc_length_pythagorean_3D(const float firstElement, const float secondElement, const float thirdElement);
diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
index d6c8fe81f39..9e50522ae3f 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -759,7 +759,9 @@ static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, fligh
     const float rateTarget = getFlightAxisRateOverride(axis, pidState->rateTarget);
 
     const float maxRate = currentControlRateProfile->stabilized.rates[axis] * 10.0f;
-    const float dampingFactor = bellCurve(MIN(rateTarget, maxRate), maxRate);
+    const float dampingFactor = attenuation(rateTarget, maxRate / 2.5f);
+
+    DEBUG_SET(DEBUG_ALWAYS, axis, dampingFactor * 1000);
 
     const float rateError = rateTarget - pidState->gyroRate;
     const float newPTerm = pTermProcess(pidState, rateError, dT) * dampingFactor;

From 37bb8580db0f7cadf109bbeba079bd2999aa1556 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Sun, 7 Apr 2024 21:46:36 +0200
Subject: [PATCH 03/14] Fix compilation for SITL

---
 src/main/common/maths.c | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/common/maths.c b/src/main/common/maths.c
index eed7b863bae..b8ec59c33a2 100644
--- a/src/main/common/maths.c
+++ b/src/main/common/maths.c
@@ -517,7 +517,7 @@ bool sensorCalibrationSolveForScale(sensorCalibrationState_t * state, float resu
 }
 
 float gaussian(const float x, const float mu, const float sigma) {
-    return exp(-pow(x - mu, 2) / (2 * pow(sigma, 2)));
+    return exp(-pow((double)(x - mu), 2) / (2 * pow((double)sigma, 2)));
 }
 
 float bellCurve(const float x, const float curveWidth)

From 5f4dbb79138eab5c5c1f598c6ca338fb89dffe5c Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Sun, 7 Apr 2024 21:54:04 +0200
Subject: [PATCH 04/14] Drop no longer previous generation dampening solution

---
 src/main/fc/settings.yaml |  6 ------
 src/main/flight/pid.c     | 27 ++++-----------------------
 src/main/flight/pid.h     |  1 -
 3 files changed, 4 insertions(+), 30 deletions(-)

diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index cb586eacf06..a3b117d7a3b 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -1934,12 +1934,6 @@ groups:
         field: fixedWingCoordinatedPitchGain
         min: 0
         max: 2
-      - name: fw_iterm_limit_stick_position
-        description: "Iterm is not allowed to grow when stick position is above threshold. This solves the problem of bounceback or followthrough when full stick deflection is applied on poorely tuned fixed wings. In other words, stabilization is partialy disabled when pilot is actively controlling the aircraft and active when sticks are not touched. `0` mean stick is in center position, `1` means it is fully deflected to either side"
-        default_value: 0.5
-        field: fixedWingItermLimitOnStickPosition
-        min: 0
-        max: 1
       - name: fw_yaw_iterm_freeze_bank_angle
         description: "Yaw Iterm is frozen when bank angle is above this threshold [degrees]. This solves the problem of the rudder counteracting turns by partially disabling yaw stabilization when making banked turns. Setting to 0 (the default) disables this feature. Only applies when autopilot is not active and TURN ASSIST is disabled."
         default_value: 0
diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
index 9e50522ae3f..b92aec705f2 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -158,7 +158,6 @@ static EXTENDED_FASTRAM uint8_t usedPidControllerType;
 typedef void (*pidControllerFnPtr)(pidState_t *pidState, flight_dynamics_index_t axis, float dT, float dT_inv);
 static EXTENDED_FASTRAM pidControllerFnPtr pidControllerApplyFn;
 static EXTENDED_FASTRAM filterApplyFnPtr dTermLpfFilterApplyFn;
-static EXTENDED_FASTRAM bool levelingEnabled = false;
 static EXTENDED_FASTRAM bool restartAngleHoldMode = true;
 static EXTENDED_FASTRAM bool angleHoldIsLevel = false;
 
@@ -170,7 +169,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, 6);
+PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 7);
 
 PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
         .bank_mc = {
@@ -271,7 +270,6 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
         .fixedWingReferenceAirspeed = SETTING_FW_REFERENCE_AIRSPEED_DEFAULT,
         .fixedWingCoordinatedYawGain = SETTING_FW_TURN_ASSIST_YAW_GAIN_DEFAULT,
         .fixedWingCoordinatedPitchGain = SETTING_FW_TURN_ASSIST_PITCH_GAIN_DEFAULT,
-        .fixedWingItermLimitOnStickPosition = SETTING_FW_ITERM_LIMIT_STICK_POSITION_DEFAULT,
         .fixedWingYawItermBankFreeze = SETTING_FW_YAW_ITERM_FREEZE_BANK_ANGLE_DEFAULT,
 
         .navVelXyDTermLpfHz = SETTING_NAV_MC_VEL_XY_DTERM_LPF_HZ_DEFAULT,
@@ -672,19 +670,6 @@ static void pidApplySetpointRateLimiting(pidState_t *pidState, flight_dynamics_i
     }
 }
 
-bool isFixedWingItermLimitActive(float stickPosition)
-{
-    /*
-     * Iterm anti windup whould be active only when pilot controls the rotation
-     * velocity directly, not when ANGLE or HORIZON are used
-     */
-    if (levelingEnabled) {
-        return false;
-    }
-
-    return fabsf(stickPosition) > pidProfile()->fixedWingItermLimitOnStickPosition;
-}
-
 static float pTermProcess(pidState_t *pidState, float rateError, float dT) {
     float newPTerm = rateError * pidState->kP;
 
@@ -1050,11 +1035,9 @@ static void pidApplyFpvCameraAngleMix(pidState_t *pidState, uint8_t fpvCameraAng
 
 void checkItermLimitingActive(pidState_t *pidState)
 {
-    bool shouldActivate;
-    if (usedPidControllerType == PID_TYPE_PIFF) {
-        shouldActivate = isFixedWingItermLimitActive(pidState->stickPosition);
-    } else
-    {
+    bool shouldActivate = false;
+
+    if (usedPidControllerType == PID_TYPE_PID) {
         shouldActivate = mixerIsOutputSaturated(); //just in case, since it is already managed by itermWindupPointPercent
     }
 
@@ -1184,7 +1167,6 @@ void FAST_CODE pidController(float dT)
 
     // Step 3: Run control for ANGLE_MODE, HORIZON_MODE and ANGLEHOLD_MODE
     const float horizonRateMagnitude = FLIGHT_MODE(HORIZON_MODE) ? calcHorizonRateMagnitude() : 0.0f;
-    levelingEnabled = false;
     angleHoldIsLevel = false;
 
     for (uint8_t axis = FD_ROLL; axis <= FD_PITCH; axis++) {
@@ -1204,7 +1186,6 @@ void FAST_CODE pidController(float dT)
             // Apply the Level PID controller
             pidLevel(angleTarget, &pidState[axis], axis, horizonRateMagnitude, dT);
             canUseFpvCameraMix = false;     // FPVANGLEMIX is incompatible with ANGLE/HORIZON
-            levelingEnabled = true;
         } else {
             restartAngleHoldMode = true;
         }
diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h
index e0a8b9d310b..2e28848d3cc 100644
--- a/src/main/flight/pid.h
+++ b/src/main/flight/pid.h
@@ -123,7 +123,6 @@ typedef struct pidProfile_s {
     float       fixedWingReferenceAirspeed;     // Reference tuning airspeed for the airplane - the speed for which PID gains are tuned
     float       fixedWingCoordinatedYawGain;    // This is the gain of the yaw rate required to keep the yaw rate consistent with the turn rate for a coordinated turn.
     float       fixedWingCoordinatedPitchGain;    // This is the gain of the pitch rate to keep the pitch angle constant during coordinated turns.
-    float       fixedWingItermLimitOnStickPosition;   //Do not allow Iterm to grow when stick position is above this point
     uint16_t    fixedWingYawItermBankFreeze;       // Freeze yaw Iterm when bank angle is more than this many degrees
 
     float       navVelXyDTermLpfHz;

From 292db1f14c9bac72fbe7ac17963f6c2a4b21e36e Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Mon, 8 Apr 2024 15:39:33 +0200
Subject: [PATCH 05/14] Allo only gradual relaxation of the Axis factor

---
 src/main/flight/pid.c | 15 ++++++++++++++-
 1 file changed, 14 insertions(+), 1 deletion(-)

diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
index b92aec705f2..19708b50f00 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -107,6 +107,8 @@ typedef struct {
     pt3Filter_t rateTargetFilter;
 
     smithPredictor_t smithPredictor;
+
+    float dampingFactor;
 } pidState_t;
 
 STATIC_FASTRAM bool pidFiltersConfigured = false;
@@ -746,7 +748,16 @@ static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, fligh
     const float maxRate = currentControlRateProfile->stabilized.rates[axis] * 10.0f;
     const float dampingFactor = attenuation(rateTarget, maxRate / 2.5f);
 
-    DEBUG_SET(DEBUG_ALWAYS, axis, dampingFactor * 1000);
+    if (fabsf(dampingFactor) < fabsf(pidState->dampingFactor)) {
+        pidState->dampingFactor = dampingFactor;
+    } else {
+        pidState->dampingFactor = pidState->dampingFactor + (dampingFactor - pidState->dampingFactor) * dT * 10;
+    }
+
+    float newDF = pidState->dampingFactor;
+    
+    DEBUG_SET(DEBUG_ALWAYS, axis * 2, dampingFactor * 1000);
+    DEBUG_SET(DEBUG_ALWAYS, (axis * 2) + 1, newDF * 1000);
 
     const float rateError = rateTarget - pidState->gyroRate;
     const float newPTerm = pTermProcess(pidState, rateError, dT) * dampingFactor;
@@ -1262,6 +1273,8 @@ void pidInit(void)
 
     for (uint8_t axis = FD_ROLL; axis <= FD_YAW; axis++) {
 
+        pidState[axis].dampingFactor = 1.0f;
+
     #ifdef USE_D_BOOST
         // Rate * 10 * 10. First 10 is to convert stick to DPS. Second 10 is to convert target to acceleration.
         // We assume, max acceleration is when pilot deflects the stick fully in 100ms

From cf10c071e072589527d072b5fa381b7e416bb148 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Mon, 8 Apr 2024 15:40:21 +0200
Subject: [PATCH 06/14] Docs update

---
 docs/Settings.md | 10 ----------
 1 file changed, 10 deletions(-)

diff --git a/docs/Settings.md b/docs/Settings.md
index b6fd431d950..c33efe6ef9d 100644
--- a/docs/Settings.md
+++ b/docs/Settings.md
@@ -1292,16 +1292,6 @@ Fixed-wing rate stabilisation I-gain for YAW
 
 ---
 
-### fw_iterm_limit_stick_position
-
-Iterm is not allowed to grow when stick position is above threshold. This solves the problem of bounceback or followthrough when full stick deflection is applied on poorely tuned fixed wings. In other words, stabilization is partialy disabled when pilot is actively controlling the aircraft and active when sticks are not touched. `0` mean stick is in center position, `1` means it is fully deflected to either side
-
-| Default | Min | Max |
-| --- | --- | --- |
-| 0.5 | 0 | 1 |
-
----
-
 ### fw_level_pitch_gain
 
 I-gain for the pitch trim for self-leveling flight modes. Higher values means that AUTOTRIM will be faster but might introduce oscillations

From 3005c69f8f99616f3822056baebfa9f6cae980a8 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Tue, 9 Apr 2024 10:30:36 +0200
Subject: [PATCH 07/14] Some progress

---
 src/main/flight/pid.c | 50 ++++++++++++++++++++++++++++++++-----------
 1 file changed, 38 insertions(+), 12 deletions(-)

diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
index 19708b50f00..50c67c86074 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -108,7 +108,10 @@ typedef struct {
 
     smithPredictor_t smithPredictor;
 
-    float dampingFactor;
+    float dampingFactorPrevious;
+    float dampingFactorLockValue;
+    float dampingFactotLockUntilMs;
+    pt1Filter_t dampingFactorFilter;
 } pidState_t;
 
 STATIC_FASTRAM bool pidFiltersConfigured = false;
@@ -322,6 +325,7 @@ bool pidInitFilters(void)
 
     for (int i = 0; i < XYZ_AXIS_COUNT; i++) {
         pt1FilterInit(&windupLpf[i], pidProfile()->iterm_relax_cutoff, US2S(refreshRate));
+        pt1FilterInit(&pidState[i].dampingFactorFilter, 2.0f, US2S(refreshRate));
     }
 
 #ifdef USE_ANTIGRAVITY
@@ -748,27 +752,49 @@ static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, fligh
     const float maxRate = currentControlRateProfile->stabilized.rates[axis] * 10.0f;
     const float dampingFactor = attenuation(rateTarget, maxRate / 2.5f);
 
-    if (fabsf(dampingFactor) < fabsf(pidState->dampingFactor)) {
-        pidState->dampingFactor = dampingFactor;
+    float dampingFactorP;
+    float dampingFactorD;
+    float dampingFactorI;
+
+    if (fabsf(dampingFactor) <= fabsf(pidState->dampingFactorPrevious)) {
+        dampingFactorP = dampingFactor;
+        dampingFactorD = dampingFactor;
+        dampingFactorI = dampingFactor;
+
+        pidState->dampingFactotLockUntilMs = millis() + scaleRangef(fabsf(dampingFactor), 1.0f, 0.0f, 0, 300);
+        pidState->dampingFactorLockValue = dampingFactor;
+
+        // pt1FilterReset(&pidState->dampingFactorFilter, pidState->dampingFactorPrevious);
+        pidState->dampingFactorPrevious = dampingFactor;
     } else {
-        pidState->dampingFactor = pidState->dampingFactor + (dampingFactor - pidState->dampingFactor) * dT * 10;
+        dampingFactorP = dampingFactor;
+        dampingFactorD = dampingFactor;
+
+        if (millis() > pidState->dampingFactorLockValue) {
+            dampingFactorI = dampingFactor;
+            pidState->dampingFactorPrevious = dampingFactor;
+        } else {
+            dampingFactorI = pidState->dampingFactorLockValue;
+        }
     }
 
-    float newDF = pidState->dampingFactor;
-    
-    DEBUG_SET(DEBUG_ALWAYS, axis * 2, dampingFactor * 1000);
-    DEBUG_SET(DEBUG_ALWAYS, (axis * 2) + 1, newDF * 1000);
+    if (axis == FD_ROLL) {
+        DEBUG_SET(DEBUG_ALWAYS, 0, pidState->dampingFactorPrevious * 1000);
+        DEBUG_SET(DEBUG_ALWAYS, 1, dampingFactorP * 1000);
+        DEBUG_SET(DEBUG_ALWAYS, 2, dampingFactorI * 1000);
+        DEBUG_SET(DEBUG_ALWAYS, 3, dampingFactorD * 1000);
+    }
 
     const float rateError = rateTarget - pidState->gyroRate;
-    const float newPTerm = pTermProcess(pidState, rateError, dT) * dampingFactor;
-    const float newDTerm = dTermProcess(pidState, rateTarget, dT, dT_inv) * dampingFactor;
+    const float newPTerm = pTermProcess(pidState, rateError, dT) * dampingFactorP;
+    const float newDTerm = dTermProcess(pidState, rateTarget, dT, dT_inv) * dampingFactorD;
     const float newFFTerm = rateTarget * pidState->kFF;
 
     /*
      * Integral should be updated only if axis Iterm is not frozen
      */
     if (!pidState->itermFreezeActive) {
-        pidState->errorGyroIf += rateError * pidState->kI * dT * dampingFactor;
+        pidState->errorGyroIf += rateError * pidState->kI * dT * dampingFactorI;
     }
 
     applyItermLimiting(pidState);
@@ -1273,7 +1299,7 @@ void pidInit(void)
 
     for (uint8_t axis = FD_ROLL; axis <= FD_YAW; axis++) {
 
-        pidState[axis].dampingFactor = 1.0f;
+        pidState[axis].dampingFactorPrevious = 1.0f;
 
     #ifdef USE_D_BOOST
         // Rate * 10 * 10. First 10 is to convert stick to DPS. Second 10 is to convert target to acceleration.

From bff23ee0e5b23110a8225535478177ed9824f88e Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Tue, 9 Apr 2024 13:53:25 +0200
Subject: [PATCH 08/14] Some updates

---
 src/main/flight/pid.c | 19 ++++++++++++++-----
 1 file changed, 14 insertions(+), 5 deletions(-)

diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
index 50c67c86074..90a31a4aa16 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -756,9 +756,16 @@ static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, fligh
     float dampingFactorD;
     float dampingFactorI;
 
+    /*
+    TODO conditions have to be reworked
+    when drowing, damp
+    when releasing, lock
+    locking procedure has to be separated from action
+    
+
+    */
+
     if (fabsf(dampingFactor) <= fabsf(pidState->dampingFactorPrevious)) {
-        dampingFactorP = dampingFactor;
-        dampingFactorD = dampingFactor;
         dampingFactorI = dampingFactor;
 
         pidState->dampingFactotLockUntilMs = millis() + scaleRangef(fabsf(dampingFactor), 1.0f, 0.0f, 0, 300);
@@ -767,9 +774,7 @@ static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, fligh
         // pt1FilterReset(&pidState->dampingFactorFilter, pidState->dampingFactorPrevious);
         pidState->dampingFactorPrevious = dampingFactor;
     } else {
-        dampingFactorP = dampingFactor;
-        dampingFactorD = dampingFactor;
-
+        
         if (millis() > pidState->dampingFactorLockValue) {
             dampingFactorI = dampingFactor;
             pidState->dampingFactorPrevious = dampingFactor;
@@ -778,6 +783,10 @@ static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, fligh
         }
     }
 
+    //P & D damping factors are always the same and based on current damping factor
+    dampingFactorP = dampingFactor;
+    dampingFactorD = dampingFactor;
+
     if (axis == FD_ROLL) {
         DEBUG_SET(DEBUG_ALWAYS, 0, pidState->dampingFactorPrevious * 1000);
         DEBUG_SET(DEBUG_ALWAYS, 1, dampingFactorP * 1000);

From aef3fbe3b966696cdd3fe5555d35dfce99749a71 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Tue, 9 Apr 2024 18:42:20 +0200
Subject: [PATCH 09/14] New conditions for Iterm dampener

---
 src/main/flight/pid.c | 10 +++++-----
 1 file changed, 5 insertions(+), 5 deletions(-)

diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
index 90a31a4aa16..436e4b17471 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -757,12 +757,12 @@ static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, fligh
     float dampingFactorI;
 
     /*
-    TODO conditions have to be reworked
-    when drowing, damp
-    when releasing, lock
-    locking procedure has to be separated from action
-    
+    New idea.... 
+    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 (fabsf(dampingFactor) <= fabsf(pidState->dampingFactorPrevious)) {

From d27f3fdaa0e5b9f676a7f4e94a1c90e659e43703 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Wed, 17 Apr 2024 15:18:09 +0200
Subject: [PATCH 10/14] Updated Iterm locking mechanism

---
 src/main/flight/pid.c | 61 ++++++++++++++++---------------------------
 1 file changed, 23 insertions(+), 38 deletions(-)

diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
index ae51385cf11..941e08a1a2e 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -107,10 +107,8 @@ typedef struct {
 
     smithPredictor_t smithPredictor;
 
-    float dampingFactorPrevious;
-    float dampingFactorLockValue;
-    float dampingFactotLockUntilMs;
-    pt1Filter_t dampingFactorFilter;
+    timeMs_t targetOverThresholdTimeMs;
+
 } pidState_t;
 
 STATIC_FASTRAM bool pidFiltersConfigured = false;
@@ -322,7 +320,6 @@ bool pidInitFilters(void)
 
     for (int i = 0; i < XYZ_AXIS_COUNT; i++) {
         pt1FilterInit(&windupLpf[i], pidProfile()->iterm_relax_cutoff, US2S(refreshRate));
-        pt1FilterInit(&pidState[i].dampingFactorFilter, 2.0f, US2S(refreshRate));
     }
 
 #ifdef USE_ANTIGRAVITY
@@ -745,53 +742,43 @@ static void nullRateController(pidState_t *pidState, flight_dynamics_index_t axi
 static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, flight_dynamics_index_t axis, float dT, float dT_inv)
 {
     const float rateTarget = getFlightAxisRateOverride(axis, pidState->rateTarget);
+    const float rateError = rateTarget - pidState->gyroRate;
 
     const float maxRate = currentControlRateProfile->stabilized.rates[axis] * 10.0f;
     const float dampingFactor = attenuation(rateTarget, maxRate / 2.5f);
 
-    float dampingFactorP;
-    float dampingFactorD;
-    float dampingFactorI;
-
     /*
-    New idea.... 
-    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)
+     * 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)
-    
-    */
+     * itermAttenuation  = MIN(curve(setpoint), (abs(error) > 10%) && (sticks were deflected in 500ms) ? 0 : 1)
+     */
 
-    if (fabsf(dampingFactor) <= fabsf(pidState->dampingFactorPrevious)) {
-        dampingFactorI = dampingFactor;
+    //If error is greater than 10% or max rate
+    const bool errorThresholdReached = fabsf(rateError) > maxRate * 0.1f;
 
-        pidState->dampingFactotLockUntilMs = millis() + scaleRangef(fabsf(dampingFactor), 1.0f, 0.0f, 0, 300);
-        pidState->dampingFactorLockValue = dampingFactor;
+    //If stick (setpoint) was moved above threshold in the last 500ms
+    if (fabsf(rateTarget) > maxRate * 0.2f) {
+        pidState->targetOverThresholdTimeMs = millis();
+    }
 
-        // pt1FilterReset(&pidState->dampingFactorFilter, pidState->dampingFactorPrevious);
-        pidState->dampingFactorPrevious = dampingFactor;
-    } else {
-        
-        if (millis() > pidState->dampingFactorLockValue) {
-            dampingFactorI = dampingFactor;
-            pidState->dampingFactorPrevious = dampingFactor;
-        } else {
-            dampingFactorI = pidState->dampingFactorLockValue;
-        }
+    //If error is below threshold, we no longer track time for lock mechanism
+    if (!errorThresholdReached) {
+        pidState->targetOverThresholdTimeMs = 0;
     }
 
+    const float dampingFactorI = MIN(dampingFactor, (errorThresholdReached && (millis() - pidState->targetOverThresholdTimeMs) < 500) ? 0.0f : 1.0f);
+
     //P & D damping factors are always the same and based on current damping factor
-    dampingFactorP = dampingFactor;
-    dampingFactorD = dampingFactor;
+    const float dampingFactorP = dampingFactor;
+    const float dampingFactorD = dampingFactor;
 
     if (axis == FD_ROLL) {
-        DEBUG_SET(DEBUG_ALWAYS, 0, pidState->dampingFactorPrevious * 1000);
-        DEBUG_SET(DEBUG_ALWAYS, 1, dampingFactorP * 1000);
-        DEBUG_SET(DEBUG_ALWAYS, 2, dampingFactorI * 1000);
-        DEBUG_SET(DEBUG_ALWAYS, 3, dampingFactorD * 1000);
+        DEBUG_SET(DEBUG_ALWAYS, 0, dampingFactorP * 1000);
+        DEBUG_SET(DEBUG_ALWAYS, 1, dampingFactorI * 1000);
+        DEBUG_SET(DEBUG_ALWAYS, 2, dampingFactorD * 1000);
     }
 
-    const float rateError = rateTarget - pidState->gyroRate;
     const float newPTerm = pTermProcess(pidState, rateError, dT) * dampingFactorP;
     const float newDTerm = dTermProcess(pidState, rateTarget, dT, dT_inv) * dampingFactorD;
     const float newFFTerm = rateTarget * pidState->kFF;
@@ -1309,8 +1296,6 @@ void pidInit(void)
 
     for (uint8_t axis = FD_ROLL; axis <= FD_YAW; axis++) {
 
-        pidState[axis].dampingFactorPrevious = 1.0f;
-
     #ifdef USE_D_BOOST
         // Rate * 10 * 10. First 10 is to convert stick to DPS. Second 10 is to convert target to acceleration.
         // We assume, max acceleration is when pilot deflects the stick fully in 100ms

From b810f99b310e6bae1840d1068eb954cab7fac101 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Wed, 17 Apr 2024 16:45:40 +0200
Subject: [PATCH 11/14] Refactor to contain logic in a single function

---
 src/main/flight/pid.c | 51 ++++++++++++++++++++++++++-----------------
 1 file changed, 31 insertions(+), 20 deletions(-)

diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
index 941e08a1a2e..805450ca3e6 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -65,6 +65,14 @@
 
 #include "programming/logic_condition.h"
 
+typedef struct {
+    float aP;
+    float aI;
+    float aD;
+    float aFF;
+    timeMs_t targetOverThresholdTimeMs;
+} fwPidAttenuation_t;
+
 typedef struct {
     uint8_t axis;
     float kP;   // Proportional gain
@@ -107,8 +115,7 @@ typedef struct {
 
     smithPredictor_t smithPredictor;
 
-    timeMs_t targetOverThresholdTimeMs;
-
+    fwPidAttenuation_t attenuation;
 } pidState_t;
 
 STATIC_FASTRAM bool pidFiltersConfigured = false;
@@ -739,12 +746,8 @@ static void nullRateController(pidState_t *pidState, flight_dynamics_index_t axi
     UNUSED(dT_inv);
 }
 
-static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, flight_dynamics_index_t axis, float dT, float dT_inv)
-{
-    const float rateTarget = getFlightAxisRateOverride(axis, pidState->rateTarget);
-    const float rateError = rateTarget - pidState->gyroRate;
-
-    const float maxRate = currentControlRateProfile->stabilized.rates[axis] * 10.0f;
+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);
 
     /*
@@ -759,35 +762,43 @@ static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, fligh
 
     //If stick (setpoint) was moved above threshold in the last 500ms
     if (fabsf(rateTarget) > maxRate * 0.2f) {
-        pidState->targetOverThresholdTimeMs = millis();
+        pidState->attenuation.targetOverThresholdTimeMs = millis();
     }
 
     //If error is below threshold, we no longer track time for lock mechanism
     if (!errorThresholdReached) {
-        pidState->targetOverThresholdTimeMs = 0;
+        pidState->attenuation.targetOverThresholdTimeMs = 0;
     }
 
-    const float dampingFactorI = MIN(dampingFactor, (errorThresholdReached && (millis() - pidState->targetOverThresholdTimeMs) < 500) ? 0.0f : 1.0f);
+    pidState->attenuation.aI = MIN(dampingFactor, (errorThresholdReached && (millis() - pidState->attenuation.targetOverThresholdTimeMs) < 500) ? 0.0f : 1.0f);
 
     //P & D damping factors are always the same and based on current damping factor
-    const float dampingFactorP = dampingFactor;
-    const float dampingFactorD = dampingFactor;
+    pidState->attenuation.aP = dampingFactor;
+    pidState->attenuation.aD = dampingFactor;
 
-    if (axis == FD_ROLL) {
-        DEBUG_SET(DEBUG_ALWAYS, 0, dampingFactorP * 1000);
-        DEBUG_SET(DEBUG_ALWAYS, 1, dampingFactorI * 1000);
-        DEBUG_SET(DEBUG_ALWAYS, 2, dampingFactorD * 1000);
+    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, flight_dynamics_index_t axis, float dT, float dT_inv)
+{
+    const float rateTarget = getFlightAxisRateOverride(axis, pidState->rateTarget);
+    const float rateError = rateTarget - pidState->gyroRate;
+
+    fwRateAttenuation(pidState, rateTarget, rateError);
 
-    const float newPTerm = pTermProcess(pidState, rateError, dT) * dampingFactorP;
-    const float newDTerm = dTermProcess(pidState, rateTarget, dT, dT_inv) * dampingFactorD;
+    const float newPTerm = pTermProcess(pidState, rateError, dT) * pidState->attenuation.aP;
+    const float newDTerm = dTermProcess(pidState, rateTarget, dT, dT_inv) * pidState->attenuation.aD;
     const float newFFTerm = rateTarget * pidState->kFF;
 
     /*
      * Integral should be updated only if axis Iterm is not frozen
      */
     if (!pidState->itermFreezeActive) {
-        pidState->errorGyroIf += rateError * pidState->kI * dT * dampingFactorI;
+        pidState->errorGyroIf += rateError * pidState->kI * dT * pidState->attenuation.aI;
     }
 
     applyItermLimiting(pidState);

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 12/14] 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 307f6d9cb53..3abe94e12c9 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 479d8e8d346..711b3a0af07 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 c8df7231ed6..93e713b61a9 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 {

From 2118cebf255d2cce359863636d68f19ad6538c0e Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Sun, 19 May 2024 20:24:41 +0200
Subject: [PATCH 13/14] Iterm Lock configurability

---
 src/main/fc/settings.yaml | 6 ++++++
 src/main/flight/pid.c     | 3 ++-
 src/main/flight/pid.h     | 1 +
 3 files changed, 9 insertions(+), 1 deletion(-)

diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index 3abe94e12c9..a2d2195bde0 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -2239,6 +2239,12 @@ groups:
         default_value: 40
         min: 10
         max: 100
+      - name: fw_iterm_lock_engage_threshold
+        description: Defines error rate (in percents of max rate) when Iterm Lock is engaged when sticks are release. Iterm Lock will stay active until error drops below this number
+        default_value: 10
+        min: 5
+        max: 25
+        field: fwItermLockEngageThreshold
 
   - name: PG_PID_AUTOTUNE_CONFIG
     type: pidAutotuneConfig_t
diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
index 711b3a0af07..a9d1291877f 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -314,6 +314,7 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
 #endif
         .fwItermLockTimeMaxMs = SETTING_FW_ITERM_LOCK_TIME_MAX_MS_DEFAULT,
         .fwItermLockRateLimit = SETTING_FW_ITERM_LOCK_RATE_THRESHOLD_DEFAULT,
+        .fwItermLockEngageThreshold = SETTING_FW_ITERM_LOCK_ENGAGE_THRESHOLD_DEFAULT,
 );
 
 bool pidInitFilters(void)
@@ -761,7 +762,7 @@ static void fwRateAttenuation(pidState_t *pidState, const float rateTarget, cons
      */
 
     //If error is greater than 10% or max rate
-    const bool errorThresholdReached = fabsf(rateError) > maxRate * 0.1f;
+    const bool errorThresholdReached = fabsf(rateError) > maxRate * pidProfile()->fwItermLockEngageThreshold / 100.0f;
 
     //If stick (setpoint) was moved above threshold in the last 500ms
     if (fabsf(rateTarget) > maxRate * 0.2f) {
diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h
index 93e713b61a9..26aeb86990d 100644
--- a/src/main/flight/pid.h
+++ b/src/main/flight/pid.h
@@ -159,6 +159,7 @@ typedef struct pidProfile_s {
 
     uint16_t fwItermLockTimeMaxMs;
     uint8_t fwItermLockRateLimit;
+    uint8_t fwItermLockEngageThreshold;
 
 } pidProfile_t;
 

From fc9457f4708b616594de728681ae072bc03bcda6 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Sun, 19 May 2024 20:25:52 +0200
Subject: [PATCH 14/14] Docs update

---
 docs/Settings.md | 30 ++++++++++++++++++++++++++++++
 1 file changed, 30 insertions(+)

diff --git a/docs/Settings.md b/docs/Settings.md
index 9f3baaed662..96141fa65d2 100644
--- a/docs/Settings.md
+++ b/docs/Settings.md
@@ -1302,6 +1302,36 @@ Fixed-wing rate stabilisation I-gain for YAW
 
 ---
 
+### fw_iterm_lock_engage_threshold
+
+Defines error rate (in percents of max rate) when Iterm Lock is engaged when sticks are release. Iterm Lock will stay active until error drops below this number
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 10 | 5 | 25 |
+
+---
+
+### 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
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 40 | 10 | 100 |
+
+---
+
+### fw_iterm_lock_time_max_ms
+
+Defines max time in milliseconds for how long ITerm Lock will shut down Iterm after sticks are release
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 500 | 100 | 1000 |
+
+---
+
 ### fw_level_pitch_gain
 
 I-gain for the pitch trim for self-leveling flight modes. Higher values means that AUTOTRIM will be faster but might introduce oscillations