Merge branch 'fw_autoland_att_tecs_navigator' into bottle_drop_navigator

Conflicts:
	src/modules/navigator/navigator_main.cpp

7 files changed, 165 insertions, 308 deletions

diff --git a/src/modules/bottle_drop/bottle_drop.c b/src/modules/bottle_drop/bottle_drop.c
index 1c2e008ba..e2194bae2 100644
--- a/src/modules/bottle_drop/bottle_drop.c
+++ b/src/modules/bottle_drop/bottle_drop.c
@@ -272,21 +272,21 @@ int bottle_drop_thread_main(int argc, char *argv[]) {
 	memset(&actuators, 0, sizeof(actuators));
 	orb_advert_t actuator_pub = orb_advertise(ORB_ID(actuator_controls_1), &actuators);
 
-	struct mission_s onboard_mission;
-	memset(&onboard_mission, 0, sizeof(onboard_mission));
-	onboard_mission.items = (struct mission_item_s*)malloc(sizeof(struct mission_item_s) * 2);
+	// struct mission_s onboard_mission;
+	// memset(&onboard_mission, 0, sizeof(onboard_mission));
+	// onboard_mission.items = (struct mission_item_s*)malloc(sizeof(struct mission_item_s) * 2);
 	
-	struct mission_item_s *flight_vector_s = &onboard_mission.items[0];
-	struct mission_item_s *flight_vector_e = &onboard_mission.items[1];
+	struct mission_item_s *flight_vector_s = NULL;//&onboard_mission.items[0];
+	struct mission_item_s *flight_vector_e = NULL;//&onboard_mission.items[1];
 
-	flight_vector_s->nav_cmd = NAV_CMD_WAYPOINT;
-	flight_vector_s->radius = 50; // TODO: make parameter
-	flight_vector_s->autocontinue = true;
-	flight_vector_e->nav_cmd = NAV_CMD_WAYPOINT;
-	flight_vector_e->radius = 50; // TODO: make parameter
-	flight_vector_e->autocontinue = true;
+	// flight_vector_s->nav_cmd = NAV_CMD_WAYPOINT;
+	// flight_vector_s->radius = 50; // TODO: make parameter
+	// flight_vector_s->autocontinue = true;
+	// flight_vector_e->nav_cmd = NAV_CMD_WAYPOINT;
+	// flight_vector_e->radius = 50; // TODO: make parameter
+	// flight_vector_e->autocontinue = true;
 
-	orb_advert_t onboard_mission_pub = orb_advertise(ORB_ID(onboard_mission), &onboard_mission);
+	// orb_advert_t onboard_mission_pub = orb_advertise(ORB_ID(onboard_mission), &onboard_mission);
 
 	struct pollfd fds[] = {
 		{ .fd = vehicle_attitude_sub, .events = POLLIN }
@@ -448,18 +448,18 @@ int bottle_drop_thread_main(int argc, char *argv[]) {
 				// 	warnx("future_distance: %.2f, precision: %.2f", future_distance, precision);
 				// }
 
-				onboard_mission.count = 2;
+				// onboard_mission.count = 2;
 
-				if (state_run && !state_drop) {
-					onboard_mission.current_index = 0;
-				} else {
-					onboard_mission.current_index = -1;
-				}
+				// if (state_run && !state_drop) {
+				// 	onboard_mission.current_index = 0;
+				// } else {
+				// 	onboard_mission.current_index = -1;
+				// }
 				
 				// if (counter % 10 ==0)
 				//	warnx("Distance real: %.2f, distance_open_door: %.2f, angle to wind: %.2f", distance_real, distance_open_door, fabsf(_wrap_pi(globalpos.yaw-atan2f(wind_speed.vy,wind_speed.vx)+M_PI_F)));
 
-				orb_publish(ORB_ID(onboard_mission), onboard_mission_pub, &onboard_mission);
+				// orb_publish(ORB_ID(onboard_mission), onboard_mission_pub, &onboard_mission);
 
 			}
 
diff --git a/src/modules/dataman/dataman.c b/src/modules/dataman/dataman.c
index 05b77da20..dd3573d9a 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 41ddfaf61..9e1f789ad 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/modules/mavlink/waypoints.c b/src/modules/mavlink/waypoints.c
index 809900d7d..7aad5038d 100644
--- a/src/modules/mavlink/waypoints.c
+++ b/src/modules/mavlink/waypoints.c
@@ -52,10 +52,8 @@
 #include <uORB/uORB.h>
 #include <uORB/topics/mission.h>
 
-
-#ifndef FM_PI
-#define FM_PI 3.1415926535897932384626433832795f
-#endif
+#include <geo/geo.h>
+#include <dataman/dataman.h>
 
 bool debug = false;
 bool verbose = false;
@@ -63,13 +61,22 @@ bool verbose = false;
 orb_advert_t mission_pub = -1;
 struct mission_s mission;
 
-#define NUM_MISSIONS_SUPPORTED 10
-
 //#define MAVLINK_WPM_NO_PRINTF
 #define MAVLINK_WPM_VERBOSE 1
 
 uint8_t mavlink_wpm_comp_id = MAV_COMP_ID_MISSIONPLANNER;
 
+void publish_mission()
+{
+	/* Initialize mission publication if necessary */
+	if (mission_pub < 0) {
+		mission_pub = orb_advertise(ORB_ID(mission), &mission);
+
+	} else {
+		orb_publish(ORB_ID(mission), mission_pub, &mission);
+	}
+}
+
 int map_mavlink_mission_item_to_mission_item(const mavlink_mission_item_t *mavlink_mission_item, struct mission_item_s *mission_item)
 {
 	/* only support global waypoints for now */
@@ -96,7 +103,7 @@ int map_mavlink_mission_item_to_mission_item(const mavlink_mission_item_t *mavli
 			return MAV_MISSION_ERROR;
 	}
 
-	mission_item->yaw = mavlink_mission_item->param4*M_DEG_TO_RAD_F;
+	mission_item->yaw = _wrap_pi(mavlink_mission_item->param4*M_DEG_TO_RAD_F);
 	mission_item->loiter_radius = fabsf(mavlink_mission_item->param3);
 	mission_item->loiter_direction = (mavlink_mission_item->param3 > 0) ? 1 : -1; /* 1 if positive CW, -1 if negative CCW */
 	mission_item->nav_cmd = mavlink_mission_item->command;
@@ -104,6 +111,7 @@ int map_mavlink_mission_item_to_mission_item(const mavlink_mission_item_t *mavli
 	mission_item->time_inside = mavlink_mission_item->param2 / 1e3f; /* from milliseconds to seconds */
 	mission_item->autocontinue = mavlink_mission_item->autocontinue;
 	mission_item->index = mavlink_mission_item->seq;
+	mission_item->origin = ORIGIN_MAVLINK;
 
 	return OK;
 }
@@ -151,14 +159,6 @@ void mavlink_wpm_init(mavlink_wpm_storage *state)
 	// state->pos_reached = false;						///< boolean for position reached
 	// state->timestamp_lastoutside_orbit = 0;///< timestamp when the MAV was last outside the orbit or had the wrong yaw value
 	// state->timestamp_firstinside_orbit = 0;///< timestamp when the MAV was the first time after a waypoint change inside the orbit and had the correct yaw value
-
-	mission.count = 0;
-	mission.items = (struct mission_item_s*)malloc(sizeof(struct mission_item_s) * NUM_MISSIONS_SUPPORTED);
-	if (!mission.items) {
-		mission.count = 0;
-		/* XXX reject waypoints if this fails */
-		warnx("no free RAM to allocate mission, rejecting any waypoints");
-	}
 }
 
 /*
@@ -597,13 +597,7 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi
 						
 						mission.current_index = wpc.seq;
 
-						/* Initialize mission publication if necessary */
-						if (mission_pub < 0) {
-							mission_pub = orb_advertise(ORB_ID(mission), &mission);
-
-						} else {
-							orb_publish(ORB_ID(mission), mission_pub, &mission);
-						}
+						publish_mission();
 						
 						mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ACCEPTED);
 
@@ -703,16 +697,24 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi
 					wpm->current_wp_id = wpr.seq;
 
 					mavlink_mission_item_t wp;
-					map_mission_item_to_mavlink_mission_item(&mission.items[wpr.seq], &wp);
 
-					if (mission.current_index == wpr.seq) {
-						wp.current = true;
+					struct mission_item_s mission_item;
+					ssize_t len = sizeof(struct mission_item_s);
+					
+					if (dm_read(DM_KEY_WAYPOINTS, wpr.seq, &mission_item, len) == len) {
+					
+						if (mission.current_index == wpr.seq) {
+							wp.current = true;
+						} else {
+							wp.current = false;
+						}
+	
+						map_mission_item_to_mavlink_mission_item(&mission_item, &wp);
+						mavlink_wpm_send_waypoint(wpm->current_partner_sysid, wpm->current_partner_compid, &wp);
 					} else {
-						wp.current = false;
+						mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ERROR);
 					}
 
-					mavlink_wpm_send_waypoint(wpm->current_partner_sysid, wpm->current_partner_compid, &wp);
-
 				} else {
 					// if (verbose)
 					{
@@ -815,7 +817,7 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi
 
 							if (MAVLINK_WPM_VERBOSE) printf("Got MAVLINK_MSG_ID_MISSION_ITEM_COUNT (%u) from %u changing state to MAVLINK_WPM_STATE_GETLIST\n", wpc.count, msg->sysid);
 
-#endif
+#endif							
 						}
 
 						if (wpm->current_state == MAVLINK_WPM_STATE_GETLIST) {
@@ -837,10 +839,9 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi
 						if (wpc.count > NUM_MISSIONS_SUPPORTED) {
 							warnx("Too many waypoints: %d, supported: %d", wpc.count, NUM_MISSIONS_SUPPORTED);
 						} else {
-							/* prepare mission topic */
-							mission.count = wpc.count;
-							/* reset current index */
-							mission.current_index = -1;
+							/* set count to 0 while copying */
+							mission.count = 0;
+							publish_mission();
 						}
 
 #ifdef MAVLINK_WPM_NO_PRINTF
@@ -962,20 +963,26 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi
 					// mavlink_mission_item_t *newwp = &(wpm->rcv_waypoints[wp.seq]);
 					// memcpy(newwp, &wp, sizeof(mavlink_mission_item_t));
 
-					int ret = map_mavlink_mission_item_to_mission_item(&wp, &mission.items[wp.seq]);
-					if (ret != OK) {
+					struct mission_item_s mission_item;
 
+					int ret = map_mavlink_mission_item_to_mission_item(&wp, &mission_item);
 
+					if (ret != OK) {
 						mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, ret);
 						wpm->current_state = MAVLINK_WPM_STATE_IDLE;
 						break;
 					}
 
+					size_t len = sizeof(struct mission_item_s);
+
+					if (dm_write(DM_KEY_WAYPOINTS, wp.seq, DM_PERSIST_IN_FLIGHT_RESET, &mission_item, len) != len) {
+						mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ERROR);
+						wpm->current_state = MAVLINK_WPM_STATE_IDLE;
+						break;
+					}
+
 					if (wp.current) {
 						mission.current_index = wp.seq;
-						warnx("current is: %d", wp.seq);
-					} else {
-						warnx("not current");
 					}
 
 					wpm->current_wp_id = wp.seq + 1;
@@ -1009,14 +1016,10 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi
 						// }
 						// TODO: update count?
 
-						/* Initialize mission publication if necessary */
-						if (mission_pub < 0) {
-							mission_pub = orb_advertise(ORB_ID(mission), &mission);
-
-						} else {
-							orb_publish(ORB_ID(mission), mission_pub, &mission);
-						}
 
+						mission.count = wpm->current_count;
+						
+						publish_mission();
 
 						wpm->size = wpm->current_count;
 
@@ -1113,20 +1116,15 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi
 
 				/* prepare mission topic */
 				mission.count = 0;
-				memset(mission.items, 0, sizeof(struct mission_item_s)*NUM_MISSIONS_SUPPORTED);
-
-				/* Initialize mission publication if necessary */
-				if (mission_pub < 0) {
-					mission_pub = orb_advertise(ORB_ID(mission), &mission);
 
+				if (dm_clear(DM_KEY_WAYPOINTS) == OK) {
+					mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ACCEPTED);
 				} else {
-					orb_publish(ORB_ID(mission), mission_pub, &mission);
+					mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ERROR);
 				}
 
+				publish_mission();
 
-				warnx("Mission cleared");
-
-				mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, MAV_MISSION_ACCEPTED);
 
 			} else if (wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_wpm_comp_id */ && wpm->current_state != MAVLINK_WPM_STATE_IDLE) {
 				// if (verbose) // printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_CLEAR_LIST from %u because i'm doing something else already (state=%i).\n", msg->sysid, wpm->current_state);
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index cd4e04883..c6aac6af1 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -63,6 +63,7 @@
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/mission.h>
 #include <uORB/topics/fence.h>
+#include <uORB/topics/navigation_capabilities.h>
 #include <systemlib/param/param.h>
 #include <systemlib/err.h>
 #include <geo/geo.h>
@@ -141,7 +142,6 @@ private:
 	int		_vstatus_sub;			/**< vehicle status subscription */
 	int		_params_sub;			/**< notification of parameter updates */
 	int		_mission_sub;			/**< notification of mission updates */
-	int 		_onboard_mission_sub;		/**< notification of onboard mission updates */
 	int		_capabilities_sub;		/**< notification of vehicle capabilities updates */
 
 	orb_advert_t	_triplet_pub;			/**< publish position setpoint triplet */
@@ -157,12 +157,8 @@ private:
 	perf_counter_t	_loop_perf;			/**< loop performance counter */
 
 	unsigned	_max_mission_item_count;	/**< maximum number of mission items supported */
-	unsigned	_max_onboard_mission_item_count;/**< maximum number of onboard mission items supported */
 	
 	unsigned	_mission_item_count;		/** number of mission items copied */
-	unsigned	_onboard_mission_item_count;	/** number of onboard mission items copied */
-	struct mission_item_s				*_mission_item;	/**< storage for mission */
-	struct mission_item_s 				*_onboard_mission_item; /**< storage for onboard mission */
 
 	struct	fence_s 				_fence;			/**< storage for fence vertices */
 	bool						_fence_valid;		/**< flag if fence is valid */
@@ -174,11 +170,9 @@ private:
 	bool		_waypoint_yaw_reached;
 	uint64_t	_time_first_inside_orbit;
 	bool		_mission_item_reached;
-	bool		_onboard_mission_item_reached;
 
 	navigation_mode_t				_mode;
 	unsigned		_current_mission_index;
-	unsigned		_current_onboard_mission_index;
 
 	struct {
 		float min_altitude;
@@ -205,11 +199,6 @@ private:
 	void		mission_update();
 
 	/**
-	* Retrieve onboard mission.
-	*/
-	void		onboard_mission_update();
-
-	/**
 	 * Shim for calling task_main from task_create.
 	 */
 	static void	task_main_trampoline(int argc, char *argv[]);
@@ -227,10 +216,6 @@ private:
 
 	void		set_mode(navigation_mode_t new_nav_mode);
 
-	bool		mission_possible();
-
-	bool		onboard_mission_possible();
-
 	int		set_waypoint_mission_item(unsigned mission_item_index, struct mission_item_s *new_mission_item);
 
 	void		publish_mission_item_triplet();
@@ -285,7 +270,6 @@ Navigator::Navigator() :
 	_vstatus_sub(-1),
 	_params_sub(-1),
 	_mission_sub(-1),
-	_onboard_mission_sub(-1),
 	_capabilities_sub(-1),
 
 /* publications */
@@ -297,19 +281,15 @@ Navigator::Navigator() :
 	_loop_perf(perf_alloc(PC_ELAPSED, "navigator")),
 /* states */
 	_max_mission_item_count(10),
-	_max_onboard_mission_item_count(10),
 	_mission_item_count(0),
-	_onboard_mission_item_count(0),
 	_fence_valid(false),
 	_inside_fence(true),
 	_waypoint_position_reached(false),
 	_waypoint_yaw_reached(false),
 	_time_first_inside_orbit(0),
 	_mission_item_reached(false),
-	_onboard_mission_item_reached(false),
 	_mode(NAVIGATION_MODE_NONE),
-	_current_mission_index(0),
-	_current_onboard_mission_index(0)
+	_current_mission_index(0)
 {
 	_global_pos.valid = false;
 	memset(&_fence, 0, sizeof(_fence));
@@ -317,9 +297,6 @@ Navigator::Navigator() :
 	_parameter_handles.loiter_radius = param_find("NAV_LOITER_RAD");
 	_parameter_handles.onboard_mission_enabled = param_find("NAV_ONB_MIS_EN");
 
-	_mission_item = (mission_item_s*)malloc(sizeof(mission_item_s) * _max_mission_item_count);
-	_onboard_mission_item = (mission_item_s*)malloc(sizeof(mission_item_s) * _max_onboard_mission_item_count);
-
 	_mission_item_triplet.previous_valid = false;
 	_mission_item_triplet.current_valid = false;
 	_mission_item_triplet.next_valid = false;
@@ -374,130 +351,49 @@ 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 <= _max_mission_item_count) {
-			
-			/* Check if first part of mission (up to _current_mission_index - 1) changed:
-			 * if the first part changed: start again at first waypoint
-			 * if the first part remained unchanged: continue with the (possibly changed second part)
-			 */
-			if (mission.current_index == -1 && _current_mission_index  < _mission_item_count && _current_mission_index <  mission.count) { //check if not finished and if the new mission is not a shorter mission
-				for (unsigned i = 0; i < _current_mission_index; i++) {
-					if (!cmp_mission_item_equivalent(_mission_item[i], mission.items[i])) {
-						/* set flag to restart mission next we're in auto */
-						_current_mission_index = 0;
-						mavlink_log_info(_mavlink_fd, "[navigator] Reset to WP %d", _current_mission_index);
-						//warnx("First part of mission differs i=%d", i);
-						break;
-					}
-//					else {
-//						warnx("Mission item is equivalent i=%d", i);
-//					}
-				}
-			} else if (mission.current_index >= 0 && mission.current_index < mission.count) {
-				/* set flag to restart mission next we're in auto */
-				_current_mission_index = mission.current_index;
-				mavlink_log_info(_mavlink_fd, "[navigator] Reset to WP %d", _current_mission_index);
-			} else {
-				_current_mission_index = 0;
-				mavlink_log_info(_mavlink_fd, "[navigator] Reset to WP %d", _current_mission_index);
-			}
-
-			/*
-			 * Perform an atomic copy & state update
-			 */
-			irqstate_t flags = irqsave();
-
-			memcpy(_mission_item, mission.items, mission.count * sizeof(struct mission_item_s));
-			_mission_item_count = mission.count;
-
-			irqrestore(flags);
-
-			
-			
-		} else {
-			warnx("ERROR: too many waypoints, not supported");
-			mavlink_log_critical(_mavlink_fd, "[navigator] too many waypoints, not supported");
-			_mission_item_count = 0;
-		}
 
-		if (_mode == NAVIGATION_MODE_WAYPOINT) {
-			start_waypoint();
-		}
+// 		/* Check if first part of mission (up to _current_mission_index - 1) changed:
+// 		 * if the first part changed: start again at first waypoint
+// 		 * if the first part remained unchanged: continue with the (possibly changed second part)
+// 		 */
+// 		if (mission.current_index == -1 && _current_mission_index  < _mission_item_count && _current_mission_index <  mission.count) { //check if not finished and if the new mission is not a shorter mission
+// 			for (unsigned i = 0; i < _current_mission_index; i++) {
+// 				if (!cmp_mission_item_equivalent(_mission_item[i], mission.items[i])) {
+// 					/* set flag to restart mission next we're in auto */
+// 					_current_mission_index = 0;
+// 					mavlink_log_info(_mavlink_fd, "[navigator] Reset to WP %d", _current_mission_index);
+// 					//warnx("First part of mission differs i=%d", i);
+// 					break;
+// 				}
+// //					else {
+// //						warnx("Mission item is equivalent i=%d", i);
+// //					}
+// 			}
+// 		} else if (mission.current_index >= 0 && mission.current_index < mission.count) {
+// 			/* set flag to restart mission next we're in auto */
+// 			_current_mission_index = mission.current_index;
+// 			mavlink_log_info(_mavlink_fd, "[navigator] Reset to WP %d", _current_mission_index);
+// 		} else {
+// 			_current_mission_index = 0;
+// 			mavlink_log_info(_mavlink_fd, "[navigator] Reset to WP %d", _current_mission_index);
+// 		}
+
+		_mission_item_count = mission.count;
+		_current_mission_index = mission.current_index;
 
-		/* TODO add checks if and how the mission has changed */
 	} else {
 		_mission_item_count = 0;
 		_current_mission_index = 0;
 	}
+	if (_mission_item_count == 0 && _mode == NAVIGATION_MODE_WAYPOINT) {
+		set_mode(NAVIGATION_MODE_LOITER);
+	}
+	else if (_mode == NAVIGATION_MODE_WAYPOINT) {
+		start_waypoint();
+	}
 }
 
-void
-Navigator::onboard_mission_update()
-{
-	struct mission_s onboard_mission;
-	if (orb_copy(ORB_ID(onboard_mission), _onboard_mission_sub, &onboard_mission) == OK) {
-		// XXX this is not optimal yet, but a first prototype /
-		// test implementation
-
-		if (onboard_mission.count <= _max_onboard_mission_item_count) {
-			
-			/* Check if first part of mission (up to _current_onboard_mission_index - 1) changed:
-			 * if the first part changed: start again at first waypoint
-			 * if the first part remained unchanged: continue with the (possibly changed second part)
-			 */
-			if (onboard_mission.current_index == -1 && _current_onboard_mission_index  < _onboard_mission_item_count && _current_onboard_mission_index <  onboard_mission.count) { //check if not finished and if the new mission is not a shorter mission
-				for (unsigned i = 0; i < _current_onboard_mission_index; i++) {
-					if (!cmp_mission_item_equivalent(_onboard_mission_item[i], onboard_mission.items[i])) {
-						/* set flag to restart mission next we're in auto */
-						_current_onboard_mission_index = 0;
-						mavlink_log_info(_mavlink_fd, "[navigator] Reset to onboard WP %d", _current_onboard_mission_index);
-						//warnx("First part of onboard mission differs i=%d", i);
-						break;
-					}
-//					else {
-//						warnx("Onboard mission item is equivalent i=%d", i);
-//					}
-				}
-			} else if (onboard_mission.current_index >= 0 && onboard_mission.current_index < onboard_mission.count) {
-				/* set flag to restart mission next we're in auto */
-				_current_onboard_mission_index = onboard_mission.current_index;
-				mavlink_log_info(_mavlink_fd, "[navigator] Reset to onboard WP %d", _current_onboard_mission_index);
-			} else {
-				_current_onboard_mission_index = 0;
-				mavlink_log_info(_mavlink_fd, "[navigator] Reset to onboard WP %d", _current_onboard_mission_index);
-			}
-
-			/*
-			 * Perform an atomic copy & state update
-			 */
-			irqstate_t flags = irqsave();
-
-			memcpy(_onboard_mission_item, onboard_mission.items, onboard_mission.count * sizeof(struct mission_item_s));
-			_onboard_mission_item_count = onboard_mission.count;
-
-			irqrestore(flags);
-
-			
-			
-		} else {
-			warnx("ERROR: too many waypoints, not supported");
-			mavlink_log_critical(_mavlink_fd, "[navigator] too many waypoints, not supported");
-			_onboard_mission_item_count = 0;
-		}
-
-		if (_mode == NAVIGATION_MODE_WAYPOINT) {
-			start_waypoint();
-		}
 
-		/* TODO add checks if and how the mission has changed */
-	} else {
-		_onboard_mission_item_count = 0;
-		_current_onboard_mission_index = 0;
-	}
-}
 
 void
 Navigator::task_main_trampoline(int argc, char *argv[])
@@ -518,7 +414,6 @@ Navigator::task_main()
 	 */
 	_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
 	_mission_sub = orb_subscribe(ORB_ID(mission));
-	_onboard_mission_sub = orb_subscribe(ORB_ID(onboard_mission));
 	_capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities));
 	_vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
 	_params_sub = orb_subscribe(ORB_ID(parameter_update));
@@ -530,7 +425,6 @@ Navigator::task_main()
 	}
 
 	mission_update();
-	onboard_mission_update();
 
 	/* rate limit vehicle status updates to 5Hz */
 	orb_set_interval(_vstatus_sub, 200);
@@ -545,7 +439,7 @@ Navigator::task_main()
 	orb_copy(ORB_ID(navigation_capabilities), _capabilities_sub, &_nav_caps);
 
 	/* wakeup source(s) */
-	struct pollfd fds[7];
+	struct pollfd fds[6];
 
 	/* Setup of loop */
 	fds[0].fd = _params_sub;
@@ -558,10 +452,8 @@ Navigator::task_main()
 	fds[3].events = POLLIN;
 	fds[4].fd = _mission_sub;
 	fds[4].events = POLLIN;
-	fds[5].fd = _onboard_mission_sub;
+	fds[5].fd = _vstatus_sub;
 	fds[5].events = POLLIN;
-	fds[6].fd = _vstatus_sub;
-	fds[6].events = POLLIN;
 
 
 	while (!_task_should_exit) {
@@ -583,7 +475,7 @@ Navigator::task_main()
 		perf_begin(_loop_perf);
 
 		/* only update vehicle status if it changed */
-		if (fds[6].revents & POLLIN) {
+		if (fds[5].revents & POLLIN) {
 			/* read from param to clear updated flag */
 			orb_copy(ORB_ID(vehicle_status), _vstatus_sub, &_vstatus);
 
@@ -613,8 +505,8 @@ Navigator::task_main()
 
 					case NAVIGATION_STATE_AUTO_MISSION:
 
-						if (onboard_mission_possible() || mission_possible()) {
-							/* Start mission or onboard mission if available */
+						if (_mission_item_count > 0 && !(_current_mission_index >= _mission_item_count)) {
+							/* Start mission if there is a mission available and the last waypoint has not been reached */
 							set_mode(NAVIGATION_MODE_WAYPOINT);
 						} else {
 							/* else fallback to loiter */
@@ -664,10 +556,6 @@ Navigator::task_main()
 			mission_update();
 		}
 
-		if (fds[5].revents & POLLIN) {
-			onboard_mission_update();
-		}
-
 		if (fds[2].revents & POLLIN) {
 			orb_copy(ORB_ID(home_position), _home_pos_sub, &_home_pos);
 		}
@@ -975,61 +863,31 @@ Navigator::set_mode(navigation_mode_t new_nav_mode)
 	}
 }
 
-bool
-Navigator::mission_possible()
-{
-	return _mission_item_count > 0 &&
-	       !(_current_mission_index >= _mission_item_count);
-}
-
-bool
-Navigator::onboard_mission_possible()
-{
-	return _onboard_mission_item_count > 0 &&
-	       !(_current_onboard_mission_index >= _onboard_mission_item_count) &&
-	       _parameters.onboard_mission_enabled;
-}
-
 int
-Navigator::set_waypoint_mission_item(unsigned index, struct mission_item_s *new_item) 
+Navigator::set_waypoint_mission_item(unsigned mission_item_index, struct mission_item_s *new_mission_item) 
 {
-	if (onboard_mission_possible()) {
-
-		if (index < _onboard_mission_item_count) {
-			memcpy(new_item, &_onboard_mission_item[index], sizeof(mission_item_s));
-
-			if (new_item->nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) {
-				/* if it is a RTL waypoint, append the home position */
-				new_item->lat = (double)_home_pos.lat / 1e7;
-				new_item->lon = (double)_home_pos.lon / 1e7;
-				new_item->altitude = (float)_home_pos.alt / 1e3f + _parameters.min_altitude;
-				new_item->loiter_radius = _parameters.loiter_radius; // TODO: get rid of magic number
-				new_item->radius = 50.0f; // TODO: get rid of magic number
-			}
-			// warnx("added mission item: %d", index);
-			return OK;
-		}
-
-	} else if (mission_possible()) {
+	if (mission_item_index >= _mission_item_count) {
+		return ERROR;
+	}
+	
+	struct mission_item_s mission_item;
+	
+	if (dm_read(DM_KEY_WAYPOINTS, mission_item_index, &mission_item, sizeof(struct mission_item_s)) != sizeof(struct mission_item_s)) {
+		return ERROR;
+	}
 
-		if (index < _mission_item_count) {
-			memcpy(new_item, &_mission_item[index], sizeof(mission_item_s));
+	memcpy(new_mission_item, &mission_item, sizeof(struct mission_item_s));
 
-			if (new_item->nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) {
-				/* if it is a RTL waypoint, append the home position */
-				new_item->lat = (double)_home_pos.lat / 1e7;
-				new_item->lon = (double)_home_pos.lon / 1e7;
-				new_item->altitude = (float)_home_pos.alt / 1e3f + _parameters.min_altitude;
-				new_item->loiter_radius = _parameters.loiter_radius; // TODO: get rid of magic number
-				new_item->radius = 50.0f; // TODO: get rid of magic number
-			}
-			// warnx("added mission item: %d", index);
-			return OK;
-		}
+	if (new_mission_item->nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) {
+		/* if it is a RTL waypoint, append the home position */
+		new_mission_item->lat = (double)_home_pos.lat / 1e7;
+		new_mission_item->lon = (double)_home_pos.lon / 1e7;
+		new_mission_item->altitude = (float)_home_pos.alt / 1e3f + _parameters.min_altitude;
+		new_mission_item->loiter_radius = _parameters.loiter_radius; // TODO: get rid of magic number
+		new_mission_item->radius = 50.0f; // TODO: get rid of magic number
 	}
-	
-	// warnx("could not add mission item: %d", index);
-	return ERROR;
+
+	return OK;
 }
 
 void
@@ -1076,8 +934,6 @@ Navigator::advance_current_mission_item()
 		return ERROR;
 	}
 
-	reset_mission_item_reached();
-
 	/* copy current mission to previous item */
 	memcpy(&_mission_item_triplet.previous, &_mission_item_triplet.current, sizeof(mission_item_s));
 	_mission_item_triplet.previous_valid = _mission_item_triplet.current_valid;
@@ -1222,14 +1078,11 @@ Navigator::start_waypoint()
 {
 	reset_mission_item_reached();
 
-	/* this means we should start fresh */
-	if (_current_mission_index == 0) {
-
-		_mission_item_triplet.previous_valid = false;
-
-	} else {
+	if (_current_mission_index > 0) {
 		set_waypoint_mission_item(_current_mission_index - 1, &_mission_item_triplet.previous);
 		_mission_item_triplet.previous_valid = true;
+	} else {
+		_mission_item_triplet.previous_valid = false;
 	}
 	
 	set_waypoint_mission_item(_current_mission_index, &_mission_item_triplet.current);
diff --git a/src/modules/uORB/topics/mission.h b/src/modules/uORB/topics/mission.h
index 2427a1d57..30f06c359 100644
--- a/src/modules/uORB/topics/mission.h
+++ b/src/modules/uORB/topics/mission.h
@@ -46,6 +46,8 @@
 #include <stdbool.h>
 #include "../uORB.h"
 
+#define NUM_MISSIONS_SUPPORTED 256
+
 /* compatible to mavlink MAV_CMD */
 enum NAV_CMD {
 	NAV_CMD_WAYPOINT=16,
@@ -59,6 +61,11 @@ enum NAV_CMD {
 	NAV_CMD_PATHPLANNING=81
 };
 
+enum ORIGIN {
+	ORIGIN_MAVLINK = 0,
+	ORIGIN_ONBOARD
+};
+
 /**
  * @addtogroup topics
  * @{
@@ -84,11 +91,11 @@ struct mission_item_s
 	float time_inside;		/**< time that the MAV should stay inside the radius before advancing in seconds */
 	bool autocontinue;		/**< true if next waypoint should follow after this one */
 	int index;			/**< index matching the mavlink waypoint                */
+	enum ORIGIN origin;		/**< where the waypoint has been generated		*/
 };
 
 struct mission_s
 {
-	struct mission_item_s *items;
 	unsigned count;
 	int current_index;		/**< default -1, start at the one changed latest */
 };
@@ -99,6 +106,5 @@ struct mission_s
 
 /* register this as object request broker structure */
 ORB_DECLARE(mission);
-ORB_DECLARE(onboard_mission);
 
 #endif
diff --git a/src/systemcmds/tests/test_dataman.c b/src/systemcmds/tests/test_dataman.c
index e33c5aceb..7de87b476 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;
 		}