From 61f462bb7c10aeb92ccdc0717c524cdd42f292e2 Mon Sep 17 00:00:00 2001
From: Andy Piper <github@andypiper.com>
Date: Wed, 26 Jun 2024 10:09:26 +0100
Subject: [PATCH] Copter: support FENCE_OPTIONS on copter

---
 ArduCopter/mode.cpp | 14 ++++++++++++++
 1 file changed, 14 insertions(+)

diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp
index 69cda906803ec..97bac5da02ffd 100644
--- a/ArduCopter/mode.cpp
+++ b/ArduCopter/mode.cpp
@@ -347,6 +347,20 @@ bool Copter::set_mode(Mode::Number mode, ModeReason reason)
         return false;
     }
 
+#if AP_FENCE_ENABLED
+    // may not be allowed to change mode if recovering from fence breach
+    if (!ignore_checks &&
+        fence.enabled() &&
+        fence.option_enabled(AC_Fence::OPTIONS::DISABLE_MODE_CHANGE) &&
+        fence.get_breaches() &&
+        !flightmode->is_landing() &&
+        get_control_mode_reason() == ModeReason::FENCE_BREACHED &&
+        !ap.land_complete) {
+        mode_change_failed(new_flightmode, "in fence recovery");
+        return false;
+    }
+#endif
+
     if (!new_flightmode->init(ignore_checks)) {
         mode_change_failed(new_flightmode, "init failed");
         return false;