Fixed wing Auto Speed Mode

docs/Settings.md

@@ -1356,6 +1356,56 @@ S.Port telemetry: If `ON`, send the legacy telemetry IDs for modes (Tmp1) and GN
 
 ---
 
+### fw_auto_speed_channel
+
+Channel number used to set desired Auto Speed demand value. Defaults to throttle channel 4.
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 4 | 4 | MAX_SUPPORTED_RC_CHANNEL_COUNT |
+
+---
+
+### fw_auto_speed_level_min_thr
+
+Minimum throttle for LEVEL flight in auto speed mode when using ground speed as speed reference [us]. This should be set with just enough margin to avoid stalling. Throttle can go lower than this setting when pitching down to a minimum of nav_fw_min_thr.
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 1300 | PWM_RANGE_MIN | PWM_RANGE_MAX |
+
+---
+
+### fw_auto_speed_max_speed
+
+Maximum allowed speed demand for Auto Speed mode [m/s].
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 22 | 5 | 50 |
+
+---
+
+### fw_auto_speed_min_speed
+
+Minimum allowed speed demand for Auto Speed mode [m/s].
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 11 | 5 | 50 |
+
+---
+
+### fw_auto_speed_thr_smoothing
+
+Changes how smoothly the throttle responds in Auto Speed mode. Increasing the setting makes throttle changes smoother but also reduces throttle response.
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 4 | 1 | 10 |
+
+---
+
 ### fw_autotune_max_rate_deflection
 
 The target percentage of maximum mixer output used for determining the rates in `AUTO` and `LIMIT`.
@@ -3417,6 +3467,36 @@ Maximum climb/descent rate that UAV is allowed to reach during navigation modes.
 
 ---
 
+### nav_fw_auto_speed_d
+
+D gain of auto speed PID controller.
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 10 | 0 | 255 |
+
+---
+
+### nav_fw_auto_speed_i
+
+I gain of auto speed PID controller.
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 5 | 0 | 255 |
+
+---
+
+### nav_fw_auto_speed_p
+
+P gain of auto speed PID controller.
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 30 | 0 | 255 |
+
+---
+
 ### nav_fw_bank_angle
 
 Max roll angle when rolling / turning in GPS assisted modes, is also restrained by global max_angle_inclination_rll

src/main/fc/fc_msp.c

@@ -166,7 +166,9 @@ static const char pidnames[] =
     "NavR;"
     "LEVEL;"
     "MAG;"
-    "VEL;";
+    "VEL;"
+    "HEADING;"
+    "SPEED;";
 
 typedef enum {
     MSP_SDCARD_STATE_NOT_PRESENT = 0,
@@ -1772,7 +1774,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
                     sbufWriteU8(dst, timer2id(timerHardware[i].tim));
                     #endif
                     sbufWriteU32(dst, timerHardware[i].usageFlags);
-                  
+
                     #if defined(SITL_BUILD) || defined(WASM_BUILD)
                     sbufWriteU8(dst, 0);
                     #else
@@ -3054,7 +3056,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
             osdDrawCustomItem(item);
 
             return MSP_RESULT_ACK;
-            
+
         } else{
             return MSP_RESULT_ERROR;
         }
@@ -4410,7 +4412,7 @@ static void readMspSimulatorValues(sbuf_t *src, const int dataSize, const uint8_
         }
         // Feed data to navigation
         gpsProcessNewDriverData();
-        gpsProcessNewSolutionData(false);                          
+        gpsProcessNewSolutionData(false);
     } else {
         sbufAdvance(src, sizeof(uint8_t) + sizeof(uint8_t) + sizeof(uint32_t) + sizeof(uint32_t) + sizeof(uint32_t) + sizeof(uint16_t) + sizeof(uint16_t) + sizeof(uint16_t) * 3);
     }
@@ -4463,26 +4465,26 @@ static void readMspSimulatorValues(sbuf_t *src, const int dataSize, const uint8_
         sbufReadU16(src);
     }
 
-    if (simMspVersion == SIMULATOR_MSP_VERSION_3) {  
-        
+    if (simMspVersion == SIMULATOR_MSP_VERSION_3) {
+
         if (SIMULATOR_HAS_OPTION(HITL_RANGEFINDER)) {
             simulatorData.rangefinder = sbufReadU16(src);
             if (simulatorData.rangefinder == 0xFFFF) {
                 fakeRangefindersSetData(-1);
             } else {
-                fakeRangefindersSetData(simulatorData.rangefinder); 
+                fakeRangefindersSetData(simulatorData.rangefinder);
             }
-            
+
         } else {
             sbufReadU16(src);
         }
-        
+
         if (SIMULATOR_HAS_OPTION(HITL_CURRENT_SENSOR)) {
             simulatorData.current = sbufReadU16(src);
         } else {
             sbufReadU16(src);
         }
-        
+
         if (SIMULATOR_HAS_OPTION(HITL_SIM_RC_INPUT)) {
             for (int i = 0; i < HITL_SIM_MAX_RC_INPUTS; i++) {
                 simulatorData.rcInput[i] = sbufReadU16(src);

src/main/fc/fc_msp_box.c

@@ -109,6 +109,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = {
     { .boxId = BOXGIMBALRLOCK,      .boxName = "GIMBAL LEVEL ROLL", .permanentId = 66 },
     { .boxId = BOXGIMBALCENTER,     .boxName = "GIMBAL CENTER",     .permanentId = 67 },
     { .boxId = BOXGIMBALHTRK,       .boxName = "GIMBAL HEADTRACKER", .permanentId = 68 },
+    { .boxId = BOXAUTOSPEED,        .boxName = "AUTO SPEED",        .permanentId = 69 },
     { .boxId = CHECKBOX_ITEM_COUNT, .boxName = NULL,                .permanentId = 0xFF }
 };
 
@@ -248,6 +249,7 @@ void initActiveBoxIds(void)
 
         if (STATE(AIRPLANE) || platformTypeConfigured(PLATFORM_AIRPLANE)) {
             ADD_ACTIVE_BOX(BOXSOARING);
+            ADD_ACTIVE_BOX(BOXAUTOSPEED);
         }
     }
 
@@ -449,7 +451,6 @@ void packBoxModeFlags(boxBitmask_t * mspBoxModeFlags)
     CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXMIXERTRANSITION)), BOXMIXERTRANSITION);
 #endif
     CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXANGLEHOLD)),       BOXANGLEHOLD);
-
 #ifdef USE_SERIAL_GIMBAL
     if(IS_RC_MODE_ACTIVE(BOXGIMBALCENTER)) {
         CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGIMBALCENTER)), BOXGIMBALCENTER);
@@ -463,6 +464,7 @@ void packBoxModeFlags(boxBitmask_t * mspBoxModeFlags)
         CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGIMBALHTRK) && !IS_RC_MODE_ACTIVE(BOXGIMBALCENTER)),     BOXGIMBALRLOCK);
     }
 #endif
+    CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXAUTOSPEED)),    BOXAUTOSPEED);
 
     memset(mspBoxModeFlags, 0, sizeof(boxBitmask_t));
     for (uint32_t i = 0; i < activeBoxIdCount; i++) {

src/main/fc/rc_modes.h

@@ -85,6 +85,7 @@ typedef enum {
     BOXGIMBALRLOCK   = 57,
     BOXGIMBALCENTER  = 58,
     BOXGIMBALHTRK    = 59,
+    BOXAUTOSPEED     = 60,
     CHECKBOX_ITEM_COUNT
 } boxId_e;
 

src/main/fc/settings.yaml

@@ -1177,6 +1177,12 @@ groups:
         field: nav.fw.launch_idle_throttle
         min: 1000
         max: 2000
+      - name: fw_auto_speed_level_min_thr
+        description: "Minimum throttle for LEVEL flight in auto speed mode when using ground speed as speed reference [us]. This should be set with just enough margin to avoid stalling. Throttle can go lower than this setting when pitching down to a minimum of nav_fw_min_thr."
+        default_value: 1300
+        field: nav.fw.auto_speed_level_min_thr
+        min: PWM_RANGE_MIN
+        max: PWM_RANGE_MAX
       - name: limit_cont_current
         description: "Continous current limit (dA), set to 0 to disable"
         condition: USE_POWER_LIMITS
@@ -2263,6 +2269,24 @@ groups:
         field: navFwPosHdgPidsumLimit
         min: PID_SUM_LIMIT_MIN
         max: PID_SUM_LIMIT_MAX
+      - name: nav_fw_auto_speed_p
+        description: "P gain of auto speed PID controller."
+        default_value: 30
+        field: bank_fw.pid[PID_AUTO_SPEED].P
+        min: 0
+        max: 255
+      - name: nav_fw_auto_speed_i
+        description: "I gain of auto speed PID controller."
+        default_value: 5
+        field: bank_fw.pid[PID_AUTO_SPEED].I
+        min: 0
+        max: 255
+      - name: nav_fw_auto_speed_d
+        description: "D gain of auto speed PID controller."
+        default_value: 10
+        field: bank_fw.pid[PID_AUTO_SPEED].D
+        min: 0
+        max: 255
       - name: mc_iterm_relax
         description: "Iterm relax type. When enabled, Iterm will be relaxed when stick is centered. This will help to reduce bounceback and followthrough on multirotors. It is recommended to enable this feature on all multirotors."
         field: iterm_relax
@@ -3001,6 +3025,30 @@ groups:
         field: fw.cruise_speed
         min: 0
         max: 65535
+      - name: fw_auto_speed_min_speed
+        description: "Minimum allowed speed demand for Auto Speed mode [m/s]."
+        default_value: 11
+        field: fw.auto_speed_min_speed
+        min: 5
+        max: 50
+      - name: fw_auto_speed_max_speed
+        description: "Maximum allowed speed demand for Auto Speed mode [m/s]."
+        default_value: 22
+        field: fw.auto_speed_max_speed
+        min: 5
+        max: 50
+      - name: fw_auto_speed_channel
+        description: "Channel number used to set desired Auto Speed demand value. Defaults to throttle channel 4."
+        default_value: 4
+        field: fw.auto_speed_channel
+        min: 4
+        max: MAX_SUPPORTED_RC_CHANNEL_COUNT
+      - name: fw_auto_speed_thr_smoothing
+        description: "Changes how smoothly the throttle responds in Auto Speed mode. Increasing the setting makes throttle changes smoother but also reduces throttle response."
+        default_value: 4
+        field: fw.auto_speed_thr_smoothing
+        min: 1
+        max: 10
       - name: nav_fw_control_smoothness
         description: "How smoothly the autopilot controls the airplane to correct the navigation error"
         default_value: 0

src/main/flight/imu.h

@@ -32,9 +32,9 @@ typedef union {
     int16_t raw[XYZ_AXIS_COUNT];
     struct {
         // absolute angle inclination in multiple of 0.1 degree    180 deg = 1800
-        int16_t roll;
-        int16_t pitch;
-        int16_t yaw;
+        int16_t roll;       // +ve = Roll right
+        int16_t pitch;      // +ve = Pitch down !
+        int16_t yaw;        // +ve = Yaw right
     } values;
 } attitudeEulerAngles_t;
 

src/main/flight/mixer.c

@@ -676,7 +676,7 @@ motorStatus_e getMotorStatus(void)
 
     const bool fixedWingOrAirmodeNotActive = STATE(FIXED_WING_LEGACY) || !STATE(AIRMODE_ACTIVE);
 
-    if (throttleStickIsLow() && fixedWingOrAirmodeNotActive) {
+    if (throttleStickIsLow() && fixedWingOrAirmodeNotActive && !isFixedwingAutoSpeedActive()) {
         if ((navConfig()->general.flags.nav_overrides_motor_stop == NOMS_OFF_ALWAYS) && failsafeIsActive()) {
             // If we are in failsafe and user was holding stick low before it was triggered and nav_overrides_motor_stop is set to OFF_ALWAYS
             // and either on a plane or on a quad with inactive airmode - stop motor
@@ -688,7 +688,7 @@ motorStatus_e getMotorStatus(void)
 
             switch (navConfig()->general.flags.nav_overrides_motor_stop) {
                 case NOMS_ALL_NAV:
-                    return navigationInAutomaticThrottleMode() ? MOTOR_RUNNING : MOTOR_STOPPED_USER;
+                    return navigationRequiresAutoThrottleMode() ? MOTOR_RUNNING : MOTOR_STOPPED_USER;
 
                 case NOMS_AUTO_ONLY:
                     return navigationIsFlyingAutonomousMode() ? MOTOR_RUNNING : MOTOR_STOPPED_USER;
@@ -724,6 +724,11 @@ bool areMotorsRunning(void)
     return false;
 }
 
+bool areMotorsStopped(void)
+{
+    return motor[0] == motorZeroCommand;
+}
+
 uint16_t getMaxThrottle(void) {
 
     static uint16_t throttle = 0;

src/main/flight/mixer.h

@@ -131,5 +131,6 @@ void stopPwmAllMotors(void);
 
 void loadPrimaryMotorMixer(void);
 bool areMotorsRunning(void);
+bool areMotorsStopped(void);
 
 uint16_t getMaxThrottle(void);

src/main/flight/pid.c

@@ -224,6 +224,12 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
                     .FF = 0,
                 },
                 [PID_POS_HEADING] = {
+                    .P = 0,
+                    .I = 0,
+                    .D = 0,
+                    .FF = 0,
+                },
+                [PID_AUTO_SPEED] = {
                     .P = 0,
                     .I = 0,
                     .D = 0,
@@ -260,6 +266,12 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
                     .P = SETTING_NAV_FW_POS_HDG_P_DEFAULT,
                     .I = SETTING_NAV_FW_POS_HDG_I_DEFAULT,
                     .D = SETTING_NAV_FW_POS_HDG_D_DEFAULT,
+                    .FF = 0,
+                },
+                [PID_AUTO_SPEED] = {
+                    .P = SETTING_NAV_FW_AUTO_SPEED_P_DEFAULT,
+                    .I = SETTING_NAV_FW_AUTO_SPEED_I_DEFAULT,
+                    .D = SETTING_NAV_FW_AUTO_SPEED_D_DEFAULT,
                     .FF = 0
                 }
             }

src/main/flight/pid.h

@@ -68,6 +68,7 @@ typedef enum {
     PID_HEADING,    //   +       +
     PID_VEL_Z,      //   +       n/a
     PID_POS_HEADING,//   n/a     +
+    PID_AUTO_SPEED, //   n/a     +
     PID_ITEM_COUNT
 } pidIndex_e;
 

src/main/io/osd.c

@@ -1165,7 +1165,7 @@ static void osdFormatThrottlePosition(char *buff, bool useScaled, textAttributes
 #endif
     int8_t throttlePercent = getThrottlePercent(useScaled);
     if ((useScaled && throttlePercent <= 0) || !ARMING_FLAG(ARMED)) {
-        const char* message = ARMING_FLAG(ARMED) ? (throttlePercent == 0 && !ifMotorstopFeatureEnabled()) ? "IDLE" : "STOP" : "DARM";
+        const char* message = ARMING_FLAG(ARMED) ? areMotorsStopped() ? "STOP" : "IDLE" : "DARM";
         buff[0] = SYM_THR;
         strcpy(buff + 1, message);
         return;
@@ -1991,6 +1991,23 @@ static bool osdDrawSingleElement(uint8_t item)
         osdFormatVelocityStr(buff, stats.max_3D_speed, OSD_SPEED_TYPE_3D, true);
         break;
 
+     case OSD_AUTO_SPEED:
+        if (IS_RC_MODE_ACTIVE(BOXAUTOSPEED)) {
+            buff[0] = posControl.autoSpeedSpdSource == FW_AUTO_SPD_GROUND ? 'G' : 'A';
+            strcpy(buff + 1, ": OFF");
+            if (isFixedwingAutoSpeedActive()) {
+                osdFormatVelocityStr(buff + 2, posControl.desiredState.autoSpeedDemand, OSD_SPEED_TYPE_3D, false);
+                buff[6] = '\0';
+                if (posControl.autoSpeedSpdSource == FW_AUTO_SPD_GROUND_OVERRIDE && OSD_ALTERNATING_CHOICES(1000, 2) == 0) {
+                    buff[0] = 'G';
+                }
+            }
+            break;
+        } else {
+            displayWrite(osdDisplayPort, elemPosX, elemPosY, "      ");
+            return true;
+        }
+
     case OSD_GLIDESLOPE:
         {
             float horizontalSpeed = gpsSol.groundSpeed;
@@ -4362,6 +4379,7 @@ void pgResetFn_osdLayoutsConfig(osdLayoutsConfig_t *osdLayoutsConfig)
     osdLayoutsConfig->item_pos[0][OSD_MAIN_BATT_CELL_VOLTAGE] = OSD_POS(12, 1);
     osdLayoutsConfig->item_pos[0][OSD_MAIN_BATT_SAG_COMPENSATED_CELL_VOLTAGE] = OSD_POS(12, 1);
     osdLayoutsConfig->item_pos[0][OSD_GPS_SPEED] = OSD_POS(23, 1);
+    osdLayoutsConfig->item_pos[0][OSD_AUTO_SPEED] = OSD_POS(23, 0);
     osdLayoutsConfig->item_pos[0][OSD_3D_SPEED] = OSD_POS(23, 1);
     osdLayoutsConfig->item_pos[0][OSD_GLIDESLOPE] = OSD_POS(23, 2);