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

AC_Fence: convert degrees to meters correctly

Browse Source
This commit is contained in:
Andy Piper 2024-12-19 21:06:52 +00:00
parent 2ca827f66c
commit 1bca12ab26
2 changed files with 5 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
libraries/AC_Fence/AC_Fence.cpp
View File

@ -669,13 +669,10 @@ bool AC_Fence::check_fence_polygon()
const bool was_breached = _breached_fences & AC_FENCE_TYPE_POLYGON;
Location loc;
if (!AP::ahrs().get_location(loc)) {
return false;
}
bool have_location = AP::ahrs().get_location(loc);
bool inside_margin = false;
if (_poly_loader.breached(loc, _margin, inside_margin)) {
if (have_location && _poly_loader.breached(loc, _margin, inside_margin)) {
if (!was_breached) {
record_breach(AC_FENCE_TYPE_POLYGON);
return true;

4
libraries/AC_Fence/AC_PolyFence_loader.cpp
View File

@ -234,16 +234,18 @@ bool AC_PolyFence_loader::breached(const Location& loc, float margin, bool& insi
Vector2l pos;
pos.x = loc.lat;
pos.y = loc.lng;
margin = margin * 1.0e7 / 11132.0; // convert margin to degrees
margin = margin * 1.0e7 / 111132.954; // convert margin to degrees
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)) {
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%f %f", Polygon_closest_distance_point(boundary.points_lla, boundary.count, pos), margin);
num_inclusion_outside++;
} else if (Polygon_closest_distance_point(boundary.points_lla, boundary.count, pos) < margin) {
inside_margin = true;