Copter: SmartRTL mode restores point if interrupted

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;
 };
 
 

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