From 9d91b6c3bea6086094e8792dddf371098db6174d Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 9 Feb 2021 12:51:38 +0900 Subject: [PATCH] Copter: smart-rtl fix for final point altitude --- ArduCopter/mode_smart_rtl.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/ArduCopter/mode_smart_rtl.cpp b/ArduCopter/mode_smart_rtl.cpp index 36d3b6ff00..7932bbc75c 100644 --- a/ArduCopter/mode_smart_rtl.cpp +++ b/ArduCopter/mode_smart_rtl.cpp @@ -107,6 +107,10 @@ void ModeSmartRTL::path_follow_run() Vector3f next_dest_NED; if (g2.smart_rtl.peek_point(next_dest_NED)) { wp_nav->set_wp_destination_NED(dest_NED); + if (g2.smart_rtl.get_num_points() == 1) { + // this is the very last point, add 2m to the target alt + next_dest_NED.z -= 2.0f; + } wp_nav->set_wp_destination_next_NED(next_dest_NED); } else { // this should never happen but send next point anyway