Rover: add loiter gain and max vehicle speed params

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
 };
 

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[];

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;

APMrover2/mode_loiter.cpp

@@ -31,9 +31,7 @@ void ModeLoiter::update()
         _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());
-
+        _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);