Fixed wing Auto Speed Mode

docs/Settings.md

@@ -1346,6 +1346,26 @@ S.Port telemetry: If `ON`, send the legacy telemetry IDs for modes (Tmp1) and GN
 
 ---
 
+### 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`.
@@ -3407,6 +3427,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
@@ -5936,6 +5986,7 @@ Selection of pitot hardware. VIRTUAL only works if a GPS is enabled.
 | FAKE |  |
 | MSP |  |
 | DLVR-L10D |  |
+| MS5525 |  |
 
 ---
 

src/main/fc/fc_core.c

@@ -985,6 +985,9 @@ void taskMainPidLoop(timeUs_t currentTimeUs)
     // Apply throttle tilt compensation
     applyThrottleTiltCompensation();
 
+    // Get auto speed throttle demand if active
+    getAutoSpeedThrottleDemand(&rcCommand[THROTTLE]);
+
 #ifdef USE_POWER_LIMITS
     powerLimiterApply(&rcCommand[THROTTLE]);
 #endif

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

@@ -2262,6 +2262,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
@@ -3000,6 +3018,18 @@ 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: nav_fw_control_smoothness
         description: "How smoothly the autopilot controls the airplane to correct the navigation error"
         default_value: 0

src/main/flight/mixer.c

@@ -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

@@ -257,6 +257,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
                 }
             }
@@ -550,7 +556,7 @@ void updatePIDCoefficients(void)
     for (int axis = 0; axis < 3; axis++) {
         pidState[axis].stickPosition = constrain(rxGetChannelValue(axis) - PWM_RANGE_MIDDLE, -500, 500) / 500.0f;
     }
-    
+
     float tpaFactor=1.0f;
     float iTermFactor=1.0f;  // Separate factor for I-term scaling
     if(usedPidControllerType == PID_TYPE_PIFF){ // Fixed wing TPA calculation
@@ -570,13 +576,13 @@ void updatePIDCoefficients(void)
     }
     tpaFactorprev = tpaFactor;
 
-    
+
     // If nothing changed - don't waste time recalculating coefficients
     if (!pidGainsUpdateRequired) {
         return;
     }
 
-    
+
     // PID coefficients can be update only with THROTTLE and TPA or inflight PID adjustments
     //TODO: Next step would be to update those only at THROTTLE or inflight adjustments change
     for (int axis = 0; axis < 3; axis++) {

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

@@ -1161,7 +1161,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;
@@ -1988,6 +1988,18 @@ static bool osdDrawSingleElement(uint8_t item)
         osdFormatVelocityStr(buff, stats.max_3D_speed, OSD_SPEED_TYPE_3D, true);
         break;
 
+     case OSD_AUTO_SPEED:
+        if (isFixedwingAutoSpeedActive()) {
+            buff[0] = '(';
+            osdFormatVelocityStr(buff + 1, getDesiredAutoSpeed(), OSD_SPEED_TYPE_3D, false);
+            buff[5] = ')';
+            buff[6] = '\0';
+            break;
+        } else {
+            displayWrite(osdDisplayPort, elemPosX, elemPosY, "      ");
+            return true;
+        }
+
     case OSD_GLIDESLOPE:
         {
             float horizontalSpeed = gpsSol.groundSpeed;
@@ -2216,7 +2228,7 @@ static bool osdDrawSingleElement(uint8_t item)
     case OSD_GPS_EXTRA_STATS:
         {
             gpsSetOsdMonRfWidgetEnabled(true);
-            
+
             // Collect satellite stats grouped by GNSS ID: 0,2,3,6
             uint8_t stats[4][2] = { {0,0}, {0,0}, {0,0}, {0,0} }; // [group][0]=total, [group][1]=good
             for (int i = 0; i < UBLOX_MAX_SIGNALS; ++i) {
@@ -4360,6 +4372,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);
 
@@ -4418,7 +4431,7 @@ void pgResetFn_osdLayoutsConfig(osdLayoutsConfig_t *osdLayoutsConfig)
 
     osdLayoutsConfig->item_pos[0][OSD_MISSION] = OSD_POS(0, 10);
     osdLayoutsConfig->item_pos[0][OSD_GPS_SATS] = OSD_POS(0, 11) | OSD_VISIBLE_FLAG;
-    osdLayoutsConfig->item_pos[0][OSD_GPS_HDOP] = OSD_POS(0, 10);    
+    osdLayoutsConfig->item_pos[0][OSD_GPS_HDOP] = OSD_POS(0, 10);
     osdLayoutsConfig->item_pos[0][OSD_GPS_EXTRA_STATS] = OSD_POS(3, 11);
 
     osdLayoutsConfig->item_pos[0][OSD_GPS_LAT] = OSD_POS(0, 12);

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;
 

src/main/navigation/navigation.c

@@ -119,7 +119,7 @@ STATIC_ASSERT(NAV_MAX_WAYPOINTS < 254, NAV_MAX_WAYPOINTS_exceeded_allowable_rang
 PG_REGISTER_ARRAY(navWaypoint_t, NAV_MAX_WAYPOINTS, nonVolatileWaypointList, PG_WAYPOINT_MISSION_STORAGE, 2);
 #endif
 
-PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 7);
+PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 8);
 
 PG_RESET_TEMPLATE(navConfig_t, navConfig,
     .general = {
@@ -211,6 +211,8 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
         .max_climb_angle = SETTING_NAV_FW_CLIMB_ANGLE_DEFAULT,                              // degrees
         .max_dive_angle = SETTING_NAV_FW_DIVE_ANGLE_DEFAULT,                                // degrees
         .cruise_speed = SETTING_NAV_FW_CRUISE_SPEED_DEFAULT,                                // cm/s
+        .auto_speed_min_speed = SETTING_FW_AUTO_SPEED_MIN_SPEED_DEFAULT,                    // 11 m/s
+        .auto_speed_max_speed = SETTING_FW_AUTO_SPEED_MAX_SPEED_DEFAULT,                    // 22 m/s
         .control_smoothness = SETTING_NAV_FW_CONTROL_SMOOTHNESS_DEFAULT,
         .pitch_to_throttle_smooth = SETTING_NAV_FW_PITCH2THR_SMOOTHING_DEFAULT,
         .pitch_to_throttle_thresh = SETTING_NAV_FW_PITCH2THR_THRESHOLD_DEFAULT,
@@ -3874,10 +3876,10 @@ void getWaypoint(uint8_t wpNumber, navWaypoint_t * wpData)
 
 int isGCSValid(void)
 {
-    return (ARMING_FLAG(ARMED) && 
-            (posControl.flags.estPosStatus >= EST_TRUSTED) && 
-            posControl.gpsOrigin.valid && 
-            posControl.flags.isGCSAssistedNavigationEnabled && 
+    return (ARMING_FLAG(ARMED) &&
+            (posControl.flags.estPosStatus >= EST_TRUSTED) &&
+            posControl.gpsOrigin.valid &&
+            posControl.flags.isGCSAssistedNavigationEnabled &&
             (posControl.navState == NAV_STATE_POSHOLD_3D_IN_PROGRESS));
 }
 
@@ -5056,6 +5058,14 @@ void navigationUsePIDs(void)
                                         2.0f,
                                         0.0f
     );
+
+    navPidInit(&posControl.pids.fw_autoSpeed, (float)pidProfile()->bank_fw.pid[PID_AUTO_SPEED].P / 30.0f,
+                                        (float)pidProfile()->bank_fw.pid[PID_AUTO_SPEED].I / 50.0f,
+                                        (float)pidProfile()->bank_fw.pid[PID_AUTO_SPEED].D / 50.0f,
+                                        0.0f,
+                                        2.0f,
+                                        0.0f
+    );
 }
 
 void navigationInit(void)
@@ -5255,7 +5265,8 @@ bool navigationInAutomaticThrottleMode(void)
 {
     navigationFSMStateFlags_t stateFlags = navGetCurrentStateFlags();
     return (stateFlags & (NAV_CTL_ALT | NAV_CTL_EMERG | NAV_CTL_LAND)) ||
-           ((stateFlags & NAV_CTL_LAUNCH) && !navConfig()->fw.launch_manual_throttle);
+           ((stateFlags & NAV_CTL_LAUNCH) && !navConfig()->fw.launch_manual_throttle) ||
+           isFixedwingAutoSpeedActive();
 }
 
 bool navigationIsControllingThrottle(void)

src/main/navigation/navigation.h

@@ -197,7 +197,7 @@ typedef struct geozone_s {
     int32_t distanceHorToNearestZone;
     int32_t distanceVertToNearestZone;
     int32_t zoneInfo;
-    int32_t currentzoneMaxAltitude; 
+    int32_t currentzoneMaxAltitude;
     int32_t currentzoneMinAltitude;
     bool nearestHorZoneHasAction;
     bool sticksLocked;
@@ -473,6 +473,8 @@ typedef struct navConfig_s {
         uint8_t  max_climb_angle;            // Fixed wing max banking angle (deg)
         uint8_t  max_dive_angle;             // Fixed wing max banking angle (deg)
         uint16_t cruise_speed;               // Speed at cruise throttle (cm/s), used for time/distance left before RTH
+        uint16_t auto_speed_min_speed;       // Minimum allowed speed for auto speed mode (m/s)
+        uint16_t auto_speed_max_speed;       // Maximum allowed speed for auto speed mode (m/s)
         uint8_t  control_smoothness;         // The amount of smoothing to apply to controls for navigation
         uint16_t pitch_to_throttle_smooth;   // How smoothly the autopilot makes pitch to throttle correction inside a deadband defined by pitch_to_throttle_thresh.
         uint8_t  pitch_to_throttle_thresh;   // Threshold from average pitch where momentary pitch_to_throttle correction kicks in. [decidegrees]
@@ -598,6 +600,7 @@ typedef struct navigationPIDControllers_s {
     pidController_t fw_alt;
     pidController_t fw_nav;
     pidController_t fw_heading;
+    pidController_t fw_autoSpeed;
 } navigationPIDControllers_t;
 
 /* MultiWii-compatible params for telemetry */
@@ -820,6 +823,10 @@ int8_t navCheckActiveAngleHoldAxis(void);
 uint8_t getActiveWpNumber(void);
 uint16_t getFlownLoiterRadius(void);
 
+uint16_t getDesiredAutoSpeed(void);
+bool isFixedwingAutoSpeedActive(void);
+void getAutoSpeedThrottleDemand(int16_t *throttleCommand);
+
 /* Returns the heading recorded when home position was acquired.
  * Note that the navigation system uses deg*100 as unit and angles
  * are in the [0, 360 * 100) interval.