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()