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;