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..64550e5d400 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); @@ -624,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(); @@ -641,7 +649,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 diff --git a/src/main/flight/servos.h b/src/main/flight/servos.h index bc001455a91..b16dd7ca915 100644 --- a/src/main/flight/servos.h +++ b/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);