geofence: can select gps instead of global position

5 files changed, 89 insertions, 12 deletions

diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp
index 7897c7ba0..4a02a0c3f 100644
--- a/src/modules/navigator/geofence.cpp
+++ b/src/modules/navigator/geofence.cpp
@@ -63,7 +63,8 @@ Geofence::Geofence() :
 		_altitude_max(0),
 		_verticesCount(0),
 		_param_geofence_on(this, "ON"),
-		_param_altitude_mode(this, "ALTMODE")
+		_param_altitude_mode(this, "ALTMODE"),
+		_param_source(this, "SOURCE")
 {
 	/* Load initial params */
 	updateParams();
@@ -85,6 +86,26 @@ bool Geofence::inside(const struct vehicle_global_position_s &global_position, f
 	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) {
+	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)
 {
 	/* Return true if geofence is disabled */
diff --git a/src/modules/navigator/geofence.h b/src/modules/navigator/geofence.h
index e2c0f08d8..91c74572e 100644
--- a/src/modules/navigator/geofence.h
+++ b/src/modules/navigator/geofence.h
@@ -43,6 +43,7 @@
 
 #include <uORB/topics/fence.h>
 #include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/vehicle_gps_position.h>
 #include <uORB/topics/sensor_combined.h>
 #include <controllib/blocks.hpp>
 #include <controllib/block/BlockParam.hpp>
@@ -57,10 +58,16 @@ public:
 
 	/* Altitude mode, corresponding to the param GF_ALTMODE */
 	enum {
-		GF_ALT_MODE_GPS = 0,
+		GF_ALT_MODE_WGS84 = 0,
 		GF_ALT_MODE_AMSL = 1
 	};
 
+	/* Source, corresponding to the param GF_SOURCE */
+	enum {
+		GF_SOURCE_GLOBALPOS = 0,
+		GF_SOURCE_GPS = 1
+	};
+
 	/**
 	 * Return whether system is inside geofence.
 	 *
@@ -71,6 +78,8 @@ 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);
 
 	int clearDm();
 
@@ -88,6 +97,8 @@ public:
 	bool isEmpty() {return _verticesCount == 0;}
 
 	int getAltitudeMode() { return _param_altitude_mode.get(); }
+	
+	int getSource() { return _param_source.get(); }
 
 private:
 	orb_advert_t	_fence_pub;			/**< publish fence topic */
@@ -100,6 +111,7 @@ private:
 	/* Params */
 	control::BlockParamInt _param_geofence_on;
 	control::BlockParamInt _param_altitude_mode;
+	control::BlockParamInt _param_source;
 };
 
 
diff --git a/src/modules/navigator/geofence_params.c b/src/modules/navigator/geofence_params.c
index 29b42cd54..32902ee97 100644
--- a/src/modules/navigator/geofence_params.c
+++ b/src/modules/navigator/geofence_params.c
@@ -63,10 +63,23 @@ PARAM_DEFINE_INT32(GF_ON, 1);
  * Geofence altitude mode
  *
  * Select which altitude reference should be used
- * 0 = GPS, 1 = AMSL
+ * 0 = WGS84, 1 = AMSL
  *
  * @min 0
  * @max 1
  * @group Geofence
  */
-PARAM_DEFINE_INT32(GF_ALTMODE, 1);
+PARAM_DEFINE_INT32(GF_ALTMODE, 0);
+
+/**
+ * Geofence source
+ *
+ * Select which position source should be used. Selecting GPS instead of global position makes sure that there is
+ * no dependence on the position estimator
+ * 0 = global position, 1 = GPS
+ *
+ * @min 0
+ * @max 1
+ * @group Geofence
+ */
+PARAM_DEFINE_INT32(GF_SOURCE, 0);
diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h
index 709e3e724..840b43f1b 100644
--- a/src/modules/navigator/navigator.h
+++ b/src/modules/navigator/navigator.h
@@ -51,6 +51,7 @@
 #include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/position_setpoint_triplet.h>
 #include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/vehicle_gps_position.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/mission_result.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
@@ -129,6 +130,7 @@ public:
 	struct vehicle_status_s*	    get_vstatus() { return &_vstatus; }
 	struct vehicle_control_mode_s*	    get_control_mode() { return &_control_mode; }
 	struct vehicle_global_position_s*   get_global_position() { return &_global_pos; }
+	struct vehicle_gps_position_s*	    get_gps_position() { return &_gps_pos; }
 	struct sensor_combined_s*	    get_sensor_combined() { return &_sensor_combined; }
 	struct home_position_s*		    get_home_position() { return &_home_pos; }
 	struct position_setpoint_triplet_s* get_position_setpoint_triplet() { return &_pos_sp_triplet; }
@@ -152,6 +154,7 @@ private:
 	int		_mavlink_fd;			/**< the file descriptor to send messages over mavlink */
 
 	int		_global_pos_sub;		/**< global position subscription */
+	int		_gps_pos_sub;		/**< gps position subscription */
 	int		_sensor_combined_sub;		/**< sensor combined subscription */
 	int		_home_pos_sub;			/**< home position subscription */
 	int		_vstatus_sub;			/**< vehicle status subscription */
@@ -171,6 +174,7 @@ private:
 	vehicle_status_s				_vstatus;		/**< vehicle status */
 	vehicle_control_mode_s				_control_mode;		/**< vehicle control mode */
 	vehicle_global_position_s			_global_pos;		/**< global vehicle position */
+	vehicle_gps_position_s				_gps_pos;		/**< gps position */
 	sensor_combined_s				_sensor_combined;	/**< sensor values */
 	home_position_s					_home_pos;		/**< home position for RTL */
 	mission_item_s 					_mission_item;		/**< current mission item */
@@ -217,6 +221,11 @@ private:
 	void		global_position_update();
 
 	/**
+	 * Retrieve gps position
+	 */
+	void		gps_position_update();
+
+	/**
 	 * Retrieve sensor values
 	 */
 	void		sensor_combined_update();
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index c569ee7b5..81ceaaca2 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -100,6 +100,7 @@ Navigator::Navigator() :
 	_navigator_task(-1),
 	_mavlink_fd(-1),
 	_global_pos_sub(-1),
+	_gps_pos_sub(-1),
 	_home_pos_sub(-1),
 	_vstatus_sub(-1),
 	_capabilities_sub(-1),
@@ -114,6 +115,7 @@ Navigator::Navigator() :
 	_vstatus{},
 	_control_mode{},
 	_global_pos{},
+	_gps_pos{},
 	_sensor_combined{},
 	_home_pos{},
 	_mission_item{},
@@ -188,6 +190,12 @@ Navigator::global_position_update()
 }
 
 void
+Navigator::gps_position_update()
+{
+	orb_copy(ORB_ID(vehicle_gps_position), _gps_pos_sub, &_gps_pos);
+}
+
+void
 Navigator::sensor_combined_update()
 {
 	orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined);
@@ -263,6 +271,7 @@ Navigator::task_main()
 
 	/* do subscriptions */
 	_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
+	_gps_pos_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
 	_sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
 	_capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities));
 	_vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
@@ -277,6 +286,7 @@ Navigator::task_main()
 	vehicle_status_update();
 	vehicle_control_mode_update();
 	global_position_update();
+	gps_position_update();
 	sensor_combined_update();
 	home_position_update();
 	navigation_capabilities_update();
@@ -289,7 +299,7 @@ Navigator::task_main()
 	const hrt_abstime mavlink_open_interval = 500000;
 
 	/* wakeup source(s) */
-	struct pollfd fds[7];
+	struct pollfd fds[8];
 
 	/* Setup of loop */
 	fds[0].fd = _global_pos_sub;
@@ -306,6 +316,8 @@ Navigator::task_main()
 	fds[5].events = POLLIN;
 	fds[6].fd = _sensor_combined_sub;
 	fds[6].events = POLLIN;
+	fds[7].fd = _gps_pos_sub;
+	fds[7].events = POLLIN;
 
 	while (!_task_should_exit) {
 
@@ -330,6 +342,16 @@ Navigator::task_main()
 			_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
 		}
 
+		bool have_geofence_position_data = false;
+
+		/* gps updated */
+		if (fds[7].revents & POLLIN) {
+			gps_position_update();
+			if (_geofence.getSource() == Geofence::GF_SOURCE_GPS) {
+				have_geofence_position_data = true;
+			}
+		}
+
 		/* sensors combined updated */
 		if (fds[6].revents & POLLIN) {
 			sensor_combined_update();
@@ -364,14 +386,14 @@ Navigator::task_main()
 		/* global position updated */
 		if (fds[0].revents & POLLIN) {
 			global_position_update();
-
-			/* Check geofence violation */
-			bool inside = false;
-			if (_geofence.getAltitudeMode() == Geofence::GF_ALT_MODE_GPS) {
-				inside = _geofence.inside(_global_pos);
-			} else {
-				inside = _geofence.inside(_global_pos, _sensor_combined.baro_alt_meter);
+			if (_geofence.getSource() == Geofence::GF_SOURCE_GLOBALPOS) {
+				have_geofence_position_data = true;
 			}
+		}
+
+		/* Check geofence violation */
+		if (have_geofence_position_data) {
+			bool inside = _geofence.inside(_global_pos, _gps_pos, _sensor_combined.baro_alt_meter);
 			if (!inside) {
 				/* inform other apps via the sp triplet */
 				_pos_sp_triplet.geofence_violated = true;