From f4f9b8ebf2ef1236765ea29758649d9ea22444c2 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Fri, 22 Oct 2021 17:28:34 +0200 Subject: [PATCH] RTL: replace deny_mission_landing logic by getDestinationTypeMissionLanding() The rtl instance decides on destination of RTL, and Navigator then switches to mission mode if required. Signed-off-by: Silvan Fuhrer --- src/modules/navigator/navigator_main.cpp | 4 ++-- src/modules/navigator/rtl.cpp | 6 +----- src/modules/navigator/rtl.h | 6 +++--- 3 files changed, 6 insertions(+), 10 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index c823571bd5..9e910591c2 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -589,8 +589,8 @@ Navigator::run() } - if (!rtl_activated && !_rtl.denyMissionLanding() && _rtl.getClimbAndReturnDone() - && get_mission_start_land_available()) { + if (!rtl_activated && _rtl.getClimbAndReturnDone() + && _rtl.getDestinationTypeMissionLanding()) { _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 ea641521b0..de7330560b 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -138,7 +138,7 @@ void RTL::find_RTL_destination() double dist_squared = coord_dist_sq(dlat, dlon); // set destination to mission landing if closest or in RTL_TYPE_MISSION_LANDING or RTL_TYPE_MISSION_LANDING_REVERSED (so not in RTL_TYPE_CLOSEST) - if (dist_squared < min_dist_squared || _param_rtl_type.get() != RTL_TYPE_CLOSEST) { + if (dist_squared < min_dist_squared || (_param_rtl_type.get() != RTL_TYPE_CLOSEST && !vtol_in_rw_mode)) { min_dist_squared = dist_squared; _destination.lat = mission_landing_lat; _destination.lon = mission_landing_lon; @@ -235,10 +235,6 @@ 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: diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h index 6c2e85d34c..a7a48cb115 100644 --- a/src/modules/navigator/rtl.h +++ b/src/modules/navigator/rtl.h @@ -97,10 +97,11 @@ public: bool getClimbAndReturnDone() { return _climb_and_return_done; } - bool denyMissionLanding() { return _deny_mission_landing; } - void get_rtl_xy_z_speed(float &xy, float &z); matrix::Vector2f get_wind(); + + bool getDestinationTypeMissionLanding() { return _destination.type == RTL_DESTINATION_MISSION_LANDING; } + private: void set_rtl_item(); @@ -149,7 +150,6 @@ private: float _rtl_loiter_rad{50.0f}; // radius at which a fixed wing would loiter while descending 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}; bool _rtl_alt_min{false}; DEFINE_PARAMETERS(