From 3878e4d9d1901466cf8ab7200dc7a8aa94d4e76c Mon Sep 17 00:00:00 2001 From: Ray Morris Date: Sun, 28 Dec 2025 13:57:23 -0600 Subject: [PATCH 1/2] Fix #9912: Add I-term stability check to servo autotrim MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 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 --- docs/Settings.md | 10 ++++++++++ src/main/fc/settings.yaml | 5 +++++ src/main/flight/servos.c | 11 ++++++++++- src/main/flight/servos.h | 1 + 4 files changed, 26 insertions(+), 1 deletion(-) diff --git a/docs/Settings.md b/docs/Settings.md index b886f6702ac..1d08d833a0b 100644 --- a/docs/Settings.md +++ b/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`. diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 01ca6149bfb..de8813bbae8 100644 --- a/src/main/fc/settings.yaml +++ b/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 diff --git a/src/main/flight/servos.c b/src/main/flight/servos.c index 9d2720bec09..141976ca895 100755 --- a/src/main/flight/servos.c +++ b/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 diff --git a/src/main/flight/servos.h b/src/main/flight/servos.h index bc001455a91..08e1454d04c 100644 --- a/src/main/flight/servos.h +++ b/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; From 4ae6da9292c9f4abe979fc4cac88e5bdd4855a2d Mon Sep 17 00:00:00 2001 From: Ray Morris Date: Sun, 28 Dec 2025 21:08:11 -0600 Subject: [PATCH 2/2] Address Copilot review comments - Move I-term tracking outside stable flight check for accurate rate calculation - Move servo_autotrim_iterm_rate_limit to end of struct for EEPROM compatibility --- src/main/flight/servos.c | 15 ++++++++------- src/main/flight/servos.h | 2 +- 2 files changed, 9 insertions(+), 8 deletions(-) diff --git a/src/main/flight/servos.c b/src/main/flight/servos.c index 141976ca895..64550e5d400 100755 --- a/src/main/flight/servos.c +++ b/src/main/flight/servos.c @@ -625,6 +625,13 @@ void processContinuousServoAutotrim(const float 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(); @@ -642,13 +649,7 @@ 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); - - // 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; + 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; diff --git a/src/main/flight/servos.h b/src/main/flight/servos.h index 08e1454d04c..b16dd7ca915 100644 --- a/src/main/flight/servos.h +++ b/src/main/flight/servos.h @@ -170,8 +170,8 @@ 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 + 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);