From 076782f6f18bc200385f6d4ee362c121aa7d6b79 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Tue, 10 Dec 2024 08:41:05 +1100
Subject: [PATCH] AC_Fence: remember manual disable of fence for min-alt

the automatic min-alt fence should not auto-enable based on altitude
if the fence has been manually disabled. This is needed to allow for a
manual landing by disabling the fence before descending
---
 libraries/AC_Fence/AC_Fence.cpp | 26 ++++++++++++++++++++++++--
 libraries/AC_Fence/AC_Fence.h   |  7 +++++++
 2 files changed, 31 insertions(+), 2 deletions(-)

diff --git a/libraries/AC_Fence/AC_Fence.cpp b/libraries/AC_Fence/AC_Fence.cpp
index c967684476..d6255bd449 100644
--- a/libraries/AC_Fence/AC_Fence.cpp
+++ b/libraries/AC_Fence/AC_Fence.cpp
@@ -237,9 +237,9 @@ void AC_Fence::update()
 }
 
 // enable or disable configured fences present in fence_types
-// also updates the bitmask of auto enabled fences if update_auto_mask is true
+// also updates the _min_alt_state enum if update_auto_enable is true
 // returns a bitmask of fences that were changed
-uint8_t AC_Fence::enable(bool value, uint8_t fence_types, bool update_auto_mask)
+uint8_t AC_Fence::enable(bool value, uint8_t fence_types, bool update_auto_enable)
 {
     uint8_t fences = _configured_fences.get() & fence_types;
     uint8_t enabled_fences = _enabled_fences;
@@ -249,6 +249,11 @@ uint8_t AC_Fence::enable(bool value, uint8_t fence_types, bool update_auto_mask)
         enabled_fences &= ~fences;
     }
 
+    if (update_auto_enable && (fences & AC_FENCE_TYPE_ALT_MIN) != 0) {
+        // remember that min-alt fence was manually enabled/disabled
+        _min_alt_state = value ? MinAltState::MANUALLY_ENABLED : MinAltState::MANUALLY_DISABLED;
+    }
+
     uint8_t fences_to_change = _enabled_fences ^ enabled_fences;
 
     if (!fences_to_change) {
@@ -300,6 +305,10 @@ void AC_Fence::auto_enable_fence_on_arming(void)
         return;
     }
 
+    // reset min alt state, after an auto-enable the min alt fence can be auto-enabled on
+    // reaching altitude
+    _min_alt_state = MinAltState::DEFAULT;
+
     const uint8_t fences = enable(true, AC_FENCE_ARMING_FENCES, false);
     print_fence_message("auto-enabled", fences);
 }
@@ -327,6 +336,9 @@ void AC_Fence::auto_enable_fence_after_takeoff(void)
         return;
     }
 
+    // reset min-alt state
+    _min_alt_state = MinAltState::DEFAULT;
+
     const uint8_t fences = enable(true, AC_FENCE_ALL_FENCES, false);
     print_fence_message("auto-enabled", fences);
 }
@@ -345,6 +357,10 @@ uint8_t AC_Fence::get_auto_disable_fences(void) const
             auto_disable = AC_FENCE_TYPE_ALT_MIN;
             break;
     }
+    if (_min_alt_state == MinAltState::MANUALLY_ENABLED) {
+        // don't auto-disable min alt fence if manually enabled
+        auto_disable &= ~AC_FENCE_TYPE_ALT_MIN;
+    }
     return auto_disable;
 }
 
@@ -610,6 +626,7 @@ bool AC_Fence::auto_enable_fence_floor()
     // altitude fence check
     if (!(_configured_fences & AC_FENCE_TYPE_ALT_MIN)       // not configured
         || (get_enabled_fences() & AC_FENCE_TYPE_ALT_MIN)   // already enabled
+        || _min_alt_state == MinAltState::MANUALLY_DISABLED // user has manually disabled the fence
         || (!_enabled && (auto_enabled() == AC_Fence::AutoEnable::ALWAYS_DISABLED
             || auto_enabled() == AutoEnable::ENABLE_ON_AUTO_TAKEOFF))) {
         // not enabled
@@ -715,6 +732,11 @@ uint8_t AC_Fence::check(bool disable_auto_fences)
     // clear any breach from disabled fences
     clear_breach(fences_to_disable);
 
+    if (_min_alt_state == MinAltState::MANUALLY_ENABLED) {
+        // if user has manually enabled the min-alt fence then don't auto-disable
+        fences_to_disable &= ~AC_FENCE_TYPE_ALT_MIN;
+    }
+
     // report on any fences that were auto-disabled
     if (fences_to_disable) {
         print_fence_message("auto-disabled", fences_to_disable);
diff --git a/libraries/AC_Fence/AC_Fence.h b/libraries/AC_Fence/AC_Fence.h
index 84c1ddef5a..c4a821f6e2 100644
--- a/libraries/AC_Fence/AC_Fence.h
+++ b/libraries/AC_Fence/AC_Fence.h
@@ -259,6 +259,13 @@ private:
 
     uint32_t        _manual_recovery_start_ms;  // system time in milliseconds that pilot re-took manual control
 
+    enum class MinAltState
+    {
+        DEFAULT = 0,
+        MANUALLY_ENABLED,
+        MANUALLY_DISABLED
+    } _min_alt_state;
+    
 
     AC_PolyFence_loader _poly_loader{_total, _options}; // polygon fence
 };