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(); } }