Merge pull request #11215 from sensei-hacker/fix-issue-9912-autotrim-iterm

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

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

@@ -614,13 +614,21 @@ 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);
 
     if (ARMING_FLAG(ARMED)) {
         trimState = AUTOTRIM_COLLECTING;
         if ((millis() - lastUpdateTimeMs) > 500) {
+            static float itermRateOfChange[2];
+            for (int axis = FD_ROLL; axis <= FD_PITCH; axis++) {
+                const float currentIterm = getAxisIterm(axis);
+                itermRateOfChange[axis] = fabsf(currentIterm - prevAxisIterm[axis]) / 0.5f;
+                prevAxisIterm[axis] = currentIterm;
+            }
+
             const bool planeIsFlyingStraight = rotRateMagnitudeFiltered <= DEGREES_TO_RADIANS(servoConfig()->servo_autotrim_rotation_limit);
             const bool noRotationCommanded = targetRateMagnitudeFiltered <= servoConfig()->servo_autotrim_rotation_limit;
             const bool sticksAreCentered = !areSticksDeflected();
@@ -638,7 +646,9 @@ 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) {
+                    const bool itermIsStable = itermRateOfChange[axis] < 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

@@ -171,6 +171,7 @@ typedef struct servoConfig_s {
     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_threshold; // How much of the Iterm is carried over to the servo midpoints on each update
+    uint8_t servo_autotrim_iterm_rate_limit; // Max I-term rate of change (units/sec) to apply autotrim
 } servoConfig_t;
 
 PG_DECLARE(servoConfig_t, servoConfig);