diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index 8b46a62473ec3..fb379a3be15a2 100644 --- a/Tools/autotest/arducopter.py +++ b/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) diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index 7fabd6cd07816..ae1249507b90b 100644 --- a/Tools/autotest/arduplane.py +++ b/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) diff --git a/libraries/AC_Fence/AC_Fence.cpp b/libraries/AC_Fence/AC_Fence.cpp index db214b2e494e1..d6255bd44912b 100644 --- a/libraries/AC_Fence/AC_Fence.cpp +++ b/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,9 +249,9 @@ 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; @@ -260,6 +259,7 @@ uint8_t AC_Fence::enable(bool value, uint8_t fence_types, bool update_auto_mask) 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; diff --git a/libraries/AC_Fence/AC_Fence.h b/libraries/AC_Fence/AC_Fence.h index 3165d7b73d4d3..c4a821f6e2aa4 100644 --- a/libraries/AC_Fence/AC_Fence.h +++ b/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 };