Fix iNavFlight#9912: Add I-term stability check to servo autotrim

Prevent continuous servo autotrim from applying trim adjustments during
maneuver transitions when I-term is changing rapidly due to transient error.

Root Cause:
The autotrim code verified all flight conditions (level attitude, centered
sticks, low rotation rate) but failed to check that the I-term was in a
steady state before transferring it to servo trim. During maneuver transitions
(e.g., exiting a turn), I-term accumulates transient error. When the plane
momentarily satisfies all level-flight conditions, this transient I-term is
incorrectly transferred to servo midpoints, causing the aircraft to fly
out-of-trim.

Changes:
- Added I-term rate-of-change tracking in processContinuousServoAutotrim()
- Added stability threshold check before applying autotrim
- Added configurable parameter: servo_autotrim_iterm_rate_limit (default: 2)
- Only transfers I-term to trim when rate of change is below threshold

The fix ensures trim updates only occur during true steady-level flight,
not during transient conditions following maneuvers.

🤖 Generated with [Claude Code](https://claude.com/claude-code)

Co-Authored-By: Claude Sonnet 4.5 <noreply@anthropic.com>

Author: sensei-hacker

docs/Settings.md

@@ -6052,6 +6052,16 @@ When feature SERIALRX is enabled, this allows connection to several receivers wh
 
 ---
 
+### servo_autotrim_iterm_rate_limit
+
+Maximum I-term rate of change (units/sec) for autotrim to be applied. Prevents trim updates during maneuver transitions when I-term is changing rapidly. Only applies when using `feature FW_AUTOTRIM`.
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 2 | 0 | 50 |
+
+---
+
 ### servo_autotrim_rotation_limit
 
 Servo midpoints are only updated when total aircraft rotation is less than this threshold [deg/s]. Only applies when using `feature FW_AUTOTRIM`.

src/main/fc/settings.yaml

@@ -1361,6 +1361,11 @@ groups:
         default_value: 15
         min: 1
         max: 60
+      - name: servo_autotrim_iterm_rate_limit
+        description: "Maximum I-term rate of change (units/sec) for autotrim to be applied. Prevents trim updates during maneuver transitions when I-term is changing rapidly. Only applies when using `feature FW_AUTOTRIM`."
+        default_value: 2
+        min: 0
+        max: 50
 
   - name: PG_CONTROL_PROFILES
     type: controlConfig_t

src/main/flight/servos.c

@@ -617,6 +617,7 @@ void processContinuousServoAutotrim(const float dT)
     static timeMs_t lastUpdateTimeMs;
     static servoAutotrimState_e trimState = AUTOTRIM_IDLE;
     static uint32_t servoMiddleUpdateCount;
+    static float prevAxisIterm[2] = {0};  // Track previous I-term for rate-of-change calculation
 
     const float rotRateMagnitudeFiltered = pt1FilterApply4(&rotRateFilter, fast_fsqrtf(vectorNormSquared(&imuMeasuredRotationBF)), SERVO_AUTOTRIM_FILTER_CUTOFF, dT);
     const float targetRateMagnitudeFiltered = pt1FilterApply4(&targetRateFilter, getTotalRateTarget(), SERVO_AUTOTRIM_FILTER_CUTOFF, dT);
@@ -641,7 +642,15 @@ void processContinuousServoAutotrim(const float dT)
                 for (int axis = FD_ROLL; axis <= FD_PITCH; axis++) {
                     // For each stabilized axis, add 5 units of I-term to all associated servo midpoints
                     const float axisIterm = getAxisIterm(axis);
-                    if (fabsf(axisIterm) > SERVO_AUTOTRIM_UPDATE_SIZE) {
+
+                    // Calculate I-term rate of change (per second) to detect stability
+                    const float itermRateOfChange = fabsf(axisIterm - prevAxisIterm[axis]) / 0.5f;
+                    prevAxisIterm[axis] = axisIterm;
+
+                    // Only apply trim if I-term is stable (not changing rapidly due to recent maneuver)
+                    const bool itermIsStable = itermRateOfChange < servoConfig()->servo_autotrim_iterm_rate_limit;
+
+                    if (fabsf(axisIterm) > SERVO_AUTOTRIM_UPDATE_SIZE && itermIsStable) {
                         const int8_t ItermUpdate = axisIterm > 0.0f ? SERVO_AUTOTRIM_UPDATE_SIZE : -SERVO_AUTOTRIM_UPDATE_SIZE;
                         for (int i = 0; i < servoRuleCount; i++) {
 #ifdef USE_PROGRAMMING_FRAMEWORK

src/main/flight/servos.h

@@ -170,6 +170,7 @@ typedef struct servoConfig_s {
     uint8_t servo_protocol;                 // See servoProtocolType_e
     uint8_t tri_unarmed_servo;              // send tail servo correction pulses even when unarmed
     uint8_t servo_autotrim_rotation_limit;  // Max rotation for servo midpoints to be updated
+    uint8_t servo_autotrim_iterm_rate_limit; // Max I-term rate of change (units/sec) to apply autotrim
     uint8_t servo_autotrim_iterm_threshold; // How much of the Iterm is carried over to the servo midpoints on each update
 } servoConfig_t;