RTL: cone: never climb more than to RTL_RETURN_ALT (#23558)

This is to prevent that a large NAV_ACC_RAD leads to very high return altitudes.

Signed-off-by: Silvan Fuhrer <[email protected]>

src/modules/navigator/rtl.cpp

@@ -530,13 +530,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.
@@ -551,7 +552,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()