From 2091ca11c3868f174a3296081b2dcdcbd72a4eae Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Fri, 7 Feb 2025 00:16:24 +0000 Subject: [PATCH] Cotper: RTL correct height for `RTL_ALT_FINAL` > 0. --- ArduCopter/mode_rtl.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/ArduCopter/mode_rtl.cpp b/ArduCopter/mode_rtl.cpp index 8d7bba2dd3..1bc0b559cc 100644 --- a/ArduCopter/mode_rtl.cpp +++ b/ArduCopter/mode_rtl.cpp @@ -312,7 +312,7 @@ void ModeRTL::descent_run() attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.get_heading()); // check if we've reached within 20cm of final altitude - _state_complete = labs(rtl_path.descent_target.alt - copter.current_loc.alt) < 20; + _state_complete = labs(rtl_path.descent_target.alt - copter.inertial_nav.get_position_z_up_cm()) < 20; } // land_start - initialise controllers to loiter over home @@ -389,6 +389,9 @@ void ModeRTL::build_path() // descent target is below return target at rtl_alt_final rtl_path.descent_target = Location(rtl_path.return_target.lat, rtl_path.return_target.lng, g.rtl_alt_final, Location::AltFrame::ABOVE_HOME); + // Target altitude is passed directly to the position controller so must be relative to origin + rtl_path.descent_target.change_alt_frame(Location::AltFrame::ABOVE_ORIGIN); + // set land flag rtl_path.land = g.rtl_alt_final <= 0; }