From e81d5daa66c8307050bce23bf11458f0fc53d3e6 Mon Sep 17 00:00:00 2001 From: RomanBapst Date: Thu, 26 Mar 2020 15:37:58 +0300 Subject: [PATCH] rtl: if very close to home then do not descend if higher than RTL_DESCEND_ALT before reaching home Signed-off-by: RomanBapst --- src/modules/navigator/rtl.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index 570275f805..afbd25b501 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -516,7 +516,8 @@ float RTL::calculate_return_alt_from_cone_half_angle(float cone_half_angle_deg) float rtl_altitude; if (destination_dist <= _param_rtl_min_dist.get()) { - rtl_altitude = _destination.alt + _param_rtl_descend_alt.get(); + // we are very close to home, make sure we are above the RTL descend altitude + rtl_altitude = math::max(_destination.alt + _param_rtl_descend_alt.get(), gpos.alt); } else if (gpos.alt > _destination.alt + _param_rtl_return_alt.get() || cone_half_angle_deg >= 90.0f) { rtl_altitude = gpos.alt;