Add data manager module and fence support to navigator

- Add function to geo.c to determine if global position is inside fence
- Add navigator support/commands for maintaining fence coords.
- Add data manager module to support persistence fence storage. Can
store other data, but only used for fence at this time.
- Add unit tests for data manager

1 files changed, 264 insertions, 57 deletions

diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index f6c44444a..2181001c4 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -2,6 +2,7 @@
  *
  *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
  *   Author: 	Lorenz Meier
+ *              Jean Cyr
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -48,6 +49,8 @@
 #include <math.h>
 #include <poll.h>
 #include <time.h>
+#include <sys/ioctl.h>
+#include <drivers/device/device.h>
 #include <drivers/drv_hrt.h>
 #include <arch/board/board.h>
 #include <uORB/uORB.h>
@@ -62,12 +65,14 @@
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/mission.h>
+#include <uORB/topics/fence.h>
 #include <systemlib/param/param.h>
 #include <systemlib/err.h>
 #include <geo/geo.h>
 #include <systemlib/perf_counter.h>
 #include <systemlib/systemlib.h>
 #include <mathlib/mathlib.h>
+#include <dataman/dataman.h>
 
 /**
  * navigator app start / stop handling function
@@ -90,12 +95,27 @@ public:
 	~Navigator();
 
 	/**
-	 * Start the sensors task.
+	* Start the navigator task.
 	 *
 	 * @return		OK on success.
 	 */
 	int		start();
 
+	/**
+	 * Display the navigator status.
+	 */
+	void		status();
+
+	/**
+	 * Load fence parameters.
+	 */
+	bool		load_fence(unsigned vertices);
+
+	/**
+	 * Specify fence vertex position.
+	 */
+        void		fence_point(int argc, char *argv[]);
+
 private:
 
 	bool		_task_should_exit;		/**< if true, sensor task should exit */
@@ -108,7 +128,10 @@ private:
 	int		_vstatus_sub;			/**< vehicle status subscription */
 	int 		_params_sub;			/**< notification of parameter updates */
 	int 		_manual_control_sub;		/**< notification of manual control updates */
-	int		_mission_sub;
+        int		_mission_sub;
+	int		_capabilities_sub;
+        int		_fence_sub;
+        int		_fence_pub;
 
 	orb_advert_t	_triplet_pub;			/**< position setpoint */
 
@@ -122,9 +145,16 @@ private:
 
 	perf_counter_t	_loop_perf;			/**< loop performance counter */
 
-	unsigned	_mission_items_maxcount;				/**< maximum number of mission items supported */
-	struct	mission_item_s 				* _mission_items;	/**< storage for mission items */
-	bool		_mission_valid;						/**< flag if mission is valid */
+	unsigned        _mission_items_maxcount;                                /**< maximum number of mission items supported */
+	struct        mission_item_s                                 * _mission_items;        /**< storage for mission items */
+	bool                _mission_valid;                                                /**< flag if mission is valid */
+
+
+	struct	fence_s 				_fence;			/**< storage for fence vertices */
+	bool						_fence_valid;		/**< flag if fence is valid */
+        bool						_inside_fence;		/**< vehicle is inside fence */
+
+	struct navigation_capabilities_s		_nav_caps;
 
 	/** manual control states */
 	float		_seatbelt_hold_heading;		/**< heading the system should hold in seatbelt mode */
@@ -170,6 +200,11 @@ private:
 	void		mission_poll();
 
 	/**
+	* Retrieve mission.
+	*/
+	void		mission_update();
+
+	/**
 	 * Control throttle.
 	 */
 	float		control_throttle(float energy_error);
@@ -192,6 +227,14 @@ private:
 	 * Main sensor collection task.
 	 */
 	void		task_main() __attribute__((noreturn));
+
+        void		publish_fence(unsigned vertices);
+
+        void		publish_mission(unsigned points);
+
+        void		publish_safepoints(unsigned points);
+
+	bool		fence_valid(const struct fence_s &fence);
 };
 
 namespace navigator
@@ -218,6 +261,10 @@ Navigator::Navigator() :
 	_vstatus_sub(-1),
 	_params_sub(-1),
 	_manual_control_sub(-1),
+        _fence_sub(-1),
+        _fence_pub(-1),
+        _mission_sub(-1),
+	_capabilities_sub(-1),
 
 /* publications */
 	_triplet_pub(-1),
@@ -227,17 +274,15 @@ Navigator::Navigator() :
 /* states */
 	_mission_items_maxcount(20),
 	_mission_valid(false),
-	_loiter_hold(false)
+	_loiter_hold(false),
+	_fence_valid(false),
+	_inside_fence(true)
 {
-	_mission_items = (mission_item_s*)malloc(sizeof(mission_item_s) * _mission_items_maxcount);
-	if (!_mission_items) {
-		_mission_items_maxcount = 0;
-		warnx("no free RAM to allocate mission, rejecting any waypoints");
-	}
-
+	_global_pos.valid = false;
+	memset(&_fence, 0, sizeof(_fence));
 	_parameter_handles.throttle_cruise = param_find("NAV_DUMMY");
 
-	/* fetch initial parameter values */
+	/* fetch initial values */
 	parameters_update();
 }
 
@@ -283,10 +328,8 @@ Navigator::vehicle_status_poll()
 	/* Check HIL state if vehicle status has changed */
 	orb_check(_vstatus_sub, &vstatus_updated);
 
-	if (vstatus_updated) {
-
+	if (vstatus_updated)
 		orb_copy(ORB_ID(vehicle_status), _vstatus_sub, &_vstatus);
-	}
 }
 
 void
@@ -296,9 +339,8 @@ Navigator::vehicle_attitude_poll()
 	bool att_updated;
 	orb_check(_att_sub, &att_updated);
 
-	if (att_updated) {
+	if (att_updated)
 		orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att);
-	}
 }
 
 void
@@ -308,18 +350,22 @@ Navigator::mission_poll()
 	bool mission_updated;
 	orb_check(_mission_sub, &mission_updated);
 
-	if (mission_updated) {
-
-		struct mission_s mission;
-		orb_copy(ORB_ID(mission), _mission_sub, &mission);
+	if (mission_updated)
+		mission_update();
+}
 
+void
+Navigator::mission_update()
+{
+        struct mission_s mission;
+        if (orb_copy(ORB_ID(mission), _mission_sub, &mission) == OK) {
 		// XXX this is not optimal yet, but a first prototype /
 		// test implementation
 
 		if (mission.count <= _mission_items_maxcount) {
 			/*
-			 * Perform an atomic copy & state update
-			 */
+				* Perform an atomic copy & state update
+				*/
 			irqstate_t flags = irqsave();
 
 			memcpy(_mission_items, mission.items, mission.count * sizeof(struct mission_item_s));
@@ -351,12 +397,22 @@ Navigator::task_main()
 	 */
 	_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
 	_mission_sub = orb_subscribe(ORB_ID(mission));
+	_capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities));
+        _fence_sub = orb_subscribe(ORB_ID(fence));
 	_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
 	_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
 	_vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
 	_params_sub = orb_subscribe(ORB_ID(parameter_update));
 	_manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
 
+	// Load initial states
+	if (orb_copy(ORB_ID(vehicle_status), _vstatus_sub, &_vstatus) != OK) {
+		_vstatus.arming_state = ARMING_STATE_STANDBY;    // for testing... commander may not be running
+	}
+
+	orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att);
+	mission_update();
+
 	/* rate limit vehicle status updates to 5Hz */
 	orb_set_interval(_vstatus_sub, 200);
 	/* rate limit position updates to 50 Hz */
@@ -364,23 +420,35 @@ Navigator::task_main()
 
 	parameters_update();
 
+	_fence_valid = load_fence(GEOFENCE_MAX_VERTICES);
+
+	/* load the craft capabilities */
+	orb_copy(ORB_ID(navigation_capabilities), _capabilities_sub, &_nav_caps);
+
 	/* wakeup source(s) */
-	struct pollfd fds[2];
+	struct pollfd fds[5];
 
 	/* Setup of loop */
 	fds[0].fd = _params_sub;
 	fds[0].events = POLLIN;
 	fds[1].fd = _global_pos_sub;
 	fds[1].events = POLLIN;
+	fds[2].fd = _fence_sub;
+	fds[2].events = POLLIN;
+        fds[3].fd = _capabilities_sub;
+        fds[3].events = POLLIN;
+        fds[4].fd = _mission_sub;
+        fds[4].events = POLLIN;
 
 	while (!_task_should_exit) {
 
-		/* wait for up to 500ms for data */
+		/* wait for up to 100ms for data */
 		int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
 
 		/* timed out - periodic check for _task_should_exit, etc. */
-		if (pret == 0)
+		if (pret == 0) {
 			continue;
+		}
 
 		/* this is undesirable but not much we can do - might want to flag unhappy status */
 		if (pret < 0) {
@@ -403,17 +471,34 @@ Navigator::task_main()
 			parameters_update();
 		}
 
+		/* only update fence if it has changed */
+		if (fds[2].revents & POLLIN) {
+			/* read from fence to clear updated flag */
+			unsigned vertices;
+			orb_copy(ORB_ID(fence), _fence_sub, &vertices);
+			_fence_valid = load_fence(vertices);
+		}
+
+		/* only update craft capabilities if they have changed */
+                if (fds[3].revents & POLLIN) {
+                        orb_copy(ORB_ID(navigation_capabilities), _capabilities_sub, &_nav_caps);
+                }
+
+                if (fds[4].revents & POLLIN) {
+                        mission_update();
+                }
+
 		/* only run controller if position changed */
 		if (fds[1].revents & POLLIN) {
 
-
 			static uint64_t last_run = 0;
 			float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
 			last_run = hrt_absolute_time();
 
 			/* guard against too large deltaT's */
-			if (deltaT > 1.0f)
+			if (deltaT > 1.0f) {
 				deltaT = 0.01f;
+			}
 
 			/* load local copies */
 			orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos);
@@ -422,6 +507,10 @@ Navigator::task_main()
 
 			mission_poll();
 
+			if (_fence_valid && _global_pos.valid) {
+				_inside_fence = inside_geofence(&_global_pos, &_fence);
+			}
+
 			math::Vector2f ground_speed(_global_pos.vx, _global_pos.vy);
 			// Current waypoint
 			math::Vector2f next_wp(_global_triplet.current.lat / 1e7f, _global_triplet.current.lon / 1e7f);
@@ -460,14 +549,14 @@ Navigator::task_main()
 
 					if (_global_triplet.current.nav_cmd == NAV_CMD_WAYPOINT) {
 						/* waypoint is a plain navigation waypoint */
-						
+
 
 					} else if (_global_triplet.current.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
-						_global_triplet.current.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
-						_global_triplet.current.nav_cmd == NAV_CMD_LOITER_UNLIMITED) {
+						   _global_triplet.current.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
+						   _global_triplet.current.nav_cmd == NAV_CMD_LOITER_UNLIMITED) {
 
 						/* waypoint is a loiter waypoint */
-						
+
 					}
 
 					// XXX at this point we always want no loiter hold if a
@@ -528,9 +617,10 @@ Navigator::task_main()
 		}
 
 		perf_end(_loop_perf);
+
 	}
 
-	warnx("exiting.\n");
+	warnx("exiting.");
 
 	_navigator_task = -1;
 	_exit(0);
@@ -543,11 +633,11 @@ Navigator::start()
 
 	/* start the task */
 	_navigator_task = task_spawn_cmd("navigator",
-				       SCHED_DEFAULT,
-				       SCHED_PRIORITY_MAX - 5,
-				       2048,
-				       (main_t)&Navigator::task_main_trampoline,
-				       nullptr);
+					 SCHED_DEFAULT,
+					 SCHED_PRIORITY_MAX - 5,
+					 2048,
+					 (main_t)&Navigator::task_main_trampoline,
+					 nullptr);
 
 	if (_navigator_task < 0) {
 		warn("task start failed");
@@ -557,20 +647,139 @@ Navigator::start()
 	return OK;
 }
 
+void
+Navigator::status() 
+{
+	warnx("Global position is %svalid", _global_pos.valid ? "" : "in");
+	if (_global_pos.valid) {
+		warnx("Longitude %5.5f degrees, latitude %5.5f degrees", _global_pos.lon / 1e7, _global_pos.lat / 1e7);
+		warnx("Altitude %5.5f meters, altitude above home %5.5f meters",
+			(double)_global_pos.alt, (double)_global_pos.relative_alt);
+		warnx("Ground velocity in m/s, x %5.5f, y %5.5f, z %5.5f",
+			(double)_global_pos.vx, (double)_global_pos.vy, (double)_global_pos.vz);
+		warnx("Compass heading in degrees %5.5f", (double)_global_pos.yaw * 57.2957795);
+	}
+        if (_fence_valid) {
+                warnx("Geofence is valid");
+                warnx("Vertex longitude latitude");
+                for (unsigned i = 0; i < _fence.count; i++)
+                        warnx("%6u %9.5f %8.5f", i, (double)_fence.vertices[i].lon, (double)_fence.vertices[i].lat);
+        } else
+                warnx("Geofence not set");
+}
+
+void
+Navigator::publish_fence(unsigned vertices)
+{
+        if (_fence_pub == -1)
+                _fence_pub = orb_advertise(ORB_ID(fence), &vertices);
+        else
+                orb_publish(ORB_ID(fence), _fence_pub, &vertices);
+}
+
+bool
+Navigator::fence_valid(const struct fence_s &fence)
+{
+	struct vehicle_global_position_s pos;
+
+	// NULL fence is valid
+	if (fence.count == 0) {
+		return true;
+	}
+
+	// Otherwise
+	if ((fence.count < 4) || (fence.count > GEOFENCE_MAX_VERTICES)) {
+		warnx("Fence must have at least 3 sides and not more than %d", GEOFENCE_MAX_VERTICES - 1);
+		return false;
+	}
+
+	return true;
+}
+
+bool
+Navigator::load_fence(unsigned vertices)
+{
+	struct fence_s temp_fence;
+
+	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;
+		}
+	}
+
+	temp_fence.count = i;
+
+	if (fence_valid(temp_fence))
+		memcpy(&_fence, &temp_fence, sizeof(_fence));
+	else
+		warnx("Invalid fence file, ignored!");
+
+	return _fence.count != 0;
+}
+
+void
+Navigator::fence_point(int argc, char *argv[])
+{
+        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);
+		publish_fence(0);
+                return;
+        }
+
+        if (argc < 3)
+                errx(1, "Specify: -clear | sequence latitude longitude [-publish]");
+
+        ix = atoi(argv[0]);
+        if (ix >= DM_KEY_FENCE_POINTS_MAX)
+                errx(1, "Sequence must be less than %d", DM_KEY_FENCE_POINTS_MAX);
+
+        lat = strtod(argv[1], &end);
+        lon = strtod(argv[2], &end);
+
+        last = 0;
+        if ((argc > 3) && (strcmp(argv[3], "-publish") == 0))
+                last = 1;
+
+        vertex.lat = (float)lat;
+        vertex.lon = (float)lon;
+
+        if (dm_write(DM_KEY_FENCE_POINTS, ix, DM_PERSIST_POWER_ON_RESET, &vertex, sizeof(vertex)) == sizeof(vertex)) {
+                if (last)
+                        publish_fence((unsigned)ix + 1);
+                return;
+        }
+
+        errx(1, "can't store fence point");
+}
+
+static void usage()
+{
+	errx(1, "usage: navigator {start|stop|status|fence}");
+}
+
 int navigator_main(int argc, char *argv[])
 {
-	if (argc < 1)
-		errx(1, "usage: navigator {start|stop|status}");
+	if (argc < 2) {
+		usage();
+	}
 
 	if (!strcmp(argv[1], "start")) {
 
-		if (navigator::g_navigator != nullptr)
+		if (navigator::g_navigator != nullptr) {
 			errx(1, "already running");
+		}
 
 		navigator::g_navigator = new Navigator;
 
-		if (navigator::g_navigator == nullptr)
+		if (navigator::g_navigator == nullptr) {
 			errx(1, "alloc failed");
+		}
 
 		if (OK != navigator::g_navigator->start()) {
 			delete navigator::g_navigator;
@@ -578,27 +787,25 @@ int navigator_main(int argc, char *argv[])
 			err(1, "start failed");
 		}
 
-		exit(0);
+		return 0;
 	}
 
-	if (!strcmp(argv[1], "stop")) {
-		if (navigator::g_navigator == nullptr)
-			errx(1, "not running");
+	if (navigator::g_navigator == nullptr)
+		errx(1, "not running");
 
+	if (!strcmp(argv[1], "stop")) {
 		delete navigator::g_navigator;
 		navigator::g_navigator = nullptr;
-		exit(0);
-	}
 
-	if (!strcmp(argv[1], "status")) {
-		if (navigator::g_navigator) {
-			errx(0, "running");
+	} else if (!strcmp(argv[1], "status")) {
+		navigator::g_navigator->status();
 
-		} else {
-			errx(1, "not running");
-		}
+        } else if (!strcmp(argv[1], "fence")) {
+                navigator::g_navigator->fence_point(argc - 2, argv + 2);
+
+	} else {
+		usage();
 	}
 
-	warnx("unrecognized command");
-	return 1;
+	return 0;
 }