Geofence max horizontal and vertical distance

-changes GF_ON to GF_MODE
-adds GF_MAX_HOR_DIST and GF_MAX_VER_DIST params

5 files changed, 91 insertions, 14 deletions

diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp
index 9bc9be245..efbc2689b 100644
--- a/src/modules/navigator/geofence.cpp
+++ b/src/modules/navigator/geofence.cpp
@@ -39,6 +39,7 @@
  */
 #include "geofence.h"
 
+#include <uORB/topics/home_position.h>
 #include <uORB/topics/vehicle_global_position.h>
 #include <string.h>
 #include <dataman/dataman.h>
@@ -49,6 +50,12 @@
 #include <nuttx/config.h>
 #include <unistd.h>
 #include <mavlink/mavlink_log.h>
+#include <geo/geo.h>
+
+#define GEOFENCE_OFF 0
+#define GEOFENCE_FILE_ONLY 1
+#define GEOFENCE_MAX_DISTANCES_ONLY 2
+#define GEOFENCE_FILE_AND_MAX_DISTANCES 3
 
 
 /* Oddly, ERROR is not defined for C++ */
@@ -60,13 +67,17 @@ static const int ERROR = -1;
 Geofence::Geofence() :
 	SuperBlock(NULL, "GF"),
 	_fence_pub(-1),
+	_home_pos{},
+	_home_pos_set(false),
 	_altitude_min(0),
 	_altitude_max(0),
 	_verticesCount(0),
-	_param_geofence_on(this, "ON"),
+	_param_geofence_mode(this, "MODE"),
 	_param_altitude_mode(this, "ALTMODE"),
 	_param_source(this, "SOURCE"),
 	_param_counter_threshold(this, "COUNT"),
+	_param_max_hor_distance(this, "MAX_HOR_DIST"),
+	_param_max_ver_distance(this, "MAX_VER_DIST"),
 	_outside_counter(0),
 	_mavlinkFd(-1)
 {
@@ -92,10 +103,14 @@ bool Geofence::inside(const struct vehicle_global_position_s &global_position, f
 
 
 bool Geofence::inside(const struct vehicle_global_position_s &global_position,
-		      const struct vehicle_gps_position_s &gps_position, float baro_altitude_amsl)
+		      const struct vehicle_gps_position_s &gps_position, float baro_altitude_amsl,
+		      const struct home_position_s home_pos, bool home_position_set)
 {
 	updateParams();
 
+	_home_pos = home_pos;
+	_home_pos_set = home_position_set;
+
 	if (getAltitudeMode() == Geofence::GF_ALT_MODE_WGS84) {
 		if (getSource() == Geofence::GF_SOURCE_GLOBALPOS) {
 			return inside(global_position);
@@ -118,13 +133,40 @@ bool Geofence::inside(const struct vehicle_global_position_s &global_position,
 
 bool Geofence::inside(double lat, double lon, float altitude)
 {
+	if (_param_geofence_mode.get() >= GEOFENCE_MAX_DISTANCES_ONLY) {
+		int32_t max_horizontal_distance = _param_max_hor_distance.get();
+		int32_t max_vertical_distance = _param_max_ver_distance.get();
+
+		if (max_horizontal_distance > 0 || max_vertical_distance > 0) {
+			if (_home_pos_set) {
+				float dist_xy = -1.0f;
+				float dist_z = -1.0f;
+				get_distance_to_point_global_wgs84(lat, lon, altitude,
+								   _home_pos.lat, _home_pos.lon, _home_pos.alt,
+								   &dist_xy, &dist_z);
+
+				if (max_vertical_distance > 0 && (dist_z > max_vertical_distance)) {
+					mavlink_log_critical(_mavlinkFd, "#audio: Geofence exceeded max vertical distance by %.0f m",
+							     (double)(dist_z - max_vertical_distance));
+					return false;
+				}
+
+				if (max_horizontal_distance > 0 && (dist_xy > max_horizontal_distance)) {
+					mavlink_log_critical(_mavlinkFd, "#audio: Geofence exceeded max horizontal distance by %.0f m",
+							     (double)(dist_xy - max_horizontal_distance));
+					return false;
+				}
+			}
+		}
+	}
+
 	bool inside_fence = inside_polygon(lat, lon, altitude);
 
 	if (inside_fence) {
 		_outside_counter = 0;
 		return inside_fence;
-	} {
 
+	} else {
 		_outside_counter++;
 
 		if (_outside_counter > _param_counter_threshold.get()) {
@@ -139,8 +181,9 @@ bool Geofence::inside(double lat, double lon, float altitude)
 
 bool Geofence::inside_polygon(double lat, double lon, float altitude)
 {
-	/* Return true if geofence is disabled */
-	if (_param_geofence_on.get() != 1) {
+	/* Return true if geofence is disabled or only checking max distances */
+	if ((_param_geofence_mode.get() == GEOFENCE_OFF)
+	    || (_param_geofence_mode.get() == GEOFENCE_MAX_DISTANCES_ONLY)) {
 		return true;
 	}
 
diff --git a/src/modules/navigator/geofence.h b/src/modules/navigator/geofence.h
index 37a41b68a..effce9e97 100644
--- a/src/modules/navigator/geofence.h
+++ b/src/modules/navigator/geofence.h
@@ -45,6 +45,7 @@
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/vehicle_gps_position.h>
 #include <uORB/topics/sensor_combined.h>
+#include <uORB/topics/home_position.h>
 #include <controllib/blocks.hpp>
 #include <controllib/block/BlockParam.hpp>
 
@@ -76,7 +77,9 @@ public:
 	 * @return true: system is inside fence, false: system is outside fence
 	 */
 	bool inside(const struct vehicle_global_position_s &global_position,
-		    const struct vehicle_gps_position_s &gps_position, float baro_altitude_amsl);
+		    const struct vehicle_gps_position_s &gps_position, float baro_altitude_amsl,
+		    const struct home_position_s home_pos, bool home_position_set);
+
 	bool inside_polygon(double lat, double lon, float altitude);
 
 	int clearDm();
@@ -103,16 +106,21 @@ public:
 private:
 	orb_advert_t	_fence_pub;			/**< publish fence topic */
 
+	home_position_s _home_pos;
+	bool _home_pos_set;
+
 	float			_altitude_min;
 	float			_altitude_max;
 
 	unsigned 			_verticesCount;
 
 	/* Params */
-	control::BlockParamInt _param_geofence_on;
+	control::BlockParamInt _param_geofence_mode;
 	control::BlockParamInt _param_altitude_mode;
 	control::BlockParamInt _param_source;
 	control::BlockParamInt _param_counter_threshold;
+	control::BlockParamInt _param_max_hor_distance;
+	control::BlockParamInt _param_max_ver_distance;
 
 	uint8_t			_outside_counter;
 
diff --git a/src/modules/navigator/geofence_params.c b/src/modules/navigator/geofence_params.c
index fca3918e1..f3e7d4c84 100644
--- a/src/modules/navigator/geofence_params.c
+++ b/src/modules/navigator/geofence_params.c
@@ -48,16 +48,15 @@
  */
 
 /**
- * Enable geofence.
+ * Geofence mode.
  *
- * Set to 1 to enable geofence.
- * Defaults to 1 because geofence is only enabled when the geofence.txt file is present.
+ * 0 = disabled, 1 = geofence file only, 2 = max horizontal (GF_MAX_HOR_DIST) and vertical (GF_MAX_VER_DIST) distances, 3 = both
  *
  * @min 0
- * @max 1
+ * @max 3
  * @group Geofence
  */
-PARAM_DEFINE_INT32(GF_ON, 1);
+PARAM_DEFINE_INT32(GF_MODE, 0);
 
 /**
  * Geofence altitude mode
@@ -94,3 +93,21 @@ PARAM_DEFINE_INT32(GF_SOURCE, 0);
  * @group Geofence
  */
 PARAM_DEFINE_INT32(GF_COUNT, -1);
+
+/**
+ * Max horizontal distance in meters.
+ *
+ * Set to > 0 to activate RTL if horizontal distance to home exceeds this value.
+ *
+ * @group Geofence
+ */
+PARAM_DEFINE_INT32(GF_MAX_HOR_DIST, -1);
+
+/**
+ * Max vertical distance in meters.
+ *
+ * Set to > 0 to activate RTL if vertical distance to home exceeds this value.
+ *
+ * @group Geofence
+ */
+PARAM_DEFINE_INT32(GF_MAX_VER_DIST, -1);
diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h
index d9d911d9c..d2acce789 100644
--- a/src/modules/navigator/navigator.h
+++ b/src/modules/navigator/navigator.h
@@ -186,6 +186,8 @@ private:
 	geofence_result_s				_geofence_result;
 	vehicle_attitude_setpoint_s			_att_sp;
 
+	bool 		_home_position_set;
+
 	bool 		_mission_item_valid;		/**< flags if the current mission item is valid */
 
 	perf_counter_t	_loop_perf;			/**< loop performance counter */
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index ad2349c94..05fac7b81 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -123,6 +123,7 @@ Navigator::Navigator() :
 	_pos_sp_triplet{},
 	_mission_result{},
 	_att_sp{},
+	_home_position_set(false),
 	_mission_item_valid(false),
 	_loop_perf(perf_alloc(PC_ELAPSED, "navigator")),
 	_geofence{},
@@ -203,7 +204,13 @@ Navigator::sensor_combined_update()
 void
 Navigator::home_position_update()
 {
-	orb_copy(ORB_ID(home_position), _home_pos_sub, &_home_pos);
+	bool updated = false;
+	orb_check(_home_pos_sub, &updated);
+
+	if (updated) {
+		orb_copy(ORB_ID(home_position), _home_pos_sub, &_home_pos);
+		_home_position_set = true;
+	}
 }
 
 void
@@ -392,7 +399,7 @@ Navigator::task_main()
 		/* Check geofence violation */
 		static hrt_abstime last_geofence_check = 0;
 		if (have_geofence_position_data && hrt_elapsed_time(&last_geofence_check) > GEOFENCE_CHECK_INTERVAL) {
-			bool inside = _geofence.inside(_global_pos, _gps_pos, _sensor_combined.baro_alt_meter);
+			bool inside = _geofence.inside(_global_pos, _gps_pos, _sensor_combined.baro_alt_meter, _home_pos, _home_position_set);
 			last_geofence_check = hrt_absolute_time();
 			have_geofence_position_data = false;
 			if (!inside) {