From 97f632a4087d377181305da8559f809f1a87b0b7 Mon Sep 17 00:00:00 2001 From: RomanBapst Date: Mon, 4 Jul 2022 12:27:27 +0300 Subject: [PATCH] vtol_takeoff: reset reposition triplet before handing over to loiter mode Signed-off-by: RomanBapst --- src/modules/navigator/vtol_takeoff.cpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/src/modules/navigator/vtol_takeoff.cpp b/src/modules/navigator/vtol_takeoff.cpp index 9f642d1acf43..acfb64abffb8 100644 --- a/src/modules/navigator/vtol_takeoff.cpp +++ b/src/modules/navigator/vtol_takeoff.cpp @@ -142,6 +142,13 @@ VtolTakeoff::on_active() case vtol_takeoff_state::CLIMB: { + // reset any potentially valid reposition triplet which was not handled + // we do this to avoid random loiter locations after switching to loiter mode after this + position_setpoint_triplet_s *reposition_triplet = _navigator->get_reposition_triplet(); + _navigator->reset_position_setpoint(reposition_triplet->previous); + _navigator->reset_position_setpoint(reposition_triplet->current); + _navigator->reset_position_setpoint(reposition_triplet->next); + // the VTOL takeoff is done, proceed loitering and upate the navigation state to LOITER _navigator->get_mission_result()->finished = true; _navigator->set_mission_result_updated();