diff --git a/Rover/mode_auto.cpp b/Rover/mode_auto.cpp
index c8bdfe82f5..552ced2db5 100644
--- a/Rover/mode_auto.cpp
+++ b/Rover/mode_auto.cpp
@@ -76,21 +76,15 @@ void ModeAuto::update()
     switch (_submode) {
         case Auto_WP:
         {
-            // check if we've reached the destination
-            if (!g2.wp_nav.reached_destination() || g2.wp_nav.is_fast_waypoint()) {
-                // update navigation controller
-                navigate_to_waypoint();
-            } else {
-                // we have reached the destination so stay here
-                if (rover.is_boat()) {
-                    if (!start_loiter()) {
-                        start_stop();
-                    }
-                } else {
-                    start_stop();
-                }
+            // boats loiter once the waypoint is reached
+            if (rover.is_boat() && g2.wp_nav.reached_destination() && !g2.wp_nav.is_fast_waypoint()) {
+                start_loiter();
+
                 // update distance to destination
                 _distance_to_destination = rover.current_loc.get_distance(g2.wp_nav.get_destination());
+            } else {
+                // update navigation controller
+                navigate_to_waypoint();
             }
             break;
         }