geofence: support AMSL mode

5 files changed, 86 insertions, 23 deletions

diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp
index 266215308..2b9ce752b 100644
--- a/src/modules/navigator/geofence.cpp
+++ b/src/modules/navigator/geofence.cpp
@@ -62,7 +62,8 @@ 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")
 {
 	/* Load initial params */
 	updateParams();
@@ -74,19 +75,26 @@ Geofence::~Geofence()
 }
 
 
-bool Geofence::inside(const struct vehicle_global_position_s *vehicle)
+bool Geofence::inside(const struct vehicle_global_position_s &global_position)
 {
-	double lat = vehicle->lat / 1e7d;
-	double lon = vehicle->lon / 1e7d;
-	//float	alt = vehicle->alt;
+	double lat = global_position.lat / 1e7d;
+	double lon = global_position.lon / 1e7d;
 
-	return inside(lat, lon, vehicle->alt);
+	return inside(lat, lon, global_position.alt);
+}
+
+bool Geofence::inside(const struct vehicle_global_position_s &global_position, float baro_altitude_amsl) {
+
+	double lat = global_position.lat / 1e7d;
+	double lon = global_position.lon / 1e7d;
+
+	return inside(lat, lon, baro_altitude_amsl);
 }
 
 bool Geofence::inside(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()) {
diff --git a/src/modules/navigator/geofence.h b/src/modules/navigator/geofence.h
index 2eb126ab5..e2c0f08d8 100644
--- a/src/modules/navigator/geofence.h
+++ b/src/modules/navigator/geofence.h
@@ -42,6 +42,8 @@
 #define GEOFENCE_H_
 
 #include <uORB/topics/fence.h>
+#include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/sensor_combined.h>
 #include <controllib/blocks.hpp>
 #include <controllib/block/BlockParam.hpp>
 
@@ -49,29 +51,25 @@
 
 class Geofence : public control::SuperBlock
 {
-private:
-	orb_advert_t	_fence_pub;			/**< publish fence topic */
-
-	float			_altitude_min;
-	float			_altitude_max;
-
-	unsigned 			_verticesCount;
-
-	/* Params */
-	control::BlockParamInt param_geofence_on;
 public:
 	Geofence();
 	~Geofence();
 
+	/* Altitude mode, corresponding to the param GF_ALTMODE */
+	enum {
+		GF_ALT_MODE_GPS = 0,
+		GF_ALT_MODE_AMSL = 1
+	};
+
 	/**
-	 * Return whether craft is inside geofence.
+	 * Return whether system is inside geofence.
 	 *
 	 * Calculate whether point is inside arbitrary polygon
 	 * @param craft pointer craft coordinates
-	 * @param fence pointer to array of coordinates, one per vertex. First and last vertex are assumed connected
-	 * @return true: craft is inside fence, false:craft is outside fence
+	 * @return true: system is inside fence, false: system is outside fence
 	 */
-	bool inside(const struct vehicle_global_position_s *craft);
+	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);
 
 	int clearDm();
@@ -88,6 +86,20 @@ public:
 	int loadFromFile(const char *filename);
 
 	bool isEmpty() {return _verticesCount == 0;}
+
+	int getAltitudeMode() { return _param_altitude_mode.get(); }
+
+private:
+	orb_advert_t	_fence_pub;			/**< publish fence topic */
+
+	float			_altitude_min;
+	float			_altitude_max;
+
+	unsigned 			_verticesCount;
+
+	/* Params */
+	control::BlockParamInt _param_geofence_on;
+	control::BlockParamInt _param_altitude_mode;
 };
 
 
diff --git a/src/modules/navigator/geofence_params.c b/src/modules/navigator/geofence_params.c
index 653b1ad84..29b42cd54 100644
--- a/src/modules/navigator/geofence_params.c
+++ b/src/modules/navigator/geofence_params.c
@@ -58,3 +58,15 @@
  * @group Geofence
  */
 PARAM_DEFINE_INT32(GF_ON, 1);
+
+/**
+ * Geofence altitude mode
+ *
+ * Select which altitude reference should be used
+ * 0 = GPS, 1 = AMSL
+ *
+ * @min 0
+ * @max 1
+ * @group Geofence
+ */
+PARAM_DEFINE_INT32(GF_ALTMODE, 1);
diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h
index 536977972..59a6752a9 100644
--- a/src/modules/navigator/navigator.h
+++ b/src/modules/navigator/navigator.h
@@ -120,6 +120,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 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; }
 	struct mission_result_s*	    get_mission_result() { return &_mission_result; }
@@ -141,6 +142,7 @@ private:
 	int		_mavlink_fd;			/**< the file descriptor to send messages over mavlink */
 
 	int		_global_pos_sub;		/**< global position subscription */
+	int		_sensor_combined_sub;		/**< sensor combined subscription */
 	int		_home_pos_sub;			/**< home position subscription */
 	int		_vstatus_sub;			/**< vehicle status subscription */
 	int		_capabilities_sub;		/**< notification of vehicle capabilities updates */
@@ -156,6 +158,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 */
+	sensor_combined_s				_sensor_combined;	/**< sensor values */
 	home_position_s					_home_pos;		/**< home position for RTL */
 	mission_item_s 					_mission_item;		/**< current mission item */
 	navigation_capabilities_s			_nav_caps;		/**< navigation capabilities */
@@ -196,6 +199,11 @@ private:
 	void		global_position_update();
 
 	/**
+	 * Retrieve sensor values
+	 */
+	void		sensor_combined_update();
+
+	/**
 	 * Retrieve home position
 	 */
 	void		home_position_update();
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index 18bfc2cec..d77acf74e 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -69,6 +69,7 @@
 #include <uORB/topics/fence.h>
 #include <uORB/topics/navigation_capabilities.h>
 #include <uORB/topics/offboard_control_setpoint.h>
+#include <drivers/drv_baro.h>
 
 #include <systemlib/err.h>
 #include <systemlib/systemlib.h>
@@ -112,6 +113,7 @@ Navigator::Navigator() :
 	_vstatus{},
 	_control_mode{},
 	_global_pos{},
+	_sensor_combined{},
 	_home_pos{},
 	_mission_item{},
 	_nav_caps{},
@@ -179,6 +181,12 @@ Navigator::global_position_update()
 }
 
 void
+Navigator::sensor_combined_update()
+{
+	orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined);
+}
+
+void
 Navigator::home_position_update()
 {
 	orb_copy(ORB_ID(home_position), _home_pos_sub, &_home_pos);
@@ -248,6 +256,7 @@ Navigator::task_main()
 
 	/* do subscriptions */
 	_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_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));
 	_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
@@ -261,6 +270,7 @@ Navigator::task_main()
 	vehicle_status_update();
 	vehicle_control_mode_update();
 	global_position_update();
+	sensor_combined_update();
 	home_position_update();
 	navigation_capabilities_update();
 	params_update();
@@ -272,7 +282,7 @@ Navigator::task_main()
 	const hrt_abstime mavlink_open_interval = 500000;
 
 	/* wakeup source(s) */
-	struct pollfd fds[6];
+	struct pollfd fds[7];
 
 	/* Setup of loop */
 	fds[0].fd = _global_pos_sub;
@@ -287,6 +297,8 @@ Navigator::task_main()
 	fds[4].events = POLLIN;
 	fds[5].fd = _param_update_sub;
 	fds[5].events = POLLIN;
+	fds[6].fd = _sensor_combined_sub;
+	fds[6].events = POLLIN;
 
 	while (!_task_should_exit) {
 
@@ -311,6 +323,11 @@ Navigator::task_main()
 			_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
 		}
 
+		/* sensors combined updated */
+		if (fds[6].revents & POLLIN) {
+			sensor_combined_update();
+		}
+
 		/* parameters updated */
 		if (fds[5].revents & POLLIN) {
 			params_update();
@@ -342,7 +359,13 @@ Navigator::task_main()
 			global_position_update();
 
 			/* Check geofence violation */
-			if (!_geofence.inside(&_global_pos)) {
+			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 (!inside) {
 				/* inform other apps via the sp triplet */
 				_pos_sp_triplet.geofence_violated = true;
 				if (_pos_sp_triplet.geofence_violated != true) {