Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-05 07:28:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_Fence: add margin checks to polyfence loader

Browse Source
This commit is contained in:
Andy Piper 2024-12-18 18:26:04 +00:00
parent f41ce5f6ff
commit 339f36897c
3 changed files with 31 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
libraries/AC_Fence/AC_Fence.cpp
View File

@ -667,28 +667,24 @@ bool AC_Fence::check_fence_polygon()
}
const bool was_breached = _breached_fences & AC_FENCE_TYPE_POLYGON;
if (_poly_loader.breached()) {
Location loc;
if (!AP::ahrs().get_location(loc)) {
return false;
}
bool inside_margin = false;
if (_poly_loader.breached(loc, _margin, inside_margin)) {
if (!was_breached) {
record_breach(AC_FENCE_TYPE_POLYGON);
return true;
}
return false;
} else if (option_enabled(OPTIONS::MARGIN_BREACH)) { // check is relatively expensive
Location loc;
if (AP::ahrs().get_location(loc)) {
Location l1 = loc, l2 = loc, l3 = loc, l4 = loc;
// quite approximate
l1.offset(_margin, _margin);
l2.offset(_margin, -_margin);
l3.offset(-_margin, _margin);
l4.offset(-_margin, -_margin);
if (_poly_loader.breached(l1) || _poly_loader.breached(l2)
|| _poly_loader.breached(l3) || _poly_loader.breached(l4)) {
record_margin_breach(AC_FENCE_TYPE_POLYGON);
} else {
clear_margin_breach(AC_FENCE_TYPE_POLYGON);
}
}
} else if (option_enabled(OPTIONS::MARGIN_BREACH) && inside_margin) {
record_margin_breach(AC_FENCE_TYPE_POLYGON);
} else {
clear_margin_breach(AC_FENCE_TYPE_POLYGON);
}
if (was_breached) {

11
libraries/AC_Fence/AC_PolyFence_loader.cpp
View File

@ -225,7 +225,7 @@ bool AC_PolyFence_loader::breached() const
// check if a position (expressed as lat/lng) is within the boundary
// returns true if location is outside the boundary
bool AC_PolyFence_loader::breached(const Location& loc) const
bool AC_PolyFence_loader::breached(const Location& loc, float margin, bool& inside_margin) const
{
if (!loaded() || total_fence_count() == 0) {
return false;
@ -237,12 +237,15 @@ bool AC_PolyFence_loader::breached(const Location& loc) const
const uint16_t num_inclusion = _num_loaded_circle_inclusion_boundaries + _num_loaded_inclusion_boundaries;
uint16_t num_inclusion_outside = 0;
inside_margin = false;
// check we are inside each inclusion zone:
for (uint8_t i=0; i<_num_loaded_inclusion_boundaries; i++) {
const InclusionBoundary &boundary = _loaded_inclusion_boundary[i];
if (Polygon_outside(pos, boundary.points_lla, boundary.count)) {
num_inclusion_outside++;
} else if (Polygon_closest_distance_point(boundary.points_lla, boundary.count, pos) < margin) {
inside_margin = true;
}
}
@ -251,6 +254,8 @@ bool AC_PolyFence_loader::breached(const Location& loc) const
const ExclusionBoundary &boundary = _loaded_exclusion_boundary[i];
if (!Polygon_outside(pos, boundary.points_lla, boundary.count)) {
return true;
} else if (Polygon_closest_distance_point(boundary.points_lla, boundary.count, pos) < margin) {
inside_margin = true;
}
}
@ -262,6 +267,8 @@ bool AC_PolyFence_loader::breached(const Location& loc) const
const float diff_cm = loc.get_distance(circle_center)*100.0f;
if (diff_cm < circle.radius * 100.0f) {
return true;
} else if (diff_cm < (circle.radius + margin) * 100.0f) {
inside_margin = true;
}
}
@ -273,6 +280,8 @@ bool AC_PolyFence_loader::breached(const Location& loc) const
const float diff_cm = loc.get_distance(circle_center)*100.0f;
if (diff_cm > circle.radius * 100.0f) {
num_inclusion_outside++;
} else if (diff_cm > (circle.radius - margin) * 100.0f) {
inside_margin = true;
}
}

9
libraries/AC_Fence/AC_PolyFence_loader.h
View File

@ -132,8 +132,15 @@ public:
// breached() - returns true if the vehicle has breached any fence
bool breached() const WARN_IF_UNUSED;
// breached(Location&, float margin, bool& inside_margin) - returns true if location is outside the boundary
// sets if loc is inside the margin specified
bool breached(const Location& loc, float margin, bool& inside_margin) const WARN_IF_UNUSED;
// breached(Location&) - returns true if location is outside the boundary
bool breached(const Location& loc) const WARN_IF_UNUSED;
bool breached(const Location& loc) const WARN_IF_UNUSED
{
bool inside_margin;
return breached(loc, 0.0f, inside_margin);
}
// returns true if a polygonal include fence could be returned
bool inclusion_boundary_available() const WARN_IF_UNUSED {