diff options
-rw-r--r-- | src/modules/navigator/geofence.cpp | 9 | ||||
-rw-r--r-- | src/modules/navigator/geofence.h | 1 | ||||
-rw-r--r-- | src/modules/navigator/mission_feasibility_checker.cpp | 19 |
3 files changed, 26 insertions, 3 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++) diff --git a/src/modules/navigator/geofence.h b/src/modules/navigator/geofence.h index 8a1d06e71..0335f58f6 100644 --- a/src/modules/navigator/geofence.h +++ b/src/modules/navigator/geofence.h @@ -64,6 +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); /** 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; } |