Automatically load waypoints

docs/Controls.md

@@ -34,7 +34,7 @@ The stick positions are combined to activate different functions:
 | Trim Acc Forwards             | HIGH     | CENTER  | HIGH   | CENTER |
 | Trim Acc Backwards            | HIGH     | CENTER  | LOW    | CENTER |
 | Save current waypoint mission | LOW      | CENTER  | HIGH   | LOW    |
-| Load current waypoint mission | LOW      | CENTER  | HIGH   | HIGH   |
+| Load/unload current waypoint mission | LOW      | CENTER  | HIGH   | HIGH   |
 | Save setting                  | LOW      | LOW     | LOW    | HIGH   |
 | Enter OSD Menu (CMS)          | CENTER   | LOW     | HIGH   | CENTER |
 

docs/Settings.md

@@ -342,6 +342,7 @@
 | nav_use_fw_yaw_control | OFF | Enables or Disables the use of the heading PID controller on fixed wing. Heading PID controller is always enabled for rovers and boats |
 | nav_use_midthr_for_althold | OFF | If set to OFF, the FC remembers your throttle stick position when enabling ALTHOLD and treats it as a neutral midpoint for holding altitude |
 | nav_user_control_mode | ATTI | Defines how Pitch/Roll input from RC receiver affects flight in POSHOLD mode: ATTI - pitch/roll controls attitude like in ANGLE mode; CRUISE - pitch/roll controls velocity in forward and right direction. |
+| nav_wp_load_on_boot | OFF | If set to ON, waypoints will be automatically loaded from EEPROM to the FC during startup. |
 | nav_wp_radius | 100 | Waypoint radius [cm]. Waypoint would be considered reached if machine is within this radius |
 | nav_wp_safe_distance | 10000 | First waypoint in the mission should be closer than this value [cm]. A value of 0 disables this check. |
 | opflow_hardware |  | Selection of OPFLOW hardware. |

src/main/fc/settings.yaml

@@ -2072,6 +2072,11 @@ groups:
         field: general.pos_failure_timeout
         min: 0
         max: 10
+      - name: nav_wp_load_on_boot
+        description: "If set to ON, waypoints will be automatically loaded from EEPROM to the FC during startup."
+        default_value: "OFF"
+        field: general.waypoint_load_on_boot
+        type: bool
       - name: nav_wp_radius
         description: "Waypoint radius [cm]. Waypoint would be considered reached if machine is within this radius"
         default_value: "100"

src/main/navigation/navigation.c

@@ -113,6 +113,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
         .pos_failure_timeout = 5,               // 5 sec
         .waypoint_radius = 100,                 // 2m diameter
         .waypoint_safe_distance = 10000,        // centimeters - first waypoint should be closer than this
+        .waypoint_load_on_boot = false,         // load waypoints automatically during boot
         .max_auto_speed = 300,                  // 3 m/s = 10.8 km/h
         .max_auto_climb_rate = 500,             // 5 m/s
         .max_manual_speed = 500,
@@ -2828,6 +2829,12 @@ bool loadNonVolatileWaypointList(void)
     if (ARMING_FLAG(ARMED))
         return false;
 
+    // if waypoints are already loaded, just unload them.
+    if (posControl.waypointCount > 0) {
+        resetWaypointList();
+        return false;
+    }
+
     resetWaypointList();
 
     for (int i = 0; i < NAV_MAX_WAYPOINTS; i++) {
@@ -3520,6 +3527,11 @@ void navigationInit(void)
     } else {
         DISABLE_STATE(FW_HEADING_USE_YAW);
     }
+
+#if defined(NAV_NON_VOLATILE_WAYPOINT_STORAGE)
+    if (navConfig()->general.waypoint_load_on_boot)
+        loadNonVolatileWaypointList();
+#endif
 }
 
 /*-----------------------------------------------------------

src/main/navigation/navigation.h

@@ -189,6 +189,7 @@ typedef struct navConfig_s {
         uint8_t  pos_failure_timeout;           // Time to wait before switching to emergency landing (0 - disable)
         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 max_auto_climb_rate;           // max vertical speed limitation cm/sec
         uint16_t max_manual_speed;              // manual velocity control max horizontal speed