From 623b1fc4fe9ccf2441d848667734ec4e2fffd0a7 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 23 Jul 2024 21:43:59 +0900 Subject: [PATCH] Copter: SmartRTL mode restores point if interrupted --- ArduCopter/mode.h | 4 ++++ ArduCopter/mode_smart_rtl.cpp | 16 +++++++++++++++- 2 files changed, 19 insertions(+), 1 deletion(-) diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index dd06ee0149..86eeed0b15 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -1507,6 +1507,10 @@ private: // point while following our path home. If we take too long we // may choose to land the vehicle. uint32_t path_follow_last_pop_fail_ms; + + // backup last popped point so that it can be restored to the path + // if vehicle exits SmartRTL mode before reaching home. invalid if zero + Vector3f dest_NED_backup; }; diff --git a/ArduCopter/mode_smart_rtl.cpp b/ArduCopter/mode_smart_rtl.cpp index dacd7cc5f2..8c2b1988c3 100644 --- a/ArduCopter/mode_smart_rtl.cpp +++ b/ArduCopter/mode_smart_rtl.cpp @@ -35,6 +35,14 @@ bool ModeSmartRTL::init(bool ignore_checks) // perform cleanup required when leaving smart_rtl void ModeSmartRTL::exit() { + // restore last point if we hadn't reached it + if (smart_rtl_state == SubMode::PATH_FOLLOW && !dest_NED_backup.is_zero()) { + if (!g2.smart_rtl.add_point(dest_NED_backup)) { + GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "SmartRTL: lost one point"); + } + } + dest_NED_backup.zero(); + g2.smart_rtl.cancel_request_for_thorough_cleanup(); } @@ -83,10 +91,16 @@ void ModeSmartRTL::path_follow_run() { // if we are close to current target point, switch the next point to be our target. if (wp_nav->reached_wp_destination()) { - Vector3f dest_NED; + + // clear destination backup so that it cannot be restored + dest_NED_backup.zero(); + // this pop_point can fail if the IO task currently has the // path semaphore. + Vector3f dest_NED; if (g2.smart_rtl.pop_point(dest_NED)) { + // backup destination in case we exit smart_rtl mode and need to restore it to the path + dest_NED_backup = dest_NED; path_follow_last_pop_fail_ms = 0; if (g2.smart_rtl.get_num_points() == 0) { // this is the very last point, add 2m to the target alt and move to pre-land state