navigator: load geofence from textfile

src/modules/navigator/geofence.cpp

@@ -43,8 +43,21 @@
 #include <dataman/dataman.h>
 #include <systemlib/err.h>
 #include <stdlib.h>
+#include <stdio.h>
+#include <ctype.h>
+#include <nuttx/config.h>
+#include <unistd.h>
 
-Geofence::Geofence() : _fence_pub(-1)
+
+/* Oddly, ERROR is not defined for C++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
+
+Geofence::Geofence() : _fence_pub(-1),
+		_altitude_min(0),
+		_altitude_max(0)
 {
 	memset(&_fence, 0, sizeof(_fence));
 }
@@ -77,7 +90,7 @@ bool Geofence::inside(const struct vehicle_global_position_s *vehicle)
 }
 
 bool
-Geofence::load(unsigned vertices)
+Geofence::loadFromDm(unsigned vertices)
 {
 	struct fence_s temp_fence;
 
@@ -164,3 +177,87 @@ Geofence::publishFence(unsigned vertices)
 		orb_publish(ORB_ID(fence), _fence_pub, &vertices);
 }
 
+int
+Geofence::loadFromFile(const char *filename)
+{
+	FILE		*fp;
+	char		line[120];
+	int			pointCounter = 0;
+	bool		gotVertical = false;
+	const char commentChar = '#';
+
+	/* Make sure no data is left in the datamanager */
+	clearDm();
+
+	/* open the mixer definition file */
+	fp = fopen(GEOFENCE_FILENAME, "r");
+	if (fp == NULL) {
+		return ERROR;
+	}
+
+	/* create geofence points from valid lines and store in DM */
+	for (;;) {
+
+		/* get a line, bail on error/EOF */
+		if (fgets(line, sizeof(line), fp) == NULL)
+			break;
+
+		/* Trim leading whitespace */
+		size_t textStart = 0;
+		while((textStart < sizeof(line)/sizeof(char)) && isspace(line[textStart])) textStart++;
+
+		/* if the line starts with #, skip */
+		if (line[textStart] == commentChar)
+			continue;
+
+		if (gotVertical) {
+			/* Parse the line as a geofence point */
+			struct fence_vertex_s vertex;
+
+			if (sscanf(line, "%f %f", &(vertex.lat), &(vertex.lon)) != 2)
+				return ERROR;
+
+
+			if (dm_write(DM_KEY_FENCE_POINTS, pointCounter, DM_PERSIST_POWER_ON_RESET, &vertex, sizeof(vertex)) != sizeof(vertex))
+				return ERROR;
+
+			warnx("Geofence: point: %d, lat %.5f: lon: %.5f", pointCounter,  (double)vertex.lat, (double)vertex.lon);
+
+			pointCounter++;
+		} else {
+			/* Parse the line as the vertical limits */
+			if (sscanf(line, "%f %f", &_altitude_min, &_altitude_max) != 2)
+				return ERROR;
+
+
+			warnx("Geofence: alt min: %.4f, alt_max: %.4f", (double)_altitude_min, (double)_altitude_max);
+			gotVertical = true;
+		}
+
+
+	}
+
+	fclose(fp);
+
+	/* Re-Load imported geofence from DM */
+	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;
+		}
+	} else {
+		warnx("Geofence: import error");
+	}
+
+	return ERROR;
+}
+
+int Geofence::clearDm()
+{
+	dm_clear(DM_KEY_FENCE_POINTS);
+}

src/modules/navigator/geofence.h

@@ -42,10 +42,15 @@
 
 #include <uORB/topics/fence.h>
 
+#define GEOFENCE_FILENAME "/fs/microsd/etc/geofence.txt"
+
 class Geofence {
 private:
-	struct fence_s _fence;
+	struct fence_s	_fence;
 	orb_advert_t	_fence_pub;			/**< publish fence topic */
+
+	float			_altitude_min;
+	float			_altitude_max;
 public:
 	Geofence();
 	~Geofence();
@@ -64,7 +69,9 @@ public:
 	/**
 	 * Load fence parameters.
 	 */
-	bool load(unsigned vertices);
+	bool loadFromDm(unsigned vertices);
+
+	int clearDm();
 
 	bool valid();
 
@@ -74,6 +81,8 @@ public:
 	void addPoint(int argc, char *argv[]);
 
 	void publishFence(unsigned vertices);
+
+	int loadFromFile(const char *filename);
 };
 
 

src/modules/navigator/navigator_main.cpp

@@ -75,6 +75,8 @@
 #include <mathlib/mathlib.h>
 #include <dataman/dataman.h>
 #include <mavlink/mavlink_log.h>
+#include <sys/types.h>
+#include <sys/stat.h>
 
 #include "navigator_mission.h"
 #include "mission_feasibility_checker.h"
@@ -124,6 +126,11 @@ public:
 	 */
 	void add_fence_point(int argc, char *argv[]);
 
+	/**
+	 * Load fence from file
+	 */
+	void load_fence_from_file(const char *filename);
+
 private:
 
 	bool		_task_should_exit;		/**< if true, sensor task should exit */
@@ -497,7 +504,22 @@ Navigator::task_main()
 
 	mavlink_log_info(_mavlink_fd, "[navigator] started");
 
-	_fence_valid = _geofence.load(GEOFENCE_MAX_VERTICES);
+	_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
+	 */
+	struct stat buffer;
+	if( stat (GEOFENCE_FILENAME, &buffer) == 0 ) {
+		warnx("Try to load geofence.txt");
+		_geofence.loadFromFile(GEOFENCE_FILENAME);
+	} else {
+		if (_geofence.clearDm() > 0 )
+			warnx("Geofence cleared");
+		else
+			warnx("Could not clear geofence");
+	}
 
 	/*
 	 * do subscriptions
@@ -1252,6 +1274,11 @@ void Navigator::add_fence_point(int argc, char *argv[])
 	_geofence.addPoint(argc, argv);
 }
 
+void Navigator::load_fence_from_file(const char *filename)
+{
+	_geofence.loadFromFile(filename);
+}
+
 
 static void usage()
 {
@@ -1297,6 +1324,8 @@ int navigator_main(int argc, char *argv[])
 
 	} 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);
 
 	} else {
 		usage();