From 461c5196e72984e2c3352756918b0236dcc01121 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Fri, 24 Dec 2021 11:27:01 +0900
Subject: [PATCH] AR_WPNav: add ACCEL and JERK params and accessors

allows users to specify a different acceleration and jerk for Auto, Guided, RTL, etc compared with manual modes (Acro, etc)
---
 libraries/AR_WPNav/AR_WPNav.cpp | 42 ++++++++++++++++++++++++++++++---
 libraries/AR_WPNav/AR_WPNav.h   |  8 +++++++
 2 files changed, 47 insertions(+), 3 deletions(-)

diff --git a/libraries/AR_WPNav/AR_WPNav.cpp b/libraries/AR_WPNav/AR_WPNav.cpp
index 72d0dfde48..5b05a197d5 100644
--- a/libraries/AR_WPNav/AR_WPNav.cpp
+++ b/libraries/AR_WPNav/AR_WPNav.cpp
@@ -68,6 +68,24 @@ const AP_Param::GroupInfo AR_WPNav::var_info[] = {
     // @Path: AR_PivotTurn.cpp
     AP_SUBGROUPINFO(_pivot, "PIVOT_", 8, AR_WPNav, AR_PivotTurn),
 
+    // @Param: ACCEL
+    // @DisplayName: Waypoint acceleration
+    // @Description: Waypoint acceleration.  If zero then ATC_ACCEL_MAX is used
+    // @Units: m/s/s
+    // @Range: 0 100
+    // @Increment: 0.1
+    // @User: Standard
+    AP_GROUPINFO("ACCEL", 9, AR_WPNav, _accel_max, 0),
+
+    // @Param: JERK
+    // @DisplayName: Waypoint jerk
+    // @Description: Waypoint jerk (change in acceleration).  If zero then jerk is same as acceleration
+    // @Units: m/s/s/s
+    // @Range: 0 100
+    // @Increment: 0.1
+    // @User: Standard
+    AP_GROUPINFO("JERK", 10, AR_WPNav, _jerk_max, 0),
+
     AP_GROUPEND
 };
 
@@ -91,18 +109,18 @@ void AR_WPNav::init(float speed_max, float accel_max, float lat_accel_max, float
         speed_max = _speed_max;
     }
     if (!is_positive(accel_max)) {
-        accel_max = _atc.get_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 = _atc.get_accel_max();
+        jerk_max = get_jerk_max();
     }
     _scurve_jerk = jerk_max;
 
     // initialise position controller
-    _pos_control.set_limits(speed_max, accel_max, lat_accel_max);
+    _pos_control.set_limits(speed_max, accel_max, lat_accel_max, jerk_max);
 
     _scurve_prev_leg.init();
     _scurve_this_leg.init();
@@ -284,6 +302,24 @@ bool AR_WPNav::set_desired_location_NED(const Vector3f &destination, const Vecto
     return set_desired_location(dest_loc, next_dest_loc);
 }
 
+// get max acceleration in m/s/s
+float AR_WPNav::get_accel_max() const
+{
+    if (is_positive(_accel_max)) {
+        return _accel_max;
+    }
+    return _atc.get_accel_max();
+}
+
+// get max jerk in m/s/s/s
+float AR_WPNav::get_jerk_max() const
+{
+    if (is_positive(_jerk_max)) {
+        return _jerk_max;
+    }
+    return get_accel_max();
+}
+
 // calculate vehicle stopping point using current location, velocity and maximum acceleration
 bool AR_WPNav::get_stopping_location(Location& stopping_loc)
 {
diff --git a/libraries/AR_WPNav/AR_WPNav.h b/libraries/AR_WPNav/AR_WPNav.h
index 30b62374a9..ee4e680a7e 100644
--- a/libraries/AR_WPNav/AR_WPNav.h
+++ b/libraries/AR_WPNav/AR_WPNav.h
@@ -88,6 +88,12 @@ public:
     float get_radius() const { return _radius; }
     float get_pivot_rate() const { return _pivot.get_rate_max(); }
 
+    // get max acceleration in m/s/s
+    float get_accel_max() const;
+
+    // get max jerk in m/s/s/s
+    float get_jerk_max() const;
+
     // calculate stopping location using current position and attitude controller provided maximum deceleration
     // returns true on success, false on failure
     bool get_stopping_location(Location& stopping_loc) WARN_IF_UNUSED;
@@ -125,6 +131,8 @@ protected:
     AP_Float _speed_min;            // target speed minimum in m/s.  Vehicle will not slow below this speed for corners
     AP_Float _radius;               // distance in meters from a waypoint when we consider the waypoint has been reached
     AR_PivotTurn _pivot;            // pivot turn controller
+    AP_Float _accel_max;            // max acceleration.  If zero then attitude controller's specified max accel is used
+    AP_Float _jerk_max;             // max jerk (change in acceleration).  If zero then value is same as accel_max
 
     // references
     AR_AttitudeControl& _atc;       // rover attitude control library