AC_Fence: disable fences for landing by suppressing in the fence check rather than using a state machine

simplify takeoff auto-enablement

libraries/AC_Fence/AC_Fence.cpp

@@ -248,11 +248,6 @@ uint8_t AC_Fence::enable(bool value, uint8_t fence_types, bool update_auto_mask)
 
     // fences that were manually changed are no longer eligible for auto-enablement or disablement
     if (update_auto_mask) {
-        // if we are explicitly enabling or disabling the alt min fence and it was auto-enabled then make sure
-        // it doesn't get re-enabled or disabled
-        if (fence_types & _auto_enable_mask & AC_FENCE_TYPE_ALT_MIN) {
-            _floor_disabled_for_landing = !value;
-        }
         _auto_enable_mask &= ~fences;
     }
 
@@ -328,49 +323,30 @@ void AC_Fence::auto_disable_fence_on_disarming(void)
 */
 void AC_Fence::auto_enable_fence_after_takeoff(void)
 {
-    switch(auto_enabled()) {
+    if (auto_enabled() != AC_Fence::AutoEnable::ENABLE_ON_AUTO_TAKEOFF) {
-        case AC_Fence::AutoEnable::ENABLE_ON_AUTO_TAKEOFF:
+        return;
-        case AC_Fence::AutoEnable::ENABLE_DISABLE_FLOOR_ONLY:
-        case AC_Fence::AutoEnable::ONLY_WHEN_ARMED: {
-            // auto-enable fences that aren't currently auto-enabled
-            if (_auto_enable_mask & AC_FENCE_TYPE_ALT_MIN) {
-                _floor_disabled_for_landing = false;
-            }
-            const uint8_t fences = enable(true, _auto_enable_mask, false);
-            print_fence_message("auto-enabled", fences);
-            break;
-        }
-        default:
-            // fence does not auto-enable in other takeoff conditions
-            break;
     }
+
+    const uint8_t fences = enable(true, _auto_enable_mask, false);
+    print_fence_message("auto-enabled", fences);
 }
 
-/*
+// return fences that should be auto-disabled when requested
-  called when performing an auto landing
+uint8_t AC_Fence::get_auto_disable_fences(void) const
- */
-void AC_Fence::auto_disable_fence_for_landing(void)
 {
+    uint8_t auto_disable = 0;
     switch (auto_enabled()) {
-        case AC_Fence::AutoEnable::ENABLE_ON_AUTO_TAKEOFF: {
+        case AC_Fence::AutoEnable::ENABLE_ON_AUTO_TAKEOFF:
-            // disable only those fences which are allowed to be disabled
+            auto_disable = _auto_enable_mask;
-            if (_auto_enable_mask & _enabled_fences & AC_FENCE_TYPE_ALT_MIN) {
-                _floor_disabled_for_landing = true;
-            }
-            const uint8_t fences = enable(false, _auto_enable_mask & _enabled_fences, false);
-            print_fence_message("auto-disabled", fences);
             break;
-        }
         case AC_Fence::AutoEnable::ENABLE_DISABLE_FLOOR_ONLY:
         case AC_Fence::AutoEnable::ONLY_WHEN_ARMED:
-            enable(false, AC_FENCE_TYPE_ALT_MIN, false);
+            auto_disable = _auto_enable_mask & AC_FENCE_TYPE_ALT_MIN;
-            _floor_disabled_for_landing = true;
-            GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Min Alt fence auto-disabled");
             break;
         default:
-            // fence does not auto-disable in other landing conditions
             break;
     }
+    return auto_disable;
 }
 
 uint8_t AC_Fence::present() const
@@ -609,9 +585,11 @@ bool AC_Fence::check_fence_alt_min()
 bool AC_Fence::auto_enable_fence_floor()
 {
     // altitude fence check
-    if (!(_configured_fences & AC_FENCE_TYPE_ALT_MIN)
+    if (!(_configured_fences & AC_FENCE_TYPE_ALT_MIN)       // not configured
-        || (get_enabled_fences() & AC_FENCE_TYPE_ALT_MIN)
+        || (get_enabled_fences() & AC_FENCE_TYPE_ALT_MIN)   // already enabled
-        || (!_enabled && (auto_enabled() != AC_Fence::AutoEnable::ONLY_WHEN_ARMED))) {
+        || !(_auto_enable_mask & AC_FENCE_TYPE_ALT_MIN)     // has been manually disabled
+        || (!_enabled && (auto_enabled() == AC_Fence::AutoEnable::ALWAYS_DISABLED
+            || auto_enabled() == AutoEnable::ENABLE_ON_AUTO_TAKEOFF))) {
         // not enabled
         return false;
     }
@@ -621,7 +599,7 @@ bool AC_Fence::auto_enable_fence_floor()
     _curr_alt = -alt; // translate Down to Up
 
     // check if we are over the altitude fence
-    if (!floor_enabled() && !_floor_disabled_for_landing && _curr_alt >= _alt_min + _margin) {
+    if (!floor_enabled() && _curr_alt >= _alt_min + _margin) {
         enable(true, AC_FENCE_TYPE_ALT_MIN, false);
         gcs().send_text(MAV_SEVERITY_NOTICE, "Min Alt fence enabled (auto enable)");
         return true;
@@ -630,30 +608,6 @@ bool AC_Fence::auto_enable_fence_floor()
     return false;
 }
 
-/// reset fence floor auto-enablement 
-bool AC_Fence::reset_fence_floor_enable()
-{
-    // altitude fence check
-    if (!(_configured_fences & AC_FENCE_TYPE_ALT_MIN) || (!_enabled && !_auto_enabled)) {
-        // not enabled
-        return false;
-    }
-
-    float alt;
-    AP::ahrs().get_relative_position_D_home(alt);
-    _curr_alt = -alt; // translate Down to Up
-
-    // check if we are under the altitude fence
-    if ((floor_enabled() || _floor_disabled_for_landing) && _curr_alt <= _alt_min - _margin) {
-        enable(false, AC_FENCE_TYPE_ALT_MIN, false);
-        _floor_disabled_for_landing = false;
-        gcs().send_text(MAV_SEVERITY_NOTICE, "Min Alt fence disabled (auto disable)");
-        return true;
-    } 
-
-    return false;
-}
-
 // check_fence_polygon - returns true if the poly fence is freshly
 // breached.  That includes being inside exclusion zones and outside
 // inclusions zones
@@ -728,41 +682,53 @@ bool AC_Fence::check_fence_circle()
 
 
 /// check - returns bitmask of fence types breached (if any)
-uint8_t AC_Fence::check()
+uint8_t AC_Fence::check(bool disable_auto_fences)
 {
     uint8_t ret = 0;
+    uint8_t disabled_fences = disable_auto_fences ? get_auto_disable_fences() : 0;
+    uint8_t fences_to_disable = disabled_fences & _enabled_fences;
 
     // clear any breach from a non-enabled fence
     clear_breach(~_configured_fences);
+    // clear any breach from disabled fences
+    clear_breach(fences_to_disable);
+
+    // report on any fences that were auto-disabled
+    if (fences_to_disable) {
+        print_fence_message("auto-disabled", fences_to_disable);
+    }
 
     // return immediately if disabled
     if ((!enabled() && !_auto_enabled && !(_configured_fences & AC_FENCE_TYPE_ALT_MIN)) || !_configured_fences) {
         return 0;
     }
 
+    // disable the (temporarily) disabled fences
+    enable(false, disabled_fences, false);
+
     // maximum altitude fence check
-    if (check_fence_alt_max()) {
+    if (!(disabled_fences & AC_FENCE_TYPE_ALT_MAX) && check_fence_alt_max()) {
         ret |= AC_FENCE_TYPE_ALT_MAX;
     }
 
     // minimum altitude fence check, do this before auto-disabling (e.g. because falling)
     // so that any action can be taken
-    if (floor_enabled() && check_fence_alt_min()) {
+    if (!(disabled_fences & AC_FENCE_TYPE_ALT_MIN) && check_fence_alt_min()) {
         ret |= AC_FENCE_TYPE_ALT_MIN;
     }
 
-    // auto enable floor unless auto enable has been set (which means other behaviour is required)
+    // auto enable floor unless auto enable on auto takeoff has been set (which means other behaviour is required)
-    if (auto_enabled() != AutoEnable::ENABLE_ON_AUTO_TAKEOFF && (_configured_fences & AC_FENCE_TYPE_ALT_MIN)) {
+    if (!(disabled_fences & AC_FENCE_TYPE_ALT_MIN)) {
         auto_enable_fence_floor();
     }
 
     // circle fence check
-    if (check_fence_circle()) {
+    if (!(disabled_fences & AC_FENCE_TYPE_CIRCLE) && check_fence_circle()) {
         ret |= AC_FENCE_TYPE_CIRCLE;
     }
 
     // polygon fence check
-    if (check_fence_polygon()) {
+    if (!(disabled_fences & AC_FENCE_TYPE_POLYGON) && check_fence_polygon()) {
         ret |= AC_FENCE_TYPE_POLYGON;
     }
 
@@ -937,7 +903,6 @@ void AC_Fence::disable_floor() {}
 void AC_Fence::update() {}
 
 void AC_Fence::auto_enable_fence_after_takeoff() {}
-void AC_Fence::auto_disable_fence_for_landing() {}
 void AC_Fence::auto_enable_fence_on_arming() {}
 void AC_Fence::auto_disable_fence_on_disarming() {}
 
@@ -947,7 +912,7 @@ uint8_t AC_Fence::get_enabled_fences() const { return 0; }
 
 bool AC_Fence::pre_arm_check(const char* &fail_msg) const  { return true; }
 
-uint8_t AC_Fence::check() { return 0; }
+uint8_t AC_Fence::check(bool disable_auto_fences) { return 0; }
 bool AC_Fence::check_destination_within_fence(const Location& loc) { return true; }
 float AC_Fence::get_breach_distance(uint8_t fence_type) const { return 0.0; }
 void AC_Fence::get_fence_names(uint8_t fences, ExpandingString& msg) { }

libraries/AC_Fence/AC_Fence.h

@@ -73,7 +73,7 @@ public:
     uint8_t enable_configured(bool value) { return enable(value, _configured_fences, true); }
 
     /// auto_enabled - automaticaly enable/disable fence depending of flight status
-    AutoEnable auto_enabled() { return static_cast<AutoEnable>(_auto_enabled.get()); }
+    AutoEnable auto_enabled() const { return static_cast<AutoEnable>(_auto_enabled.get()); }
 
     /// enable_floor - allows fence floor to be enabled/disabled. Note this does not update the eeprom saved value
     void enable_floor();
@@ -84,21 +84,17 @@ public:
     /// auto_enable_fence_on_takeoff - auto enables the fence. Called after takeoff conditions met
     void auto_enable_fence_after_takeoff();
 
-    /// auto_disable_fence_for_landing - auto disables respective fence. Called prior to landing.
-    void auto_disable_fence_for_landing();
-
     /// auto_enable_fences_on_arming - auto enables all applicable fences on arming
     void auto_enable_fence_on_arming();
 
     /// auto_disable_fences_on_disarming - auto disables all applicable fences on disarming
     void auto_disable_fence_on_disarming();
 
+    uint8_t get_auto_disable_fences(void) const;
+
     /// auto_enable_fence_floor - auto enables fence floor once desired altitude has been reached.
     bool auto_enable_fence_floor();
 
-    /// reset_fence_floor_enable - auto disables the fence floor if below the desired altitude.
-    bool reset_fence_floor_enable();
-
     /// enabled - returns whether fencing is enabled or not
     bool enabled() const { return _enabled_fences; }
 
@@ -119,7 +115,8 @@ public:
     ///
 
     /// check - returns the fence type that has been breached (if any)
-    uint8_t check();
+    /// disabled_fences can be used to disable fences for certain conditions (e.g. landing)
+    uint8_t check(bool disable_auto_fence = false);
 
     // returns true if the destination is within fence (used to reject waypoints outside the fence)
     bool check_destination_within_fence(const class Location& loc);
@@ -252,7 +249,6 @@ private:
     float           _circle_breach_distance;    // distance beyond the circular fence
 
     // other internal variables
-    bool            _floor_disabled_for_landing;  // fence floor is disabled for landing
     uint8_t         _auto_enable_mask = AC_FENCE_ALL_FENCES;  // fences that can be auto-enabled or auto-disabled
     float           _home_distance;         // distance from home in meters (provided by main code)
     float           _curr_alt;