From 5ea3f83e19118bfc8091ee46f812b73b94022cb2 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 25 Nov 2024 13:55:07 +1100 Subject: [PATCH] AC_Fence: fixed pre-arm check for polygon fences for polygon fences we need to check if the vehicle has a position and is inside the polygon --- libraries/AC_Fence/AC_Fence.cpp | 14 +++++++++++++- 1 file changed, 13 insertions(+), 1 deletion(-) diff --git a/libraries/AC_Fence/AC_Fence.cpp b/libraries/AC_Fence/AC_Fence.cpp index ba057dc96b..cdbf55e51f 100644 --- a/libraries/AC_Fence/AC_Fence.cpp +++ b/libraries/AC_Fence/AC_Fence.cpp @@ -470,8 +470,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);