diff options
author | Thomas Gubler <thomasgubler@gmail.com> | 2014-01-04 15:17:07 +0100 |
---|---|---|
committer | Thomas Gubler <thomasgubler@gmail.com> | 2014-01-04 15:17:07 +0100 |
commit | 31d1f436adad9dd963797872fa3575319099e940 (patch) | |
tree | 1e827e4a364997766c4c013e505b9ae434798f47 /src | |
parent | ec60a254d279e826c1d3d8097dcbe4bfa89d1e89 (diff) | |
download | px4-firmware-31d1f436adad9dd963797872fa3575319099e940.tar.gz px4-firmware-31d1f436adad9dd963797872fa3575319099e940.tar.bz2 px4-firmware-31d1f436adad9dd963797872fa3575319099e940.zip |
geofence: add simple vertical check
Diffstat (limited to 'src')
-rw-r--r-- | src/modules/navigator/geofence.cpp | 10 | ||||
-rw-r--r-- | src/modules/navigator/geofence.h | 2 | ||||
-rw-r--r-- | src/modules/navigator/mission_feasibility_checker.cpp | 2 |
3 files changed, 10 insertions, 4 deletions
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) */ diff --git a/src/modules/navigator/geofence.h b/src/modules/navigator/geofence.h index 0335f58f6..d834280cc 100644 --- a/src/modules/navigator/geofence.h +++ b/src/modules/navigator/geofence.h @@ -64,7 +64,7 @@ public: * @return true: craft is inside fence, false:craft is outside fence */ bool inside(const struct vehicle_global_position_s *craft); - bool inside(double lat, double lon); + bool inside(double lat, double lon, float altitude); /** diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index 798e95faa..eaafa217d 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -108,7 +108,7 @@ bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMiss return false; } - if (!geofence.inside(missionitem.lat, missionitem.lon)) { + if (!geofence.inside(missionitem.lat, missionitem.lon, missionitem.altitude)) { //xxx: handle relative altitude mavlink_log_info(_mavlink_fd, "#audio: Geofence violation waypoint %d", i); return false; } |