From a5638f5699edeff9417774f9c7510020e961aa29 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Thu, 6 Jan 2022 09:01:49 +0900
Subject: [PATCH] Rover: auto, guided, rtl call wpnav.init instead of
 set-desired-speed

also smart-rtl
---
 Rover/mode_auto.cpp      |  4 ++--
 Rover/mode_guided.cpp    |  4 ++--
 Rover/mode_rtl.cpp       | 10 +++-------
 Rover/mode_smart_rtl.cpp | 10 +++-------
 4 files changed, 10 insertions(+), 18 deletions(-)

diff --git a/Rover/mode_auto.cpp b/Rover/mode_auto.cpp
index 27cff1e894..38e4152738 100644
--- a/Rover/mode_auto.cpp
+++ b/Rover/mode_auto.cpp
@@ -11,8 +11,8 @@ bool ModeAuto::_enter()
         return false;
     }
 
-    // initialise waypoint speed
-    g2.wp_nav.set_desired_speed_to_default();
+    // initialise waypoint navigation library
+    g2.wp_nav.init();
 
     // other initialisation
     auto_triggered = false;
diff --git a/Rover/mode_guided.cpp b/Rover/mode_guided.cpp
index 7d3d53633a..20575e4153 100644
--- a/Rover/mode_guided.cpp
+++ b/Rover/mode_guided.cpp
@@ -12,8 +12,8 @@ bool ModeGuided::_enter()
         start_stop();
     }
 
-    // initialise waypoint speed
-    g2.wp_nav.set_desired_speed_to_default();
+    // initialise waypoint navigation library
+    g2.wp_nav.init();
 
     send_notification = false;
 
diff --git a/Rover/mode_rtl.cpp b/Rover/mode_rtl.cpp
index 9da8bc5c45..fe9d4200f5 100644
--- a/Rover/mode_rtl.cpp
+++ b/Rover/mode_rtl.cpp
@@ -8,6 +8,9 @@ bool ModeRTL::_enter()
         return false;
     }
 
+    // initialise waypoint navigation library
+    g2.wp_nav.init(MAX(0, g2.rtl_speed));
+
     // set target to the closest rally point or home
 #if AP_RALLY == ENABLED
     if (!g2.wp_nav.set_desired_location(g2.rally.calc_best_rally_or_home_location(rover.current_loc, ahrs.get_home().alt))) {
@@ -20,13 +23,6 @@ bool ModeRTL::_enter()
     }
 #endif
 
-    // initialise waypoint speed
-    if (is_positive(g2.rtl_speed)) {
-        g2.wp_nav.set_desired_speed(g2.rtl_speed);
-    } else {
-        g2.wp_nav.set_desired_speed_to_default();
-    }
-
     send_notification = true;
     _loitering = false;
     return true;
diff --git a/Rover/mode_smart_rtl.cpp b/Rover/mode_smart_rtl.cpp
index f1e76697ac..4112e906c3 100644
--- a/Rover/mode_smart_rtl.cpp
+++ b/Rover/mode_smart_rtl.cpp
@@ -14,18 +14,14 @@ bool ModeSmartRTL::_enter()
         return false;
     }
 
+    // initialise waypoint navigation library
+    g2.wp_nav.init(MAX(0, g2.rtl_speed));
+
     // set desired location to reasonable stopping point
     if (!g2.wp_nav.set_desired_location_to_stopping_location()) {
         return false;
     }
 
-    // initialise waypoint speed
-    if (is_positive(g2.rtl_speed)) {
-        g2.wp_nav.set_desired_speed(g2.rtl_speed);
-    } else {
-        g2.wp_nav.set_desired_speed_to_default();
-    }
-
     // init state
     smart_rtl_state = SmartRTL_WaitForPathCleanup;
     _loitering = false;