navigator: add pre mission geofence check

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