Merge pull request #6708 from avsaase/avs-automatic-servo-trim

Automatic servo trim

src/main/build/debug.h

@@ -85,5 +85,6 @@ typedef enum {
     DEBUG_ALTITUDE,
     DEBUG_GYRO_ALPHA_BETA_GAMMA,
     DEBUG_SMITH_PREDICTOR,
+    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

@@ -62,6 +62,7 @@
     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

@@ -118,6 +118,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;
 
@@ -838,8 +839,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;
+    }
 
     gyroFilter();
 
@@ -901,7 +906,7 @@ void taskMainPidLoop(timeUs_t currentTimeUs)
 
     if (isMixerUsingServos()) {
         servoMixer(dT);
-        processServoAutotrim();
+        processServoAutotrim(dT);
     }
 
     //Servos should be filtered or written only when mixer is using servos or special feaures are enabled
@@ -958,6 +963,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

@@ -45,5 +45,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

@@ -242,7 +242,11 @@ void initActiveBoxIds(void)
         if (!feature(FEATURE_FW_LAUNCH)) {
            activeBoxIds[activeBoxIdCount++] = BOXNAVLAUNCH;
         }
-        activeBoxIds[activeBoxIdCount++] = BOXAUTOTRIM;
+
+        if (!feature(FEATURE_FW_AUTOTRIM)) {
+            activeBoxIds[activeBoxIdCount++] = BOXAUTOTRIM;
+        }
+        
 #if defined(USE_AUTOTUNE_FIXED_WING)
         activeBoxIds[activeBoxIdCount++] = BOXAUTOTUNE;
 #endif

src/main/fc/settings.yaml

@@ -94,7 +94,7 @@ tables:
       "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",
-      "GYRO_ALPHA_BETA_GAMMA", "SMITH_PREDICTOR"]
+      "GYRO_ALPHA_BETA_GAMMA", "SMITH_PREDICTOR", "AUTOTRIM"]
   - name: async_mode
     values: ["NONE", "GYRO", "ALL"]
   - name: aux_operator
@@ -1215,6 +1215,11 @@ 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: 15
+        min: 1
+        max: 60
 
   - name: PG_CONTROL_RATE_PROFILES
     type: controlRateConfig_t

src/main/flight/mixer.h

@@ -121,7 +121,9 @@ void mixerUpdateStateFlags(void);
 void mixerResetDisarmedMotors(void);
 void mixTable(const float dT);
 void writeMotors(void);
-void processServoAutotrim(void);
+void processServoAutotrim(const float dT);
+void processServoAutotrimMode(void);
+void processContinuousServoAutotrim(const float dT);
 void stopMotors(void);
 void stopPwmAllMotors(void);
 

src/main/flight/pid.c

@@ -404,6 +404,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

@@ -188,6 +188,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,8 @@ 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
 );
 
 PG_REGISTER_ARRAY_WITH_RESET_FN(servoMixer_t, MAX_SERVO_RULES, customServoMixers, PG_SERVO_MIXER, 1);
@@ -83,7 +87,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)
 {
@@ -93,7 +97,7 @@ void pgResetFn_servoParams(servoParam_t *instance)
             .max = DEFAULT_SERVO_MAX,
             .middle = DEFAULT_SERVO_MIDDLE,
             .rate = 100
-        );
+        );        
     }
 }
 
@@ -113,6 +117,9 @@ static bool servoFilterIsSet;
 static servoMetadata_t servoMetadata[MAX_SUPPORTED_SERVOS];
 static rateLimitFilter_t servoSpeedLimitFilter[MAX_SERVO_RULES];
 
+STATIC_FASTRAM pt1Filter_t rotRateFilter;
+STATIC_FASTRAM pt1Filter_t targetRateFilter;
+
 int16_t getFlaperonDirection(uint8_t servoPin)
 {
     if (servoPin == SERVO_FLAPPERON_2) {
@@ -385,27 +392,31 @@ typedef enum {
     AUTOTRIM_DONE,
 } servoAutotrimState_e;
 
-void processServoAutotrim(void)
+void processServoAutotrimMode(void)
 {
     static servoAutotrimState_e trimState = AUTOTRIM_IDLE;
     static timeMs_t trimStartedAt;
 
     static int16_t servoMiddleBackup[MAX_SUPPORTED_SERVOS];
     static int32_t servoMiddleAccum[MAX_SUPPORTED_SERVOS];
-    static int32_t servoMiddleAccumCount;
+    static int32_t servoMiddleAccumCount[MAX_SUPPORTED_SERVOS];
 
     if (IS_RC_MODE_ACTIVE(BOXAUTOTRIM)) {
         switch (trimState) {
             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 source = currentServoMixer[i].inputSource;
+                            if (source == axis) {
+                                servoMiddleBackup[target] = servoParams(target)->middle;
+                                servoMiddleAccum[target] = 0;
+                                servoMiddleAccumCount[target] = 0;
+                            }
+                        }
                     }
-
                     trimStartedAt = millis();
-                    servoMiddleAccumCount = 0;
                     trimState = AUTOTRIM_COLLECTING;
                 }
                 else {
@@ -415,15 +426,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 source = currentServoMixer[i].inputSource;
+                            if (source == axis) {
+                                servoMiddleAccum[target] += servo[target];
+                                servoMiddleAccumCount[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 source = currentServoMixer[i].inputSource;
+                                if (source == axis) {
+                                    servoParamsMutable(target)->middle = servoMiddleAccum[target] / servoMiddleAccumCount[target];
+                                }
+                            }
                         }
                         trimState = AUTOTRIM_SAVE_PENDING;
                         pidResetErrorAccumulators(); //Reset Iterm since new midpoints override previously acumulated errors
@@ -449,8 +471,14 @@ 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 source = currentServoMixer[i].inputSource;
+                    if (source == axis) {
+                        servoParamsMutable(target)->middle = servoMiddleBackup[target];
+                    }
+                }
             }
         }
 
@@ -458,6 +486,90 @@ void processServoAutotrim(void)
     }
 }
 
+#define SERVO_AUTOTRIM_FILTER_CUTOFF    1       // LPF cutoff frequency
+#define SERVO_AUTOTRIM_CENTER_MIN       1300
+#define SERVO_AUTOTRIM_CENTER_MAX       1700
+#define SERVO_AUTOTRIM_UPDATE_SIZE      5
+
+void processContinuousServoAutotrim(const float dT)
+{
+    static timeMs_t lastUpdateTimeMs;
+    static servoAutotrimState_e trimState = AUTOTRIM_IDLE;    
+    static uint32_t servoMiddleUpdateCount;
+
+    const float rotRateMagnitude = sqrtf(vectorNormSquared(&imuMeasuredRotationBF));
+    const float rotRateMagnitudeFiltered = pt1FilterApply4(&rotRateFilter, rotRateMagnitude, SERVO_AUTOTRIM_FILTER_CUTOFF, dT);
+    const float targetRateMagnitude = getTotalRateTarget();
+    const float targetRateMagnitudeFiltered = pt1FilterApply4(&targetRateFilter, targetRateMagnitude, SERVO_AUTOTRIM_FILTER_CUTOFF, dT);
+
+    if (ARMING_FLAG(ARMED)) {
+        trimState = AUTOTRIM_COLLECTING;
+        if ((millis() - lastUpdateTimeMs) > 500) {
+            const bool planeIsFlyingStraight = rotRateMagnitudeFiltered <= DEGREES_TO_RADIANS(servoConfig()->servo_autotrim_rotation_limit);
+            const bool noRotationCommanded = targetRateMagnitudeFiltered <= servoConfig()->servo_autotrim_rotation_limit;
+            const bool planeIsFlyingLevel = calculateCosTiltAngle() >= 0.878153032f;
+            if (
+                planeIsFlyingStraight && 
+                noRotationCommanded && 
+                planeIsFlyingLevel &&
+                !FLIGHT_MODE(MANUAL_MODE) && 
+                isGPSHeadingValid() // TODO: proper flying detection
+            ) { 
+                // Plane is flying straight and level: trim servos
+                for (int axis = FD_ROLL; axis <= FD_YAW; 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 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
+                            if (!logicConditionGetValue(currentServoMixer[i].conditionId)) {
+                                continue;
+                            }
+#endif
+                            const uint8_t target = currentServoMixer[i].targetChannel;
+                            const uint8_t source = currentServoMixer[i].inputSource;
+                            if (source == 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();
+        }
+    } else if (trimState == AUTOTRIM_COLLECTING) {
+        // We have disarmed, save midpoints to EEPROM
+        writeEEPROM();
+        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, MAX(RADIANS_TO_DEGREES(rotRateMagnitudeFiltered), targetRateMagnitudeFiltered));
+    DEBUG_SET(DEBUG_AUTOTRIM, 5, axisPID_I[FD_ROLL]);
+    DEBUG_SET(DEBUG_AUTOTRIM, 7, axisPID_I[FD_PITCH]);    
+}
+
+void processServoAutotrim(const float dT) {
+    if (feature(FEATURE_FW_AUTOTRIM)) {
+        processContinuousServoAutotrim(dT);
+    } else {
+        processServoAutotrimMode();
+    }
+}
+
 bool isServoOutputEnabled(void)
 {
     return servoOutputEnabled;

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);