From 97b5e4083a8e2eb55572df40bcd64b7d08377b8f Mon Sep 17 00:00:00 2001 From: Tom Pittenger Date: Fri, 25 Oct 2024 20:17:12 -0700 Subject: [PATCH] Plane: trajectory loiter last point --- ArduPlane/mode_guided.cpp | 15 ++++++++++----- 1 file changed, 10 insertions(+), 5 deletions(-) diff --git a/ArduPlane/mode_guided.cpp b/ArduPlane/mode_guided.cpp index 5299c6313e9c8..d4a8dd8b789d7 100644 --- a/ArduPlane/mode_guided.cpp +++ b/ArduPlane/mode_guided.cpp @@ -123,16 +123,21 @@ void ModeGuided::navigate_trajectory() const bool wp_has_been_reached = plane.verify_nav_wp(mission_cmd); if (wp_has_been_reached) { - // we have reached it, remove the first index - trajectory.pop_front(); + // we have reached it. plane.gcs().send_mission_item_reached_message(0); + // TODO: notify AP_DDS of waypoint reached + + // remove it from the list + const Location prev_loc = trajectory.front(); + trajectory.pop_front(); + if (is_doing_trajectory()) { - // start the next one + // there are more point(s), start the next one plane.do_nav_wp(trajectory_to_mission_cmd()); } else { - // act as if we just entered Guided and loiter acount current point - _enter(); + // act as if we just entered Guided and loiter around the last waypoint + plane.set_guided_WP(prev_loc); } } }