From 70d4ef480ac5461ef54ac72a54bd335007e233cc Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Sat, 4 Jan 2014 18:49:31 +0100
Subject: [PATCH] geofence: do not keep fence in memory

---
 src/modules/navigator/geofence.cpp       | 86 +++++++++++-------------
 src/modules/navigator/geofence.h         |  9 +--
 src/modules/navigator/navigator_main.cpp |  2 -
 3 files changed, 41 insertions(+), 56 deletions(-)

diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp
index 373429c084..1063fd2e2c 100644
--- a/src/modules/navigator/geofence.cpp
+++ b/src/modules/navigator/geofence.cpp
@@ -57,9 +57,10 @@ static const int ERROR = -1;
 
 Geofence::Geofence() : _fence_pub(-1),
 		_altitude_min(0),
-		_altitude_max(0)
+		_altitude_max(0),
+		_verticesCount(0)
 {
-	memset(&_fence, 0, sizeof(_fence));
+
 }
 
 Geofence::~Geofence()
@@ -79,59 +80,56 @@ bool Geofence::inside(const struct vehicle_global_position_s *vehicle)
 
 bool Geofence::inside(double lat, double lon, float altitude)
 {
-	/* Vertical check */
-	if (altitude > _altitude_max || altitude < _altitude_min)
-		return false;
+	if (valid()) {
+		/* 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) */
+		/*Horizontal check */
+		/* Adaptation of algorithm originally presented as
+		 * PNPOLY - Point Inclusion in Polygon Test
+		 * W. Randolph Franklin (WRF) */
 
-	unsigned int i, j, vertices = _fence.count;
-	bool c = false;
+		bool c = false;
 
-	// skip vertex 0 (return point)
-	for (i = 0, j = vertices - 1; i < vertices; j = i++)
-		if (((_fence.vertices[i].lon) >= lon != (_fence.vertices[j].lon >= lon)) &&
-		    (lat <= (_fence.vertices[j].lat - _fence.vertices[i].lat) * (lon - _fence.vertices[i].lon) /
-		     (_fence.vertices[j].lon - _fence.vertices[i].lon) + _fence.vertices[i].lat))
-			c = !c;
-	return c;
-}
+		struct fence_vertex_s temp_vertex_i;
+		struct fence_vertex_s temp_vertex_j;
 
-bool
-Geofence::loadFromDm(unsigned vertices)
-{
-	struct fence_s temp_fence;
+		/* Red until fence is finished */
+		for (int i = 0, j = _verticesCount - 1; i < _verticesCount; j = i++) {
+			if (dm_read(DM_KEY_FENCE_POINTS, i, &temp_vertex_i, sizeof(struct fence_vertex_s)) != sizeof(struct fence_vertex_s)) {
+				break;
+			}
+			if (dm_read(DM_KEY_FENCE_POINTS, j, &temp_vertex_j, sizeof(struct fence_vertex_s)) != sizeof(struct fence_vertex_s)) {
+				break;
+			}
+
+			// skip vertex 0 (return point)
+			if (((temp_vertex_i.lon) >= lon != (temp_vertex_j.lon >= lon)) &&
+						(lat <= (temp_vertex_j.lat - temp_vertex_i.lat) * (lon - temp_vertex_i.lon) /
+						 (temp_vertex_j.lon - temp_vertex_i.lon) + temp_vertex_i.lat)) {
+						c = !c;
+			}
 
-	unsigned i;
-	for (i = 0; i < vertices; i++) {
-		if (dm_read(DM_KEY_FENCE_POINTS, i, temp_fence.vertices + i, sizeof(struct fence_vertex_s)) != sizeof(struct fence_vertex_s)) {
-			break;
 		}
+
+		return c;
+
+	} else {
+		return true;
 	}
-
-	temp_fence.count = i;
-
-	if (valid())
-		memcpy(&_fence, &temp_fence, sizeof(_fence));
-	else
-		warnx("Invalid fence file, ignored!");
-
-	return _fence.count != 0;
 }
 
 bool
 Geofence::valid()
 {
 	// NULL fence is valid
-	if (_fence.count == 0) {
+	if (_verticesCount == 0) {
 		return true;
 	}
 
 	// Otherwise
-	if ((_fence.count < 4) || (_fence.count > GEOFENCE_MAX_VERTICES)) {
+	if ((_verticesCount < 4) || (_verticesCount > GEOFENCE_MAX_VERTICES)) {
 		warnx("Fence must have at least 3 sides and not more than %d", GEOFENCE_MAX_VERTICES - 1);
 		return false;
 	}
@@ -266,17 +264,11 @@ Geofence::loadFromFile(const char *filename)
 
 	fclose(fp);
 
-	/* Re-Load imported geofence from DM */
+	/* Check if import was successful */
 	if(gotVertical && pointCounter > 0)
 	{
-		bool fence_valid = loadFromDm(GEOFENCE_MAX_VERTICES);
-		if (fence_valid) {
-			warnx("Geofence: imported and loaded successfully");
-			return OK;
-		} else {
-			warnx("Geofence: datamanager read error");
-			return ERROR;
-		}
+		_verticesCount = pointCounter;
+		warnx("Geofence: imported successfully");
 	} else {
 		warnx("Geofence: import error");
 	}
diff --git a/src/modules/navigator/geofence.h b/src/modules/navigator/geofence.h
index d834280ccf..9c753a11de 100644
--- a/src/modules/navigator/geofence.h
+++ b/src/modules/navigator/geofence.h
@@ -46,11 +46,12 @@
 
 class Geofence {
 private:
-	struct fence_s	_fence;
 	orb_advert_t	_fence_pub;			/**< publish fence topic */
 
 	float			_altitude_min;
 	float			_altitude_max;
+
+	int 			_verticesCount;
 public:
 	Geofence();
 	~Geofence();
@@ -66,12 +67,6 @@ public:
 	bool inside(const struct vehicle_global_position_s *craft);
 	bool inside(double lat, double lon, float altitude);
 
-
-	/**
-	 * Load fence parameters.
-	 */
-	bool loadFromDm(unsigned vertices);
-
 	int clearDm();
 
 	bool valid();
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index 80e006d147..8b5cc45767 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -547,8 +547,6 @@ Navigator::task_main()
 
 	mavlink_log_info(_mavlink_fd, "[navigator] started");
 
-	_fence_valid = _geofence.loadFromDm(GEOFENCE_MAX_VERTICES);
-
 	/* Try to load the geofence:
 	 * if /fs/microsd/etc/geofence.txt load from this file
 	 * else clear geofence data in datamanager