Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_Fence: ensure fence enablement on arming is inverted on disarming

Browse Source 
correct detection of polyfence
This commit is contained in:
Andy Piper 2024-03-11 12:40:31 +00:00 committed by Peter Barker
parent f0f8187c7f
commit 174d5f07bb
2 changed files with 26 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
libraries/AC_Fence/AC_Fence.cpp
View File

@ -229,7 +229,8 @@ void AC_Fence::update()
#ifdef AC_FENCE_DEBUG #ifdef AC_FENCE_DEBUG
static uint32_t last_msg_count = 0; static uint32_t last_msg_count = 0;
if (get_enabled_fences() && last_msg_count++ % 10 == 0) { if (get_enabled_fences() && last_msg_count++ % 10 == 0) {
print_fence_message("fences active", get_enabled_fences()); print_fence_message("active", get_enabled_fences());
print_fence_message("breached", get_breaches());
} }
#endif #endif
} }
@ -309,6 +310,19 @@ void AC_Fence::auto_enable_fence_on_arming(void)
print_fence_message("auto-enabled", fences); print_fence_message("auto-enabled", fences);
} }
/*
called on disarming
*/
void AC_Fence::auto_disable_fence_on_disarming(void)
{
if (auto_enabled() != AC_Fence::AutoEnable::ONLY_WHEN_ARMED) {
return;
}
const uint8_t fences = enable(false, _auto_enable_mask, false);
print_fence_message("auto-disabled", fences);
}
/* /*
called when an auto-takeoff is complete called when an auto-takeoff is complete
*/ */
@ -349,7 +363,7 @@ void AC_Fence::auto_disable_fence_for_landing(void)
} }
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:
disable_floor(); enable(false, AC_FENCE_TYPE_ALT_MIN, false);
_floor_disabled_for_landing = true; _floor_disabled_for_landing = true;
GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Min Alt fence auto-disabled"); GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Min Alt fence auto-disabled");
break; break;
@ -619,10 +633,13 @@ bool AC_Fence::auto_enable_fence_floor()
// inclusions zones // inclusions zones
bool AC_Fence::check_fence_polygon() bool AC_Fence::check_fence_polygon()
{ {
if (!(get_enabled_fences() & AC_FENCE_TYPE_POLYGON)) {
// not enabled; no breach
return false;
}
const bool was_breached = _breached_fences & AC_FENCE_TYPE_POLYGON; const bool was_breached = _breached_fences & AC_FENCE_TYPE_POLYGON;
const bool breached = ((_configured_fences & AC_FENCE_TYPE_POLYGON) && if (_poly_loader.breached()) {
_poly_loader.breached());
if (breached) {
if (!was_breached) { if (!was_breached) {
record_breach(AC_FENCE_TYPE_POLYGON); record_breach(AC_FENCE_TYPE_POLYGON);
return true; return true;
@ -895,6 +912,7 @@ void AC_Fence::update() {}
void AC_Fence::auto_enable_fence_after_takeoff() {} void AC_Fence::auto_enable_fence_after_takeoff() {}
void AC_Fence::auto_disable_fence_for_landing() {} void AC_Fence::auto_disable_fence_for_landing() {}
void AC_Fence::auto_enable_fence_on_arming() {} void AC_Fence::auto_enable_fence_on_arming() {}
void AC_Fence::auto_disable_fence_on_disarming() {}
uint8_t AC_Fence::present() const { return 0; } uint8_t AC_Fence::present() const { return 0; }

3
libraries/AC_Fence/AC_Fence.h
View File

@ -90,6 +90,9 @@ public:
/// auto_enable_fences_on_arming - auto enables all applicable fences on arming /// auto_enable_fences_on_arming - auto enables all applicable fences on arming
void auto_enable_fence_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();
/// auto_enable_fence_floor - auto enables fence floor once desired altitude has been reached. /// auto_enable_fence_floor - auto enables fence floor once desired altitude has been reached.
bool auto_enable_fence_floor(); bool auto_enable_fence_floor();