From f8740863f471946caa9a3af03727f63277dad46c Mon Sep 17 00:00:00 2001 From: RomanBapst Date: Mon, 28 Feb 2022 15:49:11 +0300 Subject: [PATCH] mission: improve in-air mission upload experience for VTOL - enforce active wayoint to be after the transition waypoint after uploading a new mission in fixed wing flight Signed-off-by: RomanBapst --- src/modules/navigator/mission.cpp | 45 +++++++++++++++++++++---------- src/modules/navigator/mission.h | 6 +++-- 2 files changed, 35 insertions(+), 16 deletions(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 77186d89f9..62e16571d7 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -140,8 +140,7 @@ Mission::on_inactive() _mission.count = mission_state.count; _current_mission_index = mission_state.current_seq; - // find and store landing start marker (if available) - find_mission_land_start(); + find_mission_land_start_and_first_fw_waypoint(); } /* On init let's check the mission, maybe there is already one available. */ @@ -403,16 +402,14 @@ Mission::set_execution_mode(const uint8_t mode) } } -bool -Mission::find_mission_land_start() +void +Mission::find_mission_land_start_and_first_fw_waypoint() { - /* return true if a MAV_CMD_DO_LAND_START, NAV_CMD_VTOL_LAND or NAV_CMD_LAND is found and internally save the index - * return false if not found - */ - const dm_item_t dm_current = (dm_item_t)_mission.dataman_id; struct mission_item_s missionitem = {}; struct mission_item_s missionitem_prev = {}; //to store mission item before currently checked on, needed to get pos of wp before NAV_CMD_DO_LAND_START + bool found_vtol_transition_to_fw = false; + _first_fixed_wing_waypoint_index = -1; _land_start_available = false; @@ -461,9 +458,19 @@ Mission::find_mission_land_start() } } - } - return _land_start_available; + { + // try to find the index of the first position waypoint after a potential vtol transition + if (found_vtol_transition_to_fw && _first_fixed_wing_waypoint_index < 0 && item_contains_position(missionitem)) { + _first_fixed_wing_waypoint_index = i; + } + + if ((!found_vtol_transition_to_fw && missionitem.nav_cmd == NAV_CMD_VTOL_TAKEOFF) || + (missionitem.nav_cmd == NAV_CMD_DO_VTOL_TRANSITION && int(missionitem.params[0]) == 4)) { + found_vtol_transition_to_fw = true; + } + } + } } bool @@ -570,8 +577,19 @@ Mission::update_mission() _current_mission_index = 0; } - // find and store landing start marker (if available) - find_mission_land_start(); + find_mission_land_start_and_first_fw_waypoint(); + + if (_navigator->get_vstatus()->arming_state == vehicle_status_s::ARMING_STATE_ARMED + && _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) { + + // if we are already in fixed wing flight then make sure that the current active waypoint + // is after the transition to forward flight waypoint + + if (_current_mission_index < _first_fixed_wing_waypoint_index) { + _current_mission_index = _first_fixed_wing_waypoint_index; + } + + } set_current_mission_item(); } @@ -1751,8 +1769,7 @@ Mission::check_mission_valid(bool force) _navigator->set_mission_result_updated(); _home_inited = _navigator->home_position_valid(); - // find and store landing start marker (if available) - find_mission_land_start(); + find_mission_land_start_and_first_fw_waypoint(); } } diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index 24dd8bf1c9..8148dcd1e9 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -221,9 +221,9 @@ private: bool need_to_reset_mission(); /** - * Find and store the index of the landing sequence (DO_LAND_START) + * Find and store the index of the landing sequence (DO_LAND_START) and the index of the first fixed wing waypoint after vtol transition. */ - bool find_mission_land_start(); + void find_mission_land_start_and_first_fw_waypoint(); /** * Return the index of the closest mission item to the current global position. @@ -254,6 +254,8 @@ private: double _landing_start_lon{0.0}; float _landing_start_alt{0.0f}; + int _first_fixed_wing_waypoint_index{-1}; // index of the first position waypoint after a transition to forward flight, < 0 is invalid + double _landing_lat{0.0}; double _landing_lon{0.0}; float _landing_alt{0.0f};