From 02e50bb5dc133014d5a18ee89947f41c18e4fb9f Mon Sep 17 00:00:00 2001 From: Konrad Date: Mon, 23 Sep 2024 10:03:50 +0200 Subject: [PATCH] rtl_direct: loiter hold should track altitude as best effort but not enforce it --- src/modules/navigator/rtl_direct.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/src/modules/navigator/rtl_direct.cpp b/src/modules/navigator/rtl_direct.cpp index 36a6e2243b..7050abacd7 100644 --- a/src/modules/navigator/rtl_direct.cpp +++ b/src/modules/navigator/rtl_direct.cpp @@ -220,6 +220,8 @@ void RtlDirect::set_rtl_item() const bool is_close_to_destination = destination_dist < _param_rtl_min_dist.get(); + float altitude_acceptance_radius = static_cast(NAN); + switch (_rtl_state) { case RTLState::CLIMBING: { PositionYawSetpoint pos_yaw_sp { @@ -293,6 +295,11 @@ void RtlDirect::set_rtl_item() if (_param_rtl_land_delay.get() < -FLT_EPSILON) { mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: completed, loitering\t"); events::send(events::ID("rtl_completed_loiter"), events::Log::Info, "RTL: completed, loitering"); + + } else { + /* Set the altitude tracking to best effort but not strictly enforce it */ + altitude_acceptance_radius = FLT_MAX; + } break; @@ -370,6 +377,7 @@ void RtlDirect::set_rtl_item() } else { // Convert mission item to current position setpoint and make it valid. if (mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current)) { + pos_sp_triplet->current.alt_acceptance_radius = altitude_acceptance_radius; _navigator->set_position_setpoint_triplet_updated(); } }