From 3d342604f8b3680d4c3676bb338490c6460628d4 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 24 Nov 2023 10:10:11 +0900 Subject: [PATCH] Rover: auto navigates while stopped --- Rover/mode_auto.cpp | 20 +++++++------------- 1 file changed, 7 insertions(+), 13 deletions(-) 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; }