Merge remote-tracking branch 'jean-m-cyr/master' into navigator_wip

7 files changed, 1246 insertions, 57 deletions

diff --git a/src/modules/dataman/dataman.c b/src/modules/dataman/dataman.c
new file mode 100644
index 000000000..05b77da20
--- /dev/null
+++ b/src/modules/dataman/dataman.c
@@ -0,0 +1,739 @@
+/****************************************************************************
+ *
+ *   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
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+/**
+ * @file navigator_main.c
+ * Implementation of the main navigation state machine.
+ *
+ * Handles missions, geo fencing and failsafe navigation behavior.
+ */
+
+#include <nuttx/config.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <math.h>
+#include <poll.h>
+#include <time.h>
+#include <sys/ioctl.h>
+#include <systemlib/systemlib.h>
+#include <systemlib/err.h>
+#include <queue.h>
+
+#include "dataman.h"
+
+/**
+ * data manager app start / stop handling function
+ *
+ * @ingroup apps
+ */
+
+__EXPORT int dataman_main(int argc, char *argv[]);
+__EXPORT ssize_t dm_read(dm_item_t item, unsigned char index, void *buffer, size_t buflen);
+__EXPORT ssize_t dm_write(dm_item_t  item, unsigned char index, dm_persitence_t persistence, const void *buffer, size_t buflen);
+__EXPORT int dm_clear(dm_item_t item);
+__EXPORT int dm_restart(dm_reset_reason restart_type);
+
+/* Types of function calls supported by the worker task */
+typedef enum {
+	dm_write_func = 0,
+	dm_read_func,
+	dm_clear_func,
+	dm_restart_func,
+	dm_number_of_funcs
+} dm_function_t;
+
+/* Work task work item */
+typedef struct {
+	sq_entry_t link;	/**< list linkage */
+	sem_t wait_sem;
+	dm_function_t func;
+	ssize_t result;
+	union {
+		struct {
+			dm_item_t item;
+			unsigned char index;
+			dm_persitence_t persistence;
+			const void *buf;
+			size_t count;
+		} write_params;
+		struct {
+			dm_item_t item;
+			unsigned char index;
+			void *buf;
+			size_t count;
+		} read_params;
+		struct {
+			dm_item_t item;
+		} clear_params;
+		struct {
+			dm_reset_reason reason;
+		} restart_params;
+	};
+} work_q_item_t;
+
+/* Usage statistics */
+static unsigned g_func_counts[dm_number_of_funcs];
+
+/* table of maximum number of instances for each item type */
+static const unsigned g_per_item_max_index[DM_KEY_NUM_KEYS] = {
+	DM_KEY_SAFE_POINTS_MAX,
+	DM_KEY_FENCE_POINTS_MAX,
+	DM_KEY_WAY_POINTS_MAX,
+};
+
+/* Table of offset for index 0 of each item type */
+static unsigned int g_key_offsets[DM_KEY_NUM_KEYS];
+
+/* The data manager store file handle and file name */
+static int g_fd = -1, g_task_fd = -1;
+static const char *k_data_manager_device_path = "/fs/microsd/dataman";
+
+/* The data manager work queues */
+
+typedef struct {
+	sq_queue_t q;
+	sem_t mutex;	/* Mutual exclusion on work queue adds and deletes */
+	unsigned size;
+	unsigned max_size;
+} work_q_t;
+
+static work_q_t g_free_q;
+static work_q_t g_work_q;
+
+sem_t g_work_queued_sema;
+sem_t g_init_sema;
+
+static bool g_task_should_exit;		/**< if true, sensor task should exit */
+
+#define DM_SECTOR_HDR_SIZE 4
+static const unsigned k_sector_size = DM_MAX_DATA_SIZE + DM_SECTOR_HDR_SIZE;
+
+static void init_q(work_q_t *q)
+{
+	sq_init(&(q->q));
+	sem_init(&(q->mutex), 1, 1);
+	q->size = q->max_size = 0;
+}
+
+static void destroy_q(work_q_t *q)
+{
+	sem_destroy(&(q->mutex));
+}
+
+static inline void
+lock_queue(work_q_t *q)
+{
+	sem_wait(&(q->mutex));
+}
+
+static inline void
+unlock_queue(work_q_t *q)
+{
+	sem_post(&(q->mutex));
+}
+
+static work_q_item_t *
+create_work_item(void)
+{
+	work_q_item_t *item;
+
+	lock_queue(&g_free_q);
+	if ((item = (work_q_item_t *)sq_remfirst(&(g_free_q.q))))
+		g_free_q.size--;
+	unlock_queue(&g_free_q);
+
+	if (item == NULL)
+		item = (work_q_item_t *)malloc(sizeof(work_q_item_t));
+
+	if (item)
+		sem_init(&item->wait_sem, 1, 0); /* Caller will wait on this... initially locked */
+
+	return item;
+}
+
+/* Work queue management functions */
+static void
+enqueue_work_item(work_q_item_t *item)
+{
+	/* put the work item on the work queue */
+	lock_queue(&g_work_q);
+	sq_addlast(&item->link, &(g_work_q.q));
+
+	if (++g_work_q.size > g_work_q.max_size)
+		g_work_q.max_size = g_work_q.size;
+
+	unlock_queue(&g_work_q);
+
+	/* tell the work thread that work is available */
+	sem_post(&g_work_queued_sema);
+}
+
+static void
+destroy_work_item(work_q_item_t *item)
+{
+	sem_destroy(&item->wait_sem);
+	lock_queue(&g_free_q);
+	sq_addfirst(&item->link, &(g_free_q.q));
+
+	if (++g_free_q.size > g_free_q.max_size)
+		g_free_q.max_size = g_free_q.size;
+
+	unlock_queue(&g_free_q);
+}
+
+static work_q_item_t *
+dequeue_work_item(void)
+{
+	work_q_item_t *work;
+	lock_queue(&g_work_q);
+
+	if ((work = (work_q_item_t *)sq_remfirst(&g_work_q.q)))
+		g_work_q.size--;
+
+	unlock_queue(&g_work_q);
+	return work;
+}
+
+/* Calculate the offset in file of specific item */
+static int
+calculate_offset(dm_item_t item, unsigned char index)
+{
+
+	/* Make sure the item type is valid */
+	if (item >= DM_KEY_NUM_KEYS)
+		return -1;
+
+	/* Make sure the index for this item type is valid */
+	if (index >= g_per_item_max_index[item])
+		return -1;
+
+	/* Calculate and return the item index based on type and index */
+	return g_key_offsets[item] + (index * k_sector_size);
+}
+
+/* Each data item is stored as follows
+ *
+ * byte 0: Length of user data item
+ * byte 1: Persistence of this data item
+ * byte DM_SECTOR_HDR_SIZE... : data item value
+ *
+ * The total size must not exceed k_sector_size
+ */
+
+/* write to the data manager file */
+static ssize_t
+_write(dm_item_t item, unsigned char index, dm_persitence_t persistence, const void *buf, size_t count)
+{
+	unsigned char buffer[k_sector_size];
+	size_t len;
+	int offset;
+
+	/* Get the offset for this item */
+	offset = calculate_offset(item, index);
+
+	if (offset < 0)
+		return -1;
+
+	/* Make sure caller has not given us more data than we can handle */
+	if (count > DM_MAX_DATA_SIZE)
+		return -1;
+
+	/* Write out the data, prefixed with length and persistence level */
+	buffer[0] = count;
+	buffer[1] = persistence;
+	buffer[2] = 0;
+	buffer[3] = 0;
+	memcpy(buffer + DM_SECTOR_HDR_SIZE, buf, count);
+	count += DM_SECTOR_HDR_SIZE;
+
+	len = -1;
+
+	if (lseek(g_task_fd, offset, SEEK_SET) == offset)
+		if ((len = write(g_task_fd, buffer, count)) == count)
+			fsync(g_task_fd);
+
+	if (len != count)
+		return -1;
+
+	/* All is well... return the number of user data written */
+	return count - DM_SECTOR_HDR_SIZE;
+}
+
+/* Retrieve from the data manager file */
+static ssize_t
+_read(dm_item_t item, unsigned char index, void *buf, size_t count)
+{
+	unsigned char buffer[k_sector_size];
+	int len, offset;
+
+	/* Get the offset for this item */
+	offset = calculate_offset(item, index);
+
+	if (offset < 0)
+		return -1;
+
+	/* Make sure the caller hasn't asked for more data than we can handle */
+	if (count > DM_MAX_DATA_SIZE)
+		return -1;
+
+	/* Read the prefix and data */
+	len = -1;
+	if (lseek(g_task_fd, offset, SEEK_SET) == offset)
+		len = read(g_task_fd, buffer, count + DM_SECTOR_HDR_SIZE);
+
+	/* Check for length issues */
+	if (len < 0)
+		return -1;
+
+	if (len == 0)
+		buffer[0] = 0;
+
+	if (buffer[0] > 0) {
+		if (buffer[0] > count)
+			return -1;
+
+		/* Looks good, copy it to the caller's buffer */
+		memcpy(buf, buffer + DM_SECTOR_HDR_SIZE, buffer[0]);
+	}
+
+	/* Return the number of bytes of caller data read */
+	return buffer[0];
+}
+
+static int
+_clear(dm_item_t item)
+{
+	int i, result = 0;
+
+	int offset = calculate_offset(item, 0);
+
+	if (offset < 0)
+		return -1;
+
+	for (i = 0; (unsigned)i < g_per_item_max_index[item]; i++) {
+		char buf[1];
+
+		if (lseek(g_task_fd, offset, SEEK_SET) != offset) {
+			result = -1;
+			break;
+		}
+
+		if (read(g_task_fd, buf, 1) < 1)
+			break;
+
+		if (buf[0]) {
+			if (lseek(g_task_fd, offset, SEEK_SET) != offset) {
+				result = -1;
+				break;
+			}
+
+			buf[0] = 0;
+
+			if (write(g_task_fd, buf, 1) != 1) {
+				result = -1;
+				break;
+			}
+		}
+
+		offset += k_sector_size;
+	}
+
+	fsync(g_task_fd);
+	return result;
+}
+
+/* Tell the data manager about the type of the last reset */
+static int
+_restart(dm_reset_reason reason)
+{
+	unsigned char buffer[2];
+	int offset, result = 0;
+
+	/* We need to scan the entire file and invalidate and data that should not persist after the last reset */
+
+	/* Loop through all of the data segments and delete those that are not persistent */
+	offset = 0;
+
+	while (1) {
+		size_t len;
+
+		/* Get data segment at current offset */
+		if (lseek(g_task_fd, offset, SEEK_SET) != offset) {
+			result = -1;
+			break;
+		}
+
+		len = read(g_task_fd, buffer, sizeof(buffer));
+
+		if (len == 0)
+			break;
+
+		/* check if segment contains data */
+		if (buffer[0]) {
+			int clear_entry = 0;
+
+			/* Whether data gets deleted depends on reset type and data segment's persistence setting */
+			if (reason == DM_INIT_REASON_POWER_ON) {
+				if (buffer[1] != DM_PERSIST_POWER_ON_RESET) {
+					clear_entry = 1;
+				}
+
+			} else {
+				if ((buffer[1] != DM_PERSIST_POWER_ON_RESET) && (buffer[1] != DM_PERSIST_IN_FLIGHT_RESET)) {
+					clear_entry = 1;
+				}
+			}
+
+			/* Set segment to unused if data does not persist */
+			if (clear_entry) {
+				if (lseek(g_task_fd, offset, SEEK_SET) != offset) {
+					result = -1;
+					break;
+				}
+
+				buffer[0] = 0;
+
+				len = write(g_task_fd, buffer, 1);
+
+				if (len != 1) {
+					result = -1;
+					break;
+				}
+			}
+		}
+
+		offset += k_sector_size;
+	}
+
+	fsync(g_task_fd);
+
+	/* tell the caller how it went */
+	return result;
+}
+
+/* write to the data manager file */
+__EXPORT ssize_t
+dm_write(dm_item_t item, unsigned char index, dm_persitence_t persistence, const void *buf, size_t count)
+{
+	work_q_item_t *work;
+
+	if ((g_fd < 0) || g_task_should_exit)
+		return -1;
+
+	/* Will return with queues locked */
+	if ((work = create_work_item()) == NULL)
+		return -1; /* queues unlocked on failure */
+
+	work->func = dm_write_func;
+	work->write_params.item = item;
+	work->write_params.index = index;
+	work->write_params.persistence = persistence;
+	work->write_params.buf = buf;
+	work->write_params.count = count;
+	enqueue_work_item(work);
+
+	sem_wait(&work->wait_sem);
+	ssize_t result = work->result;
+	destroy_work_item(work);
+	return result;
+}
+
+/* Retrieve from the data manager file */
+__EXPORT ssize_t
+dm_read(dm_item_t item, unsigned char index, void *buf, size_t count)
+{
+	work_q_item_t *work;
+
+	if ((g_fd < 0) || g_task_should_exit)
+		return -1;
+
+	/* Will return with queues locked */
+	if ((work = create_work_item()) == NULL)
+		return -1; /* queues unlocked on failure */
+
+	work->func = dm_read_func;
+	work->read_params.item = item;
+	work->read_params.index = index;
+	work->read_params.buf = buf;
+	work->read_params.count = count;
+	enqueue_work_item(work);
+
+	sem_wait(&work->wait_sem);
+	ssize_t result = work->result;
+	destroy_work_item(work);
+	return result;
+}
+
+__EXPORT int
+dm_clear(dm_item_t item)
+{
+	work_q_item_t *work;
+
+	if ((g_fd < 0) || g_task_should_exit)
+		return -1;
+
+	/* Will return with queues locked */
+	if ((work = create_work_item()) == NULL)
+		return -1; /* queues unlocked on failure */
+
+	work->func = dm_clear_func;
+	work->clear_params.item = item;
+	enqueue_work_item(work);
+
+	sem_wait(&work->wait_sem);
+	int result = work->result;
+	destroy_work_item(work);
+	return result;
+}
+
+/* Tell the data manager about the type of the last reset */
+__EXPORT int
+dm_restart(dm_reset_reason reason)
+{
+	work_q_item_t *work;
+
+	if ((g_fd < 0) || g_task_should_exit)
+		return -1;
+
+	/* Will return with queues locked */
+	if ((work = create_work_item()) == NULL)
+		return -1; /* queues unlocked on failure */
+
+	work->func = dm_restart_func;
+	work->restart_params.reason = reason;
+	enqueue_work_item(work);
+
+	sem_wait(&work->wait_sem);
+	int result = work->result;
+	destroy_work_item(work);
+	return result;
+}
+
+static int
+task_main(int argc, char *argv[])
+{
+	work_q_item_t *work;
+
+	/* inform about start */
+	warnx("Initializing..");
+
+	/* Initialize global variables */
+	g_key_offsets[0] = 0;
+
+	for (unsigned i = 0; i < (DM_KEY_NUM_KEYS - 1); i++)
+		g_key_offsets[i + 1] = g_key_offsets[i] + (g_per_item_max_index[i] * k_sector_size);
+
+	unsigned max_offset = g_key_offsets[DM_KEY_NUM_KEYS - 1] + (g_per_item_max_index[DM_KEY_NUM_KEYS - 1] * k_sector_size);
+
+	for (unsigned i = 0; i < dm_number_of_funcs; i++)
+		g_func_counts[i] = 0;
+
+	g_task_should_exit = false;
+
+	init_q(&g_work_q);
+	init_q(&g_free_q);
+
+	sem_init(&g_work_queued_sema, 1, 0);
+
+	g_task_fd = open(k_data_manager_device_path, O_RDWR | O_CREAT | O_BINARY);
+	if (g_task_fd < 0) {
+		warnx("Could not open data manager file %s", k_data_manager_device_path);
+		return -1;
+	}
+	if (lseek(g_task_fd, max_offset, SEEK_SET) != max_offset) {
+		close(g_task_fd);
+		warnx("Could not seek data manager file %s", k_data_manager_device_path);
+		return -1;
+	}
+	fsync(g_task_fd);
+
+	g_fd = g_task_fd;
+
+	warnx("Initialized, data manager file '%s' size is %d bytes", k_data_manager_device_path, max_offset);
+
+	sem_post(&g_init_sema);
+
+	/* Start the endless loop, waiting for then processing work requests */
+	while (true) {
+
+		/* do we need to exit ??? */
+		if ((g_task_should_exit) && (g_fd >= 0)) {
+			/* Close the file handle to stop further queueing */
+			g_fd = -1;
+		}
+
+		if (!g_task_should_exit) {
+			/* wait for work */
+			sem_wait(&g_work_queued_sema);
+		}
+
+		/* Empty the work queue */
+		while ((work = dequeue_work_item())) {
+
+			switch (work->func) {
+			case dm_write_func:
+				g_func_counts[dm_write_func]++;
+				work->result =
+					_write(work->write_params.item, work->write_params.index, work->write_params.persistence, work->write_params.buf, work->write_params.count);
+				break;
+
+			case dm_read_func:
+				g_func_counts[dm_read_func]++;
+				work->result =
+					_read(work->read_params.item, work->read_params.index, work->read_params.buf, work->read_params.count);
+				break;
+
+			case dm_clear_func:
+				g_func_counts[dm_clear_func]++;
+				work->result = _clear(work->clear_params.item);
+				break;
+
+			case dm_restart_func:
+				g_func_counts[dm_restart_func]++;
+				work->result = _restart(work->restart_params.reason);
+				break;
+
+			default: /* should never happen */
+				work->result = -1;
+				break;
+			}
+
+			/* Inform the caller that work is done */
+			sem_post(&work->wait_sem);
+		}
+
+		/* time to go???? */
+		if ((g_task_should_exit) && (g_fd < 0))
+			break;
+	}
+
+	close(g_task_fd);
+	g_task_fd = -1;
+
+	/* Empty the work queue */
+	for (;;) {
+		if ((work = (work_q_item_t *)sq_remfirst(&(g_free_q.q))) == NULL)
+			break;
+
+		free(work);
+	}
+
+	destroy_q(&g_work_q);
+	destroy_q(&g_free_q);
+	sem_destroy(&g_work_queued_sema);
+
+	return 0;
+}
+
+static int
+start(void)
+{
+	int task;
+
+	sem_init(&g_init_sema, 1, 0);
+
+	/* start the task */
+	if ((task = task_spawn_cmd("dataman", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 2048, task_main, NULL)) <= 0) {
+		warn("task start failed");
+		return -1;
+	}
+
+	/* wait for the thread to actuall initialize */
+	sem_wait(&g_init_sema);
+	sem_destroy(&g_init_sema);
+
+	return 0;
+}
+
+static void
+status(void)
+{
+	/* display usage statistics */
+	warnx("Writes   %d", g_func_counts[dm_write_func]);
+	warnx("Reads    %d", g_func_counts[dm_read_func]);
+	warnx("Clears   %d", g_func_counts[dm_clear_func]);
+	warnx("Restarts %d", g_func_counts[dm_restart_func]);
+	warnx("Max Q lengths work %d, free %d", g_work_q.max_size, g_free_q.max_size);
+}
+
+static void
+stop(void)
+{
+	/* Tell the worker task to shut down */
+	g_task_should_exit = true;
+	sem_post(&g_work_queued_sema);
+}
+
+static void
+usage(void)
+{
+	errx(1, "usage: dataman {start|stop|status}");
+}
+
+int
+dataman_main(int argc, char *argv[])
+{
+	if (argc < 2)
+		usage();
+
+	if (!strcmp(argv[1], "start")) {
+
+		if (g_fd >= 0)
+			errx(1, "already running");
+
+		start();
+
+		if (g_fd < 0)
+			errx(1, "start failed");
+
+		return 0;
+	}
+
+	if (g_fd < 0)
+		errx(1, "not running");
+
+	if (!strcmp(argv[1], "stop"))
+		stop();
+	else if (!strcmp(argv[1], "status"))
+		status();
+	else
+		usage();
+
+	return 0;
+}
+
diff --git a/src/modules/dataman/dataman.h b/src/modules/dataman/dataman.h
new file mode 100644
index 000000000..41ddfaf61
--- /dev/null
+++ b/src/modules/dataman/dataman.h
@@ -0,0 +1,116 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 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
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file dataman.h
+ *
+ * DATAMANAGER driver.
+ */
+#ifndef _DATAMANAGER_H
+#define _DATAMANAGER_H
+
+#include <uORB/topics/mission.h>
+#include <uORB/topics/fence.h>
+#include <mavlink/waypoints.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+	/* Types of items that the data manager can store */
+	typedef enum {
+		DM_KEY_SAFE_POINTS = 0,		/* Safe points coordinates, safe point 0 is home point */
+		DM_KEY_FENCE_POINTS,		/* Fence vertex coordinates */
+		DM_KEY_WAY_POINTS,		/* Mission way point coordinates */
+		DM_KEY_NUM_KEYS			/* Total number of item types defined */
+	} dm_item_t;
+
+	/* The maximum number of instances for each item type */
+	enum {
+		DM_KEY_SAFE_POINTS_MAX = 8,
+		DM_KEY_FENCE_POINTS_MAX = GEOFENCE_MAX_VERTICES,
+		DM_KEY_WAY_POINTS_MAX = MAVLINK_WPM_MAX_WP_COUNT,
+	};
+
+	/* Data persistence levels */
+	typedef enum {
+		DM_PERSIST_POWER_ON_RESET = 0,	/* Data survives all resets */
+		DM_PERSIST_IN_FLIGHT_RESET,     /* Data survives in-flight resets only */
+		DM_PERSIST_VOLATILE             /* Data does not survive resets */
+	} dm_persitence_t;
+
+	/* The reason for the last reset */
+	typedef enum {
+		DM_INIT_REASON_POWER_ON = 0,	/* Data survives resets */
+		DM_INIT_REASON_IN_FLIGHT	/* Data survives in-flight resets only */
+	} dm_reset_reason;
+
+	/* Maximum size in bytes of a single item instance */
+	#define DM_MAX_DATA_SIZE 126
+
+	/* Retrieve from the data manager store */
+	__EXPORT ssize_t
+	dm_read(
+		dm_item_t item,			/* The item type to retrieve */
+		unsigned char index,		/* The index of the item */
+		void *buffer,			/* Pointer to caller data buffer */
+		size_t buflen			/* Length in bytes of data to retrieve */
+	);
+
+	/* write to the data manager store */
+	__EXPORT ssize_t
+	dm_write(
+		dm_item_t  item,		/* The item type to store */
+		unsigned char index,		/* The index of the item */
+		dm_persitence_t persistence,	/* The persistence level of this item */
+		const void *buffer,		/* Pointer to caller data buffer */
+		size_t buflen			/* Length in bytes of data to retrieve */
+	);
+
+	/* Retrieve from the data manager store */
+	__EXPORT int
+	dm_clear(
+		dm_item_t item			/* The item type to clear */
+		);
+
+	/* Tell the data manager about the type of the last reset */
+	__EXPORT int
+	dm_restart(
+		dm_reset_reason restart_type	/* The last reset type */
+	);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/src/modules/dataman/module.mk b/src/modules/dataman/module.mk
new file mode 100644
index 000000000..dce7a6235
--- /dev/null
+++ b/src/modules/dataman/module.mk
@@ -0,0 +1,42 @@
+############################################################################
+#
+#   Copyright (c) 2013 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
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+#
+# Main Navigation Controller
+#
+
+MODULE_COMMAND	= dataman
+
+SRCS		= dataman.c
+
+INCLUDE_DIRS	 += $(MAVLINK_SRC)/include/mavlink
diff --git a/src/modules/navigator/module.mk b/src/modules/navigator/module.mk
index 0404b06c7..7f7443c43 100644
--- a/src/modules/navigator/module.mk
+++ b/src/modules/navigator/module.mk
@@ -39,3 +39,5 @@ MODULE_COMMAND	= navigator
 
 SRCS		= navigator_main.cpp \
 		  navigator_params.c
+
+INCLUDE_DIRS	 += $(MAVLINK_SRC)/include/mavlink
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;
 }
diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp
index 3514dca24..769fe9045 100644
--- a/src/modules/uORB/objects_common.cpp
+++ b/src/modules/uORB/objects_common.cpp
@@ -129,6 +129,9 @@ ORB_DEFINE(vehicle_global_velocity_setpoint, struct vehicle_global_velocity_setp
 #include "topics/mission.h"
 ORB_DEFINE(mission, struct mission_s);
 
+#include "topics/fence.h"
+ORB_DEFINE(fence, unsigned);
+
 #include "topics/vehicle_attitude_setpoint.h"
 ORB_DEFINE(vehicle_attitude_setpoint, struct vehicle_attitude_setpoint_s);
 
diff --git a/src/modules/uORB/topics/fence.h b/src/modules/uORB/topics/fence.h
new file mode 100644
index 000000000..6f16c51cf
--- /dev/null
+++ b/src/modules/uORB/topics/fence.h
@@ -0,0 +1,80 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Author: @author Jean Cyr <jean.m.cyr@gmail.com>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file fence.h
+ * Definition of geofence.
+ */
+
+#ifndef TOPIC_FENCE_H_
+#define TOPIC_FENCE_H_
+
+#include <stdint.h>
+#include <stdbool.h>
+#include "../uORB.h"
+
+/**
+ * @addtogroup topics
+ * @{
+ */
+
+#define GEOFENCE_MAX_VERTICES 16
+
+/**
+ * This is the position of a geofence vertex
+ *
+ */
+struct fence_vertex_s {
+	// Worst case float precision gives us 2 meter resolution at the equator
+	float lat;			/**< latitude in degrees */
+	float lon;			/**< longitude in degrees */
+};
+
+/**
+ * This is the position of a geofence
+ *
+ */
+struct fence_s { 
+	unsigned count;     /**< number of actual vertices */
+	struct fence_vertex_s vertices[GEOFENCE_MAX_VERTICES];
+};
+
+/**
+ * @}
+ */
+
+/* register this as object request broker structure */
+ORB_DECLARE(fence);
+
+#endif