Rover: auto navigates while stopped

ArduPilot · Nov 24, 2023 · 515cabb · 515cabb
1 parent 48d040f
commit 515cabb
Showing 1 changed file with 7 additions and 13 deletions.
diff --git a/Rover/mode_auto.cpp b/Rover/mode_auto.cpp
@@ -82,21 +82,15 @@ void ModeAuto::update()
     switch (_submode) {
         case SubMode::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;
         }