navigator geofence: switch to new dataman data structure, support multiple polygons

This also removes the 'navigator fence <lon> <lat>' command to simplify
code (I don't think there's still use for that anymore). However the
file loading is still supported.

If goefence.txt does not exist, navigator will not clear the geofence
anymore on startup.

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

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;
 }
 

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_ */

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;

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
 	 */

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);