diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index f11da4d1c1..be203555f6 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -391,6 +391,10 @@ Mission::find_mission_land_start() 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 + _land_start_available = false; + + bool found_land_start_marker = false; + for (size_t i = 1; i < _mission.count; i++) { const ssize_t len = sizeof(missionitem); missionitem_prev = missionitem; // store the last mission item before reading a new one @@ -402,36 +406,41 @@ Mission::find_mission_land_start() } if (missionitem.nav_cmd == NAV_CMD_DO_LAND_START) { - _land_start_available = true; + found_land_start_marker = true; _land_start_index = i; } - if (_land_start_available && i > _land_start_index && item_contains_position(missionitem)) { + if (found_land_start_marker && !_land_start_available && i > _land_start_index + && item_contains_position(missionitem)) { // use the position of any waypoint after the land start marker which specifies a position. - _landing_lat = missionitem.lat; - _landing_lon = missionitem.lon; - _landing_alt = missionitem.altitude_is_relative ? missionitem.altitude + - _navigator->get_home_position()->alt : missionitem.altitude; - - return true; - - // if no DO_LAND_START marker available, also check for VTOL_LAND or normal LAND - - } else if (((missionitem.nav_cmd == NAV_CMD_VTOL_LAND) && _navigator->get_vstatus()->is_vtol) || - (missionitem.nav_cmd == NAV_CMD_LAND)) { - + _landing_start_lat = missionitem.lat; + _landing_start_lon = missionitem.lon; + _landing_start_alt = missionitem.altitude_is_relative ? missionitem.altitude + + _navigator->get_home_position()->alt : missionitem.altitude; _land_start_available = true; - _land_start_index = i; + } + + if (((missionitem.nav_cmd == NAV_CMD_VTOL_LAND) && _navigator->get_vstatus()->is_vtol) || + (missionitem.nav_cmd == NAV_CMD_LAND)) { + _landing_lat = missionitem.lat; _landing_lon = missionitem.lon; _landing_alt = missionitem.altitude_is_relative ? missionitem.altitude + _navigator->get_home_position()->alt : missionitem.altitude; - return true; + + // don't have a valid land start yet, use the landing item itself then + if (!_land_start_available) { + _land_start_index = i; + _landing_start_lat = _landing_lat; + _landing_start_lon = _landing_lon; + _landing_start_alt = _landing_alt; + _land_start_available = true; + } + } } - _land_start_available = false; - return false; + return _land_start_available; } bool diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index c8b59450e8..a8029e0abe 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -87,6 +87,10 @@ public: bool get_mission_finished() const { return _mission_type == MISSION_TYPE_NONE; } bool get_mission_changed() const { return _mission_changed ; } bool get_mission_waypoints_changed() const { return _mission_waypoints_changed ; } + double get_landing_start_lat() { return _landing_start_lat; } + double get_landing_start_lon() { return _landing_start_lon; } + float get_landing_start_alt() { return _landing_start_alt; } + double get_landing_lat() { return _landing_lat; } double get_landing_lon() { return _landing_lon; } float get_landing_alt() { return _landing_alt; } @@ -249,6 +253,10 @@ private: // track location of planned mission landing bool _land_start_available{false}; uint16_t _land_start_index{UINT16_MAX}; /**< index of DO_LAND_START, INVALID_DO_LAND_START if no planned landing */ + double _landing_start_lat{0.0}; + double _landing_start_lon{0.0}; + float _landing_start_alt{0.0f}; + double _landing_lat{0.0}; double _landing_lon{0.0}; float _landing_alt{0.0f}; diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 05bb60a831..87a107c8ec 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -278,6 +278,10 @@ public: bool start_mission_landing() { return _mission.land_start(); } bool get_mission_start_land_available() { return _mission.get_land_start_available(); } int get_mission_landing_index() { return _mission.get_land_start_index(); } + double get_mission_landing_start_lat() { return _mission.get_landing_start_lat(); } + double get_mission_landing_start_lon() { return _mission.get_landing_start_lon(); } + float get_mission_landing_start_alt() { return _mission.get_landing_start_alt(); } + double get_mission_landing_lat() { return _mission.get_landing_lat(); } double get_mission_landing_lon() { return _mission.get_landing_lon(); } float get_mission_landing_alt() { return _mission.get_landing_alt(); } @@ -399,6 +403,7 @@ private: float _mission_cruising_speed_fw{-1.0f}; float _mission_throttle{NAN}; + bool _mission_landing_in_progress{false}; // this flag gets set if the mission is currently executing on a landing pattern // if mission mode is inactive, this flag will be cleared after 2 seconds diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 0c132fb4b8..4201eaf54b 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -563,8 +563,8 @@ Navigator::run() } - - if (!rtl_activated && _rtl.getClimbAndReturnDone() && get_mission_start_land_available()) { + if (!rtl_activated && !_rtl.denyMissionLanding() && _rtl.getClimbAndReturnDone() + && get_mission_start_land_available()) { _mission.set_execution_mode(mission_result_s::MISSION_EXECUTION_MODE_FAST_FORWARD); if (!getMissionLandingInProgress() && _vstatus.arming_state == vehicle_status_s::ARMING_STATE_ARMED diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index f46b61e5a3..20de4b8d27 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -107,10 +107,28 @@ void RTL::find_RTL_destination() _destination.type = RTL_DESTINATION_HOME; + const bool vtol_in_rw_mode = _navigator->get_vstatus()->is_vtol + && _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING; + + // consider the mission landing if not RTL_HOME type set if (rtl_type() != RTL_HOME && _navigator->get_mission_start_land_available()) { - double mission_landing_lat = _navigator->get_mission_landing_lat(); - double mission_landing_lon = _navigator->get_mission_landing_lon(); + double mission_landing_lat; + double mission_landing_lon; + float mission_landing_alt; + RTLDestinationType destination_type = RTL_DESTINATION_MISSION_LANDING; + + if (vtol_in_rw_mode) { + mission_landing_lat = _navigator->get_mission_landing_lat(); + mission_landing_lon = _navigator->get_mission_landing_lon(); + mission_landing_alt = _navigator->get_mission_landing_alt(); + destination_type = RTL_DESTINATION_HOME; + + } else { + mission_landing_lat = _navigator->get_mission_landing_start_lat(); + mission_landing_lon = _navigator->get_mission_landing_start_lon(); + mission_landing_alt = _navigator->get_mission_landing_start_alt(); + } // compare home position to landing position to decide which is closer dlat = mission_landing_lat - global_position.lat; @@ -120,10 +138,11 @@ void RTL::find_RTL_destination() // set destination to mission landing if closest or in RTL_LAND or RTL_MISSION (so not in RTL_CLOSEST) if (dist_squared < min_dist_squared || rtl_type() != RTL_CLOSEST) { min_dist_squared = dist_squared; - _destination.lat = _navigator->get_mission_landing_lat(); - _destination.lon = _navigator->get_mission_landing_lon(); - _destination.alt = _navigator->get_mission_landing_alt(); - _destination.type = RTL_DESTINATION_MISSION_LANDING; + _destination.lat = mission_landing_lat; + _destination.lon = mission_landing_lon; + _destination.alt = mission_landing_alt; + _destination.type = destination_type; + } } @@ -198,6 +217,9 @@ void RTL::find_RTL_destination() void RTL::on_activation() { + _deny_mission_landing = _navigator->get_vstatus()->is_vtol + && _navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING; + // output the correct message, depending on where the RTL destination is switch (_destination.type) { case RTL_DESTINATION_HOME: @@ -213,8 +235,6 @@ void RTL::on_activation() break; } - - const vehicle_global_position_s &global_position = *_navigator->get_global_position(); if (_navigator->get_vstatus()->vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { @@ -316,7 +336,6 @@ void RTL::set_rtl_item() } case RTL_STATE_RETURN: { - // Don't change altitude. _mission_item.nav_cmd = NAV_CMD_WAYPOINT; _mission_item.lat = _destination.lat; diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h index eff35e4be1..7439438f1b 100644 --- a/src/modules/navigator/rtl.h +++ b/src/modules/navigator/rtl.h @@ -87,6 +87,8 @@ public: bool getClimbAndReturnDone() { return _climb_and_return_done; } + bool denyMissionLanding() { return _deny_mission_landing; } + private: /** * Set the RTL item @@ -139,6 +141,7 @@ private: float _rtl_alt{0.0f}; // AMSL altitude at which the vehicle should return to the home position bool _rtl_alt_min{false}; bool _climb_and_return_done{false}; // this flag is set to true if RTL is active and we are past the climb state and return state + bool _deny_mission_landing{false}; DEFINE_PARAMETERS( (ParamFloat) _param_rtl_return_alt,