diff options
author | Jean Cyr <jcyr@dillobits.com> | 2013-11-03 12:40:13 -0500 |
---|---|---|
committer | Jean Cyr <jcyr@dillobits.com> | 2013-11-03 12:40:13 -0500 |
commit | 1cf9f72f628c5dbdf487e464699245cab61c1750 (patch) | |
tree | de0aa6b97849d6b7c02ff72624c11e7b33b902a8 /src/lib/geo | |
parent | 64c2165e8be17a8309594a0aeacec7f3aedee4c0 (diff) | |
download | px4-firmware-1cf9f72f628c5dbdf487e464699245cab61c1750.tar.gz px4-firmware-1cf9f72f628c5dbdf487e464699245cab61c1750.tar.bz2 px4-firmware-1cf9f72f628c5dbdf487e464699245cab61c1750.zip |
Add data manager module and fence support to navigator
- Add function to geo.c to determine if global position is inside fence
- Add navigator support/commands for maintaining fence coords.
- Add data manager module to support persistence fence storage. Can
store other data, but only used for fence at this time.
- Add unit tests for data manager
Diffstat (limited to 'src/lib/geo')
-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 |