AC_Fence: fixed 2 regressions from 4.5.x

Tools/autotest/arducopter.py

@@ -1719,8 +1719,6 @@ def MinAltFence(self):
     # Also check that the vehicle will not try and ascend too fast when trying to backup from a min alt fence due to avoidance
     def MinAltFenceAvoid(self):
         '''Test Min Alt Fence Avoidance'''
-        self.takeoff(30, mode="LOITER")
-        """Hold loiter position."""
 
         # enable fence, only min altitude
         # No action, rely on avoidance to prevent the breach
@@ -1730,6 +1728,10 @@ def MinAltFenceAvoid(self):
             "FENCE_ALT_MIN": 20,
             "FENCE_ACTION": 0,
         })
+        self.reboot_sitl()
+
+        self.takeoff(30, mode="LOITER")
+        """Hold loiter position."""
 
         # Try and fly past the fence
         self.set_rc(3, 1120)

Tools/autotest/arduplane.py

@@ -3661,7 +3661,7 @@ def FenceMinAltAutoEnableAbort(self):
     def FenceAutoEnableDisableSwitch(self):
         '''Tests autoenablement of regular fences and manual disablement'''
         self.set_parameters({
-            "FENCE_TYPE": 11,     # Set fence type to min alt
+            "FENCE_TYPE": 9,     # Set fence type to min alt, max alt
             "FENCE_ACTION": 1,   # Set action to RTL
             "FENCE_ALT_MIN": 50,
             "FENCE_ALT_MAX": 100,
@@ -3672,44 +3672,88 @@ def FenceAutoEnableDisableSwitch(self):
             "FENCE_RET_ALT" : 0,
             "FENCE_RET_RALLY" : 0,
             "FENCE_TOTAL" : 0,
+            "RTL_ALTITUDE" : 75,
             "TKOFF_ALT" : 75,
             "RC7_OPTION" : 11,   # AC_Fence uses Aux switch functionality
         })
+        self.reboot_sitl()
+        self.context_collect("STATUSTEXT")
+
         fence_bit = mavutil.mavlink.MAV_SYS_STATUS_GEOFENCE
         # Grab Home Position
         self.mav.recv_match(type='HOME_POSITION', blocking=True)
-        self.set_rc_from_map({7: 1000}) # Turn fence off with aux function
+        self.set_rc(7, 1000) # Turn fence off with aux function, does not impact later auto-enable
 
         self.wait_ready_to_arm()
+
+        self.progress("Check fence disabled at boot")
+        m = self.mav.recv_match(type='SYS_STATUS', blocking=True)
+        if (m.onboard_control_sensors_enabled & fence_bit):
+            raise NotAchievedException("Fence is enabled at boot")
+
         cruise_alt = 75
         self.takeoff(cruise_alt, mode='TAKEOFF')
 
-        self.progress("Fly above ceiling and check there is no breach")
+        self.progress("Fly above ceiling and check there is a breach")
+        self.change_mode('FBWA')
         self.set_rc(3, 2000)
-        self.change_altitude(cruise_alt + 80, relative=True)
+        self.set_rc(2, 1000)
+
+        self.wait_statustext("Max Alt fence breached", timeout=10, check_context=True)
+        self.wait_mode('RTL')
+
         m = self.mav.recv_match(type='SYS_STATUS', blocking=True)
-        self.progress("Got (%s)" % str(m))
-        if (not (m.onboard_control_sensors_health & fence_bit)):
-            raise NotAchievedException("Fence Ceiling breached")
+        if (m.onboard_control_sensors_health & fence_bit):
+            raise NotAchievedException("Fence ceiling not breached")
+
+        self.set_rc(3, 1500)
+        self.set_rc(2, 1500)
+
+        self.progress("Wait for RTL alt reached")
+        self.wait_altitude(cruise_alt-5, cruise_alt+5, relative=True, timeout=30)
 
         self.progress("Return to cruise alt")
         self.set_rc(3, 1500)
         self.change_altitude(cruise_alt, relative=True)
 
-        self.progress("Fly below floor and check for no breach")
-        self.change_altitude(25, relative=True)
+        self.progress("Check fence breach cleared")
         m = self.mav.recv_match(type='SYS_STATUS', blocking=True)
-        self.progress("Got (%s)" % str(m))
         if (not (m.onboard_control_sensors_health & fence_bit)):
-            raise NotAchievedException("Fence Ceiling breached")
+            raise NotAchievedException("Fence breach not cleared")
+
+        self.progress("Fly below floor and check for breach")
+        self.set_rc(2, 2000)
+        self.wait_statustext("Min Alt fence breached", timeout=10, check_context=True)
+        self.wait_mode("RTL")
+        m = self.mav.recv_match(type='SYS_STATUS', blocking=True)
+        if (m.onboard_control_sensors_health & fence_bit):
+            raise NotAchievedException("Fence floor not breached")
+
+        self.change_mode("FBWA")
 
-        self.progress("Fly above floor and check fence is not re-enabled")
+        self.progress("Fly above floor and check fence is enabled")
         self.set_rc(3, 2000)
         self.change_altitude(75, relative=True)
         m = self.mav.recv_match(type='SYS_STATUS', blocking=True)
-        self.progress("Got (%s)" % str(m))
+        if (not (m.onboard_control_sensors_enabled & fence_bit)):
+            raise NotAchievedException("Fence Floor not enabled")
+
+        self.progress("Toggle fence enable/disable")
+        self.set_rc(7, 2000)
+        self.delay_sim_time(2)
+        self.set_rc(7, 1000)
+        self.delay_sim_time(2)
+
+        self.progress("Check fence is disabled")
+        m = self.mav.recv_match(type='SYS_STATUS', blocking=True)
         if (m.onboard_control_sensors_enabled & fence_bit):
-            raise NotAchievedException("Fence Ceiling re-enabled")
+            raise NotAchievedException("Fence disable with switch failed")
+
+        self.progress("Fly below floor and check for no breach")
+        self.change_altitude(40, relative=True)
+        m = self.mav.recv_match(type='SYS_STATUS', blocking=True)
+        if (not (m.onboard_control_sensors_health & fence_bit)):
+            raise NotAchievedException("Fence floor breached")
 
         self.progress("Return to cruise alt")
         self.set_rc(3, 1500)

libraries/AC_Fence/AC_Fence.cpp

@@ -219,7 +219,6 @@ void AC_Fence::update()
     // if someone changes the parameter we want to enable or disable everything
     if (_enabled != _last_enabled || _auto_enabled != _last_auto_enabled) {
         // reset the auto mask since we just reconfigured all of fencing
-        _auto_enable_mask = AC_FENCE_ALL_FENCES;
         _last_enabled = _enabled;
         _last_auto_enabled = _auto_enabled;
         if (_enabled) {
@@ -238,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;
@@ -250,16 +249,17 @@ uint8_t AC_Fence::enable(bool value, uint8_t fence_types, bool update_auto_mask)
         enabled_fences &= ~fences;
     }
 
-    // fences that were manually changed are no longer eligible for auto-enablement or disablement
-    if (update_auto_mask) {
-        _auto_enable_mask &= ~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) {
         return 0;
     }
+
 #if HAL_LOGGING_ENABLED
     AP::logger().Write_Event(value ? LogEvent::FENCE_ENABLE : LogEvent::FENCE_DISABLE);
     if (fences_to_change & AC_FENCE_TYPE_ALT_MAX) {
@@ -305,7 +305,11 @@ void AC_Fence::auto_enable_fence_on_arming(void)
         return;
     }
 
-    const uint8_t fences = enable(true, _auto_enable_mask & ~AC_FENCE_TYPE_ALT_MIN, false);
+    // 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);
 }
 
@@ -318,7 +322,7 @@ void AC_Fence::auto_disable_fence_on_disarming(void)
         return;
     }
 
-    const uint8_t fences = enable(false, _auto_enable_mask, false);
+    const uint8_t fences = enable(false, AC_FENCE_ALL_FENCES, false);
     print_fence_message("auto-disabled", fences);
 }
 
@@ -332,7 +336,10 @@ void AC_Fence::auto_enable_fence_after_takeoff(void)
         return;
     }
 
-    const uint8_t fences = enable(true, _auto_enable_mask, false);
+    // 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);
 }
 
@@ -342,14 +349,18 @@ uint8_t AC_Fence::get_auto_disable_fences(void) const
     uint8_t auto_disable = 0;
     switch (auto_enabled()) {
         case AC_Fence::AutoEnable::ENABLE_ON_AUTO_TAKEOFF:
-            auto_disable = _auto_enable_mask;
+            auto_disable = AC_FENCE_ALL_FENCES;
             break;
         case AC_Fence::AutoEnable::ENABLE_DISABLE_FLOOR_ONLY:
         case AC_Fence::AutoEnable::ONLY_WHEN_ARMED:
         default: // when auto disable is not set we still need to disable the altmin fence on landing
-            auto_disable = _auto_enable_mask & AC_FENCE_TYPE_ALT_MIN;
+            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;
 }
 
@@ -469,8 +480,20 @@ bool AC_Fence::pre_arm_check(char *failure_msg, const uint8_t failure_msg_len) c
         return false;
     }
 
+    auto breached_fences = _breached_fences;
+    if (auto_enabled() == AC_Fence::AutoEnable::ONLY_WHEN_ARMED) {
+        Location loc;
+        if (!AP::ahrs().get_location(loc)) {
+            hal.util->snprintf(failure_msg, failure_msg_len, "Fence requires position");
+            return false;
+        }
+        if (_poly_loader.breached(loc)) {
+            breached_fences |= AC_FENCE_TYPE_POLYGON;
+        }
+    }
+
     // check no limits are currently breached
-    if (_breached_fences) {
+    if (breached_fences) {
         char msg[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1];
         ExpandingString e(msg, MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1);
         AC_Fence::get_fence_names(_breached_fences, e);
@@ -511,7 +534,7 @@ bool AC_Fence::check_fence_alt_max()
 
     float alt;
     AP::ahrs().get_relative_position_D_home(alt);
-    _curr_alt = -alt; // translate Down to Up
+    const float _curr_alt = -alt; // translate Down to Up
 
     // check if we are over the altitude fence
     if (_curr_alt >= _alt_max) {
@@ -560,7 +583,7 @@ bool AC_Fence::check_fence_alt_min()
 
     float alt;
     AP::ahrs().get_relative_position_D_home(alt);
-    _curr_alt = -alt; // translate Down to Up
+    const float _curr_alt = -alt; // translate Down to Up
 
     // check if we are under the altitude fence
     if (_curr_alt <= _alt_min) {
@@ -603,7 +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
-        || !(_auto_enable_mask & AC_FENCE_TYPE_ALT_MIN)     // has been manually disabled
+        || _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
@@ -612,7 +635,7 @@ bool AC_Fence::auto_enable_fence_floor()
 
     float alt;
     AP::ahrs().get_relative_position_D_home(alt);
-    _curr_alt = -alt; // translate Down to Up
+    const float _curr_alt = -alt; // translate Down to Up
 
     // check if we are over the altitude fence
     if (!floor_enabled() && _curr_alt >= _alt_min + _margin) {
@@ -709,11 +732,36 @@ 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);
     }
 
+#if 0
+    /*
+      this debug log message is very useful both when developing tests
+      and doing manual SITL fence testing
+     */
+    {
+        float alt;
+        AP::ahrs().get_relative_position_D_home(alt);
+
+        AP::logger().WriteStreaming("FENC", "TimeUS,EN,AE,CF,EF,DF,Alt", "QIIIIIf",
+                                    AP_HAL::micros64(),
+                                    enabled(),
+                                    _auto_enabled,
+                                    _configured_fences,
+                                    get_enabled_fences(),
+                                    disabled_fences,
+                                    alt*-1);
+    }
+#endif
+
     // return immediately if disabled
     if ((!enabled() && !_auto_enabled && !(_configured_fences & AC_FENCE_TYPE_ALT_MIN)) || !_configured_fences) {
         return 0;

libraries/AC_Fence/AC_Fence.h

@@ -249,10 +249,7 @@ class AC_Fence
     float           _circle_breach_distance;    // distance beyond the circular fence
 
     // other internal variables
-    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;
-
 
     // breach information
     uint8_t         _breached_fences;       // bitmask holding the fence type that was breached (i.e. AC_FENCE_TYPE_ALT_MIN, AC_FENCE_TYPE_CIRCLE)
@@ -262,6 +259,13 @@ class AC_Fence
 
     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
 };