diff options
author | Thomas Gubler <thomasgubler@gmail.com> | 2014-01-04 14:46:17 +0100 |
---|---|---|
committer | Thomas Gubler <thomasgubler@gmail.com> | 2014-01-04 15:04:24 +0100 |
commit | ec60a254d279e826c1d3d8097dcbe4bfa89d1e89 (patch) | |
tree | b50f65474e9070f150e69cd7a9ee82fdf9def638 /src/modules/navigator/geofence.cpp | |
parent | a65de1e0b9412708c862fbe87d459250a7a3d5fd (diff) | |
download | px4-firmware-ec60a254d279e826c1d3d8097dcbe4bfa89d1e89.tar.gz px4-firmware-ec60a254d279e826c1d3d8097dcbe4bfa89d1e89.tar.bz2 px4-firmware-ec60a254d279e826c1d3d8097dcbe4bfa89d1e89.zip |
navigator: add pre mission geofence check
Diffstat (limited to 'src/modules/navigator/geofence.cpp')
-rw-r--r-- | src/modules/navigator/geofence.cpp | 9 |
1 files changed, 7 insertions, 2 deletions
diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index 199ccb41b..666d9076e 100644 --- a/src/modules/navigator/geofence.cpp +++ b/src/modules/navigator/geofence.cpp @@ -70,15 +70,20 @@ Geofence::~Geofence() bool Geofence::inside(const struct vehicle_global_position_s *vehicle) { + double lat = vehicle->lat / 1e7d; + double lon = vehicle->lon / 1e7d; + + return inside(lat, lon); +} +bool Geofence::inside(double lat, double lon) +{ /* 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++) |