fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-06 11:28:52 +08:00
Code Issues Packages Projects Releases Wiki Activity

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

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

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer
2024-08-19 07:51:33 +02:00
committed by GitHub
parent ea0ef154d8
commit 435e9665b3
1 changed files with 4 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/rtl.cpp
+4 -3
View File
@@ -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()