From 199608528221d8799a621fe323f7e6557e2010de Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Thu, 6 Jan 2022 09:01:11 +0900
Subject: [PATCH] AR_WPNav: init accepts speed max

also remove unused set_desired_speed_to_default
also init previous leg in set_desired_location
init accepts speed but inforces minimum
---
 libraries/AR_WPNav/AR_WPNav.cpp | 48 ++++++++++++++++++++++++---------
 libraries/AR_WPNav/AR_WPNav.h   | 10 +++----
 2 files changed, 40 insertions(+), 18 deletions(-)

diff --git a/libraries/AR_WPNav/AR_WPNav.cpp b/libraries/AR_WPNav/AR_WPNav.cpp
index 87cf46d94f..15d36c787f 100644
--- a/libraries/AR_WPNav/AR_WPNav.cpp
+++ b/libraries/AR_WPNav/AR_WPNav.cpp
@@ -26,6 +26,7 @@ extern const AP_HAL::HAL& hal;
 
 #define AR_WPNAV_TIMEOUT_MS             100
 #define AR_WPNAV_SPEED_DEFAULT          2.0f
+#define AR_WPNAV_SPEED_MIN              0.05f   // minimum speed between waypoints in m/s
 #define AR_WPNAV_RADIUS_DEFAULT         2.0f
 #define AR_WPNAV_OVERSPEED_RATIO_MAX    5.0f    // if _overspeed_enabled the vehicle may travel as quickly as 5x WP_SPEED
 
@@ -89,12 +90,16 @@ AR_WPNav::AR_WPNav(AR_AttitudeControl& atc, AR_PosControl &pos_control) :
     AP_Param::setup_object_defaults(this, var_info);
 }
 
-// initialise waypoint controller
-void AR_WPNav::init()
+// initialise waypoint controller.  speed_max should be set to the maximum speed in m/s (or left at zero to use the default speed)
+void AR_WPNav::init(float speed_max)
 {
-    // sanity check parameters
-    const float speed_max = MAX(0, _speed_max);
-    const float accel_max = is_positive(_accel_max) ? _accel_max : _atc.get_accel_max();
+    // determine max speed, acceleration and jerk
+    if (!is_positive(speed_max)) {
+        speed_max = _speed_max;
+    }
+    speed_max = MAX(AR_WPNAV_SPEED_MIN, speed_max);
+    const float atc_accel_max = MIN(_atc.get_accel_max(), _atc.get_decel_max());
+    const float accel_max = is_positive(_accel_max) ? MIN(_accel_max, atc_accel_max) : atc_accel_max;
     const float jerk_max = is_positive(_jerk_max) ? _jerk_max : accel_max;
 
     // initialise position controller
@@ -114,12 +119,8 @@ void AR_WPNav::init()
     _pivot_at_next_wp = false;
 
     // initialise origin and destination to stopping point
-    Location stopping_loc;
-    if (get_stopping_location(stopping_loc)) {
-        _origin = _destination = stopping_loc;
-    } else {
-        // handle not current location
-    }
+    _orig_and_dest_valid = false;
+    set_origin_and_destination_to_stopping_point();
 }
 
 // update navigation
@@ -166,7 +167,13 @@ bool AR_WPNav::set_desired_location(const struct Location& destination, Location
 {
     // re-initialise if inactive, previous destination has been interrupted or different controller was used
     if (!is_active() || !_reached_destination || (_nav_control_type != NavControllerType::NAV_SCURVE)) {
-        init();
+        if (!set_origin_and_destination_to_stopping_point()) {
+            return false;
+        }
+        // clear scurves
+        _scurve_prev_leg.init();
+        _scurve_this_leg.init();
+        _scurve_next_leg.init();
     }
 
     // shift this leg to previous leg
@@ -299,7 +306,9 @@ bool AR_WPNav::set_desired_location_expect_fast_update(const Location &destinati
 {
     // initialise if not active
     if (!is_active() || (_nav_control_type != NavControllerType::NAV_PSC_INPUT_SHAPING)) {
-        init();
+        if (!set_origin_and_destination_to_stopping_point()) {
+            return false;
+        }
     }
 
     // initialise some variables
@@ -540,3 +549,16 @@ void AR_WPNav::init_pos_control_if_necessary()
         }
     }
 }
+
+// set origin and destination to stopping point
+bool AR_WPNav::set_origin_and_destination_to_stopping_point()
+{
+    // initialise origin and destination to stopping point
+    Location stopping_loc;
+    if (!get_stopping_location(stopping_loc)) {
+        return false;
+    }
+    _origin = _destination = stopping_loc;
+    _orig_and_dest_valid = true;
+    return true;
+}
diff --git a/libraries/AR_WPNav/AR_WPNav.h b/libraries/AR_WPNav/AR_WPNav.h
index e167e0b03f..e612fc11ea 100644
--- a/libraries/AR_WPNav/AR_WPNav.h
+++ b/libraries/AR_WPNav/AR_WPNav.h
@@ -13,8 +13,8 @@ public:
     // constructor
     AR_WPNav(AR_AttitudeControl& atc, AR_PosControl &pos_control);
 
-    // initialise waypoint controller
-    void init();
+    // initialise waypoint controller.  speed_max should be set to the maximum speed in m/s (or left at zero to use the default speed)
+    void init(float speed_max = 0);
 
     // update navigation
     virtual void update(float dt);
@@ -25,9 +25,6 @@ public:
     // set desired speed in m/s
     void set_desired_speed(float speed) { _desired_speed = MAX(speed, 0.0f); }
 
-    // restore desired speed to default from parameter value
-    void set_desired_speed_to_default() { _desired_speed = _speed_max; }
-
     // execute the mission in reverse (i.e. drive backwards to destination)
     bool get_reversed() const { return _reversed; }
     void set_reversed(bool reversed) { _reversed = reversed; }
@@ -132,6 +129,9 @@ protected:
     // this should be called before updating the position controller with new targets but after the EKF has a good position estimate
     void init_pos_control_if_necessary();
 
+    // set origin and destination to stopping point
+    bool set_origin_and_destination_to_stopping_point();
+
     // parameters
     AP_Float _speed_max;            // target speed between waypoints in m/s
     AP_Float _radius;               // distance in meters from a waypoint when we consider the waypoint has been reached