1 files changed, 59 insertions, 5 deletions

diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp
index 49aec21e0..0f431ded2 100644
--- a/src/modules/navigator/geofence.cpp
+++ b/src/modules/navigator/geofence.cpp
@@ -48,6 +48,7 @@
 #include <ctype.h>
 #include <nuttx/config.h>
 #include <unistd.h>
+#include <mavlink/mavlink_log.h>
 
 
 /* Oddly, ERROR is not defined for C++ */
@@ -62,7 +63,12 @@ Geofence::Geofence() :
 		_altitude_min(0),
 		_altitude_max(0),
 		_verticesCount(0),
-		param_geofence_on(this, "ON")
+		_param_geofence_on(this, "ON"),
+		_param_altitude_mode(this, "ALTMODE"),
+		_param_source(this, "SOURCE"),
+		_param_counter_threshold(this, "COUNT"),
+		_outside_counter(0),
+		_mavlinkFd(-1)
 {
 	/* Load initial params */
 	updateParams();
@@ -74,23 +80,69 @@ Geofence::~Geofence()
 }
 
 
-bool Geofence::inside(const struct vehicle_global_position_s *vehicle)
+bool Geofence::inside(const struct vehicle_global_position_s &global_position)
 {
-	return inside(vehicle->lat, vehicle->lon, vehicle->alt);
+	return inside(global_position.lat, global_position.lon, global_position.alt);
+}
+
+bool Geofence::inside(const struct vehicle_global_position_s &global_position, float baro_altitude_amsl)
+{
+	return inside(global_position.lat, global_position.lon, baro_altitude_amsl);
+}
+
+
+bool Geofence::inside(const struct vehicle_global_position_s &global_position,
+			const struct vehicle_gps_position_s &gps_position,float baro_altitude_amsl) {
+	updateParams();
+
+	if (getAltitudeMode() == Geofence::GF_ALT_MODE_WGS84) {
+		if (getSource() == Geofence::GF_SOURCE_GLOBALPOS) {
+			return inside(global_position);
+		} else {
+			return inside((double)gps_position.lat * 1.0e-7, (double)gps_position.lon * 1.0e-7,
+					(double)gps_position.alt * 1.0e-3);
+		}
+	} else {
+		if (getSource() == Geofence::GF_SOURCE_GLOBALPOS) {
+			return inside(global_position, baro_altitude_amsl);
+		} else {
+			return inside((double)gps_position.lat * 1.0e-7, (double)gps_position.lon * 1.0e-7,
+					baro_altitude_amsl);
+		}
+	}
 }
 
 bool Geofence::inside(double lat, double lon, float altitude)
 {
+	bool inside_fence = inside_polygon(lat, lon, altitude);
+
+	if (inside_fence) {
+		_outside_counter = 0;
+		return inside_fence;
+	} {
+		_outside_counter++;
+		if(_outside_counter > _param_counter_threshold.get()) {
+			return inside_fence;
+		} else {
+			return true;
+		}
+	}
+}
+
+
+bool Geofence::inside_polygon(double lat, double lon, float altitude)
+{
 	/* Return true if geofence is disabled */
-	if (param_geofence_on.get() != 1)
+	if (_param_geofence_on.get() != 1)
 		return true;
 
 	if (valid()) {
 
 		if (!isEmpty()) {
 			/* Vertical check */
-			if (altitude > _altitude_max || altitude < _altitude_min)
+			if (altitude > _altitude_max || altitude < _altitude_min) {
 				return false;
+			}
 
 			/*Horizontal check */
 			/* Adaptation of algorithm originally presented as
@@ -280,8 +332,10 @@ Geofence::loadFromFile(const char *filename)
 	{
 		_verticesCount = pointCounter;
 		warnx("Geofence: imported successfully");
+		mavlink_log_info(_mavlinkFd, "Geofence imported");
 	} else {
 		warnx("Geofence: import error");
+		mavlink_log_critical(_mavlinkFd, "#audio: Geofence import error");
 	}
 
 	return ERROR;