From dbf7d32e07887a402bdd72e092422a4cf3b2ae9d Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Tue, 2 Aug 2022 16:34:42 +0300 Subject: [PATCH] Skip VTOL_TAKEOFF mission item when in fixed wing mode (#19985) * mission: skip VTOL_TAKEOFF mission item when in fixed wing mode Signed-off-by: RomanBapst * mission: added better comment regarding skipping VTOL Takeoff in fw mode Signed-off-by: RomanBapst --- src/modules/navigator/mission.cpp | 11 +++-------- src/modules/navigator/mission_block.cpp | 7 +++++++ 2 files changed, 10 insertions(+), 8 deletions(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 3f8951e7c9..c0d764319a 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -820,15 +820,10 @@ Mission::set_mission_items() } else if (_mission_item.nav_cmd == NAV_CMD_VTOL_TAKEOFF && _work_item_type == WORK_ITEM_TYPE_DEFAULT && new_work_item_type == WORK_ITEM_TYPE_DEFAULT) { + // if the vehicle is already in fixed wing mode then the current mission item + // will be accepted immediately and the work items will be skipped + _work_item_type = WORK_ITEM_TYPE_TAKEOFF; - if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { - /* haven't transitioned yet, trigger vtol takeoff logic below */ - _work_item_type = WORK_ITEM_TYPE_TAKEOFF; - - } else { - /* already in fixed-wing, go to waypoint */ - _mission_item.nav_cmd = NAV_CMD_WAYPOINT; - } /* ignore yaw here, otherwise it might yaw before heading_sp_update takes over */ _mission_item.yaw = NAN; diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 903948e263..d04741776f 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -129,6 +129,13 @@ MissionBlock::is_mission_item_reached() return false; } + case NAV_CMD_VTOL_TAKEOFF: + if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { + return true; + } + + break; + case NAV_CMD_DELAY: // Set reached flags directly such that only the delay time is considered _waypoint_position_reached = true;