docs (+25 squashed commit)

Squashed commit:

[51a36e00b] faster updates

[7fed94eae] some cleanup

[ea00282b2] make into feature and add constraint

[a2bdfab5a] require sticks centered

[19c5c8773] defaults

[191ebe04f] fix bug

[a50a11de7] fix a dumb bug

[92d6760b7] use same servo selection logic for regular autotrim

[d4a389f78] minor cleanup

[999b968b2] build error

[a1c25b474] don't update trims when gps heading is not valid to avoid changing trims when on the ground

[317bca1f1] add check on ratetarget for when flying in other modes than acro

[8abd5682e] use time since last arm instead of total arm time since boot

[cf3179325] setting name

[78b00bb0f] comment to force new GH check

[e5527a9f4] build error

[6d0e09938] change setting name

[3824ce3f7] more docs

[9e04b5b25] docs

[591cade68] transfer I-term to servo midpoint

[ad97f692d] fix mode

[2d7438b4c] merge conflicts

[290288203] change rotation threshold

[6fc3676cf] always the docs...

[48ba7b7ae] Automatic servo trim

docs/Settings.md

@@ -474,6 +474,8 @@
 | serialrx_halfduplex | AUTO | Allow serial receiver to operate on UART TX pin. With some receivers will allow control and telemetry over a single wire. |
 | serialrx_inverted | OFF | Reverse the serial inversion of the serial RX protocol. When this value is OFF, each protocol will use its default signal (e.g. SBUS will use an inverted signal). Some OpenLRS receivers produce a non-inverted SBUS signal. This setting supports this type of receivers (including modified FrSKY). |
 | serialrx_provider | _target default_ | When feature SERIALRX is enabled, this allows connection to several receivers which output data via digital interface resembling serial. See RX section. |
+| servo_autotrim_iterm_threshold | 5 | How much of the Iterm is carried over to the servo midpoints on each update. Only applies when using `feature FW_AUTOTRIM`. |
+| servo_autotrim_rotation_limit | 10 | Servo midpoints are only updated when total aircraft rotation is less than this threshold [deg/s]. Only applies when using `feature FW_AUTOTRIM`. |
 | servo_center_pulse | 1500 | Servo midpoint |
 | servo_lpf_hz | 20 | Selects the servo PWM output cutoff frequency. Value is in [Hz] |
 | servo_protocol | PWM | An option to chose the protocol/option that would be used to output servo data. Possible options `PWM` (FC servo outputs), `SERVO_DRIVER` (I2C PCA9685 peripheral), `SBUS` (S.Bus protocol output via a configured serial port) |

src/main/build/debug.h

@@ -83,5 +83,6 @@ typedef enum {
     DEBUG_FW_D,
     DEBUG_IMU2,
     DEBUG_ALTITUDE,
+    DEBUG_AUTOTRIM,
     DEBUG_COUNT
 } debugType_e;

src/main/fc/cli.c

@@ -156,7 +156,7 @@ static const char * const featureNames[] = {
     "", "RSSI_ADC", "LED_STRIP", "DASHBOARD", "",
     "BLACKBOX", "", "TRANSPONDER", "AIRMODE",
     "SUPEREXPO", "VTX", "", "", "", "PWM_OUTPUT_ENABLE",
-    "OSD", "FW_LAUNCH", NULL
+    "OSD", "FW_LAUNCH", "FW_AUTOTRIM", NULL
 };
 
 /* Sensor names (used in lookup tables for *_hardware settings and in status command output) */

src/main/fc/config.h

@@ -67,6 +67,7 @@ typedef enum {
     FEATURE_PWM_OUTPUT_ENABLE = 1 << 28,
     FEATURE_OSD = 1 << 29,
     FEATURE_FW_LAUNCH = 1 << 30,
+    FEATURE_FW_AUTOTRIM = 1 << 31,
 } features_e;
 
 typedef struct systemConfig_s {

src/main/fc/fc_core.c

@@ -121,6 +121,7 @@ typedef struct emergencyArmingState_s {
 
 timeDelta_t cycleTime = 0;         // this is the number in micro second to achieve a full loop, it can differ a little and is taken into account in the PID loop
 static timeUs_t flightTime = 0;
+static timeUs_t armTime = 0;
 
 EXTENDED_FASTRAM float dT;
 
@@ -842,8 +843,12 @@ void taskMainPidLoop(timeUs_t currentTimeUs)
 
     if (ARMING_FLAG(ARMED) && (!STATE(FIXED_WING_LEGACY) || !isNavLaunchEnabled() || (isNavLaunchEnabled() && (isFixedWingLaunchDetected() || isFixedWingLaunchFinishedOrAborted())))) {
         flightTime += cycleTime;
+        armTime += cycleTime;
         updateAccExtremes();
     }
+    if (!ARMING_FLAG(ARMED)) {
+        armTime = 0;
+    }
 
     taskGyro(currentTimeUs);
     imuUpdateAccelerometer();
@@ -905,6 +910,8 @@ void taskMainPidLoop(timeUs_t currentTimeUs)
     if (isMixerUsingServos()) {
         servoMixer(dT);
         processServoAutotrim();
+        processContinuousServoAutotrim(dT);
+    
     }
 
     //Servos should be filtered or written only when mixer is using servos or special feaures are enabled
@@ -961,6 +968,11 @@ float getFlightTime()
     return (float)(flightTime / 1000) / 1000;
 }
 
+float getArmTime()
+{
+    return (float)(armTime / 1000) / 1000;
+}
+
 void fcReboot(bool bootLoader)
 {
     // stop motor/servo outputs

src/main/fc/fc_core.h

@@ -44,5 +44,6 @@ void emergencyArmingUpdate(bool armingSwitchIsOn);
 
 bool isCalibrating(void);
 float getFlightTime(void);
+float getArmTime(void);
 
 void fcReboot(bool bootLoader);

src/main/fc/fc_msp_box.c

@@ -90,6 +90,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = {
     { BOXMSPRCOVERRIDE, "MSP RC OVERRIDE", 50 },
     { BOXPREARM, "PREARM", 51 },
     { BOXFLIPOVERAFTERCRASH, "TURTLE", 52 },
+    { BOXCONTAUTOTRIM, "CONTINUOUS AUTOTRIM", 53 },
     { CHECKBOX_ITEM_COUNT, NULL, 0xFF }
 };
 
@@ -240,7 +241,12 @@ void initActiveBoxIds(void)
         if (!feature(FEATURE_FW_LAUNCH)) {
            activeBoxIds[activeBoxIdCount++] = BOXNAVLAUNCH;
         }
+
+        if (!feature(FEATURE_FW_AUTOTRIM)) {
             activeBoxIds[activeBoxIdCount++] = BOXAUTOTRIM;
+            activeBoxIds[activeBoxIdCount++] = BOXCONTAUTOTRIM;
+        }
+        
 #if defined(USE_AUTOTUNE_FIXED_WING)
         activeBoxIds[activeBoxIdCount++] = BOXAUTOTUNE;
 #endif
@@ -373,6 +379,7 @@ void packBoxModeFlags(boxBitmask_t * mspBoxModeFlags)
 #if defined(USE_RX_MSP) && defined(USE_MSP_RC_OVERRIDE)
     CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXMSPRCOVERRIDE)),   BOXMSPRCOVERRIDE);
 #endif
+    CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXCONTAUTOTRIM)),    BOXCONTAUTOTRIM);
 
     memset(mspBoxModeFlags, 0, sizeof(boxBitmask_t));
     for (uint32_t i = 0; i < activeBoxIdCount; i++) {

src/main/fc/rc_modes.h

@@ -70,6 +70,7 @@ typedef enum {
     BOXMSPRCOVERRIDE = 41,
     BOXPREARM        = 42,
     BOXFLIPOVERAFTERCRASH = 43,
+    BOXCONTAUTOTRIM  = 44,
     CHECKBOX_ITEM_COUNT
 } boxId_e;
 

src/main/fc/settings.yaml

@@ -87,7 +87,7 @@ tables:
       "FLOW", "SBUS", "FPORT", "ALWAYS", "SAG_COMP_VOLTAGE",
       "VIBE", "CRUISE", "REM_FLIGHT_TIME", "SMARTAUDIO", "ACC",
       "ERPM", "RPM_FILTER", "RPM_FREQ", "NAV_YAW", "DYNAMIC_FILTER", "DYNAMIC_FILTER_FREQUENCY",
-      "IRLOCK", "CD", "KALMAN_GAIN", "PID_MEASUREMENT", "SPM_CELLS", "SPM_VS600", "SPM_VARIO", "PCF8574", "DYN_GYRO_LPF", "FW_D", "IMU2", "ALTITUDE"]
+      "IRLOCK", "CD", "KALMAN_GAIN", "PID_MEASUREMENT", "SPM_CELLS", "SPM_VS600", "SPM_VARIO", "PCF8574", "DYN_GYRO_LPF", "FW_D", "IMU2", "ALTITUDE", "AUTOTRIM"]
   - name: async_mode
     values: ["NONE", "GYRO", "ALL"]
   - name: aux_operator
@@ -1176,6 +1176,16 @@ groups:
         description: "On tricopter mix only, if this is set to ON, servo will always be correcting regardless of armed state. to disable this, set it to OFF."
         default_value: ON
         type: bool
+      - name: servo_autotrim_rotation_limit
+        description: "Servo midpoints are only updated when total aircraft rotation is less than this threshold [deg/s]. Only applies when using `feature FW_AUTOTRIM`."
+        default_value: 10
+        min: 1
+        max: 60
+      - name: servo_autotrim_iterm_threshold
+        description: "How much of the Iterm is carried over to the servo midpoints on each update. Only applies when using `feature FW_AUTOTRIM`."
+        default_value: 5
+        min: 1
+        max: 25
 
   - name: PG_CONTROL_RATE_PROFILES
     type: controlRateConfig_t

src/main/flight/mixer.h

@@ -127,6 +127,7 @@ void mixerResetDisarmedMotors(void);
 void mixTable(const float dT);
 void writeMotors(void);
 void processServoAutotrim(void);
+void processContinuousServoAutotrim(const float dT);
 void stopMotors(void);
 void stopPwmAllMotors(void);
 

src/main/flight/pid.c

@@ -371,6 +371,22 @@ void pidResetErrorAccumulators(void)
     }
 }
 
+void pidReduceErrorAccumulators(int8_t delta, uint8_t axis) 
+{
+    pidState[axis].errorGyroIf -= delta;
+    pidState[axis].errorGyroIfLimit -= delta;
+}
+
+float getTotalRateTarget(void)
+{
+    return sqrtf(sq(pidState[FD_ROLL].rateTarget) + sq(pidState[FD_PITCH].rateTarget) + sq(pidState[FD_YAW].rateTarget));
+}
+
+float getAxisIterm(uint8_t axis) 
+{
+    return pidState[axis].errorGyroIf;
+}
+
 static float pidRcCommandToAngle(int16_t stick, int16_t maxInclination)
 {
     stick = constrain(stick, -500, 500);

src/main/flight/pid.h

@@ -183,6 +183,9 @@ extern int32_t axisPID_P[], axisPID_I[], axisPID_D[], axisPID_Setpoint[];
 void pidInit(void);
 bool pidInitFilters(void);
 void pidResetErrorAccumulators(void);
+void pidReduceErrorAccumulators(int8_t delta, uint8_t axis);
+float getAxisIterm(uint8_t axis);
+float getTotalRateTarget(void);
 void pidResetTPAFilter(void);
 
 struct controlRateConfig_s;

src/main/flight/servos.c

@@ -40,6 +40,7 @@
 #include "drivers/time.h"
 
 #include "fc/config.h"
+#include "fc/fc_core.h"
 #include "fc/rc_controls.h"
 #include "fc/rc_modes.h"
 #include "fc/runtime_config.h"
@@ -51,11 +52,13 @@
 #include "flight/pid.h"
 #include "flight/servos.h"
 
+#include "io/gps.h"
+
 #include "rx/rx.h"
 
 #include "sensors/gyro.h"
 
-PG_REGISTER_WITH_RESET_TEMPLATE(servoConfig_t, servoConfig, PG_SERVO_CONFIG, 1);
+PG_REGISTER_WITH_RESET_TEMPLATE(servoConfig_t, servoConfig, PG_SERVO_CONFIG, 2);
 
 PG_RESET_TEMPLATE(servoConfig_t, servoConfig,
     .servoCenterPulse = SETTING_SERVO_CENTER_PULSE_DEFAULT,
@@ -63,7 +66,9 @@ PG_RESET_TEMPLATE(servoConfig_t, servoConfig,
     .servo_lowpass_freq = SETTING_SERVO_LPF_HZ_DEFAULT,         // Default servo update rate is 50Hz, everything above Nyquist frequency (25Hz) is going to fold and cause distortions
     .servo_protocol = SETTING_SERVO_PROTOCOL_DEFAULT,
     .flaperon_throw_offset = SETTING_FLAPERON_THROW_OFFSET_DEFAULT,
-    .tri_unarmed_servo = SETTING_TRI_UNARMED_SERVO_DEFAULT
+    .tri_unarmed_servo = SETTING_TRI_UNARMED_SERVO_DEFAULT,
+    .servo_autotrim_rotation_limit = SETTING_SERVO_AUTOTRIM_ROTATION_LIMIT_DEFAULT,
+    .servo_autotrim_iterm_threshold = SETTING_SERVO_AUTOTRIM_ITERM_THRESHOLD
 );
 
 PG_REGISTER_ARRAY_WITH_RESET_FN(servoMixer_t, MAX_SERVO_RULES, customServoMixers, PG_SERVO_MIXER, 1);
@@ -83,7 +88,7 @@ void pgResetFn_customServoMixers(servoMixer_t *instance)
     }
 }
 
-PG_REGISTER_ARRAY_WITH_RESET_FN(servoParam_t, MAX_SUPPORTED_SERVOS, servoParams, PG_SERVO_PARAMS, 2);
+PG_REGISTER_ARRAY_WITH_RESET_FN(servoParam_t, MAX_SUPPORTED_SERVOS, servoParams, PG_SERVO_PARAMS, 3);
 
 void pgResetFn_servoParams(servoParam_t *instance)
 {
@@ -113,6 +118,8 @@ static bool servoFilterIsSet;
 static servoMetadata_t servoMetadata[MAX_SUPPORTED_SERVOS];
 static rateLimitFilter_t servoSpeedLimitFilter[MAX_SERVO_RULES];
 
+STATIC_FASTRAM pt1Filter_t rotRateFilter;
+
 int16_t getFlaperonDirection(uint8_t servoPin)
 {
     if (servoPin == SERVO_FLAPPERON_2) {
@@ -399,11 +406,15 @@ void processServoAutotrim(void)
             case AUTOTRIM_IDLE:
                 if (ARMING_FLAG(ARMED)) {
                     // We are activating servo trim - backup current middles and prepare to average the data
-                    for (int servoIndex = SERVO_ELEVATOR; servoIndex <= MIN(SERVO_RUDDER, MAX_SUPPORTED_SERVOS); servoIndex++) {
-                        servoMiddleBackup[servoIndex] = servoParams(servoIndex)->middle;
-                        servoMiddleAccum[servoIndex] = 0;
+                    for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
+                        for (int i = 0; i < servoRuleCount; i++) {
+                            const uint8_t target = currentServoMixer[i].targetChannel;
+                            const uint8_t from = currentServoMixer[i].inputSource;
+                            if (from == axis) {
+                                servoMiddleBackup[target] = servoParams(target)->middle;
+                            }
+                        }
                     }
-
                     trimStartedAt = millis();
                     servoMiddleAccumCount = 0;
                     trimState = AUTOTRIM_COLLECTING;
@@ -416,14 +427,26 @@ void processServoAutotrim(void)
             case AUTOTRIM_COLLECTING:
                 if (ARMING_FLAG(ARMED)) {
                     servoMiddleAccumCount++;
-
-                    for (int servoIndex = SERVO_ELEVATOR; servoIndex <= MIN(SERVO_RUDDER, MAX_SUPPORTED_SERVOS); servoIndex++) {
-                        servoMiddleAccum[servoIndex] += servo[servoIndex];
+                    for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
+                        for (int i = 0; i < servoRuleCount; i++) {
+                            const uint8_t target = currentServoMixer[i].targetChannel;
+                            const uint8_t from = currentServoMixer[i].inputSource;
+                            if (from == axis) {
+                                servoMiddleAccum[target] += servo[target];
+                            }
+                        }
                     }
 
                     if ((millis() - trimStartedAt) > SERVO_AUTOTRIM_TIMER_MS) {
-                        for (int servoIndex = SERVO_ELEVATOR; servoIndex <= MIN(SERVO_RUDDER, MAX_SUPPORTED_SERVOS); servoIndex++) {
-                            servoParamsMutable(servoIndex)->middle = servoMiddleAccum[servoIndex] / servoMiddleAccumCount;
+                        for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
+                            for (int i = 0; i < servoRuleCount; i++) {
+                                const uint8_t target = currentServoMixer[i].targetChannel;
+                                const uint8_t from = currentServoMixer[i].inputSource;
+                                if (from == axis) {
+                                    servoParamsMutable(target)->middle = servoMiddleAccum[target] / servoMiddleAccumCount;
+
+                                }
+                            }
                         }
                         trimState = AUTOTRIM_SAVE_PENDING;
                         pidResetErrorAccumulators(); //Reset Iterm since new midpoints override previously acumulated errors
@@ -449,11 +472,16 @@ void processServoAutotrim(void)
     else {
         // We are deactivating servo trim - restore servo midpoints
         if (trimState == AUTOTRIM_SAVE_PENDING) {
-            for (int servoIndex = SERVO_ELEVATOR; servoIndex <= MIN(SERVO_RUDDER, MAX_SUPPORTED_SERVOS); servoIndex++) {
-                servoParamsMutable(servoIndex)->middle = servoMiddleBackup[servoIndex];
+            for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
+                for (int i = 0; i < servoRuleCount; i++) {
+                    const uint8_t target = currentServoMixer[i].targetChannel;
+                    const uint8_t from = currentServoMixer[i].inputSource;
+                    if (from == axis) {
+                        servoParamsMutable(target)->middle = servoMiddleBackup[target];
+                    }
+                }
             }
         }
-
         trimState = AUTOTRIM_IDLE;
     }
 }
@@ -472,3 +500,115 @@ bool isMixerUsingServos(void)
 {
     return mixerUsesServos;
 }
+
+#define SERVO_AUTOTRIM_FILTER_CUTOFF    1       // LPF cutoff frequency
+#define SERVO_AUTOTRIM_CENTER_MIN       1300
+#define SERVO_AUTOTRIM_CENTER_MAX       1700
+
+void processContinuousServoAutotrim(const float dT)
+{
+    static timeMs_t lastUpdateTimeMs;
+    static servoAutotrimState_e trimState = AUTOTRIM_IDLE;    
+
+    static int16_t servoMiddleBackup[MAX_SUPPORTED_SERVOS];
+    static int32_t servoMiddleUpdateCount;
+
+    const uint8_t ItermThreshold = servoConfig()->servo_autotrim_iterm_threshold;
+
+    const float rotRateMagnitude = sqrtf(vectorNormSquared(&imuMeasuredRotationBF));
+    const float rotRateMagnitudeFiltered = pt1FilterApply4(&rotRateFilter, rotRateMagnitude, SERVO_AUTOTRIM_FILTER_CUTOFF, dT);
+
+    if (IS_RC_MODE_ACTIVE(BOXCONTAUTOTRIM) || feature(FEATURE_FW_AUTOTRIM)) {
+        switch (trimState) {
+            case AUTOTRIM_IDLE:
+                if (ARMING_FLAG(ARMED)) {
+                    // FIXME: use proper flying detection
+                    // We are activating servo trim - backup current middles and prepare to update the servo midpoints
+                    for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
+                        for (int i = 0; i < servoRuleCount; i++) {
+                            const uint8_t target = currentServoMixer[i].targetChannel;
+                            const uint8_t from = currentServoMixer[i].inputSource;
+                            if (from == axis) {
+                                servoMiddleBackup[target] = servoParams(target)->middle;
+                            }
+                        }
+                    }
+                    servoMiddleUpdateCount = 0;
+                    lastUpdateTimeMs = millis();
+                    trimState = AUTOTRIM_COLLECTING;
+                }
+                else {
+                    break;
+                }
+                // Fallthru
+
+            case AUTOTRIM_COLLECTING:
+                if (ARMING_FLAG(ARMED)) {
+                    // Every half second update servo midpoints
+                    if ((millis() - lastUpdateTimeMs) > 500 && isGPSHeadingValid()) {
+                        const bool planeFlyingStraight = rotRateMagnitudeFiltered <= DEGREES_TO_RADIANS(servoConfig()->servo_autotrim_rotation_limit);
+                        const bool zeroRotationCommanded = getTotalRateTarget() <= servoConfig()->servo_autotrim_rotation_limit;
+                        if (planeFlyingStraight && zeroRotationCommanded && !areSticksDeflectedMoreThanPosHoldDeadband() &&!FLIGHT_MODE(MANUAL_MODE)) {
+                            // Plane is flying straight and sticks are centered
+                            for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
+                                // For each stabilized axis, add x units of I-term to all associated servo midpoints
+                                const float axisIterm = getAxisIterm(axis);
+                                if (fabsf(axisIterm) > ItermThreshold) {
+                                    const int8_t ItermUpdate = axisIterm > 0.0f ? ItermThreshold : -ItermThreshold;
+                                    for (int i = 0; i < servoRuleCount; i++) {
+                                        const uint8_t target = currentServoMixer[i].targetChannel;
+                                        const uint8_t from = currentServoMixer[i].inputSource;
+                                        if (from == axis) {
+                                            // Convert axis I-term to servo PWM and add to midpoint
+                                            const float mixerRate = currentServoMixer[i].rate / 100.0f;
+                                            const float servoRate = servoParams(target)->rate / 100.0f;
+                                            servoParamsMutable(target)->middle += ItermUpdate * mixerRate * servoRate;
+                                            servoParamsMutable(target)->middle = constrain(servoParamsMutable(target)->middle, SERVO_AUTOTRIM_CENTER_MIN, SERVO_AUTOTRIM_CENTER_MAX);
+                                        }
+                                    }
+                                    pidReduceErrorAccumulators(ItermUpdate, axis);
+                                }
+                            }
+                            servoMiddleUpdateCount++;
+                        }
+                        // Reset timer
+                        lastUpdateTimeMs = millis();
+                    }
+                    break;
+                } else {
+                    // We have disarmed, save to EEPROM
+                    saveConfigAndNotify();
+                    trimState = AUTOTRIM_IDLE;
+                    break;
+                }
+            case AUTOTRIM_SAVE_PENDING:
+            case AUTOTRIM_DONE:
+                break;
+        }
+    }
+    else {
+        // We are deactivating servo trim - restore servo midpoints
+        if (trimState == AUTOTRIM_COLLECTING) {
+            for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
+                for (int i = 0; i < servoRuleCount; i++) {
+                    const uint8_t target = currentServoMixer[i].targetChannel;
+                    const uint8_t from = currentServoMixer[i].inputSource;
+                    if (from == axis) {
+                        servoParamsMutable(target)->middle = servoMiddleBackup[target];
+                    }
+                }
+            }
+        }
+        trimState = AUTOTRIM_IDLE;
+    }
+
+    // Debug
+    DEBUG_SET(DEBUG_AUTOTRIM, 0, servoParams(2)->middle);
+    DEBUG_SET(DEBUG_AUTOTRIM, 2, servoParams(3)->middle);
+    DEBUG_SET(DEBUG_AUTOTRIM, 4, servoParams(4)->middle);
+    DEBUG_SET(DEBUG_AUTOTRIM, 6, servoParams(5)->middle);
+    DEBUG_SET(DEBUG_AUTOTRIM, 1, servoMiddleUpdateCount);
+    DEBUG_SET(DEBUG_AUTOTRIM, 3, RADIANS_TO_DEGREES(rotRateMagnitudeFiltered));
+    DEBUG_SET(DEBUG_AUTOTRIM, 5, axisPID_I[FD_ROLL]);
+    DEBUG_SET(DEBUG_AUTOTRIM, 7, axisPID_I[FD_PITCH]);    
+}

src/main/flight/servos.h

@@ -138,6 +138,8 @@ typedef struct servoConfig_s {
     uint16_t flaperon_throw_offset;
     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_threshold; // How much of the Iterm is carried over to the servo midpoints on each update
 } servoConfig_t;
 
 PG_DECLARE(servoConfig_t, servoConfig);

src/main/io/osd.c

@@ -3598,7 +3598,7 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
                         // by OSD_FLYMODE.
                         messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_ALTITUDE_HOLD);
                     }
-                    if (IS_RC_MODE_ACTIVE(BOXAUTOTRIM)) {
+                    if (IS_RC_MODE_ACTIVE(BOXAUTOTRIM) || IS_RC_MODE_ACTIVE(BOXCONTAUTOTRIM)) {
                         messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_AUTOTRIM);
                     }
                     if (IS_RC_MODE_ACTIVE(BOXAUTOTUNE)) {