Fixed wing Auto Speed Mode

docs/Settings.md

@@ -1356,6 +1356,36 @@ 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 input value. Defaults to throttle channel 4. Currently only for fixed wing.
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 4 | 4 | MAX_SUPPORTED_RC_CHANNEL_COUNT |
+
+---
+
+### fw_auto_speed_max_speed
+
+Maximum ground speed for fixed wing auto speed mode [m/s].
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 22 | 5 | 50 |
+
+---
+
+### fw_auto_speed_min_speed
+
+Minimum ground speed for fixed wing auto speed mode [m/s].
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 11 | 5 | 50 |
+
+---
+
 ### fw_autotune_max_rate_deflection
 
 The target percentage of maximum mixer output used for determining the rates in `AUTO` and `LIMIT`.
@@ -3417,6 +3447,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 (Fixed wing).
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 10 | 0 | 255 |
+
+---
+
+### nav_fw_auto_speed_i
+
+I gain of auto speed PID controller (Fixed wing).
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 5 | 0 | 255 |
+
+---
+
+### nav_fw_auto_speed_p
+
+P gain of auto speed PID controller (Fixed wing).
+
+| 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
@@ -3051,7 +3053,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
             osdDrawCustomItem(item);
 
             return MSP_RESULT_ACK;
-            
+
         } else{
             return MSP_RESULT_ERROR;
         }
@@ -4407,7 +4409,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);
     }
@@ -4460,26 +4462,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

@@ -2268,6 +2268,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 (Fixed wing)."
+        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 (Fixed wing)."
+        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 (Fixed wing)."
+        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
@@ -3006,6 +3024,24 @@ groups:
         field: fw.cruise_speed
         min: 0
         max: 65535
+      - name: fw_auto_speed_min_speed
+        description: "Minimum ground speed for fixed wing 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 ground speed for fixed wing 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 input value. Defaults to throttle channel 4. Currently only for fixed wing."
+        default_value: 4
+        field: fw.auto_speed_channel
+        min: 4
+        max: MAX_SUPPORTED_RC_CHANNEL_COUNT
       - 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,20 @@ 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] = pitotValidForAirspeed() ? 'A' : 'G';
+            strcpy(buff + 1, ": OFF");
+            if (isFixedwingAutoSpeedActive()) {
+                osdFormatVelocityStr(buff + 2, getDesiredAutoSpeed(), OSD_SPEED_TYPE_3D, false);
+                buff[6] = '\0';
+            }
+            break;
+        } else {
+            displayWrite(osdDisplayPort, elemPosX, elemPosY, "      ");
+            return true;
+        }
+
     case OSD_GLIDESLOPE:
         {
             float horizontalSpeed = gpsSol.groundSpeed;
@@ -4362,6 +4376,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);
 

src/main/io/osd.h

@@ -342,7 +342,8 @@ typedef enum {
     OSD_NAV_FW_ALT_CONTROL_RESPONSE,
     OSD_NAV_MIN_GROUND_SPEED,
     OSD_THROTTLE_GAUGE,
-    OSD_GPS_EXTRA_STATS, 
+    OSD_GPS_EXTRA_STATS,
+    OSD_AUTO_SPEED,  // 170
     OSD_ITEM_COUNT // MUST BE LAST
 } osd_items_e;