Plane: fence: do not re-trigger if mode change for expected reason

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

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;

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