Autolevel flight mode and pitch trim option

docs/Settings.md

@@ -129,6 +129,8 @@
 | fw_i_yaw | 10 | 0 | 200 | Fixed-wing rate stabilisation I-gain for YAW |
 | fw_iterm_limit_stick_position | 0.5 | 0 | 1 | Iterm is not allowed to grow when stick position is above threshold. This solves the problem of bounceback or followthrough when full stick deflection is applied on poorely tuned fixed wings. In other words, stabilization is partialy disabled when pilot is actively controlling the aircraft and active when sticks are not touched. `0` mean stick is in center position, `1` means it is fully deflected to either side |
 | fw_iterm_throw_limit | 165 | FW_ITERM_THROW_LIMIT_MIN | FW_ITERM_THROW_LIMIT_MAX | Limits max/min I-term value in stabilization PID controller in case of Fixed Wing. It solves the problem of servo saturation before take-off/throwing the airplane into the air. By default, error accumulated in I-term can not exceed 1/3 of servo throw (around 165us). Set 0 to disable completely. |
+| fw_level_pitch_deadband | 5 | 0 | 20 | Deadband for automatic leveling when AUTOLEVEL mode is used. |
+| fw_level_pitch_gain | 5 | 0 | 20 | I-gain for the pitch trim for self-leveling flight modes. Higher values means that AUTOTRIM will be faster but might introduce oscillations |
 | fw_level_pitch_trim | 0 | -10 | 10 | Pitch trim for self-leveling flight modes. In degrees. +5 means airplane nose should be raised 5 deg from level |
 | fw_loiter_direction | RIGHT |  |  | Direction of loitering: center point on right wing (clockwise - default), or center point on left wing (counterclockwise). If equal YAW then can be changed in flight using a yaw stick. |
 | fw_min_throttle_down_pitch | 0 | 0 | 450 | Automatic pitch down angle when throttle is at 0 in angle mode. Progressively applied between cruise throttle and zero throttle (decidegrees) |

src/main/build/debug.h

@@ -80,6 +80,7 @@ typedef enum {
     DEBUG_SPM_VARIO,    // Smartport master variometer
     DEBUG_PCF8574,
     DEBUG_DYNAMIC_GYRO_LPF,
+    DEBUG_AUTOLEVEL,
     DEBUG_FW_D,
     DEBUG_IMU2,
     DEBUG_ALTITUDE,

src/main/common/fp_pid.c

@@ -96,7 +96,10 @@ float navPidApply3(
     pid->output_constrained = outValConstrained;
 
     /* Update I-term */
-    if (!(pidFlags & PID_ZERO_INTEGRATOR)) {
+    if (
+        !(pidFlags & PID_ZERO_INTEGRATOR) &&
+        !(pidFlags & PID_FREEZE_INTEGRATOR) 
+    ) {
         const float newIntegrator = pid->integrator + (error * pid->param.kI * gainScaler * dt) + ((outValConstrained - outVal) * pid->param.kT * dt);
 
         if (pidFlags & PID_SHRINK_INTEGRATOR) {
@@ -147,6 +150,10 @@ void navPidInit(pidController_t *pid, float _kP, float _kI, float _kD, float _kF
         float Td = _kD / _kP;
         pid->param.kT = 2.0f / (Ti + Td);
     }
+    else if (_kI > 1e-6f) {
+        pid->param.kI = _kI;
+        pid->param.kT = 0.0f;
+    }
     else {
         pid->param.kI = 0.0;
         pid->param.kT = 0.0;

src/main/common/fp_pid.h

@@ -42,6 +42,7 @@ typedef enum {
     PID_ZERO_INTEGRATOR             = 1 << 1,
     PID_SHRINK_INTEGRATOR           = 1 << 2,
     PID_LIMIT_INTEGRATOR            = 1 << 3,
+    PID_FREEZE_INTEGRATOR           = 1 << 4,
 } pidControllerFlags_e;
 
 typedef struct {

src/main/fc/fc_msp_box.c

@@ -91,6 +91,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = {
     { BOXPREARM, "PREARM", 51 },
     { BOXTURTLE, "TURTLE", 52 },
     { BOXNAVCRUISE, "NAV CRUISE", 53 },
+    { BOXAUTOLEVEL, "AUTO LEVEL", 54 },
     { CHECKBOX_ITEM_COUNT, NULL, 0xFF }
 };
 
@@ -250,6 +251,9 @@ void initActiveBoxIds(void)
 #if defined(USE_AUTOTUNE_FIXED_WING)
         activeBoxIds[activeBoxIdCount++] = BOXAUTOTUNE;
 #endif
+        if (sensors(SENSOR_BARO)) {
+            activeBoxIds[activeBoxIdCount++] = BOXAUTOLEVEL;
+        }
     }
 
     /*
@@ -380,6 +384,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(BOXAUTOLEVEL)),       BOXAUTOLEVEL);
 
     memset(mspBoxModeFlags, 0, sizeof(boxBitmask_t));
     for (uint32_t i = 0; i < activeBoxIdCount; i++) {

src/main/fc/fc_tasks.c

@@ -304,9 +304,9 @@ void taskUpdateOsd(timeUs_t currentTimeUs)
 
 void taskUpdateAux(timeUs_t currentTimeUs)
 {
-    UNUSED(currentTimeUs);
     updatePIDCoefficients();
     dynamicLpfGyroTask();
+    updateFixedWingLevelTrim(currentTimeUs);
 }
 
 void fcTasksInit(void)

src/main/fc/rc_modes.h

@@ -71,6 +71,7 @@ typedef enum {
     BOXPREARM        = 42,
     BOXTURTLE        = 43,
     BOXNAVCRUISE     = 44,
+    BOXAUTOLEVEL     = 45,
     CHECKBOX_ITEM_COUNT
 } boxId_e;
 

src/main/fc/settings.yaml

@@ -93,7 +93,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", "AUTOLEVEL", "FW_D", "IMU2", "ALTITUDE",
       "GYRO_ALPHA_BETA_GAMMA", "SMITH_PREDICTOR", "AUTOTRIM", "AUTOTUNE"]
   - name: async_mode
     values: ["NONE", "GYRO", "ALL"]
@@ -2112,6 +2112,18 @@ groups:
         condition: USE_SMITH_PREDICTOR
         min: 1
         max: 500
+      - name: fw_level_pitch_gain
+        description: "I-gain for the pitch trim for self-leveling flight modes. Higher values means that AUTOTRIM will be faster but might introduce oscillations"
+        default_value: 5
+        field: fixedWingLevelTrimGain
+        min: 0
+        max: 20
+      - name: fw_level_pitch_deadband
+        description: "Deadband for automatic leveling when AUTOLEVEL mode is used."
+        default_value: 5
+        field: fixedWingLevelTrimDeadband
+        min: 0
+        max: 20
 
   - name: PG_PID_AUTOTUNE_CONFIG
     type: pidAutotuneConfig_t

src/main/flight/pid.c

@@ -160,7 +160,14 @@ static EXTENDED_FASTRAM pidControllerFnPtr pidControllerApplyFn;
 static EXTENDED_FASTRAM filterApplyFnPtr dTermLpfFilterApplyFn;
 static EXTENDED_FASTRAM filterApplyFnPtr dTermLpf2FilterApplyFn;
 static EXTENDED_FASTRAM bool levelingEnabled = false;
+
+#define FIXED_WING_LEVEL_TRIM_MAX_ANGLE 10.0f // Max angle auto trimming can demand
+#define FIXED_WING_LEVEL_TRIM_DIVIDER 500.0f
+#define FIXED_WING_LEVEL_TRIM_MULTIPLIER 1.0f / FIXED_WING_LEVEL_TRIM_DIVIDER
+#define FIXED_WING_LEVEL_TRIM_CONTROLLER_LIMIT FIXED_WING_LEVEL_TRIM_DIVIDER * FIXED_WING_LEVEL_TRIM_MAX_ANGLE
+
 static EXTENDED_FASTRAM float fixedWingLevelTrim;
+static EXTENDED_FASTRAM pidController_t fixedWingLevelTrimController;
 
 PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 2);
 
@@ -300,6 +307,8 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
 #endif
 
         .fixedWingLevelTrim = SETTING_FW_LEVEL_PITCH_TRIM_DEFAULT,
+        .fixedWingLevelTrimGain = SETTING_FW_LEVEL_PITCH_GAIN_DEFAULT,
+        .fixedWingLevelTrimDeadband = SETTING_FW_LEVEL_PITCH_DEADBAND_DEFAULT,
 
 #ifdef USE_SMITH_PREDICTOR
         .smithPredictorStrength = SETTING_SMITH_PREDICTOR_STRENGTH_DEFAULT,
@@ -597,8 +606,28 @@ static void pidLevel(pidState_t *pidState, flight_dynamics_index_t axis, float h
     float angleTarget = pidRcCommandToAngle(rcCommand[axis], pidProfile()->max_angle_inclination[axis]);
 
     // Automatically pitch down if the throttle is manually controlled and reduced bellow cruise throttle
-    if ((axis == FD_PITCH) && STATE(AIRPLANE) && FLIGHT_MODE(ANGLE_MODE) && !navigationIsControllingThrottle())
-        angleTarget += scaleRange(MAX(0, navConfig()->fw.cruise_throttle - rcCommand[THROTTLE]), 0, navConfig()->fw.cruise_throttle - PWM_RANGE_MIN, 0, mixerConfig()->fwMinThrottleDownPitchAngle);
+    if ((axis == FD_PITCH) && STATE(AIRPLANE) && FLIGHT_MODE(ANGLE_MODE) && !navigationIsControllingThrottle()) {
+        angleTarget += scaleRange(MAX(0, navConfig()->fw.cruise_throttle - rcCommand[THROTTLE]), 0, navConfig()->fw.cruise_throttle - PWM_RANGE_MIN, 0, mixerConfig()->fwMinThrottleDownPitchAngle);        
+    }
+
+    //PITCH trim applied by a AutoLevel flight mode and manual pitch trimming
+    if (axis == FD_PITCH && STATE(AIRPLANE)) {
+        DEBUG_SET(DEBUG_AUTOLEVEL, 0, angleTarget * 10);
+        DEBUG_SET(DEBUG_AUTOLEVEL, 1, fixedWingLevelTrim * 10);
+        DEBUG_SET(DEBUG_AUTOLEVEL, 2, getEstimatedActualVelocity(Z));
+
+        /* 
+         * fixedWingLevelTrim has opposite sign to rcCommand.
+         * Positive rcCommand means nose should point downwards
+         * Negative rcCommand mean nose should point upwards
+         * This is counter intuitive and a natural way suggests that + should mean UP
+         * This is why fixedWingLevelTrim has opposite sign to rcCommand
+         * Positive fixedWingLevelTrim means nose should point upwards
+         * Negative fixedWingLevelTrim means nose should point downwards
+         */
+        angleTarget -= fixedWingLevelTrim;   
+        DEBUG_SET(DEBUG_AUTOLEVEL, 3, angleTarget * 10);
+    }
 
         //PITCH trim applied by a AutoLevel flight mode and manual pitch trimming
     if (axis == FD_PITCH && STATE(AIRPLANE)) {
@@ -1250,6 +1279,16 @@ void pidInit(void)
 #endif
 
     fixedWingLevelTrim = pidProfile()->fixedWingLevelTrim;
+
+    navPidInit(
+        &fixedWingLevelTrimController,
+        0.0f,
+        (float)pidProfile()->fixedWingLevelTrimGain / 100000.0f,
+        0.0f,
+        0.0f,
+        2.0f
+    );
+
 }
 
 const pidBank_t * pidBank(void) { 
@@ -1258,3 +1297,61 @@ const pidBank_t * pidBank(void) {
 pidBank_t * pidBankMutable(void) { 
     return usedPidControllerType == PID_TYPE_PIFF ? &pidProfileMutable()->bank_fw : &pidProfileMutable()->bank_mc;
 }
+
+void updateFixedWingLevelTrim(timeUs_t currentTimeUs)
+{
+    if (!STATE(AIRPLANE)) {
+        return;
+    }
+
+    static timeUs_t previousUpdateTimeUs;
+    static bool previousArmingState;
+    const float dT = US2S(currentTimeUs - previousUpdateTimeUs);
+
+    /*
+     * On every ARM reset the controller
+     */
+    if (ARMING_FLAG(ARMED) && !previousArmingState) {
+        navPidReset(&fixedWingLevelTrimController);
+    } 
+
+    /*
+     * On disarm update the default value
+     */
+    if (!ARMING_FLAG(ARMED) && previousArmingState) {
+        pidProfileMutable()->fixedWingLevelTrim = constrainf(fixedWingLevelTrim, -FIXED_WING_LEVEL_TRIM_MAX_ANGLE, FIXED_WING_LEVEL_TRIM_MAX_ANGLE);
+    } 
+
+    /*
+     * Prepare flags for the PID controller
+     */
+    pidControllerFlags_e flags = PID_LIMIT_INTEGRATOR;
+
+    //Iterm should freeze when pitch stick is deflected
+    if (
+        !IS_RC_MODE_ACTIVE(BOXAUTOLEVEL) ||
+        rxGetChannelValue(PITCH) > (PWM_RANGE_MIDDLE + pidProfile()->fixedWingLevelTrimDeadband) ||
+        rxGetChannelValue(PITCH) < (PWM_RANGE_MIDDLE - pidProfile()->fixedWingLevelTrimDeadband) ||
+        (!FLIGHT_MODE(ANGLE_MODE) && !FLIGHT_MODE(HORIZON_MODE)) ||
+        navigationIsControllingAltitude()
+    ) {
+        flags |= PID_FREEZE_INTEGRATOR;
+    }
+
+    const float output = navPidApply3(
+        &fixedWingLevelTrimController,
+        0,  //Setpoint is always 0 as we try to keep level flight
+        getEstimatedActualVelocity(Z),
+        dT,
+        -FIXED_WING_LEVEL_TRIM_CONTROLLER_LIMIT,
+        FIXED_WING_LEVEL_TRIM_CONTROLLER_LIMIT,
+        flags,
+        1.0f,
+        1.0f
+    );
+
+    DEBUG_SET(DEBUG_AUTOLEVEL, 4, output);
+    fixedWingLevelTrim = pidProfile()->fixedWingLevelTrim + (output * FIXED_WING_LEVEL_TRIM_MULTIPLIER);
+
+    previousArmingState = !!ARMING_FLAG(ARMED);
+}

src/main/flight/pid.h

@@ -19,6 +19,7 @@
 
 #include "config/parameter_group.h"
 #include "fc/runtime_config.h"
+#include "common/time.h"
 
 #define GYRO_SATURATION_LIMIT       1800        // 1800dps
 #define PID_SUM_LIMIT_MIN           100
@@ -54,6 +55,8 @@ FP-PID has been rescaled to match LuxFloat (and MWRewrite) from Cleanflight 1.13
 
 #define ANTI_GRAVITY_THROTTLE_FILTER_CUTOFF 15  // The anti gravity throttle highpass filter cutoff
 
+#define FIXED_WING_LEVEL_TRIM_DEADBAND_DEFAULT 5
+
 #define TASK_AUX_RATE_HZ   100 //In Hz
 
 typedef enum {
@@ -161,6 +164,8 @@ typedef struct pidProfile_s {
 #endif
 
     float fixedWingLevelTrim;
+    float fixedWingLevelTrimGain;
+    float fixedWingLevelTrimDeadband;
 #ifdef USE_SMITH_PREDICTOR
     float smithPredictorStrength;
     float smithPredictorDelay;
@@ -226,3 +231,5 @@ void autotuneUpdateState(void);
 void autotuneFixedWingUpdate(const flight_dynamics_index_t axis, float desiredRateDps, float reachedRateDps, float pidOutput);
 
 pidType_e pidIndexGetType(pidIndex_e pidIndex);
+
+void updateFixedWingLevelTrim(timeUs_t currentTimeUs);

src/main/navigation/navigation.c

@@ -3648,6 +3648,11 @@ bool navigationIsControllingThrottle(void)
     return navigationInAutomaticThrottleMode() && (getMotorStatus() != MOTOR_STOPPED_USER);
 }
 
+bool navigationIsControllingAltitude(void) {
+    navigationFSMStateFlags_t stateFlags = navGetCurrentStateFlags();
+    return (stateFlags & NAV_CTL_ALT);
+}
+
 bool navigationIsFlyingAutonomousMode(void)
 {
     navigationFSMStateFlags_t stateFlags = navGetCurrentStateFlags();

src/main/navigation/navigation.h

@@ -524,6 +524,7 @@ bool navigationIsControllingThrottle(void);
 bool isFixedWingAutoThrottleManuallyIncreased(void);
 bool navigationIsFlyingAutonomousMode(void);
 bool navigationIsExecutingAnEmergencyLanding(void);
+bool navigationIsControllingAltitude(void);
 /* Returns true iff navConfig()->general.flags.rth_allow_landing is NAV_RTH_ALLOW_LANDING_ALWAYS
  * or if it's NAV_RTH_ALLOW_LANDING_FAILSAFE and failsafe mode is active.
  */