navigator: mission class added (WIP)

12 files changed, 749 insertions, 497 deletions

diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
index a53118dec..b8d94fe18 100644
--- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
@@ -861,7 +861,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 
 		}
 
-		if (pos_sp_triplet.current.type == SETPOINT_TYPE_NORMAL) {
+		if (pos_sp_triplet.current.type == SETPOINT_TYPE_POSITION) {
 			/* waypoint is a plain navigation waypoint */
 			_l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed);
 			_att_sp.roll_body = _l1_control.nav_roll();
diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 6b45736e9..48f91481d 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -415,7 +415,7 @@ Mavlink::instance_exists(const char *device_name, Mavlink *self)
 void
 Mavlink::forward_message(mavlink_message_t *msg, Mavlink *self)
 {
-	
+
 	Mavlink *inst;
 	LL_FOREACH(_mavlink_instances, inst) {
 		if (inst != self) {
diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp
index adbb0cfa2..f3a86666f 100644
--- a/src/modules/navigator/mission.cpp
+++ b/src/modules/navigator/mission.cpp
@@ -38,30 +38,42 @@
  * @author Julian Oes <julian@oes.ch>
  */
 
+#include <sys/types.h>
 #include <string.h>
 #include <stdlib.h>
+#include <unistd.h>
+
+#include <drivers/drv_hrt.h>
 
 #include <dataman/dataman.h>
 #include <systemlib/err.h>
+#include <geo/geo.h>
 
 #include <uORB/uORB.h>
 #include <uORB/topics/mission_result.h>
 
+#include "navigator.h"
 #include "mission.h"
 
 
-Mission::Mission() :
-
-	_offboard_dataman_id(-1),
-	_current_offboard_mission_index(0),
-	_current_onboard_mission_index(0),
-	_offboard_mission_item_count(0),
-	_onboard_mission_item_count(0),
-	_onboard_mission_allowed(false),
-	_current_mission_type(MISSION_TYPE_NONE),
+Mission::Mission(Navigator *navigator) :
+	SuperBlock(NULL, "MIS"),
+	_navigator(navigator),
+	_first_run(true),
+	_param_onboard_enabled(this, "ONBOARD_EN"),
+	_onboard_mission_sub(-1),
+	_offboard_mission_sub(-1),
+	_onboard_mission({0}),
+	_offboard_mission({0}),
+	_mission_item({0}),
 	_mission_result_pub(-1),
-	_mission_result({})
+	_mission_result({0}),
+	_mission_type(MISSION_TYPE_NONE)
 {
+	/* load initial params */
+	updateParams();
+	/* set initial mission items */
+	reset();
 }
 
 Mission::~Mission()
@@ -69,134 +81,335 @@ Mission::~Mission()
 }
 
 void
-Mission::set_offboard_dataman_id(const int new_id)
+Mission::reset()
 {
-	_offboard_dataman_id = new_id;
+	_first_run = true;
 }
 
-void
-Mission::set_offboard_mission_count(int new_count)
+bool
+Mission::update(struct position_setpoint_triplet_s *pos_sp_triplet)
 {
-	_offboard_mission_item_count = new_count;
+	bool updated = false;
+	/* check if anything has changed, and reset mission items if needed */
+	if (is_onboard_mission_updated() || is_offboard_mission_updated() || _first_run) {
+		set_mission_items(pos_sp_triplet);
+		updated = true;
+		_first_run = false;
+	}
+
+	if (_mission_type != MISSION_TYPE_NONE && is_mission_item_reached()) {
+		advance_mission();
+		set_mission_items(pos_sp_triplet);
+		updated = true;
+	}
+	return updated;
 }
 
-void
-Mission::set_onboard_mission_count(int new_count)
+bool
+Mission::is_mission_item_reached()
 {
-	_onboard_mission_item_count = new_count;
+	/* TODO: count turns */
+#if 0
+	if ((_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
+	     _mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
+	     _mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED) &&
+	    _mission_item.loiter_radius > 0.01f) {
+
+		return false;
+	}
+#endif
+
+	hrt_abstime now = hrt_absolute_time();
+
+	if (!_waypoint_position_reached) {
+
+		float dist = -1.0f;
+		float dist_xy = -1.0f;
+		float dist_z = -1.0f;
+
+		float altitude_amsl = _mission_item.altitude_is_relative
+				      ? _mission_item.altitude + _navigator->get_home_position()->alt
+			              : _mission_item.altitude;
+
+		dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, altitude_amsl,
+				                          _navigator->get_global_position()->lat,
+							  _navigator->get_global_position()->lon,
+							  _navigator->get_global_position()->alt,
+				&dist_xy, &dist_z);
+
+		if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF) {
+
+			/* require only altitude for takeoff */
+			if (_navigator->get_global_position()->alt > altitude_amsl - _mission_item.acceptance_radius) {
+				_waypoint_position_reached = true;
+			}
+		} else {
+			if (dist >= 0.0f && dist <= _mission_item.acceptance_radius) {
+				_waypoint_position_reached = true;
+			}
+		}
+	}
+
+	if (_waypoint_position_reached && !_waypoint_yaw_reached) {
+
+		/* TODO: removed takeoff, why? */
+		if (_navigator->get_vstatus()->is_rotary_wing && isfinite(_mission_item.yaw)) {
+
+			/* check yaw if defined only for rotary wing except takeoff */
+			float yaw_err = _wrap_pi(_mission_item.yaw - _navigator->get_global_position()->yaw);
+
+			if (fabsf(yaw_err) < 0.2f) { /* TODO: get rid of magic number */
+				_waypoint_yaw_reached = true;
+			}
+
+		} else {
+			_waypoint_yaw_reached = true;
+		}
+	}
+
+	/* check if the current waypoint was reached */
+	if (_waypoint_position_reached && _waypoint_yaw_reached) {
+
+		if (_time_first_inside_orbit == 0) {
+			_time_first_inside_orbit = now;
+
+			// if (_mission_item.time_inside > 0.01f) {
+			// 	mavlink_log_info(_mavlink_fd, "#audio: waypoint reached, wait for %.1fs",
+			// 		(double)_mission_item.time_inside);
+			// }
+		}
+
+		/* check if the MAV was long enough inside the waypoint orbit */
+		if (now - _time_first_inside_orbit >= (hrt_abstime)_mission_item.time_inside * 1e6f) {
+			return true;
+		}
+	}
+	return false;
 }
 
 void
-Mission::set_onboard_mission_allowed(bool allowed)
-{
-	_onboard_mission_allowed = allowed;
+Mission::reset_mission_item_reached() {
+	_waypoint_position_reached = false;
+	_waypoint_yaw_reached = false;
+	_time_first_inside_orbit = 0;
 }
 
-bool
-Mission::command_current_offboard_mission_index(const int new_index)
+void
+Mission::mission_item_to_position_setpoint(const mission_item_s *item, position_setpoint_s *sp)
 {
-	if (new_index >= 0) {
-		_current_offboard_mission_index = (unsigned)new_index;
-	} else {
+	sp->valid = true;
 
-		/* if less WPs available, reset to first WP */
-		if (_current_offboard_mission_index >= _offboard_mission_item_count) {
-			_current_offboard_mission_index = 0;
-		}
+	sp->lat = item->lat;
+	sp->lon = item->lon;
+	sp->alt = item->altitude_is_relative ? item->altitude + _navigator->get_home_position()->alt : item->altitude;
+	sp->yaw = item->yaw;
+	sp->loiter_radius = item->loiter_radius;
+	sp->loiter_direction = item->loiter_direction;
+	sp->pitch_min = item->pitch_min;
+
+	if (item->nav_cmd == NAV_CMD_TAKEOFF) {
+		sp->type = SETPOINT_TYPE_TAKEOFF;
+
+	} else if (item->nav_cmd == NAV_CMD_LAND) {
+		sp->type = SETPOINT_TYPE_LAND;
+
+	} else if (item->nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
+		   item->nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
+		   item->nav_cmd == NAV_CMD_LOITER_UNLIMITED) {
+		sp->type = SETPOINT_TYPE_LOITER;
+
+	} else {
+		sp->type = SETPOINT_TYPE_POSITION;
 	}
-	report_current_offboard_mission_item();
 }
 
 bool
-Mission::command_current_onboard_mission_index(int new_index)
+Mission::is_onboard_mission_updated()
 {
-	if (new_index != -1) {
-		_current_onboard_mission_index = (unsigned)new_index;
-	} else {
+	bool updated;
+	orb_check(_onboard_mission_sub, &updated);
 
-		/* if less WPs available, reset to first WP */
-		if (_current_onboard_mission_index >= _onboard_mission_item_count) {
-			_current_onboard_mission_index = 0;
-		}
+	if (!updated) {
+		return false;
 	}
-	// TODO: implement this for onboard missions as well
-	// report_current_mission_item();
+
+	if (orb_copy(ORB_ID(onboard_mission), _onboard_mission_sub, &_onboard_mission) != OK) {
+		_onboard_mission.count = 0;
+		_onboard_mission.current_index = 0;
+	}
+	return true;
 }
 
 bool
-Mission::get_current_mission_item(struct mission_item_s *new_mission_item, bool *onboard, int *index)
+Mission::is_offboard_mission_updated()
 {
-	*onboard = false;
-	*index = -1;
-
-	/* try onboard mission first */
-	if (_current_onboard_mission_index < _onboard_mission_item_count && _onboard_mission_allowed) {
-		if (read_mission_item(DM_KEY_WAYPOINTS_ONBOARD, true, &_current_onboard_mission_index, new_mission_item)) {
-			_current_mission_type = MISSION_TYPE_ONBOARD;
-			*onboard = true;
-			*index = _current_onboard_mission_index;
-
-			return true;
-		}
+	bool updated;
+	orb_check(_offboard_mission_sub, &updated);
+	if (!updated) {
+		return false;
 	}
 
-	/* otherwise fallback to offboard */
-	if (_current_offboard_mission_index < _offboard_mission_item_count) {
+	if (orb_copy(ORB_ID(offboard_mission), _offboard_mission_sub, &_offboard_mission) == OK) {
 
+		/* Check mission feasibility, for now do not handle the return value,
+		 * however warnings are issued to the gcs via mavlink from inside the MissionFeasiblityChecker */
 		dm_item_t dm_current;
-		if (_offboard_dataman_id == 0) {
+
+		if (_offboard_mission.dataman_id == 0) {
 			dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0;
 		} else {
 			dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1;
 		}
-		if (read_mission_item(dm_current, true, &_current_offboard_mission_index, new_mission_item)) {
 
-			_current_mission_type = MISSION_TYPE_OFFBOARD;
-			*onboard = false;
-			*index = _current_offboard_mission_index;
+		missionFeasiblityChecker.checkMissionFeasible(_navigator->get_vstatus()->is_rotary_wing, dm_current,
+			                                      (size_t)_offboard_mission.count,
+							      _navigator->get_geofence());
+	} else {
+		_offboard_mission.count = 0;
+		_offboard_mission.current_index = 0;
+	}
+	return true;
+}
 
-			return true;
-		}
+
+void
+Mission::advance_mission()
+{
+	switch (_mission_type) {
+	case MISSION_TYPE_ONBOARD:
+		_onboard_mission.current_index++;
+		break;
+
+	case MISSION_TYPE_OFFBOARD:
+		_offboard_mission.current_index++;
+		break;
+
+	case MISSION_TYPE_NONE:
+	default:
+		break;
 	}
+}
+
+void
+Mission::set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet)
+{
+	warnx("set mission items");
+
+	set_previous_pos_setpoint(&pos_sp_triplet->current, &pos_sp_triplet->previous);
 
-	/* happens when no more mission items can be added as a next item */
-	_current_mission_type = MISSION_TYPE_NONE;
+	if (is_current_onboard_mission_item_set(&pos_sp_triplet->current)) {
+		/* try setting onboard mission item */
+		_mission_type = MISSION_TYPE_ONBOARD;
 
+	} else if (is_current_offboard_mission_item_set(&pos_sp_triplet->current)) {
+		/* try setting offboard mission item */
+		_mission_type = MISSION_TYPE_OFFBOARD;
+	} else {
+		_mission_type = MISSION_TYPE_NONE;
+	}
+}
+
+void
+Mission::set_previous_pos_setpoint(const struct position_setpoint_s *current_pos_sp,
+		                   struct position_setpoint_s *previous_pos_sp)
+{
+	/* reuse current setpoint as previous setpoint */
+	if (current_pos_sp->valid) {
+		memcpy(previous_pos_sp, current_pos_sp, sizeof(struct position_setpoint_s));
+	}
+}
+
+bool
+Mission::is_current_onboard_mission_item_set(struct position_setpoint_s *current_pos_sp)
+{
+	if (_param_onboard_enabled.get() > 0
+	    && _onboard_mission.current_index < (int)_onboard_mission.count) {
+		struct mission_item_s new_mission_item;
+		if (read_mission_item(DM_KEY_WAYPOINTS_ONBOARD, true, &_onboard_mission.current_index,
+					&new_mission_item)) {
+			/* convert the current mission item and set it valid */
+			mission_item_to_position_setpoint(&_mission_item, current_pos_sp);
+			current_pos_sp->valid = true;
+
+			/* TODO: report this somehow */
+			return true;
+		}
+	}
 	return false;
 }
 
 bool
-Mission::get_next_mission_item(struct mission_item_s *new_mission_item)
+Mission::is_current_offboard_mission_item_set(struct position_setpoint_s *current_pos_sp)
 {
-	int next_temp_mission_index = _current_onboard_mission_index + 1;
+	if (_offboard_mission.current_index < (int)_offboard_mission.count) {
+		dm_item_t dm_current;
+		if (_offboard_mission.dataman_id == 0) {
+			dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0;
+		} else {
+			dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1;
+		}
+		struct mission_item_s new_mission_item;
+		if (read_mission_item(dm_current, true, &_offboard_mission.current_index, &new_mission_item)) {
+			/* convert the current mission item and set it valid */
+			mission_item_to_position_setpoint(&_mission_item, current_pos_sp);
+			current_pos_sp->valid = true;
 
-	/* try onboard mission first */
-	if (next_temp_mission_index < _onboard_mission_item_count && _onboard_mission_allowed) {
-		if (read_mission_item(DM_KEY_WAYPOINTS_ONBOARD, false, &next_temp_mission_index, new_mission_item)) {
+			report_current_offboard_mission_item();
 			return true;
 		}
 	}
+	return false;
+}
 
-	/* then try offboard mission */
-	dm_item_t dm_current;
-	if (_offboard_dataman_id == 0) {
-		dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0;
-	} else {
-		dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1;
-	}
-	next_temp_mission_index = _current_offboard_mission_index + 1;
-	if (next_temp_mission_index < _offboard_mission_item_count) {
-		if (read_mission_item(dm_current, false, &next_temp_mission_index, new_mission_item)) {
-			return true;
+void
+Mission::get_next_onboard_mission_item(struct position_setpoint_s *next_pos_sp)
+{
+	int next_temp_mission_index = _onboard_mission.current_index + 1;
+
+	/* try if there is a next onboard mission */
+	if (next_temp_mission_index < (int)_onboard_mission.count) {
+		struct mission_item_s new_mission_item;
+		if (read_mission_item(DM_KEY_WAYPOINTS_ONBOARD, false, &next_temp_mission_index, &new_mission_item)) {
+			/* convert next mission item to position setpoint */
+			mission_item_to_position_setpoint(&new_mission_item, next_pos_sp);
+			next_pos_sp->valid = true;
+			return;
 		}
 	}
 
-	/* both failed, bail out */
-	return false;
+	/* give up */
+	next_pos_sp->valid = false;
+	return;
+}
+
+void
+Mission::get_next_offboard_mission_item(struct position_setpoint_s *next_pos_sp)
+{
+	/* try if there is a next offboard mission */
+	int next_temp_mission_index = _offboard_mission.current_index + 1;
+	if (next_temp_mission_index < (int)_offboard_mission.count) {
+		dm_item_t dm_current;
+		if (_offboard_mission.dataman_id == 0) {
+			dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0;
+		} else {
+			dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1;
+		}
+		struct mission_item_s new_mission_item;
+		if (read_mission_item(dm_current, false, &next_temp_mission_index, &new_mission_item)) {
+			/* convert next mission item to position setpoint */
+			mission_item_to_position_setpoint(&new_mission_item, next_pos_sp);
+			next_pos_sp->valid = true;
+			return;
+		}
+	}
+	/* give up */
+	next_pos_sp->valid = false;
+	return;
 }
 
 bool
-Mission::read_mission_item(const dm_item_t dm_item, bool is_current, int *mission_index, struct mission_item_s *new_mission_item)
+Mission::read_mission_item(const dm_item_t dm_item, bool is_current, int *mission_index,
+		           struct mission_item_s *new_mission_item)
 {
 	/* repeat several to get the mission item because we might have to follow multiple DO_JUMPS */
 	for (int i=0; i<10; i++) {
@@ -211,25 +424,25 @@ Mission::read_mission_item(const dm_item_t dm_item, bool is_current, int *missio
 		/* check for DO_JUMP item, and whether it hasn't not already been repeated enough times */
 		if (new_mission_item->nav_cmd == NAV_CMD_DO_JUMP) {
 
-			if (new_mission_item->do_jump_current_count < new_mission_item->do_jump_repeat_count) {
+			if (new_mission_item->do_jump_current_count >= new_mission_item->do_jump_repeat_count) {
+				return false;
+			}
 
-				/* only raise the repeat count if this is for the current mission item
-				 * but not for the next mission item */
-				if (is_current) {
-					(new_mission_item->do_jump_current_count)++;
+			/* only raise the repeat count if this is for the current mission item
+			 * but not for the next mission item */
+			if (is_current) {
+				(new_mission_item->do_jump_current_count)++;
 
-					/* save repeat count */
-					if (dm_write(dm_item, *mission_index, DM_PERSIST_IN_FLIGHT_RESET, new_mission_item, len) != len) {
-						/* not supposed to happen unless the datamanager can't access the SD card, etc. */
-						return false;
-					}
+				/* save repeat count */
+				if (dm_write(dm_item, *mission_index, DM_PERSIST_IN_FLIGHT_RESET,
+					     new_mission_item, len) != len) {
+					/* not supposed to happen unless the datamanager can't access the dataman */
+					return false;
 				}
-				/* set new mission item index and repeat
-				 * we don't have to validate here, if it's invalid, we should realize this later .*/
-				*mission_index = new_mission_item->do_jump_mission_index;
-			} else {
-				return false;
 			}
+			/* set new mission item index and repeat
+			 * we don't have to validate here, if it's invalid, we should realize this later .*/
+			*mission_index = new_mission_item->do_jump_mission_index;
 
 		} else {
 			/* if it's not a DO_JUMP, then we were successful */
@@ -243,31 +456,11 @@ Mission::read_mission_item(const dm_item_t dm_item, bool is_current, int *missio
 }
 
 void
-Mission::move_to_next()
-{
-	report_mission_item_reached();
-
-	switch (_current_mission_type) {
-	case MISSION_TYPE_ONBOARD:
-		_current_onboard_mission_index++;
-		break;
-
-	case MISSION_TYPE_OFFBOARD:
-		_current_offboard_mission_index++;
-		break;
-
-	case MISSION_TYPE_NONE:
-	default:
-		break;
-	}
-}
-
-void
 Mission::report_mission_item_reached()
 {
-	if (_current_mission_type == MISSION_TYPE_OFFBOARD) {
+	if (_mission_type == MISSION_TYPE_OFFBOARD) {
 		_mission_result.mission_reached = true;
-		_mission_result.mission_index_reached = _current_offboard_mission_index;
+		_mission_result.mission_index_reached = _offboard_mission.current_index;
 	}
 	publish_mission_result();
 }
@@ -275,7 +468,7 @@ Mission::report_mission_item_reached()
 void
 Mission::report_current_offboard_mission_item()
 {
-	_mission_result.index_current_mission = _current_offboard_mission_index;
+	_mission_result.index_current_mission = _offboard_mission.current_index;
 	publish_mission_result();
 }
 
@@ -294,4 +487,3 @@ Mission::publish_mission_result()
 	/* reset reached bool */
 	_mission_result.mission_reached = false;
 }
-
diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h
index 5e0e9702d..e86dd25bb 100644
--- a/src/modules/navigator/mission.h
+++ b/src/modules/navigator/mission.h
@@ -31,70 +31,172 @@
  *
  ****************************************************************************/
 /**
- * @file navigator_mission.h
+ * @file mission.h
  * Helper class to access missions
- * @author Julian Oes <julian@oes.ch>
  *
- * @author Julian Oes <joes@student.ethz.ch>
+ * @author Julian Oes <julian@oes.ch>
  */
 
 #ifndef NAVIGATOR_MISSION_H
 #define NAVIGATOR_MISSION_H
 
-#include <uORB/topics/mission.h>
-#include <uORB/topics/mission_result.h>
+#include <drivers/drv_hrt.h>
+
+#include <controllib/blocks.hpp>
+#include <controllib/block/BlockParam.hpp>
+
 #include <dataman/dataman.h>
 
+#include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/home_position.h>
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/mission_result.h>
+
+#include "mission_feasibility_checker.h"
+
+class Navigator;
 
-class __EXPORT Mission
+class Mission : public control::SuperBlock
 {
 public:
 	/**
 	 * Constructor
 	 */
-	Mission();
+	Mission(Navigator *navigator);
 
 	/**
 	 * Destructor
 	 */
-	~Mission();
+	virtual ~Mission();
 
-	void		set_offboard_dataman_id(const int new_id);
-	void		set_offboard_mission_count(const int new_count);
-	void		set_onboard_mission_count(const int new_count);
-	void		set_onboard_mission_allowed(const bool allowed);
+	/**
+	 * This function is called while the mode is inactive
+	 */
+	virtual void reset();
 
-	bool		command_current_offboard_mission_index(const int new_index);
-	bool		command_current_onboard_mission_index(const int new_index);
+	/**
+	 * This function is called while the mode is active
+	 */
+	virtual bool update(struct position_setpoint_triplet_s *pos_sp_triplet);
 
-	bool		get_current_mission_item(struct mission_item_s *mission_item, bool *onboard, int *index);
-	bool		get_next_mission_item(struct mission_item_s *mission_item);
+protected:
+	/**
+	 * Check if mission item has been reached
+	 * @return true if successfully reached
+	 */
+	bool is_mission_item_reached();
+	/**
+	 * Reset all reached flags
+	 */
+	void reset_mission_item_reached();
 
-	void		move_to_next();
+	/**
+	 * Convert a mission item to a position setpoint
+	 */
+	void mission_item_to_position_setpoint(const struct mission_item_s *item, struct position_setpoint_s *sp);
+
+	class Navigator *_navigator;
+
+	bool _waypoint_position_reached;
+	bool _waypoint_yaw_reached;
+	hrt_abstime _time_first_inside_orbit;
+
+	bool _first_run;
 
 private:
-	bool		read_mission_item(const dm_item_t dm_item, const bool is_current, int *mission_index, struct mission_item_s *new_mission_item);
+	/**
+	 * Update onboard mission topic
+	 * @return true if onboard mission has been updated
+	 */
+	bool is_onboard_mission_updated();
+
+	/**
+	 * Update offboard mission topic
+	 * @return true if offboard mission has been updated
+	 */
+	bool is_offboard_mission_updated();
+
+	/**
+	 * Move on to next mission item or switch to loiter
+	 */
+	void advance_mission();
+
+	/**
+	 * Set new mission items
+	 */
+	void set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet);
+
+	/**
+	 * Set previous position setpoint
+	 */
+	void set_previous_pos_setpoint(const struct position_setpoint_s *current_pos_sp,
+			               struct position_setpoint_s *previous_pos_sp);
+
+	/**
+	 * Try to set the current position setpoint from an onboard mission item
+	 * @return true if mission item successfully set
+	 */
+	bool is_current_onboard_mission_item_set(struct position_setpoint_s *current_pos_sp);
+
+	/**
+	 * Try to set the current position setpoint from an offboard mission item
+	 * @return true if mission item successfully set
+	 */
+	bool is_current_offboard_mission_item_set(struct position_setpoint_s *current_pos_sp);
 
-	void		report_mission_item_reached();
-	void		report_current_offboard_mission_item();
+	/**
+	 * Try to set the next position setpoint from an onboard mission item
+	 */
+	void get_next_onboard_mission_item(struct position_setpoint_s *next_pos_sp);
 
-	void		publish_mission_result();
+	/**
+	 * Try to set the next position setpoint from an offboard mission item
+	 */
+	void get_next_offboard_mission_item(struct position_setpoint_s *next_pos_sp);
 
-	int 		_offboard_dataman_id;
-	int		_current_offboard_mission_index;
-	int		_current_onboard_mission_index;
-	int		_offboard_mission_item_count;		/** number of offboard mission items available */
-	int		_onboard_mission_item_count;		/** number of onboard mission items available */
-	bool		_onboard_mission_allowed;
+	/**
+	 * Read a mission item from the dataman and watch out for DO_JUMPS
+	 * @return true if successful
+	 */
+	bool read_mission_item(const dm_item_t dm_item, bool is_current, int *mission_index,
+			       struct mission_item_s *new_mission_item);
+
+	/**
+	 * Report that a mission item has been reached
+	 */
+	void report_mission_item_reached();
+
+	/**
+	 * Rport the current mission item
+	 */
+	void report_current_offboard_mission_item();
+
+	/**
+	 * Publish the mission result so commander and mavlink know what is going on
+	 */
+	void publish_mission_result();
+
+	control::BlockParamFloat _param_onboard_enabled;
+
+	int _onboard_mission_sub;
+	int _offboard_mission_sub;
+
+	struct mission_s _onboard_mission;
+	struct mission_s _offboard_mission;
+
+	struct mission_item_s _mission_item;
+
+	orb_advert_t _mission_result_pub;
+	struct mission_result_s _mission_result;
 
 	enum {
 		MISSION_TYPE_NONE,
 		MISSION_TYPE_ONBOARD,
-		MISSION_TYPE_OFFBOARD,
-	} 		_current_mission_type;
+		MISSION_TYPE_OFFBOARD
+	} _mission_type;
+
+	MissionFeasibilityChecker missionFeasiblityChecker; /**< class that checks if a mission is feasible */
 
-	orb_advert_t	 _mission_result_pub;			/**< publish mission result topic */
-	mission_result_s _mission_result;			/**< mission result for commander/mavlink */
 };
 
 #endif
diff --git a/src/modules/navigator/module.mk b/src/modules/navigator/module.mk
index 8913d4d1c..2d07bc49c 100644
--- a/src/modules/navigator/module.mk
+++ b/src/modules/navigator/module.mk
@@ -41,7 +41,6 @@ SRCS		= navigator_main.cpp \
 		  navigator_params.c \
 		  mission.cpp \
 		  rtl.cpp \
-		  rtl_params.c \
 		  mission_feasibility_checker.cpp \
 		  geofence.cpp \
 		  geofence_params.c
diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h
new file mode 100644
index 000000000..1838fe32b
--- /dev/null
+++ b/src/modules/navigator/navigator.h
@@ -0,0 +1,205 @@
+/***************************************************************************
+ *
+ *   Copyright (c) 2013-2014 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 navigator.h
+ * Helper class to access missions
+ * @author Julian Oes <julian@oes.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ */
+
+#ifndef NAVIGATOR_H
+#define NAVIGATOR_H
+
+#include <systemlib/perf_counter.h>
+
+#include <uORB/uORB.h>
+#include <uORB/topics/mission.h>
+#include <uORB/topics/vehicle_control_mode.h>
+#include <uORB/topics/position_setpoint_triplet.h>
+#include <uORB/topics/vehicle_global_position.h>
+
+#include "mission.h"
+#include "rtl.h"
+#include "geofence.h"
+
+class Navigator
+{
+public:
+	/**
+	 * Constructor
+	 */
+	Navigator();
+
+	/**
+	 * Destructor, also kills the navigators task.
+	 */
+	~Navigator();
+
+	/**
+	 * Start the navigator task.
+	 *
+	 * @return		OK on success.
+	 */
+	int		start();
+
+	/**
+	 * Display the navigator status.
+	 */
+	void		status();
+
+	/**
+	 * Add point to geofence
+	 */
+	void		add_fence_point(int argc, char *argv[]);
+
+	/**
+	 * Load fence from file
+	 */
+	void		load_fence_from_file(const char *filename);
+
+	struct vehicle_status_s* get_vstatus() { return &_vstatus; }
+	struct vehicle_global_position_s* get_global_position() { return &_global_pos; }
+	struct home_position_s* get_home_position() { return &_home_pos; }
+	Geofence& get_geofence() { return _geofence; }
+
+private:
+
+	bool		_task_should_exit;		/**< if true, sensor task should exit */
+	int		_navigator_task;		/**< task handle for sensor task */
+
+	int		_mavlink_fd;			/**< the file descriptor to send messages over mavlink */
+
+	int		_global_pos_sub;		/**< global position subscription */
+	int		_home_pos_sub;			/**< home position subscription */
+	int		_vstatus_sub;			/**< vehicle status subscription */
+	int		_params_sub;			/**< notification of parameter updates */
+	int		_offboard_mission_sub;		/**< notification of offboard mission updates */
+	int 		_onboard_mission_sub;		/**< notification of onboard mission updates */
+	int		_capabilities_sub;		/**< notification of vehicle capabilities updates */
+	int		_control_mode_sub;		/**< vehicle control mode subscription */
+
+	orb_advert_t	_pos_sp_triplet_pub;		/**< publish position setpoint triplet */
+
+	vehicle_status_s				_vstatus;		/**< vehicle status */
+	vehicle_control_mode_s				_control_mode;		/**< vehicle control mode */
+	vehicle_global_position_s			_global_pos;		/**< global vehicle position */
+	home_position_s					_home_pos;		/**< home position for RTL */
+	mission_item_s 					_mission_item;		/**< current mission item */
+	navigation_capabilities_s			_nav_caps;		/**< navigation capabilities */
+	position_setpoint_triplet_s			_pos_sp_triplet;	/**< triplet of position setpoints */
+
+	bool 		_mission_item_valid;		/**< flags if the current mission item is valid */
+
+	perf_counter_t	_loop_perf;			/**< loop performance counter */
+
+	Geofence	_geofence;			/**< class that handles the geofence */
+	bool		_geofence_violation_warning_sent; /**< prevents spaming to mavlink */
+
+	bool		_fence_valid;			/**< flag if fence is valid */
+	bool		_inside_fence;			/**< vehicle is inside fence */
+
+	Mission		_mission;			/**< class that handles the missions */
+	//Loiter		_loiter;			/**< class that handles loiter */
+	RTL 		_rtl;				/**< class that handles RTL */
+
+	bool		_waypoint_position_reached;	/**< flags if the position of the mission item has been reached */
+	bool		_waypoint_yaw_reached;		/**< flags if the yaw position of the mission item has been reached */
+	uint64_t	_time_first_inside_orbit;	/**< the time when we were first in the acceptance radius of the mission item */
+
+	bool		_update_triplet;		/**< flags if position setpoint triplet needs to be published */
+
+	struct {
+		float acceptance_radius;
+		float loiter_radius;
+		int onboard_mission_enabled;
+		float takeoff_alt;
+	}		_parameters;			/**< local copies of parameters */
+
+	struct {
+		param_t acceptance_radius;
+		param_t loiter_radius;
+		param_t onboard_mission_enabled;
+		param_t takeoff_alt;
+	}		_parameter_handles;		/**< handles for parameters */
+
+	/**
+	 * Update our local parameter cache.
+	 */
+	void		parameters_update();
+
+	/**
+	 * Retrieve global position
+	 */
+	void		global_position_update();
+
+	/**
+	 * Retrieve home position
+	 */
+	void		home_position_update();
+
+	/**
+	 * Retreive navigation capabilities
+	 */
+	void		navigation_capabilities_update();
+
+	/**
+	 * Retrieve vehicle status
+	 */
+	void		vehicle_status_update();
+
+	/**
+	 * Retrieve vehicle control mode
+	 */
+	void		vehicle_control_mode_update();
+
+	/**
+	 * Shim for calling task_main from task_create.
+	 */
+	static void	task_main_trampoline(int argc, char *argv[]);
+
+	/**
+	 * Main task.
+	 */
+	void		task_main();
+
+	/**
+	 * Translate mission item to a position setpoint.
+	 */
+	void		mission_item_to_position_setpoint(const mission_item_s *item, position_setpoint_s *sp);
+
+	/**
+	 * Publish a new position setpoint triplet for position controllers
+	 */
+	void		publish_position_setpoint_triplet();
+};
+#endif
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index f30cd9a94..b991ffc8c 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -62,11 +62,8 @@
 #include <arch/board/board.h>
 
 #include <uORB/uORB.h>
-#include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/home_position.h>
-#include <uORB/topics/position_setpoint_triplet.h>
 #include <uORB/topics/vehicle_status.h>
-#include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/mission.h>
 #include <uORB/topics/fence.h>
@@ -74,24 +71,13 @@
 
 #include <systemlib/param/param.h>
 #include <systemlib/err.h>
-#include <systemlib/perf_counter.h>
 #include <systemlib/systemlib.h>
 #include <geo/geo.h>
 #include <dataman/dataman.h>
 #include <mathlib/mathlib.h>
 #include <mavlink/mavlink_log.h>
 
-#include "mission.h"
-#include "rtl.h"
-#include "mission_feasibility_checker.h"
-#include "geofence.h"
-
-
-/* oddly, ERROR is not defined for c++ */
-#ifdef ERROR
-# undef ERROR
-#endif
-static const int ERROR = -1;
+#include "navigator.h"
 
 /**
  * navigator app start / stop handling function
@@ -100,174 +86,10 @@ static const int ERROR = -1;
  */
 extern "C" __EXPORT int navigator_main(int argc, char *argv[]);
 
-class Navigator
-{
-public:
-	/**
-	 * Constructor
-	 */
-	Navigator();
-
-	/**
-	 * Destructor, also kills the navigators task.
-	 */
-	~Navigator();
-
-	/**
-	 * Start the navigator task.
-	 *
-	 * @return		OK on success.
-	 */
-	int		start();
-
-	/**
-	 * Display the navigator status.
-	 */
-	void		status();
-
-	/**
-	 * Add point to geofence
-	 */
-	void		add_fence_point(int argc, char *argv[]);
-
-	/**
-	 * Load fence from file
-	 */
-	void		load_fence_from_file(const char *filename);
-
-private:
-
-	bool		_task_should_exit;		/**< if true, sensor task should exit */
-	int		_navigator_task;		/**< task handle for sensor task */
-
-	int		_mavlink_fd;			/**< the file descriptor to send messages over mavlink */
-
-	int		_global_pos_sub;		/**< global position subscription */
-	int		_home_pos_sub;			/**< home position subscription */
-	int		_vstatus_sub;			/**< vehicle status subscription */
-	int		_params_sub;			/**< notification of parameter updates */
-	int		_offboard_mission_sub;		/**< notification of offboard mission updates */
-	int 		_onboard_mission_sub;		/**< notification of onboard mission updates */
-	int		_capabilities_sub;		/**< notification of vehicle capabilities updates */
-	int		_control_mode_sub;		/**< vehicle control mode subscription */
-
-	orb_advert_t	_pos_sp_triplet_pub;		/**< publish position setpoint triplet */
-
-	vehicle_status_s				_vstatus;		/**< vehicle status */
-	vehicle_control_mode_s				_control_mode;		/**< vehicle control mode */
-	vehicle_global_position_s			_global_pos;		/**< global vehicle position */
-	home_position_s					_home_pos;		/**< home position for RTL */
-	mission_item_s 					_mission_item;		/**< current mission item */
-	navigation_capabilities_s			_nav_caps;		/**< navigation capabilities */
-	position_setpoint_triplet_s			_pos_sp_triplet;	/**< triplet of position setpoints */
-
-	bool 		_mission_item_valid;		/**< flags if the current mission item is valid */
-
-	perf_counter_t	_loop_perf;			/**< loop performance counter */
-
-	Geofence	_geofence;			/**< class that handles the geofence */
-	bool		_geofence_violation_warning_sent; /**< prevents spaming to mavlink */
-
-	bool		_fence_valid;			/**< flag if fence is valid */
-	bool		_inside_fence;			/**< vehicle is inside fence */
-
-
-	Mission		_mission;			/**< class that handles the missions */
-
-	RTL 		_rtl;				/**< class that handles RTL */
-
-	bool		_waypoint_position_reached;	/**< flags if the position of the mission item has been reached */
-	bool		_waypoint_yaw_reached;		/**< flags if the yaw position of the mission item has been reached */
-	uint64_t	_time_first_inside_orbit;	/**< the time when we were first in the acceptance radius of the mission item */
-
-	MissionFeasibilityChecker			missionFeasiblityChecker; /**< class that checks if a mission is feasible */
-
-	bool		_update_triplet;		/**< flags if position setpoint triplet needs to be published */
-
-	struct {
-		float acceptance_radius;
-		float loiter_radius;
-		int onboard_mission_enabled;
-		float takeoff_alt;
-	}		_parameters;			/**< local copies of parameters */
-
-	struct {
-		param_t acceptance_radius;
-		param_t loiter_radius;
-		param_t onboard_mission_enabled;
-		param_t takeoff_alt;
-	}		_parameter_handles;		/**< handles for parameters */
-
-	/**
-	 * Update our local parameter cache.
-	 */
-	void		parameters_update();
-
-	/**
-	 * Retrieve global position
-	 */
-	void		global_position_update();
-
-	/**
-	 * Retrieve home position
-	 */
-	void		home_position_update();
-
-	/**
-	 * Retreive navigation capabilities
-	 */
-	void		navigation_capabilities_update();
-
-	/**
-	 * Retrieve offboard mission.
-	 */
-	void		offboard_mission_update();
-
-	/**
-	 * Retrieve onboard mission.
-	 */
-	void		onboard_mission_update();
-
-	/**
-	 * Retrieve vehicle status
-	 */
-	void		vehicle_status_update();
-
-	/**
-	 * Retrieve vehicle control mode
-	 */
-	void		vehicle_control_mode_update();
-
-	/**
-	 * Shim for calling task_main from task_create.
-	 */
-	static void	task_main_trampoline(int argc, char *argv[]);
-
-	/**
-	 * Main task.
-	 */
-	void		task_main();
-
-	/**
-	 * Translate mission item to a position setpoint.
-	 */
-	void		mission_item_to_position_setpoint(const mission_item_s *item, position_setpoint_s *sp);
-
-	/**
-	 * Publish a new position setpoint triplet for position controllers
-	 */
-	void		publish_position_setpoint_triplet();
-};
 
 namespace navigator
 {
 
-/* oddly, ERROR is not defined for c++ */
-#ifdef ERROR
-# undef ERROR
-#endif
-static const int ERROR = -1;
-
 Navigator	*g_navigator;
 }
 
@@ -299,8 +121,9 @@ Navigator::Navigator() :
 	_geofence_violation_warning_sent(false),
 	_fence_valid(false),
 	_inside_fence(true),
-	_mission({}),
-	_rtl({}),
+	_mission(this),
+	//_loiter(&_global_pos, &_home_pos, &_vstatus),
+	_rtl(this),
 	_waypoint_position_reached(false),
 	_waypoint_yaw_reached(false),
 	_time_first_inside_orbit(0),
@@ -351,7 +174,7 @@ Navigator::parameters_update()
 	param_get(_parameter_handles.onboard_mission_enabled, &(_parameters.onboard_mission_enabled));
 	param_get(_parameter_handles.takeoff_alt, &(_parameters.takeoff_alt));
 
-	_mission.set_onboard_mission_allowed((bool)_parameters.onboard_mission_enabled);
+	//_mission.set_onboard_mission_allowed((bool)_parameters.onboard_mission_enabled);
 
 	_geofence.updateParams();
 }
@@ -366,8 +189,6 @@ void
 Navigator::home_position_update()
 {
 	orb_copy(ORB_ID(home_position), _home_pos_sub, &_home_pos);
-
-	_rtl.set_home_position(&_home_pos);
 }
 
 void
@@ -376,54 +197,6 @@ Navigator::navigation_capabilities_update()
 	orb_copy(ORB_ID(navigation_capabilities), _capabilities_sub, &_nav_caps);
 }
 
-
-void
-Navigator::offboard_mission_update()
-{
-	struct mission_s offboard_mission;
-
-	if (orb_copy(ORB_ID(offboard_mission), _offboard_mission_sub, &offboard_mission) == OK) {
-
-		/* Check mission feasibility, for now do not handle the return value,
-		 * however warnings are issued to the gcs via mavlink from inside the MissionFeasiblityChecker */
-		dm_item_t dm_current;
-
-		if (offboard_mission.dataman_id == 0) {
-			dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0;
-
-		} else {
-			dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1;
-		}
-
-		missionFeasiblityChecker.checkMissionFeasible(_vstatus.is_rotary_wing, dm_current, (size_t)offboard_mission.count, _geofence);
-
-		_mission.set_offboard_dataman_id(offboard_mission.dataman_id);
-
-		_mission.set_offboard_mission_count(offboard_mission.count);
-		_mission.command_current_offboard_mission_index(offboard_mission.current_index);
-
-	} else {
-		_mission.set_offboard_mission_count(0);
-		_mission.command_current_offboard_mission_index(0);
-	}
-}
-
-void
-Navigator::onboard_mission_update()
-{
-	struct mission_s onboard_mission;
-
-	if (orb_copy(ORB_ID(onboard_mission), _onboard_mission_sub, &onboard_mission) == OK) {
-
-		_mission.set_onboard_mission_count(onboard_mission.count);
-		_mission.command_current_onboard_mission_index(onboard_mission.current_index);
-
-	} else {
-		_mission.set_onboard_mission_count(0);
-		_mission.command_current_onboard_mission_index(0);
-	}
-}
-
 void
 Navigator::vehicle_status_update()
 {
@@ -459,8 +232,7 @@ Navigator::task_main()
 
 	/* Try to load the geofence:
 	 * if /fs/microsd/etc/geofence.txt load from this file
-	 * else clear geofence data in datamanager
-	 */
+	 * else clear geofence data in datamanager */
 	struct stat buffer;
 
 	if (stat(GEOFENCE_FILENAME, &buffer) == 0) {
@@ -474,9 +246,7 @@ Navigator::task_main()
 			warnx("Could not clear geofence");
 	}
 
-	/*
-	 * do subscriptions
-	 */
+	/* do subscriptions */
 	_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
 	_offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission));
 	_onboard_mission_sub = orb_subscribe(ORB_ID(onboard_mission));
@@ -493,8 +263,6 @@ Navigator::task_main()
 	global_position_update();
 	home_position_update();
 	navigation_capabilities_update();
-	offboard_mission_update();
-	onboard_mission_update();
 
 	/* rate limit position updates to 50 Hz */
 	orb_set_interval(_global_pos_sub, 20);
@@ -503,7 +271,7 @@ Navigator::task_main()
 	const hrt_abstime mavlink_open_interval = 500000;
 
 	/* wakeup source(s) */
-	struct pollfd fds[8];
+	struct pollfd fds[6];
 
 	/* Setup of loop */
 	fds[0].fd = _params_sub;
@@ -514,14 +282,10 @@ Navigator::task_main()
 	fds[2].events = POLLIN;
 	fds[3].fd = _capabilities_sub;
 	fds[3].events = POLLIN;
-	fds[4].fd = _offboard_mission_sub;
+	fds[4].fd = _vstatus_sub;
 	fds[4].events = POLLIN;
-	fds[5].fd = _onboard_mission_sub;
+	fds[5].fd = _control_mode_sub;
 	fds[5].events = POLLIN;
-	fds[6].fd = _vstatus_sub;
-	fds[6].events = POLLIN;
-	fds[7].fd = _control_mode_sub;
-	fds[7].events = POLLIN;
 
 	while (!_task_should_exit) {
 
@@ -547,12 +311,12 @@ Navigator::task_main()
 		}
 
 		/* vehicle control mode updated */
-		if (fds[7].revents & POLLIN) {
+		if (fds[5].revents & POLLIN) {
 			vehicle_control_mode_update();
 		}
 
 		/* vehicle status updated */
-		if (fds[6].revents & POLLIN) {
+		if (fds[4].revents & POLLIN) {
 			vehicle_status_update();
 		}
 
@@ -567,16 +331,6 @@ Navigator::task_main()
 			navigation_capabilities_update();
 		}
 
-		/* offboard mission updated */
-		if (fds[4].revents & POLLIN) {
-			offboard_mission_update();
-		}
-
-		/* onboard mission updated */
-		if (fds[5].revents & POLLIN) {
-			onboard_mission_update();
-		}
-
 		/* home position updated */
 		if (fds[2].revents & POLLIN) {
 			home_position_update();
@@ -600,6 +354,33 @@ Navigator::task_main()
 			}
 		}
 
+		/* Do stuff according to navigation state set by commander */
+		switch (_vstatus.set_nav_state) {
+			case NAVIGATION_STATE_MANUAL:
+			case NAVIGATION_STATE_ACRO:
+			case NAVIGATION_STATE_ALTCTL:
+			case NAVIGATION_STATE_POSCTL:
+				break;
+			case NAVIGATION_STATE_AUTO_MISSION:
+				_update_triplet = _mission.update(&_pos_sp_triplet);
+				_rtl.reset();
+				break;
+			case NAVIGATION_STATE_AUTO_LOITER:
+				//_loiter.update();
+				break;
+			case NAVIGATION_STATE_AUTO_RTL:
+			case NAVIGATION_STATE_AUTO_RTL_RC:
+			case NAVIGATION_STATE_AUTO_RTL_DL:
+				_mission.reset();
+				_update_triplet = _rtl.update(&_pos_sp_triplet);
+				break;
+			case NAVIGATION_STATE_LAND:
+			case NAVIGATION_STATE_TERMINATION:
+			default:
+				break;
+
+		}
+
 		if (_update_triplet ) {
 			publish_position_setpoint_triplet();
 			_update_triplet = false;
@@ -880,47 +661,6 @@ Navigator::start_land()
 	return true;
 }
 #endif
-void
-Navigator::mission_item_to_position_setpoint(const mission_item_s *item, position_setpoint_s *sp)
-{
-	sp->valid = true;
-
-	if (item->nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) {
-
-		if (_pos_sp_triplet.previous.valid) {
-			/* if previous setpoint is valid then use it to calculate heading to home */
-			sp->yaw = get_bearing_to_next_waypoint(_pos_sp_triplet.previous.lat, _pos_sp_triplet.previous.lon, sp->lat, sp->lon);
-
-		} else {
-			/* else use current position */
-			sp->yaw = get_bearing_to_next_waypoint(_global_pos.lat, _global_pos.lon, sp->lat, sp->lon);
-		}
-
-	} else {
-		sp->lat = item->lat;
-		sp->lon = item->lon;
-		sp->alt = item->altitude_is_relative ? item->altitude + _home_pos.alt : item->altitude;
-		sp->yaw = item->yaw;
-		sp->loiter_radius = item->loiter_radius;
-		sp->loiter_direction = item->loiter_direction;
-		sp->pitch_min = item->pitch_min;
-	}
-
-	if (item->nav_cmd == NAV_CMD_TAKEOFF) {
-		sp->type = SETPOINT_TYPE_TAKEOFF;
-
-	} else if (item->nav_cmd == NAV_CMD_LAND) {
-		sp->type = SETPOINT_TYPE_LAND;
-
-	} else if (item->nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
-		   item->nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
-		   item->nav_cmd == NAV_CMD_LOITER_UNLIMITED) {
-		sp->type = SETPOINT_TYPE_LOITER;
-
-	} else {
-		sp->type = SETPOINT_TYPE_NORMAL;
-	}
-}
 #if 0
 bool
 Navigator::check_mission_item_reached()
diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp
index b8ea06275..9d7886aa6 100644
--- a/src/modules/navigator/rtl.cpp
+++ b/src/modules/navigator/rtl.cpp
@@ -51,26 +51,41 @@
 
 #include "rtl.h"
 
-
-RTL::RTL() :
-	SuperBlock(NULL, "RTL"),
+RTL::RTL(Navigator *navigator) :
+	Mission(navigator),
 	_mavlink_fd(-1),
 	_rtl_state(RTL_STATE_NONE),
 	_home_position({}),
+	_loiter_radius(50),
+	_acceptance_radius(50),
 	_param_return_alt(this, "RETURN_ALT"),
 	_param_descend_alt(this, "DESCEND_ALT"),
-	_param_land_delay(this, "LAND_DELAY"),
-	_loiter_radius(50),
-	_acceptance_radius(50)
+	_param_land_delay(this, "LAND_DELAY")
 {
 	/* load initial params */
 	updateParams();
+	/* initial reset */
+	reset();
 }
 
 RTL::~RTL()
 {
 }
 
+bool
+RTL::update(struct position_setpoint_triplet_s *pos_sp_triplet)
+{
+	bool updated = false;
+
+	return updated;
+}
+
+void
+RTL::reset()
+{
+
+}
+
 void
 RTL::set_home_position(const home_position_s *new_home_position)
 {
@@ -206,7 +221,7 @@ RTL::get_current_rtl_item(const vehicle_global_position_s *global_position, miss
 	}
 
 	default:
-		return false;	
+		return false;
 	}
 
 	return true;
@@ -226,7 +241,7 @@ RTL::move_to_next()
 	case RTL_STATE_CLIMB:
 		_rtl_state = RTL_STATE_RETURN;
 		break;
-	
+
 	case RTL_STATE_RETURN:
 		_rtl_state = RTL_STATE_DESCEND;
 		break;
diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h
index c761837fc..8d1bfec59 100644
--- a/src/modules/navigator/rtl.h
+++ b/src/modules/navigator/rtl.h
@@ -1,4 +1,4 @@
-/****************************************************************************
+/***************************************************************************
  *
  *   Copyright (c) 2013-2014 PX4 Development Team. All rights reserved.
  *
@@ -44,22 +44,32 @@
 #include <controllib/block/BlockParam.hpp>
 
 #include <uORB/topics/mission.h>
+#include <uORB/topics/mission.h>
 #include <uORB/topics/home_position.h>
 #include <uORB/topics/vehicle_global_position.h>
 
-class RTL : public control::SuperBlock
+#include "mission.h"
+
+class Navigator;
+
+class RTL : public Mission
 {
 public:
 	/**
 	 * Constructor
 	 */
-	RTL();
+	RTL(Navigator *navigator);
 
 	/**
 	 * Destructor
 	 */
-	~RTL();
+	virtual ~RTL();
+
+	virtual bool update(struct position_setpoint_triplet_s *pos_sp_triplet);
 
+	virtual void reset();
+
+private:
 	void		set_home_position(const home_position_s *home_position);
 
 	bool		get_current_rtl_item(const vehicle_global_position_s *global_position, mission_item_s *new_mission_item);
@@ -67,7 +77,6 @@ public:
 
 	void		move_to_next();
 
-private:
 	int 		_mavlink_fd;
 
 	enum RTLState {
@@ -77,7 +86,7 @@ private:
 		RTL_STATE_DESCEND,
 		RTL_STATE_LAND,
 		RTL_STATE_FINISHED,
-	}		_rtl_state;
+	} _rtl_state;
 
 	home_position_s _home_position;
 	float		_loiter_radius;
diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index 66a9e31c6..2b6caf159 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -1340,7 +1340,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		/* --- GLOBAL POSITION SETPOINT --- */
 		if (copy_if_updated(ORB_ID(position_setpoint_triplet), subs.triplet_sub, &buf.triplet)) {
 			log_msg.msg_type = LOG_GPSP_MSG;
-			log_msg.body.log_GPSP.nav_state = buf.triplet.nav_state;
+			log_msg.body.log_GPSP.nav_state = 0;  /* TODO: Fix this */
 			log_msg.body.log_GPSP.lat = (int32_t)(buf.triplet.current.lat * 1e7d);
 			log_msg.body.log_GPSP.lon = (int32_t)(buf.triplet.current.lon * 1e7d);
 			log_msg.body.log_GPSP.alt = buf.triplet.current.alt;
diff --git a/src/modules/uORB/topics/mission.h b/src/modules/uORB/topics/mission.h
index 4532b329d..d25db3a77 100644
--- a/src/modules/uORB/topics/mission.h
+++ b/src/modules/uORB/topics/mission.h
@@ -1,9 +1,6 @@
 /****************************************************************************
  *
  *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: @author Thomas Gubler <thomasgubler@student.ethz.ch>
- *           @author Julian Oes <joes@student.ethz.ch>
- *           @author Lorenz Meier <lm@inf.ethz.ch>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -37,6 +34,9 @@
 /**
  * @file mission.h
  * Definition of a mission consisting of mission items.
+ * @author Thomas Gubler <thomasgubler@student.ethz.ch>
+ * @author Julian Oes <joes@student.ethz.ch>
+ * @author Lorenz Meier <lm@inf.ethz.ch>
  */
 
 #ifndef TOPIC_MISSION_H_
@@ -92,9 +92,9 @@ struct mission_item_s {
 	float pitch_min;		/**< minimal pitch angle for fixed wing takeoff waypoints */
 	bool autocontinue;		/**< true if next waypoint should follow after this one */
 	enum ORIGIN origin;		/**< where the waypoint has been generated		*/
-	int do_jump_mission_index;	/**< the mission index that we want to jump to		*/
-	int do_jump_repeat_count;	/**< how many times the jump should be repeated		*/
-	int do_jump_current_count;	/**< how many times the jump has already been repeated	*/
+	int do_jump_mission_index;	/**< index where the do jump will go to                 */
+	unsigned do_jump_repeat_count;	/**< how many times do jump needs to be done            */
+	unsigned do_jump_current_count;	/**< count how many times the jump has been done	*/
 };
 
 struct mission_s
diff --git a/src/modules/uORB/topics/position_setpoint_triplet.h b/src/modules/uORB/topics/position_setpoint_triplet.h
index c2741a05b..ce42035ba 100644
--- a/src/modules/uORB/topics/position_setpoint_triplet.h
+++ b/src/modules/uORB/topics/position_setpoint_triplet.h
@@ -1,9 +1,6 @@
 /****************************************************************************
  *
  *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
- *   Author: @author Thomas Gubler <thomasgubler@student.ethz.ch>
- *           @author Julian Oes <joes@student.ethz.ch>
- *           @author Lorenz Meier <lm@inf.ethz.ch>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -37,6 +34,10 @@
 /**
  * @file mission_item_triplet.h
  * Definition of the global WGS84 position setpoint uORB topic.
+ *
+ * @author Thomas Gubler <thomasgubler@student.ethz.ch>
+ * @author Julian Oes <joes@student.ethz.ch>
+ * @author Lorenz Meier <lm@inf.ethz.ch>
  */
 
 #ifndef TOPIC_MISSION_ITEM_TRIPLET_H_
@@ -53,11 +54,12 @@
 
 enum SETPOINT_TYPE
 {
-	SETPOINT_TYPE_NORMAL = 0,		/**< normal setpoint */
-	SETPOINT_TYPE_LOITER,			/**< loiter setpoint */
-	SETPOINT_TYPE_TAKEOFF,			/**< takeoff setpoint */
-	SETPOINT_TYPE_LAND,			/**< land setpoint, altitude must be ignored, vehicle must descend until landing */
-	SETPOINT_TYPE_IDLE,			/**< do nothing, switch off motors or keep at idle speed (MC) */
+	SETPOINT_TYPE_POSITION = 0,	/**< position setpoint */
+	SETPOINT_TYPE_VELOCITY,		/**< velocity setpoint */
+	SETPOINT_TYPE_LOITER,		/**< loiter setpoint */
+	SETPOINT_TYPE_TAKEOFF,		/**< takeoff setpoint */
+	SETPOINT_TYPE_LAND,		/**< land setpoint, altitude must be ignored, descend until landing */
+	SETPOINT_TYPE_IDLE,		/**< do nothing, switch off motors or keep at idle speed (MC) */
 };
 
 struct position_setpoint_s
@@ -73,16 +75,6 @@ struct position_setpoint_s
 	float pitch_min;		/**< minimal pitch angle for fixed wing takeoff waypoints */
 };
 
-typedef enum {
-	NAV_STATE_MANUAL = 0,
-	NAV_STATE_POSCTL,
-	NAV_STATE_AUTO,
-	NAV_STATE_RC_LOSS,
-	NAV_STATE_DL_LOSS,
-	NAV_STATE_TERMINATION,
-	MAX_NAV_STATE
-} nav_state_t;
-
 /**
  * Global position setpoint triplet in WGS84 coordinates.
  *
@@ -93,8 +85,6 @@ struct position_setpoint_triplet_s
 	struct position_setpoint_s previous;
 	struct position_setpoint_s current;
 	struct position_setpoint_s next;
-
-	nav_state_t nav_state;			/**< navigation state */
 };
 
 /**