diff --git a/src/modules/dataman/dataman.c b/src/modules/dataman/dataman.c
index 05b77da20d..dd3573d9a3 100644
--- a/src/modules/dataman/dataman.c
+++ b/src/modules/dataman/dataman.c
@@ -33,10 +33,8 @@
  *
  ****************************************************************************/
 /**
- * @file navigator_main.c
- * Implementation of the main navigation state machine.
- *
- * Handles missions, geo fencing and failsafe navigation behavior.
+ * @file dataman.c
+ * DATAMANAGER driver.
  */
 
 #include <nuttx/config.h>
@@ -113,7 +111,7 @@ static unsigned g_func_counts[dm_number_of_funcs];
 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,
+	DM_KEY_WAYPOINTS_MAX,
 };
 
 /* Table of offset for index 0 of each item type */
@@ -138,7 +136,7 @@ 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 */
+static bool g_task_should_exit;		/**< if true, dataman task should exit */
 
 #define DM_SECTOR_HDR_SIZE 4
 static const unsigned k_sector_size = DM_MAX_DATA_SIZE + DM_SECTOR_HDR_SIZE;
@@ -266,11 +264,11 @@ _write(dm_item_t item, unsigned char index, dm_persitence_t persistence, const v
 	/* Get the offset for this item */
 	offset = calculate_offset(item, index);
 
-	if (offset < 0)
+	if (offset < 0) 
 		return -1;
 
 	/* Make sure caller has not given us more data than we can handle */
-	if (count > DM_MAX_DATA_SIZE)
+	if (count > DM_MAX_DATA_SIZE) 
 		return -1;
 
 	/* Write out the data, prefixed with length and persistence level */
@@ -456,7 +454,7 @@ dm_write(dm_item_t item, unsigned char index, dm_persitence_t persistence, const
 		return -1;
 
 	/* Will return with queues locked */
-	if ((work = create_work_item()) == NULL)
+	if ((work = create_work_item()) == NULL) 
 		return -1; /* queues unlocked on failure */
 
 	work->func = dm_write_func;
diff --git a/src/modules/dataman/dataman.h b/src/modules/dataman/dataman.h
index 41ddfaf61b..9e1f789ad3 100644
--- a/src/modules/dataman/dataman.h
+++ b/src/modules/dataman/dataman.h
@@ -41,7 +41,6 @@
 
 #include <uORB/topics/mission.h>
 #include <uORB/topics/fence.h>
-#include <mavlink/waypoints.h>
 
 #ifdef __cplusplus
 extern "C" {
@@ -51,7 +50,7 @@ extern "C" {
 	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_WAYPOINTS,		/* Mission way point coordinates */
 		DM_KEY_NUM_KEYS			/* Total number of item types defined */
 	} dm_item_t;
 
@@ -59,7 +58,7 @@ extern "C" {
 	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,
+		DM_KEY_WAYPOINTS_MAX = NUM_MISSIONS_SUPPORTED
 	};
 
 	/* Data persistence levels */
diff --git a/src/systemcmds/tests/test_dataman.c b/src/systemcmds/tests/test_dataman.c
index e33c5acebe..7de87b476f 100644
--- a/src/systemcmds/tests/test_dataman.c
+++ b/src/systemcmds/tests/test_dataman.c
@@ -83,12 +83,15 @@ task_main(int argc, char *argv[])
 	srand(hrt_absolute_time() ^ my_id);
 	unsigned hit = 0, miss = 0;
 	wstart = hrt_absolute_time();
-	for (unsigned i = 0; i < 256; i++) {
+	for (unsigned i = 0; i < NUM_MISSIONS_SUPPORTED; i++) {
 		memset(buffer, my_id, sizeof(buffer));
 		buffer[1] = i;
 		unsigned hash = i ^ my_id;
 		unsigned len = (hash & 63) + 2;
-		if (dm_write(DM_KEY_WAY_POINTS, hash, DM_PERSIST_IN_FLIGHT_RESET, buffer, len) != len) {
+
+		int ret = dm_write(DM_KEY_WAYPOINTS, hash, DM_PERSIST_IN_FLIGHT_RESET, buffer, len);
+		warnx("ret: %d", ret);
+		if (ret != len) {
 			warnx("%d write failed, index %d, length %d", my_id, hash, len);
 			goto fail;
 		}
@@ -97,10 +100,10 @@ task_main(int argc, char *argv[])
 	rstart = hrt_absolute_time();
 	wend = rstart;
 
-	for (unsigned i = 0; i < 256; i++) {
+	for (unsigned i = 0; i < NUM_MISSIONS_SUPPORTED; i++) {
 		unsigned hash = i ^ my_id;
 		unsigned len2, len = (hash & 63) + 2;
-		if ((len2 = dm_read(DM_KEY_WAY_POINTS, hash, buffer, sizeof(buffer))) < 2) {
+		if ((len2 = dm_read(DM_KEY_WAYPOINTS, hash, buffer, sizeof(buffer))) < 2) {
 			warnx("%d read failed length test, index %d", my_id, hash);
 			goto fail;
 		}
@@ -120,7 +123,7 @@ task_main(int argc, char *argv[])
 	}
 	rend = hrt_absolute_time();
 	warnx("Test %d pass, hit %d, miss %d, io time read %llums. write %llums.",
-		my_id, hit, miss, (rend - rstart) / 256000, (wend - wstart) / 256000);
+		my_id, hit, miss, (rend - rstart) / NUM_MISSIONS_SUPPORTED / 1000, (wend - wstart) / NUM_MISSIONS_SUPPORTED / 1000);
 	sem_post(sems + my_id);
 	return 0;
 fail:
@@ -159,18 +162,18 @@ int test_dataman(int argc, char *argv[])
 	}
 	free(sems);
 	dm_restart(DM_INIT_REASON_IN_FLIGHT);
-	for (i = 0; i < 256; i++) {
-		if (dm_read(DM_KEY_WAY_POINTS, i, buffer, sizeof(buffer)) != 0)
+	for (i = 0; i < NUM_MISSIONS_SUPPORTED; i++) {
+		if (dm_read(DM_KEY_WAYPOINTS, i, buffer, sizeof(buffer)) != 0)
 			break;
 	}
-	if (i >= 256) {
+	if (i >= NUM_MISSIONS_SUPPORTED) {
 		warnx("Restart in-flight failed");
 		return -1;
 
 	}
 	dm_restart(DM_INIT_REASON_POWER_ON);
-	for (i = 0; i < 256; i++) {
-		if (dm_read(DM_KEY_WAY_POINTS, i, buffer, sizeof(buffer)) != 0) {
+	for (i = 0; i < NUM_MISSIONS_SUPPORTED; i++) {
+		if (dm_read(DM_KEY_WAYPOINTS, i, buffer, sizeof(buffer)) != 0) {
 			warnx("Restart power-on failed");
 			return -1;
 		}