From f60bb2c6bc9dd347cf19a72b606535a8852779b1 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Mon, 23 Sep 2024 18:33:03 +0200 Subject: [PATCH] rtl_direct: skip loiter altitude items for multicopter without RTL land delay (#23689) This was already done before the refactor and was assumed to not be necessary. The problem is that these mission items result in a strang looking vertical slow down directly followed by an acceleration during the landing phase. --- src/modules/navigator/rtl_direct.cpp | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/src/modules/navigator/rtl_direct.cpp b/src/modules/navigator/rtl_direct.cpp index b28d7976af..f255a9f11d 100644 --- a/src/modules/navigator/rtl_direct.cpp +++ b/src/modules/navigator/rtl_direct.cpp @@ -157,6 +157,10 @@ void RtlDirect::setRtlPosition(PositionYawSetpoint rtl_position, loiter_point_s void RtlDirect::_updateRtlState() { + // RTL_LAND_DELAY > 0 -> wait seconds, < 0 wait indefinitely + const bool wait_at_rtl_descend_alt = fabsf(_param_rtl_land_delay.get()) > FLT_EPSILON; + const bool is_multicopter = (_vehicle_status_sub.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING); + RTLState new_state{RTLState::IDLE}; switch (_rtl_state) { @@ -165,7 +169,13 @@ void RtlDirect::_updateRtlState() break; case RTLState::MOVE_TO_LOITER: - new_state = RTLState::LOITER_DOWN; + if (!is_multicopter || wait_at_rtl_descend_alt) { + new_state = RTLState::LOITER_DOWN; + + } else { + new_state = RTLState::LAND; + } + break; case RTLState::LOITER_DOWN: @@ -206,7 +216,6 @@ void RtlDirect::_updateRtlState() } _rtl_state = new_state; - }