diff options
author | Julian Oes <julian@oes.ch> | 2013-11-18 13:00:09 +0100 |
---|---|---|
committer | Julian Oes <julian@oes.ch> | 2013-11-18 13:00:09 +0100 |
commit | 29578a56049cf79b7d983d11ef4b1fa87b00a5d9 (patch) | |
tree | 5b5577cb2bc9d6f804f2343c2ce46ad0135bce34 /src/lib | |
parent | 39634d100104b64f205b69017562b3ac549cf264 (diff) | |
parent | 1cf9f72f628c5dbdf487e464699245cab61c1750 (diff) | |
download | px4-firmware-29578a56049cf79b7d983d11ef4b1fa87b00a5d9.tar.gz px4-firmware-29578a56049cf79b7d983d11ef4b1fa87b00a5d9.tar.bz2 px4-firmware-29578a56049cf79b7d983d11ef4b1fa87b00a5d9.zip |
Merge remote-tracking branch 'jean-m-cyr/master' into navigator_wip
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 |