Plane: disalow mode change during fence recovery if option is set

ArduPlane/Plane.h

@@ -979,6 +979,7 @@ private:
     // fence.cpp
     void fence_check();
     bool fence_stickmixing() const;
+    bool in_fence_recovery() const;
 #endif
 
     // ArduPlane.cpp

ArduPlane/fence.cpp

@@ -43,13 +43,7 @@ void Plane::fence_check()
         return;
     }
 
-    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 (current_mode_breach || (previous_mode_breach && previous_mode_complete)) {
+    if (in_fence_recovery()) {
         // we have already triggered, don't trigger again until the
         // user disables/re-enables using the fence channel switch
         return;
@@ -131,7 +125,7 @@ bool Plane::fence_stickmixing(void) const
 {
     if (fence.enabled() &&
         fence.get_breaches() &&
-        control_mode->is_guided_mode())
+        in_fence_recovery())
     {
         // don't mix in user input
         return false;
@@ -140,4 +134,16 @@ bool Plane::fence_stickmixing(void) const
     return true;
 }
 
+bool Plane::in_fence_recovery() const
+{
+    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);
+
+    return current_mode_breach || (previous_mode_breach && previous_mode_complete);
+}
+
 #endif

ArduPlane/system.cpp

@@ -244,6 +244,16 @@ bool Plane::set_mode(Mode &new_mode, const ModeReason reason)
     }
 #endif  // HAL_QUADPLANE_ENABLED
 
+#if AP_FENCE_ENABLED
+    // may not be allowed to change mode if recovering from fence breach
+    if (hal.util->get_soft_armed() && fence.enabled() && fence.option_enabled(AC_Fence::OPTIONS::DISABLE_MODE_CHANGE) &&
+                                            fence.get_breaches() && in_fence_recovery()) {
+        gcs().send_text(MAV_SEVERITY_NOTICE,"Mode change to %s denied, in fence recovery", new_mode.name());
+        AP_Notify::events.user_mode_change_failed = 1;
+        return false;
+    }
+#endif
+
     // backup current control_mode and previous_mode
     Mode &old_previous_mode = *previous_mode;
     Mode &old_mode = *control_mode;