From 09f066a73ae1eb9e1d39afc1c444a1f3264b0a17 Mon Sep 17 00:00:00 2001 From: KonradRudin <98741601+KonradRudin@users.noreply.github.com> Date: Tue, 25 Jun 2024 16:33:45 +0200 Subject: [PATCH] mission: skip a vtol takoff mission item if already in air (#23319) * mission: skip a vtol takoff mission item if already in air and a fixed wing * MissionBase: also skip FW takeoff when already in-air Signed-off-by: Silvan Fuhrer * mission: use setNextMissionItem to skip vtol takeoff when already in air * mission: Only skip the VTOL takeoff in air for mission and rtl mission If flying RTL mission reverse it must still include the takeoff point. --------- Signed-off-by: Silvan Fuhrer Co-authored-by: Silvan Fuhrer --- src/modules/navigator/mission.cpp | 17 +++++++++++++++++ src/modules/navigator/rtl_mission_fast.cpp | 17 +++++++++++++++++ 2 files changed, 34 insertions(+) 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 &&