navigator: lot's of cleanup (WIP)

21 files changed, 957 insertions, 930 deletions

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index e7cf2b3fa..c2d24a883 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -547,15 +547,13 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 			unsigned int mav_goto = cmd->param1;
 
 			if (mav_goto == 0) {	// MAV_GOTO_DO_HOLD
-				status->set_nav_state = NAV_STATE_LOITER;
-				status->set_nav_state_timestamp = hrt_absolute_time();
+				status->set_nav_state = NAVIGATION_STATE_LOITER;
 				mavlink_log_critical(mavlink_fd, "#audio: pause mission cmd");
 				result = VEHICLE_CMD_RESULT_ACCEPTED;
 				ret = true;
 
 			} else if (mav_goto == 1) {	// MAV_GOTO_DO_CONTINUE
-				status->set_nav_state = NAV_STATE_MISSION;
-				status->set_nav_state_timestamp = hrt_absolute_time();
+				status->set_nav_state = NAVIGATION_STATE_MISSION;
 				mavlink_log_critical(mavlink_fd, "#audio: continue mission cmd");
 				result = VEHICLE_CMD_RESULT_ACCEPTED;
 				ret = true;
@@ -668,8 +666,7 @@ int commander_thread_main(int argc, char *argv[])
 	// We want to accept RC inputs as default
 	status.rc_input_blocked = false;
 	status.main_state = MAIN_STATE_MANUAL;
-	status.set_nav_state = NAV_STATE_NONE;
-	status.set_nav_state_timestamp = 0;
+	status.set_nav_state = NAVIGATION_STATE_NONE;
 	status.arming_state = ARMING_STATE_INIT;
 	status.hil_state = HIL_STATE_OFF;
 	status.failsafe_state = FAILSAFE_STATE_NORMAL;
@@ -944,7 +941,7 @@ int commander_thread_main(int argc, char *argv[])
 		check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.z_valid, &(status.condition_local_altitude_valid), &status_changed);
 
 		static bool published_condition_landed_fw = false;
-		if (status.is_rotary_wing && status.condition_local_altitude_valid) {
+		if (status.condition_local_altitude_valid) {
 			if (status.condition_landed != local_position.landed) {
 				status.condition_landed = local_position.landed;
 				status_changed = true;
@@ -1222,30 +1219,30 @@ int commander_thread_main(int argc, char *argv[])
 				mavlink_log_critical(mavlink_fd, "ERROR: main state transition denied");
 			}
 
-			/* set navigation state */
-			/* RETURN switch, overrides MISSION switch */
-			if (sp_man.return_switch == SWITCH_POS_ON) {
-				/* switch to RTL if not already landed after RTL and home position set */
-				status.set_nav_state = NAV_STATE_RTL;
-				status.set_nav_state_timestamp = hrt_absolute_time();
+			if (sp_man.mode_switch == SWITCH_POS_ON) {
 
-			} else {
-				/* MISSION switch */
-				if (sp_man.mission_switch == SWITCH_POS_ON) {
-					/* stick is in LOITER position */
-					status.set_nav_state = NAV_STATE_LOITER;
-					status.set_nav_state_timestamp = hrt_absolute_time();
-
-				} else if (sp_man.mission_switch != SWITCH_POS_NONE) {
-					/* stick is in MISSION position */
-					status.set_nav_state = NAV_STATE_MISSION;
-					status.set_nav_state_timestamp = hrt_absolute_time();
-
-				} else if ((sp_man.return_switch == SWITCH_POS_OFF || sp_man.return_switch == SWITCH_POS_MIDDLE) &&
-						pos_sp_triplet.nav_state == NAV_STATE_RTL) {
-					/* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */
-					status.set_nav_state = NAV_STATE_MISSION;
-					status.set_nav_state_timestamp = hrt_absolute_time();
+				/* set navigation state */
+				/* RETURN switch, overrides MISSION switch */
+				if (sp_man.return_switch == SWITCH_POS_ON) {
+					/* switch to RTL if not already landed after RTL and home position set */
+					status.set_nav_state = NAVIGATION_STATE_RTL;
+
+				} else {
+					/* MISSION switch */
+					if (sp_man.mission_switch == SWITCH_POS_ON) {
+						/* stick is in LOITER position */
+						status.set_nav_state = NAVIGATION_STATE_LOITER;
+
+					} else if (sp_man.mission_switch != SWITCH_POS_NONE) {
+						/* stick is in MISSION position */
+						status.set_nav_state = NAVIGATION_STATE_MISSION;
+					}
+					/* XXX: I don't understand this */					
+					//else if ((sp_man.return_switch == SWITCH_POS_OFF || sp_man.return_switch == SWITCH_POS_MIDDLE) &&
+					// 		pos_sp_triplet.nav_state == NAVIGATION_STATE_RTL) {
+					// 	/* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */
+					// 	status.set_nav_state = NAV_STATE_MISSION;
+					// }
 				}
 			}
 
diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index e6f245cf9..54f0f13f4 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -450,8 +450,7 @@ transition_result_t failsafe_state_transition(struct vehicle_status_s *status, f
 
 			/* global position and home position required for RTL */
 			if (status->condition_global_position_valid && status->condition_home_position_valid) {
-				status->set_nav_state = NAV_STATE_RTL;
-				status->set_nav_state_timestamp = hrt_absolute_time();
+				status->set_nav_state = NAVIGATION_STATE_RTL;
 				ret = TRANSITION_CHANGED;
 			}
 
@@ -461,8 +460,7 @@ transition_result_t failsafe_state_transition(struct vehicle_status_s *status, f
 
 			/* at least relative altitude estimate required for landing */
 			if (status->condition_local_altitude_valid || status->condition_global_position_valid) {
-				status->set_nav_state = NAV_STATE_LAND;
-				status->set_nav_state_timestamp = hrt_absolute_time();
+				status->set_nav_state = NAVIGATION_STATE_LAND;
 				ret = TRANSITION_CHANGED;
 			}
 
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 7f13df785..a00a388a8 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
@@ -150,8 +150,6 @@ private:
 
 	perf_counter_t	_loop_perf;			/**< loop performance counter */
 
-	bool		_setpoint_valid;		/**< flag if the position control setpoint is valid */
-
 	/** manual control states */
 	float		_seatbelt_hold_heading;		/**< heading the system should hold in seatbelt mode */
 	double		_loiter_hold_lat;
@@ -393,7 +391,6 @@ FixedwingPositionControl::FixedwingPositionControl() :
 	_loop_perf(perf_alloc(PC_ELAPSED, "fw l1 control")),
 
 /* states */
-	_setpoint_valid(false),
 	_loiter_hold(false),
 	land_noreturn_horizontal(false),
 	land_noreturn_vertical(false),
@@ -663,7 +660,6 @@ FixedwingPositionControl::vehicle_setpoint_poll()
 
 	if (pos_sp_triplet_updated) {
 		orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet);
-		_setpoint_valid = true;
 	}
 }
 
@@ -708,7 +704,7 @@ void
 FixedwingPositionControl::calculate_gndspeed_undershoot(const math::Vector<2> &current_position, const math::Vector<2> &ground_speed, const struct position_setpoint_triplet_s &pos_sp_triplet)
 {
 
-	if (_global_pos_valid && !(pos_sp_triplet.current.type == SETPOINT_TYPE_LOITER)) {
+	if (pos_sp_triplet.current.valid && !(pos_sp_triplet.current.type == SETPOINT_TYPE_LOITER)) {
 
 		/* rotate ground speed vector with current attitude */
 		math::Vector<2> yaw_vector(_R_nb(0, 0), _R_nb(1, 0));
@@ -781,7 +777,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 
 	// XXX this should only execute if auto AND safety off (actuators active),
 	// else integrators should be constantly reset.
-	if (_control_mode.flag_control_position_enabled) {
+	if (pos_sp_triplet.current.valid) {
 
 		/* get circle mode */
 		bool was_circle_mode = _l1_control.circle_mode();
diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 36d47b7ee..c60c85495 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -872,7 +872,7 @@ int Mavlink::map_mavlink_mission_item_to_mission_item(const mavlink_mission_item
 		break;
 	case MAV_CMD_DO_JUMP:
 		mission_item->do_jump_mission_index = mavlink_mission_item->param1;
-		/* TODO: also save param2 (repeat count) */
+		mission_item->do_jump_repeat_count = mavlink_mission_item->param2;
 		break;
 	default:
 		mission_item->acceptance_radius = mavlink_mission_item->param2;
@@ -889,6 +889,9 @@ int Mavlink::map_mavlink_mission_item_to_mission_item(const mavlink_mission_item
 	// mission_item->index = mavlink_mission_item->seq;
 	mission_item->origin = ORIGIN_MAVLINK;
 
+	/* reset DO_JUMP count */
+	mission_item->do_jump_current_count = 0;
+
 	return OK;
 }
 
@@ -908,7 +911,7 @@ int Mavlink::map_mission_item_to_mavlink_mission_item(const struct mission_item_
 
 	case NAV_CMD_DO_JUMP:
 		mavlink_mission_item->param1 = mission_item->do_jump_mission_index;
-		/* TODO: also save param2 (repeat count) */
+		mavlink_mission_item->param2 = mission_item->do_jump_repeat_count;
 		break;
 
 	default:
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index 648228e3b..3cf69bf7c 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -117,7 +117,8 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set
 	union px4_custom_mode custom_mode;
 	custom_mode.data = 0;
 
-	if (pos_sp_triplet->nav_state == NAV_STATE_NONE) {
+	if (pos_sp_triplet->nav_state == NAV_STATE_NONE_ON_GROUND
+		|| pos_sp_triplet->nav_state == NAV_STATE_NONE_IN_AIR) {
 		/* use main state when navigator is not active */
 		if (status->main_state == MAIN_STATE_MANUAL) {
 			*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (status->is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0);
@@ -142,7 +143,7 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set
 		*mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED;
 		custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
 
-		if (pos_sp_triplet->nav_state == NAV_STATE_READY) {
+		if (pos_sp_triplet->nav_state == NAV_STATE_AUTO_ON_GROUND) {
 			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY;
 
 		} else if (pos_sp_triplet->nav_state == NAV_STATE_LOITER) {
diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp
index 2bd2d356a..7d3d39d18 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -645,7 +645,7 @@ MulticopterPositionControl::task_main()
 				_pos_sp_triplet.current.valid = true;
 				_pos_sp_triplet.next.valid = true;
 
-				_pos_sp_triplet.nav_state = NAV_STATE_NONE;
+				// _pos_sp_triplet.nav_state = NAV_STATE_NONE;
 				_pos_sp_triplet.current.type = SETPOINT_TYPE_NORMAL;
 				_pos_sp_triplet.current.lat = _lat_sp;
 				_pos_sp_triplet.current.lon = _lon_sp;
diff --git a/src/modules/navigator/geofence_params.c b/src/modules/navigator/geofence_params.c
index 5831a0ca9..653b1ad84 100644
--- a/src/modules/navigator/geofence_params.c
+++ b/src/modules/navigator/geofence_params.c
@@ -1,7 +1,6 @@
 /****************************************************************************
  *
  *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
- *   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
diff --git a/src/modules/navigator/navigator_mission.cpp b/src/modules/navigator/mission.cpp
index ac7e604ef..e95bcfd43 100644
--- a/src/modules/navigator/navigator_mission.cpp
+++ b/src/modules/navigator/mission.cpp
@@ -1,7 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
- *   Author: @author Julian Oes <joes@student.ethz.ch>
+ *   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
@@ -33,22 +32,22 @@
  ****************************************************************************/
 /**
  * @file navigator_mission.cpp
+ *
  * Helper class to access missions
+ *
+ * @author Julian Oes <julian@oes.ch>
  */
 
 #include <string.h>
 #include <stdlib.h>
+
 #include <dataman/dataman.h>
 #include <systemlib/err.h>
+
 #include <uORB/uORB.h>
 #include <uORB/topics/mission_result.h>
-#include "navigator_mission.h"
 
-/* oddly, ERROR is not defined for c++ */
-#ifdef ERROR
-# undef ERROR
-#endif
-static const int ERROR = -1;
+#include "mission.h"
 
 
 Mission::Mission() :
@@ -60,27 +59,43 @@ Mission::Mission() :
 	_onboard_mission_item_count(0),
 	_onboard_mission_allowed(false),
 	_current_mission_type(MISSION_TYPE_NONE),
-	_mission_result_pub(-1)
+	_mission_result_pub(-1),
+	_mission_result({})
 {
-	memset(&_mission_result, 0, sizeof(struct mission_result_s));
 }
 
 Mission::~Mission()
 {
-
 }
 
 void
-Mission::set_offboard_dataman_id(int new_id)
+Mission::set_offboard_dataman_id(const int new_id)
 {
 	_offboard_dataman_id = new_id;
 }
 
 void
-Mission::set_current_offboard_mission_index(int new_index)
+Mission::set_offboard_mission_count(int new_count)
 {
-	if (new_index != -1) {
-		warnx("specifically set to %d", new_index);
+	_offboard_mission_item_count = new_count;
+}
+
+void
+Mission::set_onboard_mission_count(int new_count)
+{
+	_onboard_mission_item_count = new_count;
+}
+
+void
+Mission::set_onboard_mission_allowed(bool allowed)
+{
+	_onboard_mission_allowed = allowed;
+}
+
+bool
+Mission::command_current_offboard_mission_index(const int new_index)
+{
+	if (new_index >= 0) {
 		_current_offboard_mission_index = (unsigned)new_index;
 	} else {
 
@@ -92,8 +107,8 @@ Mission::set_current_offboard_mission_index(int new_index)
 	report_current_offboard_mission_item();
 }
 
-void
-Mission::set_current_onboard_mission_index(int new_index)
+bool
+Mission::command_current_onboard_mission_index(int new_index)
 {
 	if (new_index != -1) {
 		_current_onboard_mission_index = (unsigned)new_index;
@@ -108,153 +123,130 @@ Mission::set_current_onboard_mission_index(int new_index)
 	// report_current_mission_item();
 }
 
-void
-Mission::set_offboard_mission_count(unsigned new_count)
-{
-	_offboard_mission_item_count = new_count;
-}
-
-void
-Mission::set_onboard_mission_count(unsigned new_count)
-{
-	_onboard_mission_item_count = new_count;
-}
-
-void
-Mission::set_onboard_mission_allowed(bool allowed)
-{
-	_onboard_mission_allowed = allowed;
-}
-
-bool
-Mission::current_mission_available()
-{
-	return (current_onboard_mission_available() || current_offboard_mission_available());
-
-}
-
 bool
-Mission::next_mission_available()
+Mission::get_current_mission_item(struct mission_item_s *new_mission_item, bool *onboard, int *index)
 {
-	return (next_onboard_mission_available() || next_offboard_mission_available());
-}
-
-int
-Mission::get_current_mission_item(struct mission_item_s *new_mission_item, bool *onboard, unsigned *index)
-{
-	int ret = ERROR;
+	*onboard = false;
+	*index = -1;
 
 	/* try onboard mission first */
-	if (current_onboard_mission_available()) {
-
-		ret = get_mission_item(DM_KEY_WAYPOINTS_ONBOARD, &_current_onboard_mission_index, new_mission_item);
-		if (ret == OK) {
+	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;
+		}
+	}
+	
 	/* otherwise fallback to offboard */
-	} else if (current_offboard_mission_available()) {
+	if (_current_offboard_mission_index < _offboard_mission_item_count) {
 
 		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;
 		}
+		if (read_mission_item(dm_current, true, &_current_offboard_mission_index, new_mission_item)) {
 
-		ret = get_mission_item(dm_current, &_current_offboard_mission_index, new_mission_item);
-		if (ret == OK) {
 			_current_mission_type = MISSION_TYPE_OFFBOARD;
 			*onboard = false;
 			*index = _current_offboard_mission_index;
-		}
 
-	} else {
-		/* happens when no more mission items can be added as a next item */
-		_current_mission_type = MISSION_TYPE_NONE;
-		ret == ERROR;
+			return true;
+		}
 	}
 
-	return ret;
+	/* happens when no more mission items can be added as a next item */
+	_current_mission_type = MISSION_TYPE_NONE;
+
+	return false;
 }
 
-int
+bool
 Mission::get_next_mission_item(struct mission_item_s *new_mission_item)
 {
-	int ret = ERROR;
+	int next_temp_mission_index = _current_onboard_mission_index + 1;
 
 	/* try onboard mission first */
-	if (next_onboard_mission_available()) {
-
-		unsigned next_onboard_mission_index = _current_onboard_mission_index + 1;
-		ret = get_mission_item(DM_KEY_WAYPOINTS_ONBOARD, &next_onboard_mission_index, new_mission_item);
-
-	/* otherwise fallback to offboard */
-	} else if (next_offboard_mission_available()) {
-
-		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;
+	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)) {
+			return true;
 		}
+	}
 
-		unsigned next_offboard_mission_index = _current_offboard_mission_index + 1;
-		ret = get_mission_item(dm_current, &next_offboard_mission_index, new_mission_item);
-
+	/* then try offboard mission */
+	dm_item_t dm_current;
+	if (_offboard_dataman_id == 0) {
+		dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0;
 	} else {
-		/* happens when no more mission items can be added as a next item */
-		ret = ERROR;
+		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;
+		}
 	}
-	return ret;
-}
-
-
-bool
-Mission::current_onboard_mission_available()
-{
-	return _onboard_mission_item_count > _current_onboard_mission_index && _onboard_mission_allowed;
-}
 
-bool
-Mission::current_offboard_mission_available()
-{
-	return _offboard_mission_item_count > _current_offboard_mission_index;
+	/* both failed, bail out */
+	return false;
 }
 
 bool
-Mission::next_onboard_mission_available()
+Mission::read_mission_item(const dm_item_t dm_item, bool is_current, int *mission_index, struct mission_item_s *new_mission_item)
 {
-	unsigned next = 0;
+	/* repeat several to get the mission item because we might have to follow multiple DO_JUMPS */
+	for (int i=0; i<10; i++) {
+		const ssize_t len = sizeof(struct mission_item_s);
 
-	if (_current_mission_type != MISSION_TYPE_ONBOARD) {
-		next = 1;
-	}
+		/* read mission item from datamanager */
+		if (dm_read(dm_item, *mission_index, new_mission_item, len) != len) {
+			/* not supposed to happen unless the datamanager can't access the SD card, etc. */
+			return false;
+		}
 
-	return _onboard_mission_item_count > (_current_onboard_mission_index + next) && _onboard_mission_allowed;
-}
+		/* 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) {
 
-bool
-Mission::next_offboard_mission_available()
-{
-	unsigned next = 0;
+			if (new_mission_item->do_jump_current_count < new_mission_item->do_jump_repeat_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;
+					}
+				}
+				/* 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;
+			}
 
-	if (_current_mission_type != MISSION_TYPE_OFFBOARD) {
-		next = 1;
+		} else {
+			/* if it's not a DO_JUMP, then we were successful */
+			return true;
+		}
 	}
 
-	return _offboard_mission_item_count > (_current_offboard_mission_index + next);
+	/* we have given up, we don't want to cycle forever */
+	warnx("ERROR: cycling through mission items without success");
+	return false;
 }
 
 void
 Mission::move_to_next()
 {
+	report_mission_item_reached();
+	
 	switch (_current_mission_type) {
 	case MISSION_TYPE_ONBOARD:
 		_current_onboard_mission_index++;
@@ -277,12 +269,14 @@ Mission::report_mission_item_reached()
 		_mission_result.mission_reached = true;
 		_mission_result.mission_index_reached = _current_offboard_mission_index;
 	}
+	publish_mission_result();
 }
 
 void
 Mission::report_current_offboard_mission_item()
 {
 	_mission_result.index_current_mission = _current_offboard_mission_index;
+	publish_mission_result();
 }
 
 void
@@ -301,32 +295,3 @@ Mission::publish_mission_result()
 	_mission_result.mission_reached = false;
 }
 
-int
-Mission::get_mission_item(const dm_item_t dm_item, unsigned *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++) {
-
-		const ssize_t len = sizeof(struct mission_item_s);
-
-		/* read mission item from datamanager */
-		if (dm_read(dm_item, *mission_index, new_mission_item, len) != len) {
-			/* not supposed to happen unless the datamanager can't access the SD card, etc. */
-			return ERROR;
-		}
-
-		/* check for DO_JUMP item */
-		if (new_mission_item->nav_cmd == NAV_CMD_DO_JUMP) {
-			/* 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 */
-			return OK;
-		}
-	}
-
-	/* we have given up, we don't want to cycle forever */
-	warnx("ERROR: cycling through mission items without success");
-	return ERROR;
-}
diff --git a/src/modules/navigator/navigator_mission.h b/src/modules/navigator/mission.h
index fef145410..0fa2ff3fa 100644
--- a/src/modules/navigator/navigator_mission.h
+++ b/src/modules/navigator/mission.h
@@ -1,7 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
- *   Author: @author Julian Oes <joes@student.ethz.ch>
+ *   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
@@ -34,6 +33,7 @@
 /**
  * @file navigator_mission.h
  * Helper class to access missions
+ * @author Julian Oes <julian@oes.ch>
  */
 
 #ifndef NAVIGATOR_MISSION_H
@@ -41,6 +41,7 @@
 
 #include <uORB/topics/mission.h>
 #include <uORB/topics/mission_result.h>
+#include <dataman/dataman.h>
 
 
 class __EXPORT Mission
@@ -52,44 +53,36 @@ public:
 	Mission();
 
 	/**
-	 * Destructor, also kills the sensors task.
+	 * Destructor
 	 */
 	~Mission();
 
-	void		set_offboard_dataman_id(int new_id);
-	void		set_current_offboard_mission_index(int new_index);
-	void		set_current_onboard_mission_index(int new_index);
-	void		set_offboard_mission_count(unsigned new_count);
-	void		set_onboard_mission_count(unsigned new_count);
+	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);
 
-	void		set_onboard_mission_allowed(bool allowed);
+	bool		command_current_offboard_mission_index(const int new_index);
+	bool		command_current_onboard_mission_index(const int new_index);
 
-	bool		current_mission_available();
-	bool		next_mission_available();
-
-	int		get_current_mission_item(struct mission_item_s *mission_item, bool *onboard, unsigned *index);
-	int		get_next_mission_item(struct mission_item_s *mission_item);
+	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);
 
 	void		move_to_next();
 
+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);
+
 	void		report_mission_item_reached();
 	void		report_current_offboard_mission_item();
-	void		publish_mission_result();
 
-private:
-	int		get_mission_item(const dm_item_t dm_item, unsigned *mission_index, struct mission_item_s *new_mission_item);
-
-	bool		current_onboard_mission_available();
-	bool		current_offboard_mission_available();
-	bool		next_onboard_mission_available();
-	bool		next_offboard_mission_available();
+	void		publish_mission_result();
 
 	int 		_offboard_dataman_id;
-	unsigned	_current_offboard_mission_index;
-	unsigned	_current_onboard_mission_index;
-	unsigned	_offboard_mission_item_count;		/** number of offboard mission items available */
-	unsigned	_onboard_mission_item_count;		/** number of onboard mission items available */
-
+	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;
 
 	enum {
@@ -98,9 +91,8 @@ private:
 		MISSION_TYPE_OFFBOARD,
 	} 		_current_mission_type;
 
-	int		_mission_result_pub;
-
-	struct mission_result_s _mission_result;
+	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 55f8a4caa..5f113f686 100644
--- a/src/modules/navigator/module.mk
+++ b/src/modules/navigator/module.mk
@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+#   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
@@ -39,7 +39,9 @@ MODULE_COMMAND	= navigator
 
 SRCS		= navigator_main.cpp \
 		  navigator_params.c \
-		  navigator_mission.cpp \
+		  mission.cpp \
+		  rtl.cpp \
+		  rtl_params.c \
 		  mission_feasibility_checker.cpp \
 		  geofence.cpp \
 		  geofence_params.c
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index 494266dd3..34a28aec3 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -1,10 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
- *   Author: @author Lorenz Meier <lm@inf.ethz.ch>
- *           @author Jean Cyr <jean.m.cyr@gmail.com>
- *           @author Julian Oes <joes@student.ethz.ch>
- *           @author Anton Babushkin <anton.babushkin@me.com>
+ *   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
@@ -35,19 +31,20 @@
  *
  ****************************************************************************/
 /**
- * @file navigator_main.c
+ * @file navigator_main.cpp
  * Implementation of the main navigation state machine.
  *
- * Handles missions, geo fencing and failsafe navigation behavior.
- * Published the mission item triplet for the position controller.
+ * Handles mission items, geo fencing and failsafe navigation behavior.
+ * Published the position setpoint triplet for the position controller.
  *
  * @author Lorenz Meier <lm@inf.ethz.ch>
  * @author Jean Cyr <jean.m.cyr@gmail.com>
- * @author Julian Oes <joes@student.ethz.ch>
+ * @author Julian Oes <julian@oes.ch>
  * @author Anton Babushkin <anton.babushkin@me.com>
  */
 
 #include <nuttx/config.h>
+
 #include <stdio.h>
 #include <stdlib.h>
 #include <string.h>
@@ -58,9 +55,13 @@
 #include <poll.h>
 #include <time.h>
 #include <sys/ioctl.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+
 #include <drivers/device/device.h>
 #include <drivers/drv_hrt.h>
 #include <arch/board/board.h>
+
 #include <uORB/uORB.h>
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/home_position.h>
@@ -71,20 +72,19 @@
 #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 <systemlib/state_table.h>
 #include <systemlib/perf_counter.h>
 #include <systemlib/systemlib.h>
 #include <geo/geo.h>
-#include <mathlib/mathlib.h>
 #include <dataman/dataman.h>
+#include <mathlib/mathlib.h>
 #include <mavlink/mavlink_log.h>
-#include <sys/types.h>
-#include <sys/stat.h>
 
-#include "navigator_state.h"
-#include "navigator_mission.h"
+#include "mission.h"
+#include "rtl.h"
 #include "mission_feasibility_checker.h"
 #include "geofence.h"
 
@@ -130,12 +130,12 @@ public:
 	/**
 	 * Add point to geofence
 	 */
-	void add_fence_point(int argc, char *argv[]);
+	void		add_fence_point(int argc, char *argv[]);
 
 	/**
 	 * Load fence from file
 	 */
-	void load_fence_from_file(const char *filename);
+	void		load_fence_from_file(const char *filename);
 
 private:
 
@@ -154,15 +154,15 @@ private:
 	int		_control_mode_sub;		/**< vehicle control mode subscription */
 
 	orb_advert_t	_pos_sp_triplet_pub;		/**< publish position setpoint triplet */
-	orb_advert_t	_mission_result_pub;		/**< publish mission result topic */
 
-	struct vehicle_status_s				_vstatus;		/**< vehicle status */
-	struct vehicle_control_mode_s			_control_mode;		/**< vehicle control mode */
-	struct vehicle_global_position_s		_global_pos;		/**< global vehicle position */
-	struct home_position_s				_home_pos;		/**< home position for RTL */
-	struct position_setpoint_triplet_s		_pos_sp_triplet;	/**< triplet of position setpoints */
-	struct mission_result_s				_mission_result;	/**< mission result for commander/mavlink */
-	struct mission_item_s				_mission_item;		/**< current mission item */
+	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 */
+	position_setpoint_triplet_s			_pos_sp_triplet;	/**< triplet of position setpoints */
+	mission_item_s 					_mission_item;		/**< current mission item */
+
+	bool 						_mission_item_valid;
 
 	perf_counter_t	_loop_perf;			/**< loop performance counter */
 
@@ -174,56 +174,41 @@ private:
 
 	struct navigation_capabilities_s		_nav_caps;
 
-	class 		Mission				_mission;
+	Mission						_mission;
+
+	RTL 						_rtl;
 
-	bool		_mission_item_valid;		/**< current mission item valid */
-	bool		_global_pos_valid;		/**< track changes of global_position.global_valid flag */
-	bool		_reset_loiter_pos;		/**< if true then loiter position should be set to current position */
 	bool		_waypoint_position_reached;
 	bool		_waypoint_yaw_reached;
 	uint64_t	_time_first_inside_orbit;
-	bool		_need_takeoff;			/**< if need to perform vertical takeoff before going to waypoint (only for MISSION mode and VTOL vehicles) */
-	bool		_do_takeoff;			/**< vertical takeoff state, current mission item is generated by navigator (only for MISSION mode and VTOL vehicles) */
-
 	MissionFeasibilityChecker missionFeasiblityChecker;
 
-	uint64_t	_set_nav_state_timestamp;	/**< timestamp of last handled navigation state request */
-
-	bool		_pos_sp_triplet_updated;
-
-	const char *nav_states_str[NAV_STATE_MAX];
+	bool		_update_triplet;
 
 	struct {
-		float min_altitude;
 		float acceptance_radius;
 		float loiter_radius;
 		int onboard_mission_enabled;
 		float takeoff_alt;
-		float land_alt;
-		float rtl_alt;
-		float rtl_land_delay;
 	}		_parameters;			/**< local copies of parameters */
 
 	struct {
-		param_t min_altitude;
 		param_t acceptance_radius;
 		param_t loiter_radius;
 		param_t onboard_mission_enabled;
 		param_t takeoff_alt;
-		param_t land_alt;
-		param_t rtl_alt;
-		param_t rtl_land_delay;
 	}		_parameter_handles;		/**< handles for parameters */
 
 	enum Event {
 		EVENT_NONE_REQUESTED,
-		EVENT_READY_REQUESTED,
 		EVENT_LOITER_REQUESTED,
 		EVENT_MISSION_REQUESTED,
 		EVENT_RTL_REQUESTED,
-		EVENT_LAND_REQUESTED,
+		EVENT_TAKEN_OFF,
+		EVENT_LANDED,
 		EVENT_MISSION_CHANGED,
 		EVENT_HOME_POSITION_CHANGED,
+		EVENT_MISSION_ITEM_REACHED,
 		MAX_EVENT
 	};
 
@@ -232,15 +217,6 @@ private:
 	 */
 	static StateTable::Tran const myTable[NAV_STATE_MAX][MAX_EVENT];
 
-	enum RTLState {
-		RTL_STATE_NONE = 0,
-		RTL_STATE_CLIMB,
-		RTL_STATE_RETURN,
-		RTL_STATE_DESCEND
-	};
-
-	enum RTLState _rtl_state;
-
 	/**
 	 * Update our local parameter cache.
 	 */
@@ -264,7 +240,7 @@ private:
 	/**
 	 * Retrieve offboard mission.
 	 */
-	void		offboard_mission_update(bool isrotaryWing);
+	void		offboard_mission_update();
 
 	/**
 	 * Retrieve onboard mission.
@@ -296,19 +272,20 @@ private:
 	/**
 	 * Functions that are triggered when a new state is entered.
 	 */
-	void		start_none();
-	void		start_ready();
-	void		start_loiter();
-	void		start_mission();
-	void		start_rtl();
-	void		start_land();
-	void		start_land_home();
+	bool		start_none_on_ground();
+	bool		start_none_in_air();
+	bool		start_auto_on_ground();
+	bool		start_loiter();
+	bool		start_mission();
+	bool		advance_mission();
+	bool		start_rtl();
+	bool		advance_rtl();
+	bool		start_land();
 
 	/**
 	 * Fork for state transitions
 	 */
-	void		request_loiter_or_ready();
-	void		request_mission_if_available();
+	// void		request_loiter_or_ready();
 
 	/**
 	 * Guards offboard mission
@@ -333,12 +310,12 @@ private:
 	/**
 	 * Perform actions when current mission item reached.
 	 */
-	void		on_mission_item_reached();
+	// void		on_mission_item_reached();
 
 	/**
 	 * Move to next waypoint
 	 */
-	void		set_mission_item();
+	bool		set_mission_items();
 
 	/**
 	 * Switch to next RTL state
@@ -348,7 +325,7 @@ private:
 	/**
 	 * Set position setpoint for mission item
 	 */
-	void		position_setpoint_from_mission_item(position_setpoint_s *sp, mission_item_s *item);
+	void		mission_item_to_position_setpoint(const mission_item_s *item, position_setpoint_s *sp);
 
 	/**
 	 * Helper function to get a takeoff item
@@ -377,12 +354,9 @@ Navigator::Navigator() :
 
 /* state machine transition table */
 	StateTable(&myTable[0][0], NAV_STATE_MAX, MAX_EVENT),
-
 	_task_should_exit(false),
 	_navigator_task(-1),
 	_mavlink_fd(-1),
-
-/* subscriptions */
 	_global_pos_sub(-1),
 	_home_pos_sub(-1),
 	_vstatus_sub(-1),
@@ -391,53 +365,29 @@ Navigator::Navigator() :
 	_onboard_mission_sub(-1),
 	_capabilities_sub(-1),
 	_control_mode_sub(-1),
-
-/* publications */
 	_pos_sp_triplet_pub(-1),
-
-/* performance counters */
 	_loop_perf(perf_alloc(PC_ELAPSED, "navigator")),
-
 	_geofence_violation_warning_sent(false),
 	_fence_valid(false),
 	_inside_fence(true),
 	_mission(),
+	_pos_sp_triplet({}),
+	_mission_item({}),
 	_mission_item_valid(false),
-	_global_pos_valid(false),
-	_reset_loiter_pos(true),
 	_waypoint_position_reached(false),
 	_waypoint_yaw_reached(false),
 	_time_first_inside_orbit(0),
-	_need_takeoff(true),
-	_do_takeoff(false),
-	_set_nav_state_timestamp(0),
-	_pos_sp_triplet_updated(false),
-/* states */
-	_rtl_state(RTL_STATE_NONE)
+	_update_triplet(false)
 {
-	_parameter_handles.min_altitude = param_find("NAV_MIN_ALT");
 	_parameter_handles.acceptance_radius = param_find("NAV_ACCEPT_RAD");
 	_parameter_handles.loiter_radius = param_find("NAV_LOITER_RAD");
 	_parameter_handles.onboard_mission_enabled = param_find("NAV_ONB_MIS_EN");
 	_parameter_handles.takeoff_alt = param_find("NAV_TAKEOFF_ALT");
-	_parameter_handles.land_alt = param_find("NAV_LAND_ALT");
-	_parameter_handles.rtl_alt = param_find("NAV_RTL_ALT");
-	_parameter_handles.rtl_land_delay = param_find("NAV_RTL_LAND_T");
-
-	memset(&_pos_sp_triplet, 0, sizeof(struct position_setpoint_triplet_s));
-	memset(&_mission_item, 0, sizeof(struct mission_item_s));
-
-	memset(&nav_states_str, 0, sizeof(nav_states_str));
-	nav_states_str[0] = "NONE";
-	nav_states_str[1] = "READY";
-	nav_states_str[2] = "LOITER";
-	nav_states_str[3] = "MISSION";
-	nav_states_str[4] = "RTL";
-	nav_states_str[5] = "LAND";
 
 	/* Initialize state machine */
-	myState = NAV_STATE_NONE;
-	start_none();
+	myState = NAV_STATE_NONE_ON_GROUND;
+
+	start_none_on_ground();
 }
 
 Navigator::~Navigator()
@@ -472,16 +422,12 @@ Navigator::parameters_update()
 	struct parameter_update_s update;
 	orb_copy(ORB_ID(parameter_update), _params_sub, &update);
 
-	param_get(_parameter_handles.min_altitude, &(_parameters.min_altitude));
 	param_get(_parameter_handles.acceptance_radius, &(_parameters.acceptance_radius));
 	param_get(_parameter_handles.loiter_radius, &(_parameters.loiter_radius));
 	param_get(_parameter_handles.onboard_mission_enabled, &(_parameters.onboard_mission_enabled));
-	param_get(_parameter_handles.takeoff_alt, &(_parameters.takeoff_alt));
-	param_get(_parameter_handles.land_alt, &(_parameters.land_alt));
-	param_get(_parameter_handles.rtl_alt, &(_parameters.rtl_alt));
-	param_get(_parameter_handles.rtl_land_delay, &(_parameters.rtl_land_delay));
+	param_get(_parameter_handles.takeoff_alt, &(_parameters.takeoff_alt));	
 
-	_mission.set_onboard_mission_allowed((bool)_parameter_handles.onboard_mission_enabled);
+	_mission.set_onboard_mission_allowed((bool)_parameters.onboard_mission_enabled);
 
 	_geofence.updateParams();
 }
@@ -496,6 +442,8 @@ void
 Navigator::home_position_update()
 {
 	orb_copy(ORB_ID(home_position), _home_pos_sub, &_home_pos);
+
+	_rtl.set_home_position(&_home_pos);
 }
 
 void
@@ -506,7 +454,7 @@ Navigator::navigation_capabilities_update()
 
 
 void
-Navigator::offboard_mission_update(bool isrotaryWing)
+Navigator::offboard_mission_update()
 {
 	struct mission_s offboard_mission;
 
@@ -523,19 +471,17 @@ Navigator::offboard_mission_update(bool isrotaryWing)
 			dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1;
 		}
 
-		missionFeasiblityChecker.checkMissionFeasible(isrotaryWing, dm_current, (size_t)offboard_mission.count, _geofence);
+		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.set_current_offboard_mission_index(offboard_mission.current_index);
+		_mission.command_current_offboard_mission_index(offboard_mission.current_index);
 
 	} else {
 		_mission.set_offboard_mission_count(0);
-		_mission.set_current_offboard_mission_index(0);
+		_mission.command_current_offboard_mission_index(0);
 	}
-
-	_mission.publish_mission_result();
 }
 
 void
@@ -546,11 +492,11 @@ Navigator::onboard_mission_update()
 	if (orb_copy(ORB_ID(onboard_mission), _onboard_mission_sub, &onboard_mission) == OK) {
 
 		_mission.set_onboard_mission_count(onboard_mission.count);
-		_mission.set_current_onboard_mission_index(onboard_mission.current_index);
+		_mission.command_current_onboard_mission_index(onboard_mission.current_index);
 
 	} else {
 		_mission.set_onboard_mission_count(0);
-		_mission.set_current_onboard_mission_index(0);
+		_mission.command_current_onboard_mission_index(0);
 	}
 }
 
@@ -626,13 +572,13 @@ Navigator::task_main()
 	global_position_update();
 	home_position_update();
 	navigation_capabilities_update();
-	offboard_mission_update(_vstatus.is_rotary_wing);
+	offboard_mission_update();
 	onboard_mission_update();
 
 	/* rate limit position updates to 50 Hz */
 	orb_set_interval(_global_pos_sub, 20);
 
-	unsigned prevState = NAV_STATE_NONE;
+	unsigned prevState = NAV_STATE_NONE_ON_GROUND;
 	hrt_abstime mavlink_open_time = 0;
 	const hrt_abstime mavlink_open_interval = 500000;
 
@@ -690,55 +636,45 @@ Navigator::task_main()
 		if (fds[6].revents & POLLIN) {
 			vehicle_status_update();
 
-			/* evaluate state requested by commander */
-			if (_control_mode.flag_armed && _control_mode.flag_control_auto_enabled) {
-				if (_vstatus.set_nav_state_timestamp != _set_nav_state_timestamp) {
-					/* commander requested new navigation mode, try to set it */
-					switch (_vstatus.set_nav_state) {
-					case NAV_STATE_NONE:
-						/* nothing to do */
-						break;
-
-					case NAV_STATE_LOITER:
-						request_loiter_or_ready();
-						break;
-
-					case NAV_STATE_MISSION:
-						request_mission_if_available();
-						break;
-
-					case NAV_STATE_RTL:
-						if (!(_rtl_state == RTL_STATE_DESCEND &&
-							  (myState == NAV_STATE_LAND || myState == NAV_STATE_LOITER)) &&
-							_vstatus.condition_home_position_valid) {
-							dispatch(EVENT_RTL_REQUESTED);
-						}
-
-						break;
-
-					case NAV_STATE_LAND:
-						dispatch(EVENT_LAND_REQUESTED);
-
-						break;
-
-					default:
-						warnx("ERROR: Requested navigation state not supported");
-						break;
-					}
-
-				} else {
-					/* on first switch to AUTO try mission by default, if none is available fallback to loiter */
-					if (myState == NAV_STATE_NONE) {
-						request_mission_if_available();
-					}
-				}
-
-			} else {
-				/* navigator shouldn't act */
+			/* commander requested new navigation mode, forward it to state machine */
+			switch (_vstatus.set_nav_state) {
+			case NAVIGATION_STATE_NONE:
 				dispatch(EVENT_NONE_REQUESTED);
+				break;
+
+			case NAVIGATION_STATE_LOITER:
+				dispatch(EVENT_LOITER_REQUESTED);
+				break;
+
+			case NAVIGATION_STATE_MISSION:
+				dispatch(EVENT_MISSION_REQUESTED);
+				break;
+
+			case NAVIGATION_STATE_RTL:
+				/* TODO: what is this for? */
+				// if (!(_rtl_state == RTL_STATE_DESCEND &&
+				// 	  (myState == NAV_STATE_LAND || myState == NAV_STATE_LOITER)) &&
+				// 	_vstatus.condition_home_position_valid) {
+				dispatch(EVENT_RTL_REQUESTED);
+				// }
+				break;
+
+			case NAVIGATION_STATE_LAND:
+				/* TODO: add this */
+				// dispatch(EVENT_LAND_REQUESTED);
+				break;
+
+			default:
+				warnx("ERROR: Requested navigation state not supported");
+				break;
 			}
 
-			_set_nav_state_timestamp = _vstatus.set_nav_state_timestamp;
+			/* commander sets in-air/on-ground flag as well */
+			if (_vstatus.condition_landed) {
+				dispatch(EVENT_LANDED);
+			} else {
+				dispatch(EVENT_TAKEN_OFF);
+			}
 		}
 
 		/* parameters updated */
@@ -754,74 +690,49 @@ Navigator::task_main()
 
 		/* offboard mission updated */
 		if (fds[4].revents & POLLIN) {
-			offboard_mission_update(_vstatus.is_rotary_wing);
-			// XXX check if mission really changed
+			offboard_mission_update();
+			/* TODO: check if mission really changed */
 			dispatch(EVENT_MISSION_CHANGED);
 		}
 
 		/* onboard mission updated */
 		if (fds[5].revents & POLLIN) {
 			onboard_mission_update();
-			// XXX check if mission really changed
+			/* TODO: check if mission really changed */
 			dispatch(EVENT_MISSION_CHANGED);
 		}
 
 		/* home position updated */
 		if (fds[2].revents & POLLIN) {
 			home_position_update();
-			// XXX check if home position really changed
+			/* TODO check if home position really changed */
 			dispatch(EVENT_HOME_POSITION_CHANGED);
 		}
 
 		/* global position updated */
 		if (fds[1].revents & POLLIN) {
 			global_position_update();
-
-			/* publish position setpoint triplet on each position update if navigator active */
-			if (_control_mode.flag_armed && _control_mode.flag_control_auto_enabled) {
-				_pos_sp_triplet_updated = true;
-
-				if (myState == NAV_STATE_LAND && _global_pos.global_valid && !_global_pos_valid) {
-					/* got global position when landing, update setpoint */
-					start_land();
-				}
-
-				_global_pos_valid = _global_pos.global_valid;
-
-				/* check if waypoint has been reached in MISSION, RTL and LAND modes */
-				if (myState == NAV_STATE_MISSION || myState == NAV_STATE_RTL || myState == NAV_STATE_LAND) {
-					if (check_mission_item_reached()) {
-						on_mission_item_reached();
-					}
-				}
+			if (check_mission_item_reached()) {
+				dispatch(EVENT_MISSION_ITEM_REACHED);
 			}
 
 			/* Check geofence violation */
 			if (!_geofence.inside(&_global_pos)) {
-				//xxx: publish geofence violation here (or change local flag depending on which app handles the flight termination)
 
 				/* Issue a warning about the geofence violation once */
 				if (!_geofence_violation_warning_sent) {
 					mavlink_log_critical(_mavlink_fd, "#audio: Geofence violation");
 					_geofence_violation_warning_sent = true;
 				}
-
 			} else {
 				/* Reset the _geofence_violation_warning_sent field */
 				_geofence_violation_warning_sent = false;
 			}
 		}
 
-		/* publish position setpoint triplet if updated */
-		if (_pos_sp_triplet_updated) {
-			_pos_sp_triplet_updated = false;
+		if (_update_triplet ) {
 			publish_position_setpoint_triplet();
-		}
-
-		/* notify user about state changes */
-		if (myState != prevState) {
-			mavlink_log_info(_mavlink_fd, "#audio: navigation state: %s", nav_states_str[myState]);
-			prevState = myState;
+			_update_triplet = false;
 		}
 
 		perf_end(_loop_perf);
@@ -879,8 +790,12 @@ Navigator::status()
 	}
 
 	switch (myState) {
-	case NAV_STATE_NONE:
-		warnx("State: None");
+	case NAV_STATE_NONE_ON_GROUND:
+		warnx("State: None on ground");
+		break;
+
+	case NAV_STATE_NONE_IN_AIR:
+		warnx("State: None in air");
 		break;
 
 	case NAV_STATE_LOITER:
@@ -895,6 +810,10 @@ Navigator::status()
 		warnx("State: RTL");
 		break;
 
+	case NAV_STATE_LAND:
+		warnx("State: LAND");
+		break;
+
 	default:
 		warnx("State: Unknown");
 		break;
@@ -903,92 +822,119 @@ Navigator::status()
 
 StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = {
 	{
-		/* NAV_STATE_NONE */
-		/* EVENT_NONE_REQUESTED */		{NO_ACTION, NAV_STATE_NONE},
-		/* EVENT_READY_REQUESTED */		{ACTION(&Navigator::start_ready), NAV_STATE_READY},
+		/* NAV_STATE_NONE_ON_GROUND */
+		/* EVENT_NONE_REQUESTED */		{NO_ACTION, NAV_STATE_NONE_ON_GROUND},
+		/* EVENT_LOITER_REQUESTED */		{NO_ACTION, NAV_STATE_NONE_ON_GROUND},
+		/* EVENT_MISSION_REQUESTED */		{ACTION(&Navigator::start_auto_on_ground), NAV_STATE_AUTO_ON_GROUND},
+		/* EVENT_RTL_REQUESTED */		{NO_ACTION, NAV_STATE_NONE_ON_GROUND},
+		/* EVENT_TAKEN_OFF */			{ACTION(&Navigator::start_none_in_air), NAV_STATE_NONE_IN_AIR},
+		/* EVENT_LANDED */			{NO_ACTION, NAV_STATE_NONE_ON_GROUND},
+		/* EVENT_MISSION_CHANGED */		{NO_ACTION, NAV_STATE_NONE_ON_GROUND},
+		/* EVENT_HOME_POSITION_CHANGED */	{NO_ACTION, NAV_STATE_NONE_ON_GROUND},
+		/* EVENT_MISSION_ITEM_REACHED */	{NO_ACTION, NAV_STATE_NONE_ON_GROUND},
+	},
+	{
+		/* NAV_STATE_NONE_IN_AIR */
+		/* EVENT_NONE_REQUESTED */		{NO_ACTION, NAV_STATE_NONE_IN_AIR},
 		/* EVENT_LOITER_REQUESTED */		{ACTION(&Navigator::start_loiter), NAV_STATE_LOITER},
 		/* EVENT_MISSION_REQUESTED */		{ACTION(&Navigator::start_mission), NAV_STATE_MISSION},
 		/* EVENT_RTL_REQUESTED */		{ACTION(&Navigator::start_rtl), NAV_STATE_RTL},
-		/* EVENT_LAND_REQUESTED */		{ACTION(&Navigator::start_land), NAV_STATE_LAND},
-		/* EVENT_MISSION_CHANGED */		{NO_ACTION, NAV_STATE_NONE},
-		/* EVENT_HOME_POSITION_CHANGED */	{NO_ACTION, NAV_STATE_NONE},
+		/* EVENT_TAKEN_OFF */			{NO_ACTION, NAV_STATE_NONE_IN_AIR},
+		/* EVENT_LANDED */			{ACTION(&Navigator::start_none_on_ground), NAV_STATE_NONE_ON_GROUND},
+		/* EVENT_MISSION_CHANGED */		{NO_ACTION, NAV_STATE_NONE_IN_AIR},
+		/* EVENT_HOME_POSITION_CHANGED */	{NO_ACTION, NAV_STATE_NONE_IN_AIR},
+		/* EVENT_MISSION_ITEM_REACHED */	{NO_ACTION, NAV_STATE_NONE_IN_AIR},
 	},
 	{
-		/* NAV_STATE_READY */
-		/* EVENT_NONE_REQUESTED */		{ACTION(&Navigator::start_none), NAV_STATE_NONE},
-		/* EVENT_READY_REQUESTED */		{NO_ACTION, NAV_STATE_READY},
-		/* EVENT_LOITER_REQUESTED */		{NO_ACTION, NAV_STATE_READY},
-		/* EVENT_MISSION_REQUESTED */		{ACTION(&Navigator::start_mission), NAV_STATE_MISSION},
-		/* EVENT_RTL_REQUESTED */		{NO_ACTION, NAV_STATE_READY},
-		/* EVENT_LAND_REQUESTED */		{NO_ACTION, NAV_STATE_READY},
-		/* EVENT_MISSION_CHANGED */		{NO_ACTION, NAV_STATE_READY},
-		/* EVENT_HOME_POSITION_CHANGED */	{NO_ACTION, NAV_STATE_READY},
+		/* NAV_STATE_AUTO_ON_GROUND */
+		/* EVENT_NONE_REQUESTED */		{NO_ACTION, NAV_STATE_AUTO_ON_GROUND},
+		/* EVENT_LOITER_REQUESTED */		{NO_ACTION, NAV_STATE_AUTO_ON_GROUND},
+		/* EVENT_MISSION_REQUESTED */		{NO_ACTION, NAV_STATE_AUTO_ON_GROUND},
+		/* EVENT_RTL_REQUESTED */		{NO_ACTION, NAV_STATE_AUTO_ON_GROUND},
+		/* EVENT_TAKEN_OFF */			{ACTION(&Navigator::start_mission), NAV_STATE_MISSION},
+		/* EVENT_LANDED */			{NO_ACTION, NAV_STATE_AUTO_ON_GROUND},
+		/* EVENT_MISSION_CHANGED */		{NO_ACTION, NAV_STATE_AUTO_ON_GROUND},
+		/* EVENT_HOME_POSITION_CHANGED */	{NO_ACTION, NAV_STATE_AUTO_ON_GROUND},
+		/* EVENT_MISSION_ITEM_REACHED */	{NO_ACTION, NAV_STATE_AUTO_ON_GROUND},
 	},
 	{
 		/* NAV_STATE_LOITER */
-		/* EVENT_NONE_REQUESTED */		{ACTION(&Navigator::start_none), NAV_STATE_NONE},
-		/* EVENT_READY_REQUESTED */		{NO_ACTION, NAV_STATE_LOITER},
+		/* EVENT_NONE_REQUESTED */		{ACTION(&Navigator::start_none_in_air), NAV_STATE_NONE_IN_AIR},
 		/* EVENT_LOITER_REQUESTED */		{NO_ACTION, NAV_STATE_LOITER},
 		/* EVENT_MISSION_REQUESTED */		{ACTION(&Navigator::start_mission), NAV_STATE_MISSION},
 		/* EVENT_RTL_REQUESTED */		{ACTION(&Navigator::start_rtl), NAV_STATE_RTL},
-		/* EVENT_LAND_REQUESTED */		{ACTION(&Navigator::start_land), NAV_STATE_LAND},
+		/* EVENT_TAKEN_OFF */			{NO_ACTION, NAV_STATE_LOITER},
+		/* EVENT_LANDED */			{NO_ACTION, NAV_STATE_LOITER},
 		/* EVENT_MISSION_CHANGED */		{NO_ACTION, NAV_STATE_LOITER},
 		/* EVENT_HOME_POSITION_CHANGED */	{NO_ACTION, NAV_STATE_LOITER},
+		/* EVENT_MISSION_ITEM_REACHED */	{NO_ACTION, NAV_STATE_LOITER},
 	},
 	{
 		/* NAV_STATE_MISSION */
-		/* EVENT_NONE_REQUESTED */		{ACTION(&Navigator::start_none), NAV_STATE_NONE},
-		/* EVENT_READY_REQUESTED */		{ACTION(&Navigator::start_ready), NAV_STATE_READY},
+		/* EVENT_NONE_REQUESTED */		{ACTION(&Navigator::start_none_in_air), NAV_STATE_NONE_IN_AIR},
 		/* EVENT_LOITER_REQUESTED */		{ACTION(&Navigator::start_loiter), NAV_STATE_LOITER},
 		/* EVENT_MISSION_REQUESTED */		{NO_ACTION, NAV_STATE_MISSION},
 		/* EVENT_RTL_REQUESTED */		{ACTION(&Navigator::start_rtl), NAV_STATE_RTL},
-		/* EVENT_LAND_REQUESTED */		{ACTION(&Navigator::start_land), NAV_STATE_LAND},
+		/* EVENT_TAKEN_OFF */			{NO_ACTION, NAV_STATE_MISSION},
+		/* EVENT_LANDED */			{NO_ACTION, NAV_STATE_MISSION},
 		/* EVENT_MISSION_CHANGED */		{ACTION(&Navigator::start_mission), NAV_STATE_MISSION},
 		/* EVENT_HOME_POSITION_CHANGED */	{NO_ACTION, NAV_STATE_MISSION},
+		/* EVENT_MISSION_ITEM_REACHED */	{ACTION(&Navigator::advance_mission), NAV_STATE_MISSION},
 	},
 	{
 		/* NAV_STATE_RTL */
-		/* EVENT_NONE_REQUESTED */		{ACTION(&Navigator::start_none), NAV_STATE_NONE},
-		/* EVENT_READY_REQUESTED */		{ACTION(&Navigator::start_ready), NAV_STATE_READY},
+		/* EVENT_NONE_REQUESTED */		{ACTION(&Navigator::start_none_in_air), NAV_STATE_NONE_IN_AIR},
 		/* EVENT_LOITER_REQUESTED */		{ACTION(&Navigator::start_loiter), NAV_STATE_LOITER},
 		/* EVENT_MISSION_REQUESTED */		{ACTION(&Navigator::start_mission), NAV_STATE_MISSION},
 		/* EVENT_RTL_REQUESTED */		{NO_ACTION, NAV_STATE_RTL},
-		/* EVENT_LAND_REQUESTED */		{ACTION(&Navigator::start_land_home), NAV_STATE_LAND},
+		/* EVENT_TAKEN_OFF */			{NO_ACTION, NAV_STATE_RTL},
+		/* EVENT_LANDED */			{NO_ACTION, NAV_STATE_RTL},
 		/* EVENT_MISSION_CHANGED */		{NO_ACTION, NAV_STATE_RTL},
-		/* EVENT_HOME_POSITION_CHANGED */	{ACTION(&Navigator::start_rtl), NAV_STATE_RTL},	// TODO need to reset rtl_state
+		/* EVENT_HOME_POSITION_CHANGED */	{ACTION(&Navigator::start_rtl), NAV_STATE_RTL},
+		/* EVENT_MISSION_ITEM_REACHED */	{ACTION(&Navigator::advance_rtl), NAV_STATE_RTL},
 	},
 	{
 		/* NAV_STATE_LAND */
-		/* EVENT_NONE_REQUESTED */		{ACTION(&Navigator::start_none), NAV_STATE_NONE},
-		/* EVENT_READY_REQUESTED */		{ACTION(&Navigator::start_ready), NAV_STATE_READY},
+		/* EVENT_NONE_REQUESTED */		{ACTION(&Navigator::start_none_in_air), NAV_STATE_NONE_IN_AIR},
 		/* EVENT_LOITER_REQUESTED */		{ACTION(&Navigator::start_loiter), NAV_STATE_LOITER},
 		/* EVENT_MISSION_REQUESTED */		{ACTION(&Navigator::start_mission), NAV_STATE_MISSION},
 		/* EVENT_RTL_REQUESTED */		{ACTION(&Navigator::start_rtl), NAV_STATE_RTL},
-		/* EVENT_LAND_REQUESTED */		{NO_ACTION, NAV_STATE_LAND},
+		/* EVENT_TAKEN_OFF */			{NO_ACTION, NAV_STATE_LAND},
+		/* EVENT_LANDED */			{ACTION(&Navigator::start_auto_on_ground), NAV_STATE_AUTO_ON_GROUND},
 		/* EVENT_MISSION_CHANGED */		{NO_ACTION, NAV_STATE_LAND},
 		/* EVENT_HOME_POSITION_CHANGED */	{NO_ACTION, NAV_STATE_LAND},
+		/* EVENT_MISSION_ITEM_REACHED */	{NO_ACTION, NAV_STATE_LAND},
 	},
 };
 
-void
-Navigator::start_none()
+bool
+Navigator::start_none_on_ground()
 {
 	reset_reached();
 
 	_pos_sp_triplet.previous.valid = false;
 	_pos_sp_triplet.current.valid = false;
 	_pos_sp_triplet.next.valid = false;
-	_mission_item_valid = false;
 
-	_reset_loiter_pos = true;
-	_do_takeoff = false;
-	_rtl_state = RTL_STATE_NONE;
+	_update_triplet = true;
+	return true;
+}
+
+bool
+Navigator::start_none_in_air()
+{
+	reset_reached();
+
+	_pos_sp_triplet.previous.valid = false;
+	_pos_sp_triplet.current.valid = false;
+	_pos_sp_triplet.next.valid = false;
 
-	_pos_sp_triplet_updated = true;
+	_update_triplet = true;
+	return true;
 }
 
-void
-Navigator::start_ready()
+bool
+Navigator::start_auto_on_ground()
 {
 	reset_reached();
 
@@ -998,46 +944,26 @@ Navigator::start_ready()
 
 	_pos_sp_triplet.current.type = SETPOINT_TYPE_IDLE;
 
-	_mission_item_valid = false;
-
-	_reset_loiter_pos = true;
-	_do_takeoff = false;
 
-	if (_rtl_state != RTL_STATE_DESCEND) {
-		/* reset RTL state if landed not at home */
-		_rtl_state = RTL_STATE_NONE;
-	}
+	// if (_rtl_state != RTL_STATE_DESCEND) {
+	// 	 reset RTL state if landed not at home 
+	// 	_rtl_state = RTL_STATE_NONE;
+	// }
 
-	_pos_sp_triplet_updated = true;
+	_update_triplet = true;
+	return true;
 }
 
-void
+bool
 Navigator::start_loiter()
 {
-	reset_reached();
-
-	_do_takeoff = false;
-
-	/* set loiter position if needed */
-	if (_reset_loiter_pos || !_pos_sp_triplet.current.valid) {
-		_reset_loiter_pos = false;
+	/* if no existing item available, use current position */
+	if (!(_pos_sp_triplet.current.valid && _waypoint_position_reached)) {
 
 		_pos_sp_triplet.current.lat = _global_pos.lat;
 		_pos_sp_triplet.current.lon = _global_pos.lon;
 		_pos_sp_triplet.current.yaw = NAN;	// NAN means to use current yaw
-
-		float min_alt_amsl = _parameters.min_altitude + _home_pos.alt;
-
-		/* use current altitude if above min altitude set by parameter */
-		if (_global_pos.alt < min_alt_amsl && !_vstatus.is_rotary_wing) {
-			_pos_sp_triplet.current.alt = min_alt_amsl;
-			mavlink_log_info(_mavlink_fd, "#audio: loiter %.1fm higher", (double)(min_alt_amsl - _global_pos.alt));
-
-		} else {
-			_pos_sp_triplet.current.alt = _global_pos.alt;
-			mavlink_log_info(_mavlink_fd, "#audio: loiter at current altitude");
-		}
-
+		_pos_sp_triplet.current.alt = _global_pos.alt;
 	}
 	_pos_sp_triplet.current.type = SETPOINT_TYPE_LOITER;
 	_pos_sp_triplet.current.loiter_radius = _parameters.loiter_radius;
@@ -1045,167 +971,146 @@ Navigator::start_loiter()
 	_pos_sp_triplet.previous.valid = false;
 	_pos_sp_triplet.current.valid = true;
 	_pos_sp_triplet.next.valid = false;
-	_mission_item_valid = false;
 
-	_pos_sp_triplet_updated = true;
+	mavlink_log_info(_mavlink_fd, "#audio: loiter at current altitude");
+
+	_update_triplet = true;
+	return true;	
 }
 
-void
+bool
 Navigator::start_mission()
 {
-	_need_takeoff = true;
+	/* start fresh */
+	_pos_sp_triplet.previous.valid = false;
+	_pos_sp_triplet.current.valid = false;
+	_pos_sp_triplet.next.valid = false;
 
-	set_mission_item();
+	return set_mission_items();
 }
 
-void
-Navigator::set_mission_item()
+bool
+Navigator::advance_mission()
 {
-	reset_reached();
+	/* tell mission to move by one */
+	_mission.move_to_next();
 
-	/* copy current mission to previous item */
-	memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s));
+	/* now try to set the new mission items, if it fails, it will dispatch loiter */
+	return set_mission_items();
+}
 
-	_reset_loiter_pos = true;
-	_do_takeoff = false;
+bool
+Navigator::set_mission_items()
+{
+	if (_pos_sp_triplet.current.valid) {
+		memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s));
+		_pos_sp_triplet.previous.valid = true;
+	}
 
-	int ret;
 	bool onboard;
-	unsigned index;
+	int index;
 
-	ret = _mission.get_current_mission_item(&_mission_item, &onboard, &index);
+	/* if we fail to set the current mission, continue to loiter */
+	if (!_mission.get_current_mission_item(&_mission_item, &onboard, &index)) {
 
-	if (ret == OK) {
-		_mission.report_current_offboard_mission_item();
+		dispatch(EVENT_LOITER_REQUESTED);
+		return false;
+	}
 
-		_mission_item_valid = true;
-		position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item);
-
-		if (_mission_item.nav_cmd != NAV_CMD_RETURN_TO_LAUNCH &&
-		    _mission_item.nav_cmd != NAV_CMD_LOITER_TIME_LIMIT &&
-		    _mission_item.nav_cmd != NAV_CMD_LOITER_TURN_COUNT &&
-		    _mission_item.nav_cmd != NAV_CMD_LOITER_UNLIMITED) {
-			/* don't reset RTL state on RTL or LOITER items */
-			_rtl_state = RTL_STATE_NONE;
-		}
+	/* if we got an RTL mission item, switch to RTL mode and give up */
+	if (_mission_item.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) {
+		dispatch(EVENT_RTL_REQUESTED);
+		return false;
+	}
 
-		if (_vstatus.is_rotary_wing) {
-			if (_need_takeoff && (
-				    _mission_item.nav_cmd == NAV_CMD_TAKEOFF ||
-				    _mission_item.nav_cmd == NAV_CMD_WAYPOINT ||
-				    _mission_item.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH ||
-				    _mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
-				    _mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
-				    _mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED
-			    )) {
-				/* do special TAKEOFF handling for VTOL */
-				_need_takeoff = false;
-
-				/* calculate desired takeoff altitude AMSL */
-				float takeoff_alt_amsl = _pos_sp_triplet.current.alt;
-
-				if (_vstatus.condition_landed) {
-					/* takeoff to at least NAV_TAKEOFF_ALT from ground if landed */
-					takeoff_alt_amsl = fmaxf(takeoff_alt_amsl, _global_pos.alt + _parameters.takeoff_alt);
-				}
+	_mission_item_valid = true;
 
-				/* check if we really need takeoff */
-				if (_vstatus.condition_landed || _global_pos.alt < takeoff_alt_amsl - _mission_item.acceptance_radius) {
-					/* force TAKEOFF if landed or waypoint altitude is more than current */
-					_do_takeoff = true;
+	/* convert the current mission item and set it valid */
+	mission_item_to_position_setpoint(&_mission_item, &_pos_sp_triplet.current);
+	_pos_sp_triplet.current.valid = true;
+	
 
-					/* move current position setpoint to next */
-					memcpy(&_pos_sp_triplet.next, &_pos_sp_triplet.current, sizeof(position_setpoint_s));
+	mission_item_s next_mission_item;
 
-					/* set current setpoint to takeoff */
+	bool last_wp = false;
+	/* now try to set the next mission item as well, if there is no more next
+	 * this means we're heading to the last waypoint */
+	if (_mission.get_next_mission_item(&next_mission_item)) {
+		/* convert the next mission item and set it valid */
+		mission_item_to_position_setpoint(&next_mission_item, &_pos_sp_triplet.next);
+		_pos_sp_triplet.next.valid = true;
+	} else {
+		last_wp = true;
+	}
 
-					_pos_sp_triplet.current.lat = _global_pos.lat;
-					_pos_sp_triplet.current.lon = _global_pos.lon;
-					_pos_sp_triplet.current.alt = takeoff_alt_amsl;
-					_pos_sp_triplet.current.yaw = NAN;
-					_pos_sp_triplet.current.type = SETPOINT_TYPE_TAKEOFF;
-				}
+	/* notify user about what happened */
+	mavlink_log_info(_mavlink_fd, "#audio: heading to %s%swaypoint %d",
+		(last_wp ? "last " : "" ), (onboard ? "onboard " : ""), index);
 
-			} else if (_mission_item.nav_cmd == NAV_CMD_LAND) {
-				/* will need takeoff after landing */
-				_need_takeoff = true;
-			}
-		}
+	_update_triplet = true;
 
-		if (_do_takeoff) {
-			mavlink_log_info(_mavlink_fd, "#audio: takeoff to %.1fm above home", (double)(_pos_sp_triplet.current.alt - _home_pos.alt));
+	reset_reached();
 
-		} else {
-			if (onboard) {
-				mavlink_log_info(_mavlink_fd, "#audio: heading to onboard WP %d", index);
+	return true;
+}
 
-			} else {
-				mavlink_log_info(_mavlink_fd, "#audio: heading to offboard WP %d", index);
-			}
-		}
+bool
+Navigator::start_rtl()
+{
+	/* TODO check if RTL was successfull, if not we have a problem and need to dispatch something */
+	if (_rtl.get_current_rtl_item(&_global_pos, &_mission_item)) {
 
-	} else {
-		/* since a mission is not advanced without WPs available, this is not supposed to happen */
-		_mission_item_valid = false;
-		_pos_sp_triplet.current.valid = false;
-		warnx("ERROR: current WP can't be set");
-	}
+		_mission_item_valid = true;
 
-	if (!_do_takeoff) {
-		mission_item_s item_next;
-		ret = _mission.get_next_mission_item(&item_next);
+		mission_item_to_position_setpoint(&_mission_item, &_pos_sp_triplet.current);
+		_pos_sp_triplet.current.valid = true;
 
-		if (ret == OK) {
-			position_setpoint_from_mission_item(&_pos_sp_triplet.next, &item_next);
+		reset_reached();
 
-		} else {
-			/* this will fail for the last WP */
-			_pos_sp_triplet.next.valid = false;
-		}
+		_update_triplet = true;
+		return true;
 	}
+	dispatch(EVENT_LOITER_REQUESTED);
 
-	_pos_sp_triplet_updated = true;
+	return false;
 }
 
-void
-Navigator::start_rtl()
+bool
+Navigator::advance_rtl()
 {
-	_do_takeoff = false;
+	/* tell mission to move by one */
+	_rtl.move_to_next();
 
-	/* decide if we need climb */
-	if (_rtl_state == RTL_STATE_NONE) {
-		if (_global_pos.alt < _home_pos.alt + _parameters.rtl_alt) {
-			_rtl_state = RTL_STATE_CLIMB;
+	/* now try to set the new mission items, if it fails, it will dispatch loiter */
+	if (_rtl.get_current_rtl_item(&_global_pos, &_mission_item)) {
 
-		} else {
-			_rtl_state = RTL_STATE_RETURN;
-		}
-	}
+		_mission_item_valid = true;
 
-	/* if switching directly to return state, reset altitude setpoint */
-	if (_rtl_state == RTL_STATE_RETURN) {
-		_mission_item.altitude_is_relative = false;
-		_mission_item.altitude = _global_pos.alt;
+		mission_item_to_position_setpoint(&_mission_item, &_pos_sp_triplet.current);
+		_pos_sp_triplet.current.valid = true;
+
+		reset_reached();
+
+		_update_triplet = true;
+		return true;
 	}
 
-	_reset_loiter_pos = true;
-	set_rtl_item();
+	dispatch(EVENT_LOITER_REQUESTED);
+	return false;
 }
 
-void
+bool
 Navigator::start_land()
 {
+	/* TODO: verify/test */
+
 	reset_reached();
 
 	/* this state can be requested by commander even if no global position available,
 	 * in his case controller must perform landing without position control */
-	_do_takeoff = false;
-	_reset_loiter_pos = true;
 
 	memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s));
 
-	_mission_item_valid = true;
-
 	_mission_item.lat = _global_pos.lat;
 	_mission_item.lon = _global_pos.lon;
 	_mission_item.altitude_is_relative = false;
@@ -1220,185 +1125,22 @@ Navigator::start_land()
 	_mission_item.autocontinue = true;
 	_mission_item.origin = ORIGIN_ONBOARD;
 
-	position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item);
-
-	_pos_sp_triplet.next.valid = false;
-}
-
-void
-Navigator::start_land_home()
-{
-	reset_reached();
-
-	/* land to home position, should be called when hovering above home, from RTL state */
-	_do_takeoff = false;
-	_reset_loiter_pos = true;
-
-	memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s));
-
 	_mission_item_valid = true;
 
-	_mission_item.lat = _home_pos.lat;
-	_mission_item.lon = _home_pos.lon;
-	_mission_item.altitude_is_relative = false;
-	_mission_item.altitude = _home_pos.alt;
-	_mission_item.yaw = NAN;
-	_mission_item.loiter_radius = _parameters.loiter_radius;
-	_mission_item.loiter_direction = 1;
-	_mission_item.nav_cmd = NAV_CMD_LAND;
-	_mission_item.acceptance_radius = _parameters.acceptance_radius;
-	_mission_item.time_inside = 0.0f;
-	_mission_item.pitch_min = 0.0f;
-	_mission_item.autocontinue = true;
-	_mission_item.origin = ORIGIN_ONBOARD;
-
-	position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item);
+	mission_item_to_position_setpoint(&_mission_item, &_pos_sp_triplet.current);
 
 	_pos_sp_triplet.next.valid = false;
-}
-
-void
-Navigator::set_rtl_item()
-{
-	reset_reached();
-
-	switch (_rtl_state) {
-	case RTL_STATE_CLIMB: {
-			memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s));
-
-			float climb_alt = _home_pos.alt + _parameters.rtl_alt;
-
-			if (_vstatus.condition_landed) {
-				climb_alt = fmaxf(climb_alt, _global_pos.alt + _parameters.rtl_alt);
-			}
-
-			_mission_item_valid = true;
-
-			_mission_item.lat = _global_pos.lat;
-			_mission_item.lon = _global_pos.lon;
-			_mission_item.altitude_is_relative = false;
-			_mission_item.altitude = climb_alt;
-			_mission_item.yaw = NAN;
-			_mission_item.loiter_radius = _parameters.loiter_radius;
-			_mission_item.loiter_direction = 1;
-			_mission_item.nav_cmd = NAV_CMD_TAKEOFF;
-			_mission_item.acceptance_radius = _parameters.acceptance_radius;
-			_mission_item.time_inside = 0.0f;
-			_mission_item.pitch_min = 0.0f;
-			_mission_item.autocontinue = true;
-			_mission_item.origin = ORIGIN_ONBOARD;
-
-			position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item);
-
-			_pos_sp_triplet.next.valid = false;
-
-			mavlink_log_info(_mavlink_fd, "#audio: RTL: climb to %.1fm above home", (double)(climb_alt - _home_pos.alt));
-			break;
-		}
-
-	case RTL_STATE_RETURN: {
-			memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s));
-
-			_mission_item_valid = true;
-
-			_mission_item.lat = _home_pos.lat;
-			_mission_item.lon = _home_pos.lon;
-			// don't change altitude
-			if (_pos_sp_triplet.previous.valid) {
-				/* if previous setpoint is valid then use it to calculate heading to home */
-				_mission_item.yaw = get_bearing_to_next_waypoint(_pos_sp_triplet.previous.lat, _pos_sp_triplet.previous.lon, _mission_item.lat, _mission_item.lon);
-
-			} else {
-				/* else use current position */
-				_mission_item.yaw = get_bearing_to_next_waypoint(_global_pos.lat, _global_pos.lon, _mission_item.lat, _mission_item.lon);
-			}
-			_mission_item.loiter_radius = _parameters.loiter_radius;
-			_mission_item.loiter_direction = 1;
-			_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
-			_mission_item.acceptance_radius = _parameters.acceptance_radius;
-			_mission_item.time_inside = 0.0f;
-			_mission_item.pitch_min = 0.0f;
-			_mission_item.autocontinue = true;
-			_mission_item.origin = ORIGIN_ONBOARD;
-
-			position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item);
-
-			_pos_sp_triplet.next.valid = false;
-
-			mavlink_log_info(_mavlink_fd, "#audio: RTL: return at %.1fm above home", (double)(_mission_item.altitude - _home_pos.alt));
-			break;
-		}
-
-	case RTL_STATE_DESCEND: {
-			memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s));
-
-			_mission_item_valid = true;
-
-			_mission_item.lat = _home_pos.lat;
-			_mission_item.lon = _home_pos.lon;
-			_mission_item.altitude_is_relative = false;
-			_mission_item.altitude = _home_pos.alt + _parameters.land_alt;
-			_mission_item.yaw = NAN;
-			_mission_item.loiter_radius = _parameters.loiter_radius;
-			_mission_item.loiter_direction = 1;
-			_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
-			_mission_item.acceptance_radius = _parameters.acceptance_radius;
-			_mission_item.time_inside = _parameters.rtl_land_delay < 0.0f ? 0.0f : _parameters.rtl_land_delay;
-			_mission_item.pitch_min = 0.0f;
-			_mission_item.autocontinue = _parameters.rtl_land_delay > -0.001f;
-			_mission_item.origin = ORIGIN_ONBOARD;
-
-			position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item);
-
-			_pos_sp_triplet.next.valid = false;
-
-			mavlink_log_info(_mavlink_fd, "#audio: RTL: descend to %.1fm above home", (double)(_mission_item.altitude - _home_pos.alt));
-			break;
-		}
-
-	default: {
-			mavlink_log_critical(_mavlink_fd, "#audio: [navigator] error: unknown RTL state: %d", _rtl_state);
-			start_loiter();
-			break;
-		}
-	}
-
-	_pos_sp_triplet_updated = true;
-}
-
-void
-Navigator::request_loiter_or_ready()
-{
-	/* XXX workaround: no landing detector for fixedwing yet */
-	if (_vstatus.condition_landed && _vstatus.is_rotary_wing) {
-		dispatch(EVENT_READY_REQUESTED);
-
-	} else {
-		dispatch(EVENT_LOITER_REQUESTED);
-	}
-}
-
-void
-Navigator::request_mission_if_available()
-{
-	if (_mission.current_mission_available()) {
-		dispatch(EVENT_MISSION_REQUESTED);
 
-	} else {
-		request_loiter_or_ready();
-	}
+	_update_triplet = true;
+	return true;
 }
 
 void
-Navigator::position_setpoint_from_mission_item(position_setpoint_s *sp, mission_item_s *item)
+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) {
-		/* set home position for RTL item */
-		sp->lat = _home_pos.lat;
-		sp->lon = _home_pos.lon;
-		sp->alt = _home_pos.alt + _parameters.rtl_alt;
 
 		if (_pos_sp_triplet.previous.valid) {
 			/* if previous setpoint is valid then use it to calculate heading to home */
@@ -1408,9 +1150,6 @@ Navigator::position_setpoint_from_mission_item(position_setpoint_s *sp, mission_
 			/* else use current position */
 			sp->yaw = get_bearing_to_next_waypoint(_global_pos.lat, _global_pos.lon, sp->lat, sp->lon);
 		}
-		sp->loiter_radius = _parameters.loiter_radius;
-		sp->loiter_direction = 1;
-		sp->pitch_min = 0.0f;
 
 	} else {
 		sp->lat = item->lat;
@@ -1450,14 +1189,14 @@ Navigator::check_mission_item_reached()
 		return _vstatus.condition_landed;
 	}
 
-	/* XXX TODO count turns */
-	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) {
+	/* TODO count turns */
+	// 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;
-	}
+	// 	return false;
+	// }
 
 	uint64_t now = hrt_absolute_time();
 
@@ -1475,22 +1214,19 @@ Navigator::check_mission_item_reached()
 		float dist_xy = -1.0f;
 		float dist_z = -1.0f;
 
-		/* calculate AMSL altitude for this waypoint */
-		float wp_alt_amsl = _mission_item.altitude;
+		float altitude_amsl = _mission_item.altitude_is_relative
+					? _mission_item.altitude + _home_pos.alt : _mission_item.altitude;
 
-		if (_mission_item.altitude_is_relative)
-			wp_alt_amsl += _home_pos.alt;
-
-		dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, wp_alt_amsl,
-				(double)_global_pos.lat, (double)_global_pos.lon, _global_pos.alt,
+		dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, altitude_amsl,
+				_global_pos.lat, _global_pos.lon, _global_pos.alt,
 				&dist_xy, &dist_z);
 
-		if (_do_takeoff) {
-			if (_global_pos.alt > wp_alt_amsl - acceptance_radius) {
-				/* require only altitude for takeoff */
+		if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF) {
+
+			/* require only altitude for takeoff */
+			if (_global_pos.alt > altitude_amsl - acceptance_radius) {
 				_waypoint_position_reached = true;
 			}
-
 		} else {
 			if (dist >= 0.0f && dist <= acceptance_radius) {
 				_waypoint_position_reached = true;
@@ -1499,7 +1235,10 @@ Navigator::check_mission_item_reached()
 	}
 
 	if (_waypoint_position_reached && !_waypoint_yaw_reached) {
-		if (_vstatus.is_rotary_wing && !_do_takeoff && isfinite(_mission_item.yaw)) {
+		
+		/* TODO: removed takeoff, why? */
+		if (_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 - _global_pos.yaw);
 
@@ -1514,24 +1253,23 @@ Navigator::check_mission_item_reached()
 
 	/* 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);
+				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 >= (uint64_t)_mission_item.time_inside * 1e6)
 		    || _mission_item.nav_cmd == NAV_CMD_TAKEOFF) {
-			reset_reached();
 			return true;
 		}
 	}
-
 	return false;
-
 }
 
 void
@@ -1544,62 +1282,6 @@ Navigator::reset_reached()
 }
 
 void
-Navigator::on_mission_item_reached()
-{
-	if (myState == NAV_STATE_MISSION) {
-
-		_mission.report_mission_item_reached();
-
-		if (_do_takeoff) {
-			/* takeoff completed */
-			_do_takeoff = false;
-			mavlink_log_info(_mavlink_fd, "#audio: takeoff completed");
-
-		} else {
-			/* advance by one mission item */
-			_mission.move_to_next();
-		}
-
-		if (_mission.current_mission_available()) {
-			set_mission_item();
-
-		} else {
-			/* if no more mission items available then finish mission */
-			/* loiter at last waypoint */
-			_reset_loiter_pos = false;
-			mavlink_log_info(_mavlink_fd, "[navigator] mission completed");
-			request_loiter_or_ready();
-		}
-
-	} else if (myState == NAV_STATE_RTL) {
-		/* RTL completed */
-		if (_rtl_state == RTL_STATE_DESCEND) {
-			/* hovering above home position, land if needed or loiter */
-			mavlink_log_info(_mavlink_fd, "[navigator] RTL completed");
-
-			if (_mission_item.autocontinue) {
-				dispatch(EVENT_LAND_REQUESTED);
-
-			} else {
-				_reset_loiter_pos = false;
-				dispatch(EVENT_LOITER_REQUESTED);
-			}
-
-		} else {
-			/* next RTL step */
-			_rtl_state = (RTLState)(_rtl_state + 1);
-			set_rtl_item();
-		}
-
-	} else if (myState == NAV_STATE_LAND) {
-		/* landing completed */
-		mavlink_log_info(_mavlink_fd, "[navigator] landing completed");
-		dispatch(EVENT_READY_REQUESTED);
-	}
-	_mission.publish_mission_result();
-}
-
-void
 Navigator::publish_position_setpoint_triplet()
 {
 	/* update navigation state */
@@ -1607,11 +1289,9 @@ Navigator::publish_position_setpoint_triplet()
 
 	/* lazily publish the position setpoint triplet only once available */
 	if (_pos_sp_triplet_pub > 0) {
-		/* publish the position setpoint triplet */
 		orb_publish(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_pub, &_pos_sp_triplet);
 
 	} else {
-		/* advertise and publish */
 		_pos_sp_triplet_pub = orb_advertise(ORB_ID(position_setpoint_triplet), &_pos_sp_triplet);
 	}
 }
diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c
index 9ef359c6d..49969a382 100644
--- a/src/modules/navigator/navigator_params.c
+++ b/src/modules/navigator/navigator_params.c
@@ -53,16 +53,6 @@
  */
 
 /**
- * Minimum altitude (fixed wing only)
- *
- * Minimum altitude above home for LOITER.
- *
- * @unit meters
- * @group Navigation
- */
-PARAM_DEFINE_FLOAT(NAV_MIN_ALT, 50.0f);
-
-/**
  * Waypoint acceptance radius
  *
  * Default value of acceptance radius (if not specified in mission item).
@@ -102,37 +92,6 @@ PARAM_DEFINE_INT32(NAV_ONB_MIS_EN, 0);
 PARAM_DEFINE_FLOAT(NAV_TAKEOFF_ALT, 10.0f);
 
 /**
- * Landing altitude
- *
- * Stay at this altitude above home position after RTL descending. Land (i.e. slowly descend) from this altitude if autolanding allowed.
- *
- * @unit meters
- * @group Navigation
- */
-PARAM_DEFINE_FLOAT(NAV_LAND_ALT, 5.0f);
-
-/**
- * Return-To-Launch altitude
- *
- * Minimum altitude above home position for going home in RTL mode.
- *
- * @unit meters
- * @group Navigation
- */
-PARAM_DEFINE_FLOAT(NAV_RTL_ALT, 30.0f);
-
-/**
- * Return-To-Launch delay
- *
- * Delay after descend before landing in RTL mode.
- * If set to -1 the system will not land but loiter at NAV_LAND_ALT.
- *
- * @unit seconds
- * @group Navigation
- */
-PARAM_DEFINE_FLOAT(NAV_RTL_LAND_T, -1.0f);
-
-/**
  * Enable parachute deployment
  *
  * @group Navigation
diff --git a/src/modules/navigator/navigator_state.h b/src/modules/navigator/navigator_state.h
deleted file mode 100644
index 6a1475c9b..000000000
--- a/src/modules/navigator/navigator_state.h
+++ /dev/null
@@ -1,21 +0,0 @@
-/*
- * navigator_state.h
- *
- *  Created on: 27.01.2014
- *      Author: ton
- */
-
-#ifndef NAVIGATOR_STATE_H_
-#define NAVIGATOR_STATE_H_
-
-typedef enum {
-	NAV_STATE_NONE = 0,
-	NAV_STATE_READY,
-	NAV_STATE_LOITER,
-	NAV_STATE_MISSION,
-	NAV_STATE_RTL,
-	NAV_STATE_LAND,
-	NAV_STATE_MAX
-} nav_state_t;
-
-#endif /* NAVIGATOR_STATE_H_ */
diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp
new file mode 100644
index 000000000..6e621e39d
--- /dev/null
+++ b/src/modules/navigator/rtl.cpp
@@ -0,0 +1,253 @@
+/****************************************************************************
+ *
+ *   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_rtl.cpp
+ * Helper class to access RTL
+ * @author Julian Oes <julian@oes.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ */
+
+#include <string.h>
+#include <stdlib.h>
+#include <math.h>
+#include <fcntl.h>
+
+#include <mavlink/mavlink_log.h>
+#include <systemlib/err.h>
+
+#include <uORB/uORB.h>
+#include <uORB/topics/mission.h>
+#include <uORB/topics/home_position.h>
+
+#include "rtl.h"
+
+
+RTL::RTL() :
+	SuperBlock(NULL, "RTL"),
+	_mavlink_fd(-1),
+	_rtl_state(RTL_STATE_NONE),
+	_home_position({}),
+	_param_return_alt(this, "RETURN_ALT"),
+	_param_descend_alt(this, "DESCEND_ALT"),
+	_param_land_delay(this, "LAND_DELAY"),
+	_loiter_radius(50),
+	_acceptance_radius(50)
+{
+	/* load initial params */
+	updateParams();
+}
+
+RTL::~RTL()
+{
+}
+
+void
+RTL::set_home_position(const home_position_s *new_home_position)
+{
+	memcpy(&_home_position, new_home_position, sizeof(home_position_s));
+}
+
+bool
+RTL::get_current_rtl_item(const vehicle_global_position_s *global_position, mission_item_s *new_mission_item)
+{
+	/* Open mavlink fd */
+	if (_mavlink_fd < 0) {
+		/* try to open the mavlink log device every once in a while */
+		_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
+	}
+
+	/* decide if we need climb */
+	if (_rtl_state == RTL_STATE_NONE) {
+		if (global_position->alt < _home_position.alt + _param_return_alt.get()) {
+			_rtl_state = RTL_STATE_CLIMB;
+
+		} else {
+			_rtl_state = RTL_STATE_RETURN;
+		}
+	}
+
+	/* if switching directly to return state, set altitude setpoint to current altitude */
+	if (_rtl_state == RTL_STATE_RETURN) {
+		new_mission_item->altitude_is_relative = false;
+		new_mission_item->altitude = global_position->alt;
+	}
+
+	switch (_rtl_state) {
+	case RTL_STATE_CLIMB: {
+
+		float climb_alt = _home_position.alt + _param_return_alt.get();
+
+		/* TODO understand and fix this */
+		// if (_vstatus.condition_landed) {
+		// 	climb_alt = fmaxf(climb_alt, _global_pos.alt + _parameters.rtl_alt);
+		// }
+
+		new_mission_item->lat = global_position->lat;
+		new_mission_item->lon = global_position->lon;
+		new_mission_item->altitude_is_relative = false;
+		new_mission_item->altitude = climb_alt;
+		new_mission_item->yaw = NAN;
+		new_mission_item->loiter_radius = _loiter_radius;
+		new_mission_item->loiter_direction = 1;
+		new_mission_item->nav_cmd = NAV_CMD_TAKEOFF;
+		new_mission_item->acceptance_radius = _acceptance_radius;
+		new_mission_item->time_inside = 0.0f;
+		new_mission_item->pitch_min = 0.0f;
+		new_mission_item->autocontinue = true;
+		new_mission_item->origin = ORIGIN_ONBOARD;
+
+		mavlink_log_info(_mavlink_fd, "#audio: RTL: climb to %d meters above home",
+			(int)(climb_alt - _home_position.alt));
+		break;
+	}
+	case RTL_STATE_RETURN: {
+
+		new_mission_item->lat = _home_position.lat;
+		new_mission_item->lon = _home_position.lon;
+
+		/* TODO: add this again */
+		// don't change altitude
+		// if (_pos_sp_triplet.previous.valid) {
+		// 	/* if previous setpoint is valid then use it to calculate heading to home */
+		// 	new_mission_item->yaw = get_bearing_to_next_waypoint(_pos_sp_triplet.previous.lat, _pos_sp_triplet.previous.lon, new_mission_item->lat, new_mission_item->lon);
+
+		// } else {
+		// 	/* else use current position */
+		// 	new_mission_item->yaw = get_bearing_to_next_waypoint(_global_pos.lat, _global_pos.lon, new_mission_item->lat, new_mission_item->lon);
+		// }
+		new_mission_item->loiter_radius = _loiter_radius;
+		new_mission_item->loiter_direction = 1;
+		new_mission_item->nav_cmd = NAV_CMD_WAYPOINT;
+		new_mission_item->acceptance_radius = _acceptance_radius;
+		new_mission_item->time_inside = 0.0f;
+		new_mission_item->pitch_min = 0.0f;
+		new_mission_item->autocontinue = true;
+		new_mission_item->origin = ORIGIN_ONBOARD;
+
+		mavlink_log_info(_mavlink_fd, "#audio: RTL: return at %d meters above home",
+			(int)(new_mission_item->altitude - _home_position.alt));
+		break;
+	}
+
+	case RTL_STATE_DESCEND: {
+
+		new_mission_item->lat = _home_position.lat;
+		new_mission_item->lon = _home_position.lon;
+		new_mission_item->altitude_is_relative = false;
+		new_mission_item->altitude = _home_position.alt + _param_descend_alt.get();
+		new_mission_item->yaw = NAN;
+		new_mission_item->loiter_radius = _loiter_radius;
+		new_mission_item->loiter_direction = 1;
+		new_mission_item->nav_cmd = NAV_CMD_WAYPOINT;
+		new_mission_item->acceptance_radius = _acceptance_radius;
+		new_mission_item->time_inside = _param_land_delay.get() < 0.0f ? 0.0f : _param_land_delay.get();
+		new_mission_item->pitch_min = 0.0f;
+		new_mission_item->autocontinue = _param_land_delay.get() > -0.001f;
+		new_mission_item->origin = ORIGIN_ONBOARD;
+
+		mavlink_log_info(_mavlink_fd, "#audio: RTL: descend to %d meters above home",
+			(int)(new_mission_item->altitude - _home_position.alt));
+		break;
+	}
+
+	case RTL_STATE_LAND: {
+
+		new_mission_item->lat = _home_position.lat;
+		new_mission_item->lon = _home_position.lon;
+		new_mission_item->altitude_is_relative = false;
+		new_mission_item->altitude = _home_position.alt;
+		new_mission_item->yaw = NAN;
+		new_mission_item->loiter_radius = _loiter_radius;
+		new_mission_item->loiter_direction = 1;
+		new_mission_item->nav_cmd = NAV_CMD_LAND;
+		new_mission_item->acceptance_radius = _acceptance_radius;
+		new_mission_item->time_inside = 0.0f;
+		new_mission_item->pitch_min = 0.0f;
+		new_mission_item->autocontinue = true;
+		new_mission_item->origin = ORIGIN_ONBOARD;
+
+		mavlink_log_info(_mavlink_fd, "#audio: RTL: land at home");
+		break;
+	}
+
+	case RTL_STATE_FINISHED: {
+		/* nothing to do, report fail */
+		return false;
+	}
+
+	default:
+		return false;	
+	}
+
+	return true;
+}
+
+bool
+RTL::get_next_rtl_item(mission_item_s *new_mission_item)
+{
+	/* TODO implement */
+	return false;
+}
+
+void
+RTL::move_to_next()
+{
+	switch (_rtl_state) {
+	case RTL_STATE_CLIMB:
+		_rtl_state = RTL_STATE_RETURN;
+		break;
+	
+	case RTL_STATE_RETURN:
+		_rtl_state = RTL_STATE_DESCEND;
+		break;
+
+	case RTL_STATE_DESCEND:
+		/* only go to land if autoland is enabled */
+		if (_param_land_delay.get() < 0) {
+			_rtl_state = RTL_STATE_FINISHED;
+		} else {
+			_rtl_state = RTL_STATE_LAND;
+		}
+		break;
+
+	case RTL_STATE_LAND:
+		_rtl_state = RTL_STATE_FINISHED;
+		break;
+
+	case RTL_STATE_FINISHED:
+		break;
+
+	default:
+		break;
+	}
+}
\ No newline at end of file
diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h
new file mode 100644
index 000000000..c761837fc
--- /dev/null
+++ b/src/modules/navigator/rtl.h
@@ -0,0 +1,92 @@
+/****************************************************************************
+ *
+ *   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_mission.h
+ * Helper class to access missions
+ * @author Julian Oes <julian@oes.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ */
+
+#ifndef NAVIGATOR_RTL_H
+#define NAVIGATOR_RTL_H
+
+#include <controllib/blocks.hpp>
+#include <controllib/block/BlockParam.hpp>
+
+#include <uORB/topics/mission.h>
+#include <uORB/topics/home_position.h>
+#include <uORB/topics/vehicle_global_position.h>
+
+class RTL : public control::SuperBlock
+{
+public:
+	/**
+	 * Constructor
+	 */
+	RTL();
+
+	/**
+	 * Destructor
+	 */
+	~RTL();
+
+	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);
+	bool		get_next_rtl_item(mission_item_s *mission_item);
+
+	void		move_to_next();
+
+private:
+	int 		_mavlink_fd;
+
+	enum RTLState {
+		RTL_STATE_NONE = 0,
+		RTL_STATE_CLIMB,
+		RTL_STATE_RETURN,
+		RTL_STATE_DESCEND,
+		RTL_STATE_LAND,
+		RTL_STATE_FINISHED,
+	}		_rtl_state;
+
+	home_position_s _home_position;
+	float		_loiter_radius;
+	float		_acceptance_radius;
+
+
+	control::BlockParamFloat _param_return_alt;
+	control::BlockParamFloat _param_descend_alt;
+	control::BlockParamFloat _param_land_delay;
+};
+
+#endif
diff --git a/src/modules/navigator/rtl_params.c b/src/modules/navigator/rtl_params.c
new file mode 100644
index 000000000..7a8b1d745
--- /dev/null
+++ b/src/modules/navigator/rtl_params.c
@@ -0,0 +1,87 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 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 rtl_params.c
+ *
+ * Parameters for RTL
+ *
+ * @author Julian Oes <julian@oes.ch>
+ */
+
+#include <nuttx/config.h>
+
+#include <systemlib/param/param.h>
+
+/*
+ * RTL parameters, accessible via MAVLink
+ */
+
+/**
+ * RTL altitude
+ *
+ * Altitude to fly back in RTL in meters
+ *
+ * @unit meters
+ * @min 0
+ * @max 1
+ * @group RTL
+ */
+PARAM_DEFINE_FLOAT(RTL_RETURN_ALT, 100);
+
+
+/**
+ * RTL loiter altitude
+ *
+ * Stay at this altitude above home position after RTL descending.
+ * Land (i.e. slowly descend) from this altitude if autolanding allowed.
+ *
+ * @unit meters
+ * @min 0
+ * @max 100
+ * @group RTL
+ */
+PARAM_DEFINE_FLOAT(RTL_DESCEND_ALT, 20);
+
+/**
+ * RTL delay
+ *
+ * Delay after descend before landing in RTL mode.
+ * If set to -1 the system will not land but loiter at NAV_LAND_ALT.
+ *
+ * @unit seconds
+ * @min -1
+ * @max
+ * @group RTL
+ */
+PARAM_DEFINE_FLOAT(RTL_LAND_DELAY, -1.0f);
diff --git a/src/modules/systemlib/state_table.h b/src/modules/systemlib/state_table.h
index f2709d29f..38378651b 100644
--- a/src/modules/systemlib/state_table.h
+++ b/src/modules/systemlib/state_table.h
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   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
@@ -35,6 +35,7 @@
  * @file state_table.h
  * 
  * Finite-State-Machine helper class for state table
+ * @author: Julian Oes <julian@oes.ch>
  */
 
 #ifndef __SYSTEMLIB_STATE_TABLE_H
@@ -43,7 +44,7 @@
 class StateTable
 {
 public:
-	typedef void (StateTable::*Action)();
+	typedef bool (StateTable::*Action)();
 	struct Tran {
 		Action action;
 		unsigned nextState;
@@ -53,17 +54,23 @@ public:
 	: myTable(table), myNsignals(nSignals), myNstates(nStates) {}
 	
 	#define NO_ACTION &StateTable::doNothing
-	#define ACTION(_target) static_cast<StateTable::Action>(_target)
+	#define ACTION(_target) StateTable::Action(_target)
 
 	virtual ~StateTable() {}
 	
 	void dispatch(unsigned const sig) {
-		register Tran const *t = myTable + myState*myNsignals + sig;
-		(this->*(t->action))();
-
-		myState = t->nextState;
+		/* get transition using state table */
+		Tran const *t = myTable + myState*myNsignals + sig;
+		/* first up change state, this allows to do further dispatchs in the state functions */
+		
+		/* now execute state function, if it runs with success, accept new state */
+		if ((this->*(t->action))()) {
+			myState = t->nextState;
+		}
+	}
+	bool doNothing() {
+		return true;
 	}
-	void doNothing() {}
 protected:
 	unsigned myState;
 private:
diff --git a/src/modules/uORB/topics/mission.h b/src/modules/uORB/topics/mission.h
index dfc461ae4..4532b329d 100644
--- a/src/modules/uORB/topics/mission.h
+++ b/src/modules/uORB/topics/mission.h
@@ -93,6 +93,8 @@ struct mission_item_s {
 	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	*/
 };
 
 struct mission_s
diff --git a/src/modules/uORB/topics/position_setpoint_triplet.h b/src/modules/uORB/topics/position_setpoint_triplet.h
index 34aaa30dd..85e8ef8a5 100644
--- a/src/modules/uORB/topics/position_setpoint_triplet.h
+++ b/src/modules/uORB/topics/position_setpoint_triplet.h
@@ -45,7 +45,6 @@
 #include <stdint.h>
 #include <stdbool.h>
 #include "../uORB.h"
-#include <navigator/navigator_state.h>
 
 /**
  * @addtogroup topics
@@ -74,6 +73,17 @@ struct position_setpoint_s
 	float pitch_min;		/**< minimal pitch angle for fixed wing takeoff waypoints */
 };
 
+typedef enum {
+	NAV_STATE_NONE_ON_GROUND = 0,
+	NAV_STATE_NONE_IN_AIR,
+	NAV_STATE_AUTO_ON_GROUND,
+	NAV_STATE_LOITER,
+	NAV_STATE_MISSION,
+	NAV_STATE_RTL,
+	NAV_STATE_LAND,
+	NAV_STATE_MAX
+} nav_state_t;
+
 /**
  * Global position setpoint triplet in WGS84 coordinates.
  *
diff --git a/src/modules/uORB/topics/vehicle_global_position.h b/src/modules/uORB/topics/vehicle_global_position.h
index adcd028f0..cfab695f8 100644
--- a/src/modules/uORB/topics/vehicle_global_position.h
+++ b/src/modules/uORB/topics/vehicle_global_position.h
@@ -36,7 +36,7 @@
  * Definition of the global fused WGS84 position uORB topic.
  *
  * @author Thomas Gubler <thomasgubler@student.ethz.ch>
- * @author Julian Oes <joes@student.ethz.ch>
+ * @author Julian Oes <julian@oes.ch>
  * @author Lorenz Meier <lm@inf.ethz.ch>
  */
 
@@ -66,16 +66,16 @@ struct vehicle_global_position_s {
 	bool global_valid;		/**< true if position satisfies validity criteria of estimator */
 	bool baro_valid;		/**< true if baro_alt is valid (vel_d is also valid in this case) */
 
-	uint64_t time_gps_usec; /**< GPS timestamp in microseconds					   */
+	uint64_t time_gps_usec; 	/**< GPS timestamp in microseconds					   */
 	double lat;			/**< Latitude in degrees							 	   */
 	double lon;			/**< Longitude in degrees							 	   */
 	float alt;			/**< Altitude AMSL in meters						 	   */
-	float vel_n; 		/**< Ground north velocity, m/s				 			   */
-	float vel_e;		/**< Ground east velocity, m/s							   */
-	float vel_d;		/**< Ground downside velocity, m/s						   */
+	float vel_n; 			/**< Ground north velocity, m/s				 			   */
+	float vel_e;			/**< Ground east velocity, m/s							   */
+	float vel_d;			/**< Ground downside velocity, m/s						   */
 	float yaw; 			/**< Yaw in radians -PI..+PI.							   */
 
-	float baro_alt;		/**< Barometric altitude (not raw baro but fused with accelerometer) */
+	float baro_alt;			/**< Barometric altitude (not raw baro but fused with accelerometer) */
 };
 
 /**
diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h
index 48af4c9e2..c1bd828d8 100644
--- a/src/modules/uORB/topics/vehicle_status.h
+++ b/src/modules/uORB/topics/vehicle_status.h
@@ -54,8 +54,6 @@
 #include <stdbool.h>
 #include "../uORB.h"
 
-#include <navigator/navigator_state.h>
-
 /**
  * @addtogroup topics @{
  */
@@ -93,6 +91,14 @@ typedef enum {
 	FAILSAFE_STATE_MAX
 } failsafe_state_t;
 
+typedef enum {
+	NAVIGATION_STATE_NONE = 0,
+	NAVIGATION_STATE_MISSION,
+	NAVIGATION_STATE_LOITER,
+	NAVIGATION_STATE_RTL,
+	NAVIGATION_STATE_LAND
+} navigation_state_t;
+
 enum VEHICLE_MODE_FLAG {
 	VEHICLE_MODE_FLAG_SAFETY_ARMED = 128,
 	VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED = 64,
@@ -152,8 +158,7 @@ struct vehicle_status_s {
 	uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */
 
 	main_state_t main_state;				/**< main state machine */
-	unsigned int set_nav_state;	/**< set navigation state machine to specified value */
-	uint64_t set_nav_state_timestamp;	/**< timestamp of latest change of set_nav_state */
+	navigation_state_t set_nav_state;		/**< set navigation state machine to specified value */
 	arming_state_t arming_state;			/**< current arming state */
 	hil_state_t hil_state;					/**< current hil state */
 	failsafe_state_t failsafe_state;		/**< current failsafe state */