From dd2221338ba5636b110533025ae48343aafdbe8f Mon Sep 17 00:00:00 2001
From: Iampete1 <iampete@hotmail.co.uk>
Date: Fri, 8 Jul 2022 23:10:01 +0100
Subject: [PATCH] Plane: fence: do not re-trigger if mode change for expected
 reason

---
 ArduPlane/Plane.h    |  3 +++
 ArduPlane/fence.cpp  | 10 +++++++---
 ArduPlane/system.cpp |  7 +++----
 3 files changed, 13 insertions(+), 7 deletions(-)

diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h
index 398cdb93a8..d3de334c3b 100644
--- a/ArduPlane/Plane.h
+++ b/ArduPlane/Plane.h
@@ -1210,6 +1210,9 @@ private:
     float pitch_in_expo(bool use_dz) const;
     float rudder_in_expo(bool use_dz) const;
 
+    // mode reason for entering previous mode
+    ModeReason previous_mode_reason = ModeReason::UNKNOWN;
+
 public:
     void failsafe_check(void);
 #if AP_SCRIPTING_ENABLED
diff --git a/ArduPlane/fence.cpp b/ArduPlane/fence.cpp
index d329719048..99044f2748 100644
--- a/ArduPlane/fence.cpp
+++ b/ArduPlane/fence.cpp
@@ -43,9 +43,13 @@ void Plane::fence_check()
         return;
     }
 
-    if (orig_breaches &&
-        (control_mode->is_guided_mode()
-        || control_mode == &mode_rtl || fence.get_action() == AC_FENCE_ACTION_REPORT_ONLY)) {
+    const bool current_mode_breach = plane.control_mode_reason == ModeReason::FENCE_BREACHED;
+    const bool previous_mode_breach = plane.previous_mode_reason ==  ModeReason::FENCE_BREACHED;
+    const bool previous_mode_complete = (plane.control_mode_reason == ModeReason::RTL_COMPLETE_SWITCHING_TO_VTOL_LAND_RTL) ||
+                                        (plane.control_mode_reason == ModeReason::RTL_COMPLETE_SWITCHING_TO_FIXEDWING_AUTOLAND) ||
+                                        (plane.control_mode_reason == ModeReason::QRTL_INSTEAD_OF_RTL) ||
+                                        (plane.control_mode_reason == ModeReason::QLAND_INSTEAD_OF_RTL);
+    if (orig_breaches && (current_mode_breach || (previous_mode_breach && previous_mode_complete))) {
         // we have already triggered, don't trigger again until the
         // user disables/re-enables using the fence channel switch
         return;
diff --git a/ArduPlane/system.cpp b/ArduPlane/system.cpp
index 9551f70680..ed7e856718 100644
--- a/ArduPlane/system.cpp
+++ b/ArduPlane/system.cpp
@@ -257,7 +257,8 @@ bool Plane::set_mode(Mode &new_mode, const ModeReason reason)
     // TODO: move these to be after enter() once start_command_callback() no longer checks control_mode
     previous_mode = control_mode;
     control_mode = &new_mode;
-    const ModeReason previous_mode_reason = control_mode_reason;
+    const ModeReason  old_previous_mode_reason = previous_mode_reason;
+    previous_mode_reason = control_mode_reason;
     control_mode_reason = reason;
 
     // attempt to enter new mode
@@ -269,6 +270,7 @@ bool Plane::set_mode(Mode &new_mode, const ModeReason reason)
         previous_mode = &old_previous_mode;
         control_mode = &old_mode;
         control_mode_reason = previous_mode_reason;
+        previous_mode_reason = old_previous_mode_reason;
 
         // make sad noise
         if (reason != ModeReason::INITIALISED) {
@@ -285,9 +287,6 @@ bool Plane::set_mode(Mode &new_mode, const ModeReason reason)
     // exit previous mode
     old_mode.exit();
 
-    // record reasons
-    control_mode_reason = reason;
-
     // log and notify mode change
     logger.Write_Mode(control_mode->mode_number(), control_mode_reason);
     notify_mode(*control_mode);