1 files changed, 48 insertions, 15 deletions

diff --git a/src/modules/navigator/geofence.h b/src/modules/navigator/geofence.h
index 2eb126ab5..9d647cb68 100644
--- a/src/modules/navigator/geofence.h
+++ b/src/modules/navigator/geofence.h
@@ -42,6 +42,9 @@
 #define GEOFENCE_H_
 
 #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>
 
@@ -49,30 +52,32 @@
 
 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_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 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(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_polygon(double lat, double lon, float altitude);
 
 	int clearDm();
 
@@ -88,6 +93,34 @@ public:
 	int loadFromFile(const char *filename);
 
 	bool isEmpty() {return _verticesCount == 0;}
+
+	int getAltitudeMode() { return _param_altitude_mode.get(); }
+
+	int getSource() { return _param_source.get(); }
+
+	void setMavlinkFd(int value) { _mavlinkFd = value; }
+
+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;
+	control::BlockParamInt _param_source;
+	control::BlockParamInt _param_counter_threshold;
+
+	uint8_t			_outside_counter;
+
+	int _mavlinkFd;
+
+	bool inside(double lat, double lon, float altitude);
+	bool inside(const struct vehicle_global_position_s &global_position);
+	bool inside(const struct vehicle_global_position_s &global_position, float baro_altitude_amsl);
 };