Rover: boats stay active at destination in guided, rtl, smartrtl

APMrover2/mode_guided.cpp

@@ -21,16 +21,15 @@ void ModeGuided::update()
     switch (_guided_mode) {
         case Guided_WP:
         {
-            if (!_reached_destination) {
+            if (!_reached_destination || rover.is_boat()) {
                 // check if we've reached the destination
                 _distance_to_destination = get_distance(rover.current_loc, _destination);
-                if (_distance_to_destination <= rover.g.waypoint_radius || location_passed_point(rover.current_loc, _origin, _destination)) {
-                    // trigger reached
+                if (!_reached_destination && (_distance_to_destination <= rover.g.waypoint_radius || location_passed_point(rover.current_loc, _origin, _destination))) {
                     _reached_destination = true;
                     rover.gcs().send_mission_item_reached_message(0);
                 }
                 // drive towards destination
-                calc_steering_to_waypoint(_origin, _destination);
+                calc_steering_to_waypoint(_reached_destination ? rover.current_loc : _origin, _destination);
                 calc_throttle(calc_reduced_speed_for_turn_or_distance(_desired_speed), true);
             } else {
                 stop_vehicle();

APMrover2/mode_rtl.cpp

@@ -22,17 +22,17 @@ bool ModeRTL::_enter()
 
 void ModeRTL::update()
 {
-    if (!_reached_destination) {
+    if (!_reached_destination || rover.is_boat()) {
         // calculate distance to home
         _distance_to_destination = get_distance(rover.current_loc, _destination);
         // check if we've reached the destination
-        if (_distance_to_destination <= rover.g.waypoint_radius || location_passed_point(rover.current_loc, _origin, _destination)) {
+        if (!_reached_destination && (_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_steering_to_waypoint(_origin, _destination);
+        calc_steering_to_waypoint(_reached_destination ? rover.current_loc :_origin, _destination);
         calc_throttle(calc_reduced_speed_for_turn_or_distance(_desired_speed), true);
     } else {
         // we've reached destination so stop

APMrover2/mode_smart_rtl.cpp

@@ -17,6 +17,9 @@ bool ModeSmartRTL::_enter()
     // initialise waypoint speed
     set_desired_speed_to_default(true);
 
+    // init location target
+    set_desired_location(rover.current_loc);
+
     // RTL never reverses
     rover.set_reverse(false);
 
@@ -53,6 +56,7 @@ void ModeSmartRTL::update()
                 _load_point = false;
                 // set target destination to new point
                 if (!set_desired_location_NED(next_point)) {
+                    // this failure should never happen but we add it just in case
                     gcs().send_text(MAV_SEVERITY_INFO, "SmartRTL: failed to set destination");
                     smart_rtl_state = SmartRTL_Failure;
                 }
@@ -70,7 +74,14 @@ void ModeSmartRTL::update()
         case SmartRTL_StopAtHome:
         case SmartRTL_Failure:
             _reached_destination = true;
-            stop_vehicle();
+            if (rover.is_boat()) {
+                // boats attempt to hold position at home
+                calc_steering_to_waypoint(rover.current_loc, _destination);
+                calc_throttle(calc_reduced_speed_for_turn_or_distance(_desired_speed), true);
+            } else {
+                // rovers stop
+                stop_vehicle();
+            }
             break;
     }
 }