diff --git a/APMrover2/mode_guided.cpp b/APMrover2/mode_guided.cpp index 2a5591b157..ccd4db6bb5 100644 --- a/APMrover2/mode_guided.cpp +++ b/APMrover2/mode_guided.cpp @@ -31,8 +31,11 @@ void ModeGuided::update() calc_steering_to_waypoint(_reached_destination ? rover.current_loc : _origin, _destination, _reversed); calc_throttle(calc_reduced_speed_for_turn_or_distance(_reversed ? -_desired_speed : _desired_speed), true, true); } else { - if (rover.is_boat() && !start_loiter()) { - stop_vehicle(); + // we have reached the destination so stay here + if (rover.is_boat()) { + if (!start_loiter()) { + stop_vehicle(); + } } else { stop_vehicle(); } @@ -52,8 +55,11 @@ void ModeGuided::update() calc_steering_to_heading(_desired_yaw_cd); calc_throttle(calc_reduced_speed_for_turn_or_distance(_desired_speed), true, true); } else { - if (rover.is_boat() && !start_loiter()) { - stop_vehicle(); + // we have reached the destination so stay here + if (rover.is_boat()) { + if (!start_loiter()) { + stop_vehicle(); + } } else { stop_vehicle(); } @@ -77,8 +83,11 @@ void ModeGuided::update() g2.motors.set_steering(steering_out * 4500.0f); calc_throttle(_desired_speed, true, true); } else { - if (rover.is_boat() && !start_loiter()) { - stop_vehicle(); + // we have reached the destination so stay here + if (rover.is_boat()) { + if (!start_loiter()) { + stop_vehicle(); + } } else { stop_vehicle(); }