diff options
Diffstat (limited to 'src/lib')
| -rw-r--r-- | src/lib/geo/geo.c | 22 | ||||
| -rw-r--r-- | src/lib/geo/geo.h | 14 |
2 files changed, 36 insertions, 0 deletions
diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index 43105fdba..8aeca5be7 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -465,4 +465,26 @@ __EXPORT float _wrap_360(float bearing) return bearing; } +__EXPORT bool inside_geofence(const struct vehicle_global_position_s *vehicle, const struct fence_s *fence) +{ + + /* Adaptation of algorithm originally presented as + * PNPOLY - Point Inclusion in Polygon Test + * W. Randolph Franklin (WRF) */ + + unsigned int i, j, vertices = fence->count; + bool c = false; + double lat = vehicle->lat / 1e7; + double lon = vehicle->lon / 1e7; + + // skip vertex 0 (return point) + for (i = 0, j = vertices - 1; i < vertices; j = i++) + if (((fence->vertices[i].lon) >= lon != (fence->vertices[j].lon >= lon)) && + (lat <= (fence->vertices[j].lat - fence->vertices[i].lat) * (lon - fence->vertices[i].lon) / + (fence->vertices[j].lon - fence->vertices[i].lon) + fence->vertices[i].lat)) + c = !c; + return c; +} + + diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h index 123ff80f1..6bce9309b 100644 --- a/src/lib/geo/geo.h +++ b/src/lib/geo/geo.h @@ -47,6 +47,9 @@ #pragma once +#include "uORB/topics/fence.h" +#include "uORB/topics/vehicle_global_position.h" + __BEGIN_DECLS #include <stdbool.h> @@ -126,4 +129,15 @@ __EXPORT float _wrap_360(float bearing); __EXPORT float _wrap_pi(float bearing); __EXPORT float _wrap_2pi(float bearing); +/** + * Return whether craft is inside geofence. + * + * Calculate whether point is inside arbitrary polygon + * @param craft pointer craft coordinates + * @param fence pointer to array of coordinates, one per vertex. First and last vertex are assumed connected + * @return true: craft is inside fence, false:craft is outside fence + */ +__EXPORT bool inside_geofence(const struct vehicle_global_position_s *craft, const struct fence_s *fence); + + __END_DECLS |