From 4c6bccc8d5031fce6ff003443487cfde649109d9 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Mon, 15 Nov 2021 19:27:35 +0900
Subject: [PATCH] Rover: incorporate mission change detector

---
 Rover/mode.h        |  4 ++++
 Rover/mode_auto.cpp | 16 ++++++++++++++++
 2 files changed, 20 insertions(+)

diff --git a/Rover/mode.h b/Rover/mode.h
index c44b15bf28..94a45cfdb9 100644
--- a/Rover/mode.h
+++ b/Rover/mode.h
@@ -5,6 +5,7 @@
 #include <GCS_MAVLink/GCS_MAVLink.h>
 #include <AP_Math/AP_Math.h>
 #include <AP_Mission/AP_Mission.h>
+#include <AP_Mission/AP_Mission_ChangeDetector.h>
 #include <AR_WPNav/AR_WPNav.h>
 
 #include "defines.h"
@@ -383,6 +384,9 @@ private:
         float arg2;         // 2nd argument provided by mission command
     } nav_scripting;
 #endif
+
+    // Mission change detector
+    AP_Mission_ChangeDetector mis_change_detector;
 };
 
 
diff --git a/Rover/mode_auto.cpp b/Rover/mode_auto.cpp
index 9afbb234b4..67ee482808 100644
--- a/Rover/mode_auto.cpp
+++ b/Rover/mode_auto.cpp
@@ -53,8 +53,24 @@ void ModeAuto::update()
             // start/resume the mission (based on MIS_RESTART parameter)
             mission.start_or_resume();
             waiting_to_start = false;
+
+            // initialise mission change check
+            IGNORE_RETURN(mis_change_detector.check_for_mission_change());
         }
     } else {
+        // check for mission changes
+        if (mis_change_detector.check_for_mission_change()) {
+            // if mission is running restart the current command if it is a waypoint command
+            if ((mission.state() == AP_Mission::MISSION_RUNNING) && (_submode == AutoSubMode::Auto_WP)) {
+                if (mission.restart_current_nav_cmd()) {
+                    gcs().send_text(MAV_SEVERITY_CRITICAL, "Auto mission changed, restarted command");
+                } else {
+                    // failed to restart mission for some reason
+                    gcs().send_text(MAV_SEVERITY_CRITICAL, "Auto mission changed but failed to restart command");
+                }
+            }
+        }
+
         mission.update();
     }