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/mission_feasibility_checker.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/mission_feasibility_checker.cpp')
-rw-r--r-- | src/modules/navigator/mission_feasibility_checker.cpp | 19 |
1 files changed, 18 insertions, 1 deletions
diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index aba2dffff..798e95faa 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -97,7 +97,24 @@ bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_curre bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence) { - //xxx: check geofence + /* Check if all mission items are inside the geofence (if we have a valid geofence) */ + if (geofence.valid()) { + for (size_t i = 0; i < nMissionItems; i++) { + static struct mission_item_s missionitem; + const ssize_t len = sizeof(struct mission_item_s); + + if (dm_read(dm_current, i, &missionitem, len) != len) { + /* not supposed to happen unless the datamanager can't access the SD card, etc. */ + return false; + } + + if (!geofence.inside(missionitem.lat, missionitem.lon)) { + mavlink_log_info(_mavlink_fd, "#audio: Geofence violation waypoint %d", i); + return false; + } + } + } + return true; } |