fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-01 04:04:07 +08:00
Code Issues Packages Projects Releases Wiki Activity

rtl: if very close to home then do not descend if higher than RTL_DESCEND_ALT

Browse Source 
before reaching home

Signed-off-by: RomanBapst <bapstroman@gmail.com>
This commit is contained in:
RomanBapst 2020-03-26 15:37:58 +03:00 committed by Lorenz Meier
parent 7c727edc3f
commit e81d5daa66
1 changed files with 2 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
src/modules/navigator/rtl.cpp
View File

@ -516,7 +516,8 @@ float RTL::calculate_return_alt_from_cone_half_angle(float cone_half_angle_deg)
float rtl_altitude;
if (destination_dist <= _param_rtl_min_dist.get()) {
rtl_altitude = _destination.alt + _param_rtl_descend_alt.get();
// we are very close to home, make sure we are above the RTL descend altitude
rtl_altitude = math::max(_destination.alt + _param_rtl_descend_alt.get(), gpos.alt);
} else if (gpos.alt > _destination.alt + _param_rtl_return_alt.get() || cone_half_angle_deg >= 90.0f) {
rtl_altitude = gpos.alt;