initial build

docs/Settings.md

@@ -2944,7 +2944,7 @@ Maximum climb/descent rate that UAV is allowed to reach during navigation modes.
 
 ### nav_auto_speed
 
-Maximum velocity firmware is allowed in full auto modes (RTH, WP) [cm/s] [Multirotor only]
+Speed in fully autonomous modes (RTH, WP) [cm/s]. Used for WP mode when no specific WP speed set. [Multirotor only]
 
 | Default | Min | Max |
 | --- | --- | --- |
@@ -3444,7 +3444,7 @@ Maximum climb/descent rate firmware is allowed when processing pilot input for A
 
 ### nav_manual_speed
 
-Maximum velocity firmware is allowed when processing pilot input for POSHOLD/CRUISE control mode [cm/s] [Multirotor only]
+Maximum speed allowed when processing pilot input for POSHOLD/CRUISE control mode [cm/s] [Multirotor only]
 
 | Default | Min | Max |
 | --- | --- | --- |
@@ -3462,6 +3462,16 @@ Max allowed altitude (above Home Point) that applies to all NAV modes (including
 
 ---
 
+### nav_max_auto_speed
+
+Maximum speed allowed in fully autonomous modes (RTH, WP) [cm/s] [Multirotor only]
+
+| Default | Min | Max |
+| --- | --- | --- |
+| 1000 | 10 | 2000 |
+
+---
+
 ### nav_max_terrain_follow_alt
 
 Max allowed above the ground altitude for terrain following mode

src/main/cms/cms_menu_navigation.c

@@ -44,7 +44,8 @@ static const OSD_Entry cmsx_menuNavSettingsEntries[] =
     OSD_LABEL_ENTRY("-- BASIC SETTINGS --"),
 
     OSD_SETTING_ENTRY("CONTROL MODE", SETTING_NAV_USER_CONTROL_MODE),
-    OSD_SETTING_ENTRY("MAX NAV SPEED", SETTING_NAV_AUTO_SPEED),
+    OSD_SETTING_ENTRY("MC NAV SPEED", SETTING_NAV_AUTO_SPEED),
+    OSD_SETTING_ENTRY("MC MAX NAV SPEED", SETTING_NAV_MAX_AUTO_SPEED),
     OSD_SETTING_ENTRY("MAX CRUISE SPEED", SETTING_NAV_MANUAL_SPEED),
     OSD_SETTING_ENTRY("MAX NAV CLIMB RATE", SETTING_NAV_AUTO_CLIMB_RATE),
     OSD_SETTING_ENTRY("MAX AH CLIMB RATE", SETTING_NAV_MANUAL_CLIMB_RATE),

src/main/fc/fc_msp.c

@@ -2284,8 +2284,9 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
 
 #ifdef USE_NAV
     case MSP_SET_NAV_POSHOLD:
-        if (dataSize >= 13) {
+        if (dataSize >= 15) {
             navConfigMutable()->general.flags.user_control_mode = sbufReadU8(src);
+            navConfigMutable()->general.auto_speed = sbufReadU16(src);
             navConfigMutable()->general.max_auto_speed = sbufReadU16(src);
             navConfigMutable()->general.max_auto_climb_rate = sbufReadU16(src);
             navConfigMutable()->general.max_manual_speed = sbufReadU16(src);

src/main/fc/settings.yaml

@@ -2483,8 +2483,14 @@ groups:
         field: general.waypoint_safe_distance
         max: 65000
       - name: nav_auto_speed
-        description: "Maximum velocity firmware is allowed in full auto modes (RTH, WP) [cm/s] [Multirotor only]"
+        description: "Speed in fully autonomous modes (RTH, WP) [cm/s]. Used for WP mode when no specific WP speed set. [Multirotor only]"
         default_value: 300
+        field: general.auto_speed
+        min: 10
+        max: 2000
+      - name: nav_max_auto_speed
+        description: "Maximum speed allowed in fully autonomous modes (RTH, WP) [cm/s] [Multirotor only]"
+        default_value: 1000
         field: general.max_auto_speed
         min: 10
         max: 2000
@@ -2495,7 +2501,7 @@ groups:
         min: 10
         max: 2000
       - name: nav_manual_speed
-        description: "Maximum velocity firmware is allowed when processing pilot input for POSHOLD/CRUISE control mode [cm/s] [Multirotor only]"
+        description: "Maximum speed allowed when processing pilot input for POSHOLD/CRUISE control mode [cm/s] [Multirotor only]"
         default_value: 500
         field: general.max_manual_speed
         min: 10

src/main/navigation/navigation.c

@@ -96,7 +96,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, 0);
 #endif
 
-PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 13);
+PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 14);
 
 PG_RESET_TEMPLATE(navConfig_t, navConfig,
     .general = {
@@ -121,7 +121,8 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
         .waypoint_radius = SETTING_NAV_WP_RADIUS_DEFAULT,                             // 2m diameter
         .waypoint_safe_distance = SETTING_NAV_WP_SAFE_DISTANCE_DEFAULT,               // centimeters - first waypoint should be closer than this
         .waypoint_load_on_boot = SETTING_NAV_WP_LOAD_ON_BOOT_DEFAULT,               // load waypoints automatically during boot
-        .max_auto_speed = SETTING_NAV_AUTO_SPEED_DEFAULT,                             // 3 m/s = 10.8 km/h
+        .auto_speed = SETTING_NAV_AUTO_SPEED_DEFAULT,                                 // speed in autonomous modes (3 m/s = 10.8 km/h)
+        .max_auto_speed = SETTING_NAV_MAX_AUTO_SPEED_DEFAULT,                         // max allowed speed autonomous modes
         .max_auto_climb_rate = SETTING_NAV_AUTO_CLIMB_RATE_DEFAULT,                   // 5 m/s
         .max_manual_speed = SETTING_NAV_MANUAL_SPEED_DEFAULT,
         .max_manual_climb_rate = SETTING_NAV_MANUAL_CLIMB_RATE_DEFAULT,
@@ -3003,7 +3004,7 @@ float getActiveWaypointSpeed(void)
         return navConfig()->general.max_manual_speed;
     }
     else {
-        uint16_t waypointSpeed = navConfig()->general.max_auto_speed;
+        uint16_t waypointSpeed = navConfig()->general.auto_speed;
 
         if (navGetStateFlags(posControl.navState) & NAV_AUTO_WP) {
             if (posControl.waypointCount > 0 && (posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_WAYPOINT || posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_HOLD_TIME || posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_LAND)) {
@@ -3015,6 +3016,8 @@ float getActiveWaypointSpeed(void)
 
                 if (wpSpecificSpeed >= 50.0f && wpSpecificSpeed <= navConfig()->general.max_auto_speed) {
                     waypointSpeed = wpSpecificSpeed;
+                } else if (wpSpecificSpeed > navConfig()->general.max_auto_speed) {
+                    waypointSpeed = navConfig()->general.max_auto_speed;
                 }
             }
         }

src/main/navigation/navigation.h

@@ -205,7 +205,8 @@ typedef struct navConfig_s {
         uint16_t waypoint_radius;               // if we are within this distance to a waypoint then we consider it reached (distance is in cm)
         uint16_t waypoint_safe_distance;        // Waypoint mission sanity check distance
         bool     waypoint_load_on_boot;         // load waypoints automatically during boot
-        uint16_t max_auto_speed;                // autonomous navigation speed cm/sec
+        uint16_t auto_speed;                    // autonomous navigation speed cm/sec
+        uint16_t max_auto_speed;                // maximum allowed autonomous navigation speed cm/sec
         uint16_t max_auto_climb_rate;           // max vertical speed limitation cm/sec
         uint16_t max_manual_speed;              // manual velocity control max horizontal speed
         uint16_t max_manual_climb_rate;         // manual velocity control max vertical speed