navigator: add pre mission geofence check

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;
 }