Rover: mode auto guided and rtl slow before destination

new mode class member _desired_speed_final holds target speed at destination

main vehicle code passes heading to next waypoint into auto mode.
we do not provide heading when delaying at waypoint which signals we wish

auto-mode calculates final speed at destination which allows vehicle to
make turn within value of WP_OVERSHOOT parameter assuming vehicle turns at
maximum lateral acceleration.

APMrover2/Parameters.cpp

@@ -314,6 +314,15 @@ const AP_Param::Info Rover::var_info[] = {
     // @User: Standard
     GSCALAR(waypoint_radius,        "WP_RADIUS",        2.0f),
 
+    // @Param: WP_OVERSHOOT
+    // @DisplayName: Waypoint overshoot maximum
+    // @Description: Waypoint overshoot maximum in meters.  The vehicle will attempt to stay within this many meters of the track as it completes one waypoint and moves to the next.
+    // @Units: m
+    // @Range: 0 10
+    // @Increment: 0.1
+    // @User: Standard
+    GSCALAR(waypoint_overshoot,     "WP_OVERSHOOT", 2.0f),
+
     // @Param: TURN_MAX_G
     // @DisplayName: Turning maximum G force
     // @Description: The maximum turning acceleration (in units of gravities) that the rover can handle while remaining stable. The navigation code will keep the lateral acceleration below this level to avoid rolling over or slipping the wheels in turns

APMrover2/Parameters.h

@@ -168,6 +168,7 @@ public:
         k_param_command_total = 220,    // unused
         k_param_command_index,          // unused
         k_param_waypoint_radius,
+        k_param_waypoint_overshoot,
 
         //
         // 230: camera control
@@ -271,6 +272,7 @@ public:
     // Waypoints
     //
     AP_Float    waypoint_radius;
+    AP_Float    waypoint_overshoot;
 
     Parameters() {}
 };

APMrover2/Rover.h

@@ -259,9 +259,6 @@ private:
     // true if we have a position estimate from AHRS
     bool have_position;
 
-    // angle of our next navigation waypoint
-    int32_t next_navigation_leg_cd;
-
     // receiver RSSI
     uint8_t receiver_rssi;
 

APMrover2/commands_logic.cpp

@@ -22,9 +22,6 @@ bool Rover::start_command(const AP_Mission::Mission_Command& cmd)
 
     gcs().send_text(MAV_SEVERITY_INFO, "Executing command ID #%i", cmd.id);
 
-    // remember the course of our next navigation leg
-    next_navigation_leg_cd = mission.get_next_ground_course_cd(0);
-
     switch (cmd.id) {
     case MAV_CMD_NAV_WAYPOINT:  // Navigate to Waypoint
         do_nav_wp(cmd, false);
@@ -228,9 +225,17 @@ void Rover::do_nav_wp(const AP_Mission::Mission_Command& cmd, bool stay_active_a
     // this is the delay, stored in seconds
     loiter_duration = cmd.p1;
 
+    // get heading to following waypoint (auto mode reduces speed to allowing corning without large overshoot)
+    // in case of non-zero loiter duration, we provide heading-unknown to signal we should stop at the point
+    float next_leg_bearing_cd = MODE_NEXT_HEADING_UNKNOWN;
+    if (loiter_duration == 0) {
+    	next_leg_bearing_cd = mission.get_next_ground_course_cd(MODE_NEXT_HEADING_UNKNOWN);
+    }
+
+    // retrieve and sanitize target location
     Location cmdloc = cmd.content.location;
     location_sanitize(current_loc, cmdloc);
-    mode_auto.set_desired_location(cmdloc, stay_active_at_dest);
+    mode_auto.set_desired_location(cmdloc, next_leg_bearing_cd, stay_active_at_dest);
 }
 
 void Rover::do_loiter_unlimited(const AP_Mission::Mission_Command& cmd)

APMrover2/mode.cpp

@@ -26,7 +26,7 @@ bool Mode::enter()
 }
 
 // set desired location
-void Mode::set_desired_location(const struct Location& destination)
+void Mode::set_desired_location(const struct Location& destination, float next_leg_bearing_cd)
 {
     // record targets
     _origin = rover.current_loc;
@@ -36,6 +36,21 @@ void Mode::set_desired_location(const struct Location& destination)
     // initialise distance
     _distance_to_destination = get_distance(_origin, _destination);
     _reached_destination = false;
+
+    // set final desired speed
+    _desired_speed_final = 0.0f;
+    if (!is_equal(next_leg_bearing_cd, MODE_NEXT_HEADING_UNKNOWN)) {
+        // if not turning can continue at full speed
+        if (is_zero(next_leg_bearing_cd)) {
+            _desired_speed_final = _desired_speed;
+        } else {
+            // calculate maximum speed that keeps overshoot within bounds
+            const float curr_leg_bearing_cd = get_bearing_cd(_origin, _destination);
+            const float turn_angle_cd = wrap_180_cd(next_leg_bearing_cd - curr_leg_bearing_cd);
+            const float radius_m = fabsf(g.waypoint_overshoot / (cosf(radians(turn_angle_cd * 0.01f)) - 1.0f));
+            _desired_speed_final = MIN(_desired_speed, safe_sqrt(g.turn_max_g * GRAVITY_MSS * radius_m));
+        }
+    }
 }
 
 // set desired heading and speed
@@ -148,14 +163,17 @@ float Mode::calc_reduced_speed_for_turn_or_distance(float desired_speed)
         pivot_speed_scaling = 0.0f;
     }
 
-    // calculate a waypoint distance based scaling (default to no reduction)
-    float distance_speed_scaling = 1.0f;
-    if (is_positive(distance_to_waypoint)) {
-        distance_speed_scaling = 1.0f - yaw_error_ratio;
+    // scaled speed
+    float speed_scaled = desired_speed * MIN(lateral_accel_speed_scaling, pivot_speed_scaling);
+
+    // limit speed based on distance to waypoint and max acceleration/deceleration
+    if (is_positive(distance_to_waypoint) && is_positive(attitude_control.get_accel_max())) {
+        const float speed_max = safe_sqrt(2.0f * distance_to_waypoint * attitude_control.get_accel_max() + sq(_desired_speed_final));
+        speed_scaled = constrain_float(speed_scaled, -speed_max, speed_max);
     }
 
     // return minimum speed
-    return desired_speed * MIN(MIN(lateral_accel_speed_scaling, distance_speed_scaling), pivot_speed_scaling);
+    return speed_scaled;
 }
 
 // calculate the lateral acceleration target to cause the vehicle to drive along the path from origin to destination

APMrover2/mode.h

@@ -4,6 +4,8 @@
 #include <GCS_MAVLink/GCS_MAVLink.h>  // for MAV_SEVERITY
 #include "defines.h"
 
+#define MODE_NEXT_HEADING_UNKNOWN   99999.0f    // used to indicate to set_desired_location method that next leg's heading is unknown
+
 class Mode
 {
 public:
@@ -62,7 +64,9 @@ public:
     virtual float get_distance_to_destination() const { return 0.0f; }
 
     // set desired location and speed (used in RTL, Guided, Auto)
-    virtual void set_desired_location(const struct Location& destination);
+    //   next_leg_bearing_cd should be heading to the following waypoint (used to slow the vehicle in order to make the turn)
+    virtual void set_desired_location(const struct Location& destination, float next_leg_bearing_cd = MODE_NEXT_HEADING_UNKNOWN);
+
     // true if vehicle has reached desired location. defaults to true because this is normally used by missions and we do not want the mission to become stuck
     virtual bool reached_destination() { return true; }
 
@@ -126,6 +130,7 @@ protected:
     float _desired_yaw_cd;      // desired yaw in centi-degrees
     float _yaw_error_cd;        // error between desired yaw and actual yaw in centi-degrees
     float _desired_speed;       // desired speed in m/s
+    float _desired_speed_final; // desired speed in m/s when we reach the destination
     float _speed_error;         // ground speed error in m/s
 };
 
@@ -149,7 +154,7 @@ public:
 
     // set desired location, heading and speed
     // set stay_active_at_dest if the vehicle should attempt to maintain it's position at the destination (mostly for boats)
-    void set_desired_location(const struct Location& destination, bool stay_active_at_dest);
+    void set_desired_location(const struct Location& destination, float next_leg_bearing_cd = MODE_NEXT_HEADING_UNKNOWN, bool stay_active_at_dest = false);
     bool reached_destination() override;
 
     // heading and speed control
@@ -199,7 +204,7 @@ public:
     float get_distance_to_destination() const override;
 
     // set desired location, heading and speed
-    void set_desired_location(const struct Location& destination) override;
+    void set_desired_location(const struct Location& destination);
     void set_desired_heading_and_speed(float yaw_angle_cd, float target_speed) override;
 
     // set desired heading-delta, turn-rate and speed

APMrover2/mode_auto.cpp

@@ -10,7 +10,7 @@ bool ModeAuto::_enter()
     }
 
     // init location target
-    set_desired_location(rover.current_loc, false);
+    set_desired_location(rover.current_loc);
 
     // other initialisation
     auto_triggered = false;
@@ -81,10 +81,10 @@ void ModeAuto::update()
 }
 
 // set desired location to drive to
-void ModeAuto::set_desired_location(const struct Location& destination, bool stay_active_at_dest)
+void ModeAuto::set_desired_location(const struct Location& destination, float next_leg_bearing_cd, bool stay_active_at_dest)
 {
     // call parent
-    Mode::set_desired_location(destination);
+    Mode::set_desired_location(destination, next_leg_bearing_cd);
 
     _submode = Auto_WP;
     _stay_active_at_dest = stay_active_at_dest;