From cbf5d7116f8f85b38c33fe5df9e8c8e6535392fc Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Fri, 16 Aug 2024 16:44:02 +0200 Subject: [PATCH] RTL: cone: never climb more than to RTL_RETURN_ALT This is to prevent that a large NAV_ACC_RAD leads to very high return altitudes. Signed-off-by: Silvan Fuhrer --- src/modules/navigator/rtl.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index 6fef45f9df6e..e916f6ac9c75 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -528,13 +528,14 @@ float RTL::calculate_return_alt_from_cone_half_angle(const PositionYawSetpoint & // avoid the vehicle touching the ground while still moving horizontally. const float return_altitude_min_outside_acceptance_rad_amsl = rtl_position.alt + 2.0f * _param_nav_acc_rad.get(); - float return_altitude_amsl = rtl_position.alt + _param_rtl_return_alt.get(); + const float max_return_altitude = rtl_position.alt + _param_rtl_return_alt.get(); + + float return_altitude_amsl = max_return_altitude; if (destination_dist <= _param_nav_acc_rad.get()) { return_altitude_amsl = rtl_position.alt + 2.0f * destination_dist; } else { - if (destination_dist <= _param_rtl_min_dist.get()) { // constrain cone half angle to meaningful values. All other cases are already handled above. @@ -549,7 +550,7 @@ float RTL::calculate_return_alt_from_cone_half_angle(const PositionYawSetpoint & return_altitude_amsl = max(return_altitude_amsl, return_altitude_min_outside_acceptance_rad_amsl); } - return max(return_altitude_amsl, _global_pos_sub.get().alt); + return constrain(return_altitude_amsl, _global_pos_sub.get().alt, max_return_altitude); } void RTL::init_rtl_mission_type()