From 394a5fd2fcc97a33f56cd0ef80b2ea48d00a3a5e Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Thu, 6 Jan 2022 09:43:01 +0900
Subject: [PATCH] AR_WPNav: get/set_desired_speed renamed to get/set_speed_max

set_speed_max updates position controller limits and triggers recalculation of scurves
local _desired_speed member is no longer required because max speed is held in position controller
---
 libraries/AR_WPNav/AR_WPNav.cpp | 24 ++++++++++++++++++++++++
 libraries/AR_WPNav/AR_WPNav.h   | 10 ++++------
 2 files changed, 28 insertions(+), 6 deletions(-)

diff --git a/libraries/AR_WPNav/AR_WPNav.cpp b/libraries/AR_WPNav/AR_WPNav.cpp
index 15d36c787f..30eb35832b 100644
--- a/libraries/AR_WPNav/AR_WPNav.cpp
+++ b/libraries/AR_WPNav/AR_WPNav.cpp
@@ -161,6 +161,30 @@ void AR_WPNav::update(float dt)
     update_steering_and_speed(current_loc, dt);
 }
 
+// set maximum speed in m/s.  returns true on success
+// this should not be called at more than 3hz or else SCurve path planning may not advance properly
+bool AR_WPNav::set_speed_max(float speed_max)
+{
+    // range check target speed
+    if (speed_max < AR_WPNAV_SPEED_MIN) {
+        return false;
+    }
+
+    // ignore calls that do not change the speed
+    if (is_equal(speed_max, _pos_control.get_speed_max())) {
+        return true;
+    }
+
+    // update position controller max speed
+    _pos_control.set_limits(speed_max, _pos_control.get_accel_max(), _pos_control.get_lat_accel_max(), _pos_control.get_jerk_max());
+
+    // change track speed
+    _scurve_this_leg.set_speed_max(_pos_control.get_speed_max(), _pos_control.get_speed_max(), _pos_control.get_speed_max());
+    _scurve_next_leg.set_speed_max(_pos_control.get_speed_max(), _pos_control.get_speed_max(), _pos_control.get_speed_max());
+
+    return true;
+}
+
 // set desired location and (optionally) next_destination
 // next_destination should be provided if known to allow smooth cornering
 bool AR_WPNav::set_desired_location(const struct Location& destination, Location next_destination)
diff --git a/libraries/AR_WPNav/AR_WPNav.h b/libraries/AR_WPNav/AR_WPNav.h
index e612fc11ea..594e8c0bb3 100644
--- a/libraries/AR_WPNav/AR_WPNav.h
+++ b/libraries/AR_WPNav/AR_WPNav.h
@@ -19,11 +19,10 @@ public:
     // update navigation
     virtual void update(float dt);
 
-    // return desired speed
-    float get_desired_speed() const { return _desired_speed; }
-
-    // set desired speed in m/s
-    void set_desired_speed(float speed) { _desired_speed = MAX(speed, 0.0f); }
+    // get or set maximum speed in m/s
+    // set_speed_max should not be called at more than 3hz or else SCurve path planning may not advance properly
+    float get_speed_max() const { return _pos_control.get_speed_max(); }
+    bool set_speed_max(float speed_max);
 
     // execute the mission in reverse (i.e. drive backwards to destination)
     bool get_reversed() const { return _reversed; }
@@ -167,7 +166,6 @@ protected:
     } _nav_control_type;            // navigation controller that should be used to travel from _origin to _destination
 
     // main outputs from navigation library
-    float _desired_speed;           // desired speed in m/s
     float _desired_speed_limited;   // desired speed (above) but accel/decel limited
     float _desired_turn_rate_rads;  // desired turn-rate in rad/sec (negative is counter clockwise, positive is clockwise)
     float _desired_lat_accel;       // desired lateral acceleration (for reporting only)