From bbe35ea5192a0d95e362d89efc88dd9219b6dc2e Mon Sep 17 00:00:00 2001 From: bresch Date: Tue, 6 Aug 2024 13:29:40 +0200 Subject: [PATCH] RTL: do not RTL if RTL alt is above max HAGL --- src/modules/navigator/rtl_direct.cpp | 24 ++++++++++++++++++++++++ src/modules/navigator/rtl_direct.h | 2 ++ 2 files changed, 26 insertions(+) diff --git a/src/modules/navigator/rtl_direct.cpp b/src/modules/navigator/rtl_direct.cpp index b028c7fb358f..4b739d2942a4 100644 --- a/src/modules/navigator/rtl_direct.cpp +++ b/src/modules/navigator/rtl_direct.cpp @@ -59,6 +59,7 @@ RtlDirect::RtlDirect(Navigator *navigator) : _land_approach.lat = static_cast(NAN); _land_approach.lon = static_cast(NAN); _land_approach.height_m = NAN; + _print_return_alt_higher_than_max_hagl = false; } void RtlDirect::on_inactivation() @@ -91,6 +92,12 @@ void RtlDirect::on_activation() events::send(events::ID("vrtl_return_at"), events::Log::Info, "RTL: start return at {1m_v} ({2m_v} above destination)", (int32_t)ceilf(_rtl_alt), (int32_t)ceilf(_rtl_alt - _destination.alt)); + + if (_print_return_alt_higher_than_max_hagl) { + mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: return alt higher than max HAGL, landing\t"); + events::send(events::ID("rtl_fail_max_hagl"), events::Log::Warning, "RTL: return alt higher than max HAGL, landing"); + _print_return_alt_higher_than_max_hagl = false; + } } void RtlDirect::on_active() @@ -143,6 +150,23 @@ void RtlDirect::setRtlPosition(PositionYawSetpoint rtl_position, loiter_point_s _destination.alt = _home_pos_sub.get().alt; } + if (_global_pos_sub.get().terrain_alt_valid + && ((_rtl_alt - _global_pos_sub.get().terrain_alt) > _navigator->get_local_position()->hagl_max)) { + // Handle case where the RTL altidude is above the maximum HAGL and land in place instead of RTL + _print_return_alt_higher_than_max_hagl = true; + + _destination.lat = _global_pos_sub.get().lat; + _destination.lon = _global_pos_sub.get().lon; + _destination.alt = _global_pos_sub.get().terrain_alt; + _destination.yaw = NAN; + + _rtl_alt = _global_pos_sub.get().alt; + + loiter_pos.lat = _destination.lat; + loiter_pos.lon = _destination.lon; + loiter_pos.height_m = _destination.alt; + } + _land_approach = sanitizeLandApproach(loiter_pos); const float dist_to_destination{get_distance_to_next_waypoint(_land_approach.lat, _land_approach.lon, _destination.lat, _destination.lon)}; diff --git a/src/modules/navigator/rtl_direct.h b/src/modules/navigator/rtl_direct.h index 211b2779eae0..fa7ac7b33bf6 100644 --- a/src/modules/navigator/rtl_direct.h +++ b/src/modules/navigator/rtl_direct.h @@ -150,6 +150,8 @@ class RtlDirect : public MissionBlock, public ModuleParams float _rtl_alt{0.0f}; ///< AMSL altitude at which the vehicle should return to the home position + bool _print_return_alt_higher_than_max_hagl{false}; + DEFINE_PARAMETERS( (ParamFloat) _param_rtl_descend_alt, (ParamFloat) _param_rtl_land_delay,