diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 06b4151bdb9b..2949c88d62b3 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -216,6 +216,23 @@ void Mission::setActiveMissionItems() position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); const position_setpoint_s current_setpoint_copy = pos_sp_triplet->current; + /* Skip VTOL/FW Takeoff item if in air, fixed-wing and didn't start the takeoff already*/ + if ((_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF || _mission_item.nav_cmd == NAV_CMD_TAKEOFF) && + (_work_item_type == WorkItemType::WORK_ITEM_TYPE_DEFAULT) && + (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) && + !_land_detected_sub.get().landed) { + if (setNextMissionItem()) { + if (!loadCurrentMissionItem()) { + setEndOfMissionItems(); + return; + } + + } else { + setEndOfMissionItems(); + return; + } + } + if (item_contains_position(_mission_item)) { handleTakeoff(new_work_item_type, next_mission_items, num_found_items); diff --git a/src/modules/navigator/rtl_mission_fast.cpp b/src/modules/navigator/rtl_mission_fast.cpp index 1da091ce1bd1..0bcafd94e8fb 100644 --- a/src/modules/navigator/rtl_mission_fast.cpp +++ b/src/modules/navigator/rtl_mission_fast.cpp @@ -92,6 +92,23 @@ void RtlMissionFast::setActiveMissionItems() WorkItemType new_work_item_type{WorkItemType::WORK_ITEM_TYPE_DEFAULT}; position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + /* Skip VTOL/FW Takeoff item if in air, fixed-wing and didn't start the takeoff already*/ + if ((_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF || _mission_item.nav_cmd == NAV_CMD_TAKEOFF) && + (_work_item_type == WorkItemType::WORK_ITEM_TYPE_DEFAULT) && + (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) && + !_land_detected_sub.get().landed) { + if (setNextMissionItem()) { + if (!loadCurrentMissionItem()) { + setEndOfMissionItems(); + return; + } + + } else { + setEndOfMissionItems(); + return; + } + } + // Transition to fixed wing if necessary. if (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && _vehicle_status_sub.get().is_vtol &&