From 9499b32be6ce20846b758179ef8cb3a9d6dc5521 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Thu, 13 Mar 2025 13:15:48 +0100 Subject: [PATCH] Revert "Takeoff: remove set loiter logic, as it is handled in loiter.cpp already (#24454)" This reverts commit 8547ebb8638481b580e6741dc08f37dce1934c23. --- src/modules/navigator/takeoff.cpp | 19 +++++++++++++++++++ 1 file changed, 19 insertions(+) diff --git a/src/modules/navigator/takeoff.cpp b/src/modules/navigator/takeoff.cpp index c151369011..d9b22720f3 100644 --- a/src/modules/navigator/takeoff.cpp +++ b/src/modules/navigator/takeoff.cpp @@ -69,6 +69,25 @@ Takeoff::on_active() _navigator->get_mission_result()->finished = true; _navigator->set_mission_result_updated(); _navigator->mode_completed(getNavigatorStateId()); + + position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + + // set loiter item so position controllers stop doing takeoff logic + if (_navigator->get_land_detected()->landed) { + _mission_item.nav_cmd = NAV_CMD_IDLE; + + } else { + if (pos_sp_triplet->current.valid) { + setLoiterItemFromCurrentPositionSetpoint(&_mission_item); + + } else { + setLoiterItemFromCurrentPosition(&_mission_item); + } + } + + mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current); + + _navigator->set_position_setpoint_triplet_updated(); } }