geofence: add simple vertical check

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