Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AC_Fence: removed _auto_enable_mask 

the _auto_enable_mask was try to make AUX function overrides disable
the FENCE_AUTOENABLE functionality. This isn't the right bevaviour,
both the aux function and the auto-enable should be edge triggered,
with last function taking effect
This commit is contained in:
Andrew Tridgell 2024-12-10 08:00:33 +11:00
parent e96a5aa547
commit 75655a787c
2 changed files with 25 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

37
libraries/AC_Fence/AC_Fence.cpp
View File

@ -219,7 +219,6 @@ void AC_Fence::update()
// if someone changes the parameter we want to enable or disable everything // if someone changes the parameter we want to enable or disable everything
if (_enabled != _last_enabled || _auto_enabled != _last_auto_enabled) { if (_enabled != _last_enabled || _auto_enabled != _last_auto_enabled) {
// reset the auto mask since we just reconfigured all of fencing // reset the auto mask since we just reconfigured all of fencing
_auto_enable_mask = AC_FENCE_ALL_FENCES;
_last_enabled = _enabled; _last_enabled = _enabled;
_last_auto_enabled = _auto_enabled; _last_auto_enabled = _auto_enabled;
if (_enabled) { if (_enabled) {
@ -256,11 +255,6 @@ uint8_t AC_Fence::enable(bool value, uint8_t fence_types, bool update_auto_mask)
return 0; return 0;
} }
// fences that were manually changed are no longer eligible for auto-enablement or disablement
if (update_auto_mask) {
_auto_enable_mask &= ~fences_to_change;
}
#if HAL_LOGGING_ENABLED #if HAL_LOGGING_ENABLED
AP::logger().Write_Event(value ? LogEvent::FENCE_ENABLE : LogEvent::FENCE_DISABLE); AP::logger().Write_Event(value ? LogEvent::FENCE_ENABLE : LogEvent::FENCE_DISABLE);
if (fences_to_change & AC_FENCE_TYPE_ALT_MAX) { if (fences_to_change & AC_FENCE_TYPE_ALT_MAX) {
@ -306,7 +300,7 @@ void AC_Fence::auto_enable_fence_on_arming(void)
return; return;
} }
const uint8_t fences = enable(true, _auto_enable_mask & ~AC_FENCE_TYPE_ALT_MIN, false); const uint8_t fences = enable(true, AC_FENCE_ARMING_FENCES, false);
print_fence_message("auto-enabled", fences); print_fence_message("auto-enabled", fences);
} }
@ -319,7 +313,7 @@ void AC_Fence::auto_disable_fence_on_disarming(void)
return; 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); print_fence_message("auto-disabled", fences);
} }
@ -333,7 +327,7 @@ void AC_Fence::auto_enable_fence_after_takeoff(void)
return; return;
} }
const uint8_t fences = enable(true, _auto_enable_mask, false); const uint8_t fences = enable(true, AC_FENCE_ALL_FENCES, false);
print_fence_message("auto-enabled", fences); print_fence_message("auto-enabled", fences);
} }
@ -343,12 +337,12 @@ uint8_t AC_Fence::get_auto_disable_fences(void) const
uint8_t auto_disable = 0; uint8_t auto_disable = 0;
switch (auto_enabled()) { switch (auto_enabled()) {
case AC_Fence::AutoEnable::ENABLE_ON_AUTO_TAKEOFF: case AC_Fence::AutoEnable::ENABLE_ON_AUTO_TAKEOFF:
auto_disable = _auto_enable_mask; auto_disable = AC_FENCE_ALL_FENCES;
break; break;
case AC_Fence::AutoEnable::ENABLE_DISABLE_FLOOR_ONLY: case AC_Fence::AutoEnable::ENABLE_DISABLE_FLOOR_ONLY:
case AC_Fence::AutoEnable::ONLY_WHEN_ARMED: case AC_Fence::AutoEnable::ONLY_WHEN_ARMED:
default: // when auto disable is not set we still need to disable the altmin fence on landing 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; break;
} }
return auto_disable; return auto_disable;
@ -616,7 +610,6 @@ bool AC_Fence::auto_enable_fence_floor()
// altitude fence check // altitude fence check
if (!(_configured_fences & AC_FENCE_TYPE_ALT_MIN) // not configured if (!(_configured_fences & AC_FENCE_TYPE_ALT_MIN) // not configured
|| (get_enabled_fences() & AC_FENCE_TYPE_ALT_MIN) // already enabled || (get_enabled_fences() & AC_FENCE_TYPE_ALT_MIN) // already enabled
|| !(_auto_enable_mask & AC_FENCE_TYPE_ALT_MIN) // has been manually disabled
|| (!_enabled && (auto_enabled() == AC_Fence::AutoEnable::ALWAYS_DISABLED || (!_enabled && (auto_enabled() == AC_Fence::AutoEnable::ALWAYS_DISABLED
|| auto_enabled() == AutoEnable::ENABLE_ON_AUTO_TAKEOFF))) { || auto_enabled() == AutoEnable::ENABLE_ON_AUTO_TAKEOFF))) {
// not enabled // not enabled
@ -727,6 +720,26 @@ uint8_t AC_Fence::check(bool disable_auto_fences)
print_fence_message("auto-disabled", 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 // return immediately if disabled
if ((!enabled() && !_auto_enabled && !(_configured_fences & AC_FENCE_TYPE_ALT_MIN)) || !_configured_fences) { if ((!enabled() && !_auto_enabled && !(_configured_fences & AC_FENCE_TYPE_ALT_MIN)) || !_configured_fences) {
return 0; return 0;

1
libraries/AC_Fence/AC_Fence.h
View File

@ -249,7 +249,6 @@ private:
float _circle_breach_distance; // distance beyond the circular fence float _circle_breach_distance; // distance beyond the circular fence
// other internal variables // 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 _home_distance; // distance from home in meters (provided by main code)
// breach information // breach information