Skip to content

Commit

Permalink
AC_Fence: fixed pre-arm check for polygon fences
Browse files Browse the repository at this point in the history
for polygon fences we need to check if the vehicle has a position and
is inside the polygon
  • Loading branch information
tridge committed Nov 25, 2024
1 parent 0a32cca commit 3686029
Showing 1 changed file with 13 additions and 1 deletion.
14 changes: 13 additions & 1 deletion libraries/AC_Fence/AC_Fence.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down

0 comments on commit 3686029

Please sign in to comment.