From 31d1f436adad9dd963797872fa3575319099e940 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 4 Jan 2014 15:17:07 +0100 Subject: geofence: add simple vertical check --- src/modules/navigator/geofence.cpp | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) (limited to 'src/modules/navigator/geofence.cpp') diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index 666d9076e..f76597e60 100644 --- a/src/modules/navigator/geofence.cpp +++ b/src/modules/navigator/geofence.cpp @@ -72,12 +72,18 @@ bool Geofence::inside(const struct vehicle_global_position_s *vehicle) { double lat = vehicle->lat / 1e7d; double lon = vehicle->lon / 1e7d; + float alt = vehicle->alt; - return inside(lat, lon); + return inside(lat, lon, vehicle->alt); } -bool Geofence::inside(double lat, double lon) +bool Geofence::inside(double lat, double lon, float altitude) { + /* Vertical check */ + if (altitude > _altitude_max || altitude < _altitude_min) + return false; + + /*Horizontal check */ /* Adaptation of algorithm originally presented as * PNPOLY - Point Inclusion in Polygon Test * W. Randolph Franklin (WRF) */ -- cgit v1.2.3