From 266457711af87d22f93fc33a2a247047e9877154 Mon Sep 17 00:00:00 2001
From: Henry Wurzburg <hwurzburg@yahoo.com>
Date: Wed, 18 Sep 2019 14:23:28 -0500
Subject: [PATCH] Rover: add loiter gain and max vehicle speed params

---
 APMrover2/Parameters.cpp  | 17 +++++++++++++++++
 APMrover2/Parameters.h    |  6 ++++++
 APMrover2/mode.cpp        |  2 ++
 APMrover2/mode_loiter.cpp |  6 ++----
 4 files changed, 27 insertions(+), 4 deletions(-)

diff --git a/APMrover2/Parameters.cpp b/APMrover2/Parameters.cpp
index 93a6908f6e..adba118219 100644
--- a/APMrover2/Parameters.cpp
+++ b/APMrover2/Parameters.cpp
@@ -603,6 +603,23 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
     // @Path: ../libraries/AC_Avoidance/AP_OAPathPlanner.cpp
     AP_SUBGROUPINFO(oa, "OA_", 45, ParametersG2, AP_OAPathPlanner),
 
+    // @Param: SPEED_MAX
+    // @DisplayName: Speed maximum
+    // @Description: Maximum speed vehicle can obtain at full throttle. If 0, it will be estimated based on CRUISE_SPEED and CRUISE_THROTTLE.
+    // @Units: m/s
+    // @Range: 0 30
+    // @Increment: 0.1
+    // @User: Advanced
+    AP_GROUPINFO("SPEED_MAX", 46, ParametersG2, speed_max, 0.0f),
+
+    // @Param: LOIT_SPEED_GAIN
+    // @DisplayName: Loiter speed gain
+    // @Description: Determines how agressively LOITER tries to correct for drift from loiter point. Higher is faster but default should be acceptable.
+    // @Range: 0 5
+    // @Increment: 0.01
+    // @User: Advanced
+    AP_GROUPINFO("LOIT_SPEED_GAIN", 47, ParametersG2, loiter_speed_gain, 0.5f),
+
     AP_GROUPEND
 };
 
diff --git a/APMrover2/Parameters.h b/APMrover2/Parameters.h
index 6ff248b5d6..9ca93f4be1 100644
--- a/APMrover2/Parameters.h
+++ b/APMrover2/Parameters.h
@@ -388,6 +388,12 @@ public:
 
     // object avoidance path planning
     AP_OAPathPlanner oa;
+
+    // maximum speed for vehicle
+    AP_Float speed_max;
+
+    // gain for speed of correction in loiter
+    AP_Float loiter_speed_gain;
 };
 
 extern const AP_Param::Info var_info[];
diff --git a/APMrover2/mode.cpp b/APMrover2/mode.cpp
index 515909d582..2410f6c510 100644
--- a/APMrover2/mode.cpp
+++ b/APMrover2/mode.cpp
@@ -335,6 +335,8 @@ float Mode::calc_speed_max(float cruise_speed, float cruise_throttle) const
     // sanity checks
     if (cruise_throttle > 1.0f || cruise_throttle < 0.05f) {
         speed_max = cruise_speed;
+    } else if (is_positive(g2.speed_max)) {
+        speed_max = g2.speed_max;
     } else {
         // project vehicle's maximum speed
         speed_max = (1.0f / cruise_throttle) * cruise_speed;
diff --git a/APMrover2/mode_loiter.cpp b/APMrover2/mode_loiter.cpp
index 760305e40a..03c9d9eeb9 100644
--- a/APMrover2/mode_loiter.cpp
+++ b/APMrover2/mode_loiter.cpp
@@ -30,10 +30,8 @@ void ModeLoiter::update()
         const float desired_speed_within_radius = rover.g2.sailboat.tack_enabled() ? 0.1f : 0.0f;
         _desired_speed = attitude_control.get_desired_speed_accel_limited(desired_speed_within_radius, rover.G_Dt);
     } else {
-        // P controller with hard-coded gain to convert distance to desired speed
-        // To-Do: make gain configurable or calculate from attitude controller's maximum accelearation
-        _desired_speed = MIN((_distance_to_destination - g2.loit_radius) * 0.5f, g2.wp_nav.get_default_speed());
-
+        // P controller with hard-coded gain to convert distance to desired speed       
+        _desired_speed = MIN((_distance_to_destination - g2.loit_radius) * g2.loiter_speed_gain, g2.wp_nav.get_default_speed());
         // calculate bearing to destination
         _desired_yaw_cd = rover.current_loc.get_bearing_to(_destination);
         float yaw_error_cd = wrap_180_cd(_desired_yaw_cd - ahrs.yaw_sensor);