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

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
This commit is contained in:
Andrew Tridgell 2024-11-25 13:55:07 +11:00
parent f6726e66f8
commit 9084b48308
1 changed files with 13 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
libraries/AC_Fence/AC_Fence.cpp
View File

@ -470,8 +470,20 @@ bool AC_Fence::pre_arm_check(char *failure_msg, const uint8_t failure_msg_len) c
return false; 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 // check no limits are currently breached
if (_breached_fences) { if (breached_fences) {
char msg[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1]; char msg[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1];
ExpandingString e(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); AC_Fence::get_fence_names(_breached_fences, e);