Rover: rtl mode refactoring

slow down for turns

APMrover2/mode.h

@@ -262,12 +262,14 @@ public:
 
     // methods that affect movement of the vehicle in this mode
     void update() override;
-    void update_navigation() override;
 
     // attributes of the mode
     bool is_autopilot_mode() const override { return true; }
     bool failsafe_throttle_suppress() const override { return false; }
 
+    float get_distance_to_destination() const override { return _distance_to_destination; }
+    bool reached_destination() override { return _reached_destination; }
+
 protected:
 
     bool _enter() override;

APMrover2/mode_rtl.cpp

@@ -3,8 +3,14 @@
 
 bool ModeRTL::_enter()
 {
-    rover.prev_WP = rover.current_loc;
+    // refuse RTL if home has not been set
-    rover.next_WP = rover.home;
+    if (rover.home_is_set == HOME_UNSET) {
+        return false;
+    }
+
+    // set destination
+    set_desired_location(rover.home);
+
     g2.motors.slew_limit_throttle(true);
     return true;
 }
@@ -14,15 +20,24 @@ void ModeRTL::update()
     if (!rover.in_auto_reverse) {
         rover.set_reverse(false);
     }
-    calc_lateral_acceleration();
-    calc_nav_steer();
-    calc_throttle(g.speed_cruise);
-}
 
-void ModeRTL::update_navigation()
+    if (!_reached_destination) {
-{
+        // calculate distance to home
-    // no loitering around the wp with the rover, goes direct to the wp position
+        _distance_to_destination = get_distance(rover.current_loc, _destination);
-    if (rover.verify_RTL()) {
+        // check if we've reached the destination
-        rover.set_mode(rover.mode_hold, MODE_REASON_MISSION_END);
+        if (_distance_to_destination <= rover.g.waypoint_radius || location_passed_point(rover.current_loc, _origin, _destination)) {
+            // trigger reached
+            _reached_destination = true;
+            gcs().send_text(MAV_SEVERITY_INFO, "Reached destination");
+        }
+        // continue driving towards destination
+        calc_lateral_acceleration(_origin, _destination);
+        calc_nav_steer();
+        calc_throttle(calc_reduced_speed_for_turn_or_distance(_desired_speed));
+    } else {
+        // we've reached destination so stop
+        g2.motors.set_throttle(g.throttle_min.get());
+        g2.motors.set_steering(0.0f);
+        lateral_acceleration = 0.0f;
     }
 }