geofence: add counter threshold for subsequent detections

3 files changed, 38 insertions, 3 deletions

diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp
index 4a02a0c3f..a3805dc0f 100644
--- a/src/modules/navigator/geofence.cpp
+++ b/src/modules/navigator/geofence.cpp
@@ -64,7 +64,9 @@ Geofence::Geofence() :
 		_verticesCount(0),
 		_param_geofence_on(this, "ON"),
 		_param_altitude_mode(this, "ALTMODE"),
-		_param_source(this, "SOURCE")
+		_param_source(this, "SOURCE"),
+		_param_counter_threshold(this, "COUNT"),
+		_outside_counter(0)
 {
 	/* Load initial params */
 	updateParams();
@@ -108,6 +110,24 @@ bool Geofence::inside(const struct vehicle_global_position_s &global_position,
 
 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)
 		return true;
diff --git a/src/modules/navigator/geofence.h b/src/modules/navigator/geofence.h
index 91c74572e..65ebb0c3d 100644
--- a/src/modules/navigator/geofence.h
+++ b/src/modules/navigator/geofence.h
@@ -77,9 +77,10 @@ public:
 	 */
 	bool inside(const struct vehicle_global_position_s &global_position);
 	bool inside(const struct vehicle_global_position_s &global_position, float baro_altitude_amsl);
-	bool inside(double lat, double lon, float altitude);
 	bool inside(const struct vehicle_global_position_s &global_position,
 			const struct vehicle_gps_position_s &gps_position,float baro_altitude_amsl);
+	bool inside(double lat, double lon, float altitude);
+	bool inside_polygon(double lat, double lon, float altitude);
 
 	int clearDm();
 
@@ -97,7 +98,7 @@ public:
 	bool isEmpty() {return _verticesCount == 0;}
 
 	int getAltitudeMode() { return _param_altitude_mode.get(); }
-	
+
 	int getSource() { return _param_source.get(); }
 
 private:
@@ -112,6 +113,9 @@ private:
 	control::BlockParamInt _param_geofence_on;
 	control::BlockParamInt _param_altitude_mode;
 	control::BlockParamInt _param_source;
+	control::BlockParamInt _param_counter_threshold;
+
+	uint8_t			_outside_counter;
 };
 
 
diff --git a/src/modules/navigator/geofence_params.c b/src/modules/navigator/geofence_params.c
index 32902ee97..fca3918e1 100644
--- a/src/modules/navigator/geofence_params.c
+++ b/src/modules/navigator/geofence_params.c
@@ -83,3 +83,14 @@ PARAM_DEFINE_INT32(GF_ALTMODE, 0);
  * @group Geofence
  */
 PARAM_DEFINE_INT32(GF_SOURCE, 0);
+
+/**
+ * Geofence counter limit
+ *
+ * Set how many subsequent position measurements outside of the fence are needed before geofence violation is triggered
+ *
+ * @min -1
+ * @max 10
+ * @group Geofence
+ */
+PARAM_DEFINE_INT32(GF_COUNT, -1);