From 94d3ab28eedadbd42310154d239252bd46718632 Mon Sep 17 00:00:00 2001 From: RomanBapst Date: Fri, 23 Oct 2020 10:33:19 +0300 Subject: [PATCH] rtl: fixed computation of return altitude when mission landing is available 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 d51b817af9..c4d1d04056 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -218,7 +218,8 @@ void RTL::on_activation() _rtl_alt = calculate_return_alt_from_cone_half_angle((float)_param_rtl_cone_half_angle_deg.get()); } else { - _rtl_alt = math::max(global_position.alt, _destination.alt + _param_rtl_return_alt.get()); + _rtl_alt = max(global_position.alt, max(_destination.alt, + _navigator->get_home_position()->alt + _param_rtl_return_alt.get())); } if (_navigator->get_land_detected()->landed) {