Copter: support FENCE_OPTIONS on copter

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;