From 54eee7d311e026b5009d483bb257cf832b82e1cb Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Wed, 5 Jan 2022 20:23:19 +0900
Subject: [PATCH] AR_WPNav: simplify init

---
 libraries/AR_WPNav/AR_WPNav.cpp | 30 ++++++++----------------------
 libraries/AR_WPNav/AR_WPNav.h   |  6 +-----
 2 files changed, 9 insertions(+), 27 deletions(-)

diff --git a/libraries/AR_WPNav/AR_WPNav.cpp b/libraries/AR_WPNav/AR_WPNav.cpp
index b30a1e20b0..f5dac7082d 100644
--- a/libraries/AR_WPNav/AR_WPNav.cpp
+++ b/libraries/AR_WPNav/AR_WPNav.cpp
@@ -99,29 +99,15 @@ AR_WPNav::AR_WPNav(AR_AttitudeControl& atc, AR_PosControl &pos_control) :
 }
 
 // initialise waypoint controller
-// speed_max should be the max speed (in m/s) the vehicle will travel to waypoint.  Leave as zero to use the default speed
-// accel_max should be the max forward-back acceleration (in m/s/s).  Leave as zero to use the attitude controller's default acceleration
-// lat_accel_max should be the max right-left acceleration (in m/s/s).  Leave as zero to use the attitude controller's default acceleration
-// jerk_max should be the max forward-back and lateral jerk (in m/s/s/s).  Leave as zero to use the attitude controller's default acceleration
-void AR_WPNav::init(float speed_max, float accel_max, float lat_accel_max, float jerk_max)
+void AR_WPNav::init()
 {
-    // default max speed and accel
-    if (!is_positive(speed_max)) {
-        speed_max = _speed_max;
-    }
-    if (!is_positive(accel_max)) {
-        accel_max = get_accel_max();
-    }
-    if (!is_positive(lat_accel_max)) {
-        lat_accel_max = _atc.get_turn_lat_accel_max();
-    }
-    if (!is_positive(jerk_max)) {
-        jerk_max = get_jerk_max();
-    }
-    _scurve_jerk = jerk_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();
+    const float jerk_max = is_positive(_jerk_max) ? _jerk_max : accel_max;
 
     // initialise position controller
-    _pos_control.set_limits(speed_max, accel_max, lat_accel_max, jerk_max);
+    _pos_control.set_limits(speed_max, accel_max, _atc.get_turn_lat_accel_max(), jerk_max);
 
     _scurve_prev_leg.init();
     _scurve_this_leg.init();
@@ -189,7 +175,7 @@ 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(0,0,0,0);
+        init();
     }
 
     // shift this leg to previous leg
@@ -322,7 +308,7 @@ 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(0,0,0,0);
+        init();
     }
 
     // initialise some variables
diff --git a/libraries/AR_WPNav/AR_WPNav.h b/libraries/AR_WPNav/AR_WPNav.h
index 210816c243..df4c7ac6fa 100644
--- a/libraries/AR_WPNav/AR_WPNav.h
+++ b/libraries/AR_WPNav/AR_WPNav.h
@@ -14,11 +14,7 @@ public:
     AR_WPNav(AR_AttitudeControl& atc, AR_PosControl &pos_control);
 
     // initialise waypoint controller
-    // speed_max should be the max speed (in m/s) the vehicle will travel to waypoint.  Leave as zero to use the default speed
-    // accel_max should be the max forward-back acceleration (in m/s/s).  Leave as zero to use the attitude controller's default acceleration
-    // lat_accel_max should be the max right-left acceleration (in m/s/s).  Leave as zero to use the attitude controller's default acceleration
-    // jerk_max should be the max forward-back and lateral jerk (in m/s/s/s).  Leave as zero to use the attitude controller's default acceleration
-    void init(float speed_max, float accel_max, float lat_accel_max, float jerk_max);
+    void init();
 
     // update navigation
     virtual void update(float dt);