diff --git a/src/modules/navigator/CMakeLists.txt b/src/modules/navigator/CMakeLists.txt
index 55307ba4e2..6b17219c4e 100644
--- a/src/modules/navigator/CMakeLists.txt
+++ b/src/modules/navigator/CMakeLists.txt
@@ -30,6 +30,9 @@
 # POSSIBILITY OF SUCH DAMAGE.
 #
 ############################################################################
+
+include_directories(${PX4_SOURCE_DIR}/mavlink/include/mavlink)
+
 px4_add_module(
 	MODULE modules__navigator
 	MAIN navigator
diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp
index 8918983f7f..4314030a79 100644
--- a/src/modules/navigator/geofence.cpp
+++ b/src/modules/navigator/geofence.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013,2017 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -36,6 +36,7 @@
  *
  * @author Jean Cyr <jean.m.cyr@gmail.com>
  * @author Thomas Gubler <thomasgubler@gmail.com>
+ * @author Beat Küng <beat-kueng@gmx.net>
  */
 #include "geofence.h"
 #include "navigator.h"
@@ -46,6 +47,9 @@
 #include <drivers/drv_hrt.h>
 #include <geo/geo.h>
 #include <systemlib/mavlink_log.h>
+#include <v2.0/common/mavlink.h>
+
+#include "navigator.h"
 
 #define GEOFENCE_RANGE_WARNING_LIMIT 5000000
 
@@ -60,47 +64,135 @@ Geofence::Geofence(Navigator *navigator) :
 	_param_max_ver_distance(this, "GF_MAX_VER_DIST", false)
 {
 	updateParams();
+	updateFence();
+}
+
+Geofence::~Geofence()
+{
+	if (_polygons) {
+		delete[](_polygons);
+	}
+}
+
+void Geofence::updateFence()
+{
+	// initialize fence points count
+	mission_stats_entry_s stats;
+	int ret = dm_read(DM_KEY_FENCE_POINTS, 0, &stats, sizeof(mission_stats_entry_s));
+	int num_fence_items = 0;
+
+	if (ret == sizeof(mission_stats_entry_s)) {
+		num_fence_items = stats.num_items;
+	}
+
+	// iterate over all polygons and store their starting vertices
+	_num_polygons = 0;
+	int current_seq = 1;
+
+	while (current_seq <= num_fence_items) {
+		mission_fence_point_s mission_fence_point;
+
+		if (dm_read(DM_KEY_FENCE_POINTS, current_seq, &mission_fence_point, sizeof(mission_fence_point_s)) !=
+		    sizeof(mission_fence_point_s)) {
+			PX4_ERR("dm_read failed");
+			break;
+		}
+
+		switch (mission_fence_point.nav_cmd) {
+		case MAV_CMD_NAV_FENCE_RETURN_POINT:
+			// TODO: do we need to store this?
+			++current_seq;
+			break;
+
+		case MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION:
+		case MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION:
+			if (mission_fence_point.vertex_count == 0) {
+				++current_seq; // avoid endless loop
+				PX4_ERR("Polygon with 0 vertices. Skipping");
+
+			} else {
+				if (_polygons) {
+					// resize: this is somewhat inefficient, but we do not expect there to be many polygons
+					PolygonInfo *new_polygons = new PolygonInfo[_num_polygons + 1];
+
+					if (new_polygons) {
+						memcpy(new_polygons, _polygons, sizeof(PolygonInfo) * _num_polygons);
+					}
+
+					delete[](_polygons);
+					_polygons = new_polygons;
+
+				} else {
+					_polygons = new PolygonInfo[1];
+				}
+
+				if (!_polygons) {
+					_num_polygons = 0;
+					PX4_ERR("alloc failed");
+					return;
+				}
+
+				PolygonInfo &polygon = _polygons[_num_polygons];
+				polygon.dataman_index = current_seq;
+				polygon.fence_type = mission_fence_point.nav_cmd;
+				polygon.vertex_count = mission_fence_point.vertex_count;
+				current_seq += mission_fence_point.vertex_count;
+				++_num_polygons;
+			}
+
+			break;
+
+		default:
+			PX4_ERR("unhandled Fence command: %i", (int)mission_fence_point.nav_cmd);
+			++current_seq;
+			break;
+		}
+
+	}
+
 }
 
-bool Geofence::inside(const struct vehicle_global_position_s &global_position)
+bool Geofence::checkAll(const struct vehicle_global_position_s &global_position)
 {
-	return inside(global_position.lat, global_position.lon, global_position.alt);
+	return checkAll(global_position.lat, global_position.lon, global_position.alt);
 }
 
-bool Geofence::inside(const struct vehicle_global_position_s &global_position, float baro_altitude_amsl)
+bool Geofence::checkAll(const struct vehicle_global_position_s &global_position, float baro_altitude_amsl)
 {
-	return inside(global_position.lat, global_position.lon, baro_altitude_amsl);
+	return checkAll(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)
+
+bool Geofence::check(const struct vehicle_global_position_s &global_position,
+		     const struct vehicle_gps_position_s &gps_position, float baro_altitude_amsl,
+		     const struct home_position_s home_pos, bool home_position_set)
 {
 	if (getAltitudeMode() == Geofence::GF_ALT_MODE_WGS84) {
 		if (getSource() == Geofence::GF_SOURCE_GLOBALPOS) {
-			return inside(global_position);
+			return checkAll(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);
+			return checkAll((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);
+			return checkAll(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);
+			return checkAll((double)gps_position.lat * 1.0e-7, (double)gps_position.lon * 1.0e-7,
+					baro_altitude_amsl);
 		}
 	}
 }
 
-bool Geofence::inside(const struct mission_item_s &mission_item)
+bool Geofence::check(const struct mission_item_s &mission_item)
 {
-	return inside(mission_item.lat, mission_item.lon, mission_item.altitude);
+	return checkAll(mission_item.lat, mission_item.lon, mission_item.altitude);
 }
 
-bool Geofence::inside(double lat, double lon, float altitude)
+bool Geofence::checkAll(double lat, double lon, float altitude)
 {
 	bool inside_fence = true;
 
@@ -141,7 +233,7 @@ bool Geofence::inside(double lat, double lon, float altitude)
 
 	// to be inside the geofence both fences have to report being inside
 	// as they both report being inside when not enabled
-	inside_fence = inside_fence && inside_polygon(lat, lon, altitude);
+	inside_fence = inside_fence && checkPolygons(lat, lon, altitude);
 
 	if (inside_fence) {
 		_outside_counter = 0;
@@ -159,119 +251,91 @@ bool Geofence::inside(double lat, double lon, float altitude)
 	}
 }
 
-bool Geofence::inside_polygon(double lat, double lon, float altitude)
-{
-	if (valid()) {
-		if (!isEmpty()) {
-			/* Vertical check */
-			if (altitude > _altitude_max || altitude < _altitude_min) {
-				return false;
-			}
-
-			/*Horizontal check */
-			/* Adaptation of algorithm originally presented as
-			 * PNPOLY - Point Inclusion in Polygon Test
-			 * W. Randolph Franklin (WRF) */
 
-			bool c = false;
-
-			struct fence_vertex_s temp_vertex_i;
-			struct fence_vertex_s temp_vertex_j;
+bool Geofence::checkPolygons(double lat, double lon, float altitude)
+{
+	if (isEmpty()) {
+		/* Empty fence -> accept all points */
+		return true;
+	}
 
-			/* Red until fence is finished */
-			for (unsigned i = 0, j = _vertices_count - 1; i < _vertices_count; j = i++) {
-				if (dm_read(DM_KEY_FENCE_POINTS, i, &temp_vertex_i, sizeof(struct fence_vertex_s)) != sizeof(struct fence_vertex_s)) {
-					break;
-				}
+	/* Vertical check */
+	if (altitude > _altitude_max || altitude < _altitude_min) {
+		return false;
+	}
 
-				if (dm_read(DM_KEY_FENCE_POINTS, j, &temp_vertex_j, sizeof(struct fence_vertex_s)) != sizeof(struct fence_vertex_s)) {
-					break;
-				}
+	/* Horizontal check: iterate all polygons */
+	bool outside_exclusion = true;
+	bool inside_inclusion = false;
+	bool had_inclusion_polygons = false;
 
-				// skip vertex 0 (return point)
-				if (((double)temp_vertex_i.lon >= lon) != ((double)temp_vertex_j.lon >= lon) &&
-				    (lat <= (double)(temp_vertex_j.lat - temp_vertex_i.lat) * (lon - (double)temp_vertex_i.lon) /
-				     (double)(temp_vertex_j.lon - temp_vertex_i.lon) + (double)temp_vertex_i.lat)) {
-					c = !c;
-				}
+	for (int polygon_idx = 0; polygon_idx < _num_polygons; ++polygon_idx) {
+		bool inside = insidePolygon(_polygons[polygon_idx], lat, lon, altitude);
 
+		if (_polygons[polygon_idx].fence_type == MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION) {
+			if (inside) {
+				inside_inclusion = true;
 			}
 
-			return c;
+			had_inclusion_polygons = true;
 
-		} else {
-			/* Empty fence --> accept all points */
-			return true;
+		} else { // exclusion
+			if (inside) {
+				outside_exclusion = false;
+			}
 		}
-
-	} else {
-		/* Invalid fence --> accept all points */
-		return true;
 	}
-}
 
-bool
-Geofence::valid()
-{
-	// NULL fence is valid
-	if (isEmpty()) {
-		return true;
-	}
-
-	// Otherwise
-	if ((_vertices_count < 4) || (_vertices_count > fence_s::GEOFENCE_MAX_VERTICES)) {
-		warnx("Fence must have at least 3 sides and not more than %d", fence_s::GEOFENCE_MAX_VERTICES - 1);
-		return false;
-	}
-
-	return true;
+	return (!had_inclusion_polygons || inside_inclusion) && outside_exclusion;
 }
 
-void
-Geofence::addPoint(int argc, char *argv[])
+bool Geofence::insidePolygon(const PolygonInfo &polygon, double lat, double lon, float altitude)
 {
-	int ix, last;
-	double lon, lat;
-	struct fence_vertex_s vertex;
-	char *end;
-
-	if ((argc == 1) && (strcmp("-clear", argv[0]) == 0)) {
-		dm_clear(DM_KEY_FENCE_POINTS);
-		return;
-	}
 
-	if (argc < 3) {
-		PX4_WARN("Specify: -clear | sequence latitude longitude [-publish]");
-	}
-
-	ix = atoi(argv[0]);
-
-	if (ix >= DM_KEY_FENCE_POINTS_MAX) {
-		PX4_WARN("Sequence must be less than %d", DM_KEY_FENCE_POINTS_MAX);
-	}
+	/* Adaptation of algorithm originally presented as
+	 * PNPOLY - Point Inclusion in Polygon Test
+	 * W. Randolph Franklin (WRF)
+	 * Only supports non-complex polygons (not self intersecting)
+	 */
 
-	lat = strtod(argv[1], &end);
-	lon = strtod(argv[2], &end);
+	mission_fence_point_s temp_vertex_i;
+	mission_fence_point_s temp_vertex_j;
+	bool c = false;
 
-	last = 0;
+	for (unsigned i = 0, j = polygon.vertex_count - 1; i < polygon.vertex_count; j = i++) {
+		if (dm_read(DM_KEY_FENCE_POINTS, polygon.dataman_index + i, &temp_vertex_i,
+			    sizeof(mission_fence_point_s)) != sizeof(mission_fence_point_s)) {
+			break;
+		}
 
-	if ((argc > 3) && (strcmp(argv[3], "-publish") == 0)) {
-		last = 1;
-	}
+		if (dm_read(DM_KEY_FENCE_POINTS, polygon.dataman_index + j, &temp_vertex_j,
+			    sizeof(mission_fence_point_s)) != sizeof(mission_fence_point_s)) {
+			break;
+		}
 
-	vertex.lat = (float)lat;
-	vertex.lon = (float)lon;
+		if (temp_vertex_i.frame != MAV_FRAME_GLOBAL && temp_vertex_i.frame != MAV_FRAME_GLOBAL_INT) {
+			// TODO: handle different frames
+			PX4_ERR("Frame type %i not supported", (int)temp_vertex_i.frame);
+			break;
+		}
 
-	if (dm_write(DM_KEY_FENCE_POINTS, ix, DM_PERSIST_POWER_ON_RESET, &vertex, sizeof(vertex)) == sizeof(vertex)) {
-		if (last) {
+		if (((double)temp_vertex_i.lon >= lon) != ((double)temp_vertex_j.lon >= lon) &&
+		    (lat <= (double)(temp_vertex_j.lat - temp_vertex_i.lat) * (lon - (double)temp_vertex_i.lon) /
+		     (double)(temp_vertex_j.lon - temp_vertex_i.lon) + (double)temp_vertex_i.lat)) {
+			c = !c;
 		}
 
-		return;
+		// TODO: handle altitude
 	}
 
-	PX4_WARN("can't store fence point");
+	return c;
 }
 
+bool
+Geofence::valid()
+{
+	return true; // always valid
+}
 
 int
 Geofence::loadFromFile(const char *filename)
@@ -317,36 +381,41 @@ Geofence::loadFromFile(const char *filename)
 
 		if (gotVertical) {
 			/* Parse the line as a geofence point */
-			struct fence_vertex_s vertex;
+			mission_fence_point_s vertex;
+			vertex.frame = MAV_FRAME_GLOBAL;
+			vertex.nav_cmd = MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION;
+			vertex.vertex_count = 0; // this will be filled in a second pass
+			vertex.alt = 0; // TODO: does this make sense?
 
 			/* if the line starts with DMS, this means that the coordinate is given as degree minute second instead of decimal degrees */
 			if (line[textStart] == 'D' && line[textStart + 1] == 'M' && line[textStart + 2] == 'S') {
 				/* Handle degree minute second format */
-				float lat_d, lat_m, lat_s, lon_d, lon_m, lon_s;
+				double lat_d, lat_m, lat_s, lon_d, lon_m, lon_s;
 
-				if (sscanf(line, "DMS %f %f %f %f %f %f", &lat_d, &lat_m, &lat_s, &lon_d, &lon_m, &lon_s) != 6) {
-					warnx("Scanf to parse DMS geofence vertex failed.");
+				if (sscanf(line, "DMS %lf %lf %lf %lf %lf %lf", &lat_d, &lat_m, &lat_s, &lon_d, &lon_m, &lon_s) != 6) {
+					PX4_ERR("Scanf to parse DMS geofence vertex failed.");
 					goto error;
 				}
 
-//				warnx("Geofence DMS: %.5f %.5f %.5f ; %.5f %.5f %.5f", (double)lat_d, (double)lat_m, (double)lat_s, (double)lon_d, (double)lon_m, (double)lon_s);
+//				PX4_INFO("Geofence DMS: %.5lf %.5lf %.5lf ; %.5lf %.5lf %.5lf", lat_d, lat_m, lat_s, lon_d, lon_m, lon_s);
 
-				vertex.lat = lat_d + lat_m / 60.0f + lat_s / 3600.0f;
-				vertex.lon = lon_d + lon_m / 60.0f + lon_s / 3600.0f;
+				vertex.lat = lat_d + lat_m / 60.0 + lat_s / 3600.0;
+				vertex.lon = lon_d + lon_m / 60.0 + lon_s / 3600.0;
 
 			} else {
 				/* Handle decimal degree format */
-				if (sscanf(line, "%f %f", &(vertex.lat), &(vertex.lon)) != 2) {
-					warnx("Scanf to parse geofence vertex failed.");
+				if (sscanf(line, "%lf %lf", &vertex.lat, &vertex.lon) != 2) {
+					PX4_ERR("Scanf to parse geofence vertex failed.");
 					goto error;
 				}
 			}
 
-			if (dm_write(DM_KEY_FENCE_POINTS, pointCounter, DM_PERSIST_POWER_ON_RESET, &vertex, sizeof(vertex)) != sizeof(vertex)) {
+			if (dm_write(DM_KEY_FENCE_POINTS, pointCounter + 1, DM_PERSIST_POWER_ON_RESET, &vertex,
+				     sizeof(vertex)) != sizeof(vertex)) {
 				goto error;
 			}
 
-			warnx("Geofence: point: %d, lat %.5f: lon: %.5f", pointCounter, (double)vertex.lat, (double)vertex.lon);
+			PX4_INFO("Geofence: point: %d, lat %.5lf: lon: %.5lf", pointCounter, vertex.lat, vertex.lon);
 
 			pointCounter++;
 
@@ -356,23 +425,40 @@ Geofence::loadFromFile(const char *filename)
 				goto error;
 			}
 
-			warnx("Geofence: alt min: %.4f, alt_max: %.4f", (double)_altitude_min, (double)_altitude_max);
+			PX4_INFO("Geofence: alt min: %.4f, alt_max: %.4f", (double)_altitude_min, (double)_altitude_max);
 			gotVertical = true;
 		}
 	}
 
+
 	/* Check if import was successful */
-	if (gotVertical && pointCounter > 0) {
-		_vertices_count = pointCounter;
-		warnx("Geofence: imported successfully");
+	if (gotVertical && pointCounter > 2) {
 		mavlink_log_info(_navigator->get_mavlink_log_pub(), "Geofence imported");
 		rc = PX4_OK;
 
+		/* do a second pass, now that we know the number of vertices */
+		for (int seq = 1; seq <= pointCounter; ++seq) {
+			mission_fence_point_s mission_fence_point;
+
+			if (dm_read(DM_KEY_FENCE_POINTS, seq, &mission_fence_point, sizeof(mission_fence_point_s)) ==
+			    sizeof(mission_fence_point_s)) {
+				mission_fence_point.vertex_count = pointCounter;
+				dm_write(DM_KEY_FENCE_POINTS, seq, DM_PERSIST_POWER_ON_RESET, &mission_fence_point,
+					 sizeof(mission_fence_point_s));
+			}
+		}
+
+		mission_stats_entry_s stats;
+		stats.num_items = pointCounter;
+		rc = dm_write(DM_KEY_FENCE_POINTS, 0, DM_PERSIST_POWER_ON_RESET, &stats, sizeof(mission_stats_entry_s));
+
 	} else {
-		warnx("Geofence: import error");
+		PX4_ERR("Geofence: import error");
 		mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Geofence import error");
 	}
 
+	updateFence();
+
 error:
 	fclose(fp);
 	return rc;
@@ -381,6 +467,7 @@ error:
 int Geofence::clearDm()
 {
 	dm_clear(DM_KEY_FENCE_POINTS);
+	updateFence();
 	return PX4_OK;
 }
 
diff --git a/src/modules/navigator/geofence.h b/src/modules/navigator/geofence.h
index f329282db6..8f601e9e58 100644
--- a/src/modules/navigator/geofence.h
+++ b/src/modules/navigator/geofence.h
@@ -67,7 +67,7 @@ public:
 	Geofence(Navigator *navigator);
 	Geofence(const Geofence &) = delete;
 	Geofence &operator=(const Geofence &) = delete;
-	~Geofence() = default;
+	~Geofence();
 
 	/* Altitude mode, corresponding to the param GF_ALTMODE */
 	enum {
@@ -82,31 +82,52 @@ public:
 	};
 
 	/**
-	 * Return whether system is inside geofence.
-	 *
-	 * Calculate whether point is inside arbitrary polygon
-	 * @param craft pointer craft coordinates
-	 * @return true: system is inside fence, false: system is outside fence
+	 * update the geofence from dataman
 	 */
-	bool inside(const struct vehicle_global_position_s &global_position,
-		    const struct vehicle_gps_position_s &gps_position, float baro_altitude_amsl);
+	void updateFence();
 
-	bool inside(const struct mission_item_s &mission_item);
+	/**
+	 * Return whether the system obeys the geofence.
+	 *
+	 * @return true: system is obeying fence, false: system is violating fence
+	 */
+	bool check(const struct vehicle_global_position_s &global_position,
+		   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);
+	/**
+	 * Return whether a mission item obeys the geofence.
+	 *
+	 * @return true: system is obeying fence, false: system is violating fence
+	 */
+	bool check(const struct mission_item_s &mission_item);
 
 	int clearDm();
 
 	bool valid();
 
 	/**
-	 * Specify fence vertex position.
+	 * Load a single inclusion polygon, replacing any already existing polygons.
+	 * The file has one of the following formats:
+	 * - Decimal Degrees:
+	 * 0 900
+	 * 47.475273548913222 8.52672100067138672
+	 * 47.4608261578541359 8.53414535522460938
+	 * 47.4613484218861217 8.56444358825683594
+	 * 47.4830758091035534 8.53470325469970703
+	 *
+	 * - Degree-Minute-Second:
+	 * 0 900
+	 * DMS -26 -34 -10.4304 151 50 14.5428
+	 * DMS -26 -34 -11.8416 151 50 21.8580
+	 * DMS -26 -34 -36.5628 151 50 28.1112
+	 * DMS -26 -34 -37.1640 151 50 24.1620
+	 *
+	 * Where the first line is min, max altitude in meters AMSL.
 	 */
-	void addPoint(int argc, char *argv[]);
-
 	int loadFromFile(const char *filename);
 
-	bool isEmpty() {return _vertices_count == 0;}
+	bool isEmpty() { return _num_polygons == 0; }
 
 	int getAltitudeMode() { return _param_altitude_mode.get(); }
 	int getSource() { return _param_source.get(); }
@@ -126,7 +147,13 @@ private:
 	float _altitude_min{0.0f};
 	float _altitude_max{0.0f};
 
-	unsigned _vertices_count{0};
+	struct PolygonInfo {
+		uint16_t fence_type; ///< one of MAV_CMD_NAV_FENCE_*
+		uint16_t dataman_index;
+		uint16_t vertex_count;
+	};
+	PolygonInfo *_polygons{nullptr};
+	int _num_polygons{0};
 
 	/* Params */
 	control::BlockParamInt _param_action;
@@ -138,9 +165,34 @@ private:
 
 	int _outside_counter{0};
 
-	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);
+	/**
+	 * Check if a point passes the Geofence test.
+	 * This takes all polygons and minimum & maximum altitude into account
+	 *
+	 * The check passes if: (inside(polygon_inclusion_1) || inside(polygon_inclusion_2) || ... ) &&
+	 *                       !inside(polygon_exclusion_1) && !inside(polygon_exclusion_2) && ...
+	 *                       && (altitude within [min, max])
+	 *                  or: no polygon configured
+	 * @return result of the check above (false for a geofence violation)
+	 */
+	bool checkPolygons(double lat, double lon, float altitude);
+
+	/**
+	 * Check if a point passes the Geofence test.
+	 * In addition to checkPolygons(), this takes all additional parameters into account.
+	 *
+	 * @return false for a geofence violation
+	 */
+	bool checkAll(double lat, double lon, float altitude);
+
+	bool checkAll(const struct vehicle_global_position_s &global_position);
+	bool checkAll(const struct vehicle_global_position_s &global_position, float baro_altitude_amsl);
+
+	/**
+	 * Check if a single point is within a polygon
+	 * @return true if within polygon
+	 */
+	bool insidePolygon(const PolygonInfo &polygon, double lat, double lon, float altitude);
 };
 
 #endif /* GEOFENCE_H_ */
diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp
index cdb4724a2a..29a379b32a 100644
--- a/src/modules/navigator/mission_feasibility_checker.cpp
+++ b/src/modules/navigator/mission_feasibility_checker.cpp
@@ -188,7 +188,8 @@ MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMissionIt
 			// Geofence function checks against home altitude amsl
 			missionitem.altitude = missionitem.altitude_is_relative ? missionitem.altitude + home_alt : missionitem.altitude;
 
-			if (MissionBlock::item_contains_position(&missionitem) && !geofence.inside(missionitem)) {
+			if (MissionBlock::item_contains_position(&missionitem) &&
+			    !geofence.check(missionitem)) {
 
 				mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Geofence violation for waypoint %d", i + 1);
 				return false;
diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h
index 9ce8b9f9f9..849f1ee456 100644
--- a/src/modules/navigator/navigator.h
+++ b/src/modules/navigator/navigator.h
@@ -95,11 +95,6 @@ public:
 	 */
 	void		status();
 
-	/**
-	 * Add point to geofence
-	 */
-	void		add_fence_point(int argc, char *argv[]);
-
 	/**
 	 * Load fence from file
 	 */
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index e0a966a2c6..4e9efe705b 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -226,18 +226,12 @@ Navigator::task_main()
 	bool have_geofence_position_data = false;
 
 	/* Try to load the geofence:
-	 * if /fs/microsd/etc/geofence.txt load from this file
-	 * else clear geofence data in datamanager */
+	 * if /fs/microsd/etc/geofence.txt load from this file */
 	struct stat buffer;
 
 	if (stat(GEOFENCE_FILENAME, &buffer) == 0) {
-		PX4_INFO("Try to load geofence.txt");
+		PX4_INFO("Loading geofence from %s", GEOFENCE_FILENAME);
 		_geofence.loadFromFile(GEOFENCE_FILENAME);
-
-	} else {
-		if (_geofence.clearDm() != OK) {
-			mavlink_log_critical(&_mavlink_log_pub, "failed clearing geofence");
-		}
 	}
 
 	/* do subscriptions */
@@ -532,7 +526,8 @@ Navigator::task_main()
 		    (_geofence.getGeofenceAction() != geofence_result_s::GF_ACTION_NONE) &&
 		    (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.check(_global_pos, _gps_pos, _sensor_combined.baro_alt_meter, _home_pos,
+						      home_position_valid());
 			last_geofence_check = hrt_absolute_time();
 			have_geofence_position_data = false;
 
@@ -701,12 +696,18 @@ Navigator::start()
 void
 Navigator::status()
 {
-	if (_geofence.valid()) {
-		PX4_INFO("Geofence is valid");
+	/* TODO: add this again */
+	// PX4_INFO("Global position is %svalid", _global_pos_valid ? "" : "in");
+
+	// if (_global_pos.global_valid) {
+	// 	PX4_INFO("Longitude %5.5f degrees, latitude %5.5f degrees", _global_pos.lon, _global_pos.lat);
+	// 	PX4_INFO("Altitude %5.5f meters, altitude above home %5.5f meters",
+	// 	      (double)_global_pos.alt, (double)(_global_pos.alt - _home_pos.alt));
+	// 	PX4_INFO("Ground velocity in m/s, N %5.5f, E %5.5f, D %5.5f",
+	// 	      (double)_global_pos.vel_n, (double)_global_pos.vel_e, (double)_global_pos.vel_d);
+	// 	PX4_INFO("Compass heading in degrees %5.5f", (double)(_global_pos.yaw * M_RAD_TO_DEG_F));
+	// }
 
-	} else {
-		PX4_INFO("Geofence not set (no /etc/geofence.txt on microsd) or not valid");
-	}
 }
 
 void
@@ -819,12 +820,6 @@ Navigator::get_acceptance_radius(float mission_item_radius)
 	return radius;
 }
 
-void
-Navigator::add_fence_point(int argc, char *argv[])
-{
-	_geofence.addPoint(argc, argv);
-}
-
 void
 Navigator::load_fence_from_file(const char *filename)
 {
@@ -852,7 +847,7 @@ Navigator::abort_landing()
 
 static void usage()
 {
-	PX4_INFO("usage: navigator {start|stop|status|fence|fencefile}");
+	PX4_INFO("usage: navigator {start|stop|status|fencefile}");
 }
 
 int navigator_main(int argc, char *argv[])
@@ -898,9 +893,6 @@ int navigator_main(int argc, char *argv[])
 	} else if (!strcmp(argv[1], "status")) {
 		navigator::g_navigator->status();
 
-	} else if (!strcmp(argv[1], "fence")) {
-		navigator::g_navigator->add_fence_point(argc - 2, argv + 2);
-
 	} else if (!strcmp(argv[1], "fencefile")) {
 		navigator::g_navigator->load_fence_from_file(GEOFENCE_FILENAME);