diff options
author | Thomas Gubler <thomasgubler@gmail.com> | 2014-01-02 14:18:02 +0100 |
---|---|---|
committer | Thomas Gubler <thomasgubler@gmail.com> | 2014-01-02 14:18:02 +0100 |
commit | dca6d97a5288766e3e0da05dc5fdc98108fa7892 (patch) | |
tree | 3cc2850afdbfc79062358d9e6140b7b66bbf32bb /src/lib/geo/geo.c | |
parent | 4191ae33c264459f0a85d9c03b8cb4893c6ee33e (diff) | |
download | px4-firmware-dca6d97a5288766e3e0da05dc5fdc98108fa7892.tar.gz px4-firmware-dca6d97a5288766e3e0da05dc5fdc98108fa7892.tar.bz2 px4-firmware-dca6d97a5288766e3e0da05dc5fdc98108fa7892.zip |
create geofence class and start moving fence functionality to this class
Diffstat (limited to 'src/lib/geo/geo.c')
-rw-r--r-- | src/lib/geo/geo.c | 24 |
1 files changed, 0 insertions, 24 deletions
diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index f64bfb41a..08fe2b696 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -503,27 +503,3 @@ __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 / 1e7d; - double lon = vehicle->lon / 1e7d; - - // 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; -} - - - |