From 3bd114bf9f9c1e0c3dcb175ad9276d67299c85da Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Mon, 25 Oct 2021 15:32:28 +0200 Subject: [PATCH] RTL: if vehicle is a VTOL in hover, always consider all safe landing points Unless there is already a mission landing in progress, in that case stay on the mission landing if RTL_TYPE isn't set to closest. Signed-off-by: Silvan Fuhrer --- src/modules/navigator/rtl.cpp | 19 +++++++++++++++---- src/modules/navigator/rtl_params.c | 2 +- 2 files changed, 16 insertions(+), 5 deletions(-) diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index de7330560b..188920e726 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -132,18 +132,29 @@ void RTL::find_RTL_destination() 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; dlon = mission_landing_lon - global_position.lon; 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 && !vtol_in_rw_mode)) { + // always find closest destination if in hover and VTOL + if (_param_rtl_type.get() == RTL_TYPE_CLOSEST || (vtol_in_rw_mode && !_navigator->getMissionLandingInProgress())) { + + // compare home position to landing position to decide which is closer + if (dist_squared < min_dist_squared) { + _destination.type = RTL_DESTINATION_MISSION_LANDING; + min_dist_squared = dist_squared; + _destination.lat = mission_landing_lat; + _destination.lon = mission_landing_lon; + _destination.alt = mission_landing_alt; + } + + } else { + // it has to be the mission landing + _destination.type = RTL_DESTINATION_MISSION_LANDING; min_dist_squared = dist_squared; _destination.lat = mission_landing_lat; _destination.lon = mission_landing_lon; _destination.alt = mission_landing_alt; - _destination.type = RTL_DESTINATION_MISSION_LANDING;; } } diff --git a/src/modules/navigator/rtl_params.c b/src/modules/navigator/rtl_params.c index 23f73dbdbe..b12479d7c0 100644 --- a/src/modules/navigator/rtl_params.c +++ b/src/modules/navigator/rtl_params.c @@ -111,7 +111,7 @@ PARAM_DEFINE_FLOAT(RTL_MIN_DIST, 10.0f); * Return mode destination and flight path (home location, rally point, mission landing pattern, reverse mission) * * @value 0 Return to closest safe point (home or rally point) via direct path. - * @value 1 Return to closest safe point other than home (mission landing pattern or rally point), via direct path. If no mission landing or rally points are defined return home via direct path. + * @value 1 Return to closest safe point other than home (mission landing pattern or rally point), via direct path. If no mission landing or rally points are defined return home via direct path. Always chose closes safe landing point if vehicle is a VTOL in hover mode. * @value 2 Return to a planned mission landing, if available, using the mission path, else return to home via the reverse mission path. Do not consider rally points. * @value 3 Return via direct path to closest destination: home, start of mission landing pattern or safe point. If the destination is a mission landing pattern, follow the pattern to land. * @group Return Mode