Merge branch 'navigator_rewrite' into dataman_state_nav_rewrite

19 files changed, 2265 insertions, 1569 deletions

diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp
index bc8dbca50..266215308 100644
--- a/src/modules/navigator/geofence.cpp
+++ b/src/modules/navigator/geofence.cpp
@@ -78,7 +78,7 @@ bool Geofence::inside(const struct vehicle_global_position_s *vehicle)
 {
 	double lat = vehicle->lat / 1e7d;
 	double lon = vehicle->lon / 1e7d;
-	float	alt = vehicle->alt;
+	//float	alt = vehicle->alt;
 
 	return inside(lat, lon, vehicle->alt);
 }
@@ -116,9 +116,9 @@ bool Geofence::inside(double lat, double lon, float altitude)
 				}
 
 				// skip vertex 0 (return point)
-				if (((temp_vertex_i.lon) >= lon != (temp_vertex_j.lon >= lon)) &&
-							(lat <= (temp_vertex_j.lat - temp_vertex_i.lat) * (lon - temp_vertex_i.lon) /
-							 (temp_vertex_j.lon - temp_vertex_i.lon) + temp_vertex_i.lat)) {
+				if (((double)temp_vertex_i.lon >= lon) != ((double)temp_vertex_j.lon >= lon) &&
+							(lat <= (double)(temp_vertex_j.lat - temp_vertex_i.lat) * (lon - (double)temp_vertex_i.lon) /
+							 (double)(temp_vertex_j.lon - temp_vertex_i.lon) + (double)temp_vertex_i.lat)) {
 							c = !c;
 				}
 
@@ -294,4 +294,5 @@ Geofence::loadFromFile(const char *filename)
 int Geofence::clearDm()
 {
 	dm_clear(DM_KEY_FENCE_POINTS);
+	return OK;
 }
diff --git a/src/modules/navigator/loiter.cpp b/src/modules/navigator/loiter.cpp
new file mode 100644
index 000000000..035d035e1
--- /dev/null
+++ b/src/modules/navigator/loiter.cpp
@@ -0,0 +1,80 @@
+/****************************************************************************
+ *
+ *   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 loiter.cpp
+ *
+ * Helper class to loiter
+ *
+ * @author Julian Oes <julian@oes.ch>
+ */
+
+#include <string.h>
+#include <stdlib.h>
+#include <stdbool.h>
+#include <math.h>
+#include <fcntl.h>
+
+#include <mavlink/mavlink_log.h>
+#include <systemlib/err.h>
+
+#include <uORB/uORB.h>
+#include <uORB/topics/position_setpoint_triplet.h>
+
+#include "loiter.h"
+
+Loiter::Loiter(Navigator *navigator, const char *name) :
+	NavigatorMode(navigator, name),
+	MissionBlock(navigator)
+{
+	/* load initial params */
+	updateParams();
+	/* initial reset */
+	reset();
+}
+
+Loiter::~Loiter()
+{
+}
+
+bool
+Loiter::update(struct position_setpoint_triplet_s *pos_sp_triplet)
+{
+	/* set loiter item, don't reuse an existing position setpoint */
+	return set_loiter_item(false, pos_sp_triplet);;
+}
+
+void
+Loiter::reset()
+{
+}
+
diff --git a/src/modules/navigator/loiter.h b/src/modules/navigator/loiter.h
new file mode 100644
index 000000000..a83b53f43
--- /dev/null
+++ b/src/modules/navigator/loiter.h
@@ -0,0 +1,74 @@
+/***************************************************************************
+ *
+ *   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 loiter.h
+ *
+ * Helper class to loiter
+ *
+ * @author Julian Oes <julian@oes.ch>
+ */
+
+#ifndef NAVIGATOR_LOITER_H
+#define NAVIGATOR_LOITER_H
+
+#include <controllib/blocks.hpp>
+#include <controllib/block/BlockParam.hpp>
+
+#include "navigator_mode.h"
+#include "mission_block.h"
+
+class Loiter : public NavigatorMode, MissionBlock
+{
+public:
+	/**
+	 * Constructor
+	 */
+	Loiter(Navigator *navigator, const char *name);
+
+	/**
+	 * Destructor
+	 */
+	~Loiter();
+
+	/**
+	 * This function is called while the mode is inactive
+	 */
+	bool update(struct position_setpoint_triplet_s *pos_sp_triplet);
+
+	/**
+	 * This function is called while the mode is active
+	 */
+	void reset();
+};
+
+#endif
diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp
new file mode 100644
index 000000000..a551df9a2
--- /dev/null
+++ b/src/modules/navigator/mission.cpp
@@ -0,0 +1,472 @@
+/****************************************************************************
+ *
+ *   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.cpp
+ *
+ * Helper class to access missions
+ *
+ * @author Julian Oes <julian@oes.ch>
+ */
+
+#include <sys/types.h>
+#include <string.h>
+#include <stdlib.h>
+#include <unistd.h>
+
+#include <drivers/drv_hrt.h>
+
+#include <dataman/dataman.h>
+#include <mavlink/mavlink_log.h>
+#include <systemlib/err.h>
+#include <geo/geo.h>
+
+#include <uORB/uORB.h>
+#include <uORB/topics/mission.h>
+#include <uORB/topics/mission_result.h>
+
+#include "navigator.h"
+#include "mission.h"
+
+Mission::Mission(Navigator *navigator, const char *name) :
+	NavigatorMode(navigator, name),
+	MissionBlock(navigator),
+	_param_onboard_enabled(this, "ONBOARD_EN"),
+	_onboard_mission({0}),
+	_offboard_mission({0}),
+	_current_onboard_mission_index(-1),
+	_current_offboard_mission_index(-1),
+	_mission_result_pub(-1),
+	_mission_result({0}),
+	_mission_type(MISSION_TYPE_NONE)
+{
+	/* load initial params */
+	updateParams();
+	/* set initial mission items */
+	reset();
+
+}
+
+Mission::~Mission()
+{
+}
+
+void
+Mission::reset()
+{
+	_first_run = true;
+
+	/* check anyway if missions have changed so that feedback to groundstation is given */
+	bool onboard_updated;
+	orb_check(_navigator->get_onboard_mission_sub(), &onboard_updated);
+	if (onboard_updated) {
+		update_onboard_mission();
+	}
+
+	bool offboard_updated;
+	orb_check(_navigator->get_offboard_mission_sub(), &offboard_updated);
+	if (offboard_updated) {
+		update_offboard_mission();
+	}
+}
+
+bool
+Mission::update(struct position_setpoint_triplet_s *pos_sp_triplet)
+{
+	bool updated = false;
+
+	/* check if anything has changed */
+	bool onboard_updated;
+	orb_check(_navigator->get_onboard_mission_sub(), &onboard_updated);
+	if (onboard_updated) {
+		update_onboard_mission();
+	}
+
+	bool offboard_updated;
+	orb_check(_navigator->get_offboard_mission_sub(), &offboard_updated);
+	if (offboard_updated) {
+		update_offboard_mission();
+	}
+
+	/* reset mission items if needed */
+	if (onboard_updated || offboard_updated || _first_run) {
+		set_mission_items(pos_sp_triplet);
+		updated = true;
+		_first_run = false;
+	}
+
+	/* lets check if we reached the current mission item */
+	if (_mission_type != MISSION_TYPE_NONE && is_mission_item_reached()) {
+		advance_mission();
+		set_mission_items(pos_sp_triplet);
+		updated = true;
+	}
+
+	return updated;
+}
+
+void
+Mission::update_onboard_mission()
+{
+	if (orb_copy(ORB_ID(onboard_mission), _navigator->get_onboard_mission_sub(), &_onboard_mission) == OK) {
+		/* accept the current index set by the onboard mission if it is within bounds */
+		if (_onboard_mission.current_index >=0
+		&& _onboard_mission.current_index < (int)_onboard_mission.count) {
+			_current_onboard_mission_index = _onboard_mission.current_index;
+		} else {
+			/* if less WPs available, reset to first WP */
+			if (_current_onboard_mission_index >= (int)_onboard_mission.count) {
+				_current_onboard_mission_index = 0;
+			/* if not initialized, set it to 0 */
+			} else if (_current_onboard_mission_index < 0) {
+				_current_onboard_mission_index = 0;
+			}
+			/* otherwise, just leave it */
+		}
+	} else {
+		_onboard_mission.count = 0;
+		_onboard_mission.current_index = 0;
+		_current_onboard_mission_index = 0;
+	}
+}
+
+void
+Mission::update_offboard_mission()
+{
+	if (orb_copy(ORB_ID(offboard_mission), _navigator->get_offboard_mission_sub(), &_offboard_mission) == OK) {
+
+		/* determine current index */
+		if (_offboard_mission.current_index >= 0
+		    && _offboard_mission.current_index < (int)_offboard_mission.count) {
+			_current_offboard_mission_index = _offboard_mission.current_index;
+		} else {
+			/* if less WPs available, reset to first WP */
+			if (_current_offboard_mission_index >= (int)_offboard_mission.count) {
+				_current_offboard_mission_index = 0;
+			/* if not initialized, set it to 0 */
+			} else if (_current_offboard_mission_index < 0) {
+				_current_offboard_mission_index = 0;
+			}
+			/* otherwise, just leave it */
+		}
+
+		/* Check mission feasibility, for now do not handle the return value,
+		 * however warnings are issued to the gcs via mavlink from inside the MissionFeasiblityChecker */
+		dm_item_t dm_current;
+
+		if (_offboard_mission.dataman_id == 0) {
+			dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0;
+		} else {
+			dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1;
+		}
+
+		missionFeasiblityChecker.checkMissionFeasible(_navigator->get_vstatus()->is_rotary_wing, dm_current,
+			                                      (size_t)_offboard_mission.count,
+							      _navigator->get_geofence(),
+							      _navigator->get_home_position()->alt);
+	} else {
+		_offboard_mission.count = 0;
+		_offboard_mission.current_index = 0;
+		_current_offboard_mission_index = 0;
+	}
+	report_current_offboard_mission_item();
+}
+
+
+void
+Mission::advance_mission()
+{
+	switch (_mission_type) {
+	case MISSION_TYPE_ONBOARD:
+		_current_onboard_mission_index++;
+		break;
+
+	case MISSION_TYPE_OFFBOARD:
+		_current_offboard_mission_index++;
+		break;
+
+	case MISSION_TYPE_NONE:
+	default:
+		break;
+	}
+}
+
+void
+Mission::set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet)
+{
+	set_previous_pos_setpoint(&pos_sp_triplet->current, &pos_sp_triplet->previous);
+
+	/* try setting onboard mission item */
+	if (is_current_onboard_mission_item_set(&pos_sp_triplet->current)) {
+		/* if mission type changed, notify */
+		if (_mission_type != MISSION_TYPE_ONBOARD) {
+			mavlink_log_info(_navigator->get_mavlink_fd(),
+					"#audio: onboard mission running");
+		}
+		_mission_type = MISSION_TYPE_ONBOARD;
+		_navigator->set_is_in_loiter(false);
+
+	/* try setting offboard mission item */
+	} else if (is_current_offboard_mission_item_set(&pos_sp_triplet->current)) {
+		/* if mission type changed, notify */
+		if (_mission_type != MISSION_TYPE_OFFBOARD) {
+			mavlink_log_info(_navigator->get_mavlink_fd(),
+					"#audio: offboard mission running");
+		}
+		_mission_type = MISSION_TYPE_OFFBOARD;
+		_navigator->set_is_in_loiter(false);
+	} else {
+		if (_mission_type != MISSION_TYPE_NONE) {
+			mavlink_log_info(_navigator->get_mavlink_fd(),
+					"#audio: mission finished");
+		} else {
+			mavlink_log_info(_navigator->get_mavlink_fd(),
+					"#audio: no mission available");
+		}
+		_mission_type = MISSION_TYPE_NONE;
+
+		bool use_current_pos_sp = pos_sp_triplet->current.valid && _waypoint_position_reached;
+		set_loiter_item(use_current_pos_sp, pos_sp_triplet);
+		reset_mission_item_reached();
+		report_mission_finished();
+	}
+}
+
+void
+Mission::set_previous_pos_setpoint(const struct position_setpoint_s *current_pos_sp,
+		                   struct position_setpoint_s *previous_pos_sp)
+{
+	/* reuse current setpoint as previous setpoint */
+	if (current_pos_sp->valid) {
+		memcpy(previous_pos_sp, current_pos_sp, sizeof(struct position_setpoint_s));
+	}
+}
+
+bool
+Mission::is_current_onboard_mission_item_set(struct position_setpoint_s *current_pos_sp)
+{
+	/* make sure param is up to date */
+	updateParams();
+	if (_param_onboard_enabled.get() > 0 &&
+	    _current_onboard_mission_index >= 0&&
+	    _current_onboard_mission_index < (int)_onboard_mission.count) {
+		struct mission_item_s new_mission_item;
+		if (read_mission_item(DM_KEY_WAYPOINTS_ONBOARD, true, &_current_onboard_mission_index,
+					&new_mission_item)) {
+			/* convert the current mission item and set it valid */
+			mission_item_to_position_setpoint(&new_mission_item, current_pos_sp);
+			current_pos_sp->valid = true;
+
+			reset_mission_item_reached();
+
+			/* TODO: report this somehow */
+			memcpy(&_mission_item, &new_mission_item, sizeof(struct mission_item_s));
+			return true;
+		}
+	}
+	return false;
+}
+
+bool
+Mission::is_current_offboard_mission_item_set(struct position_setpoint_s *current_pos_sp)
+{
+	if (_current_offboard_mission_index >= 0 &&
+	    _current_offboard_mission_index < (int)_offboard_mission.count) {
+		dm_item_t dm_current;
+		if (_offboard_mission.dataman_id == 0) {
+			dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0;
+		} else {
+			dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1;
+		}
+		struct mission_item_s new_mission_item;
+		if (read_mission_item(dm_current, true, &_current_offboard_mission_index, &new_mission_item)) {
+			/* convert the current mission item and set it valid */
+			mission_item_to_position_setpoint(&new_mission_item, current_pos_sp);
+			current_pos_sp->valid = true;
+
+			reset_mission_item_reached();
+
+			report_current_offboard_mission_item();
+			memcpy(&_mission_item, &new_mission_item, sizeof(struct mission_item_s));
+			return true;
+		}
+	}
+	return false;
+}
+
+void
+Mission::get_next_onboard_mission_item(struct position_setpoint_s *next_pos_sp)
+{
+	int next_temp_mission_index = _onboard_mission.current_index + 1;
+
+	/* try if there is a next onboard mission */
+	if (_onboard_mission.current_index >= 0 &&
+	    next_temp_mission_index < (int)_onboard_mission.count) {
+		struct mission_item_s new_mission_item;
+		if (read_mission_item(DM_KEY_WAYPOINTS_ONBOARD, false, &next_temp_mission_index, &new_mission_item)) {
+			/* convert next mission item to position setpoint */
+			mission_item_to_position_setpoint(&new_mission_item, next_pos_sp);
+			next_pos_sp->valid = true;
+			return;
+		}
+	}
+
+	/* give up */
+	next_pos_sp->valid = false;
+	return;
+}
+
+void
+Mission::get_next_offboard_mission_item(struct position_setpoint_s *next_pos_sp)
+{
+	/* try if there is a next offboard mission */
+	int next_temp_mission_index = _offboard_mission.current_index + 1;
+	warnx("next index: %d, count; %d", next_temp_mission_index, _offboard_mission.count);
+	if (_offboard_mission.current_index >= 0 &&
+	    next_temp_mission_index < (int)_offboard_mission.count) {
+		dm_item_t dm_current;
+		if (_offboard_mission.dataman_id == 0) {
+			dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0;
+		} else {
+			dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1;
+		}
+		struct mission_item_s new_mission_item;
+		if (read_mission_item(dm_current, false, &next_temp_mission_index, &new_mission_item)) {
+			/* convert next mission item to position setpoint */
+			mission_item_to_position_setpoint(&new_mission_item, next_pos_sp);
+			next_pos_sp->valid = true;
+			return;
+		}
+	}
+	/* give up */
+	next_pos_sp->valid = false;
+	return;
+}
+
+bool
+Mission::read_mission_item(const dm_item_t dm_item, bool is_current, int *mission_index,
+		           struct mission_item_s *new_mission_item)
+{
+	/* 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. */
+			mavlink_log_critical(_navigator->get_mavlink_fd(),
+			                     "#audio: ERROR waypoint could not be read");
+			return false;
+		}
+
+		/* 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) {
+
+			/* do DO_JUMP as many times as requested */
+			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
+						 * dataman */
+						mavlink_log_critical(_navigator->get_mavlink_fd(),
+								"#audio: ERROR DO JUMP waypoint could not be written");
+						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 {
+				mavlink_log_info(_navigator->get_mavlink_fd(),
+						 "#audio: DO JUMP repetitions completed");
+				/* no more DO_JUMPS, therefore just try to continue with next mission item */
+				(*mission_index)++;
+			}
+
+		} else {
+			/* if it's not a DO_JUMP, then we were successful */
+			return true;
+		}
+	}
+
+	/* we have given up, we don't want to cycle forever */
+	mavlink_log_critical(_navigator->get_mavlink_fd(),
+			     "#audio: ERROR DO JUMP is cycling, giving up");
+	return false;
+}
+
+void
+Mission::report_mission_item_reached()
+{
+	if (_mission_type == MISSION_TYPE_OFFBOARD) {
+		_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
+Mission::report_mission_finished()
+{
+	_mission_result.mission_finished = true;
+	publish_mission_result();
+}
+
+void
+Mission::publish_mission_result()
+{
+	/* lazily publish the mission result only once available */
+	if (_mission_result_pub > 0) {
+		/* publish mission result */
+		orb_publish(ORB_ID(mission_result), _mission_result_pub, &_mission_result);
+
+	} else {
+		/* advertise and publish */
+		_mission_result_pub = orb_advertise(ORB_ID(mission_result), &_mission_result);
+	}
+	/* reset reached bool */
+	_mission_result.mission_reached = false;
+	_mission_result.mission_finished = false;
+}
diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h
new file mode 100644
index 000000000..a3dd09ecd
--- /dev/null
+++ b/src/modules/navigator/mission.h
@@ -0,0 +1,184 @@
+/****************************************************************************
+ *
+ *   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 mission.h
+ *
+ * Navigator mode to access missions
+ *
+ * @author Julian Oes <julian@oes.ch>
+ */
+
+#ifndef NAVIGATOR_MISSION_H
+#define NAVIGATOR_MISSION_H
+
+#include <drivers/drv_hrt.h>
+
+#include <controllib/blocks.hpp>
+#include <controllib/block/BlockParam.hpp>
+
+#include <dataman/dataman.h>
+
+#include <uORB/uORB.h>
+#include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/position_setpoint_triplet.h>
+#include <uORB/topics/home_position.h>
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/mission.h>
+#include <uORB/topics/mission_result.h>
+
+#include "navigator_mode.h"
+#include "mission_block.h"
+#include "mission_feasibility_checker.h"
+
+class Navigator;
+
+class Mission : public NavigatorMode, MissionBlock
+{
+public:
+	/**
+	 * Constructor
+	 */
+	Mission(Navigator *navigator, const char *name);
+
+	/**
+	 * Destructor
+	 */
+	virtual ~Mission();
+
+	/**
+	 * This function is called while the mode is inactive
+	 */
+	virtual void reset();
+
+	/**
+	 * This function is called while the mode is active
+	 */
+	virtual bool update(struct position_setpoint_triplet_s *pos_sp_triplet);
+
+private:
+	/**
+	 * Update onboard mission topic
+	 */
+	void update_onboard_mission();
+
+	/**
+	 * Update offboard mission topic
+	 */
+	void update_offboard_mission();
+
+	/**
+	 * Move on to next mission item or switch to loiter
+	 */
+	void advance_mission();
+
+	/**
+	 * Set new mission items
+	 */
+	void set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet);
+
+	/**
+	 * Set previous position setpoint
+	 */
+	void set_previous_pos_setpoint(const struct position_setpoint_s *current_pos_sp,
+			               struct position_setpoint_s *previous_pos_sp);
+
+	/**
+	 * Try to set the current position setpoint from an onboard mission item
+	 * @return true if mission item successfully set
+	 */
+	bool is_current_onboard_mission_item_set(struct position_setpoint_s *current_pos_sp);
+
+	/**
+	 * Try to set the current position setpoint from an offboard mission item
+	 * @return true if mission item successfully set
+	 */
+	bool is_current_offboard_mission_item_set(struct position_setpoint_s *current_pos_sp);
+
+	/**
+	 * Try to set the next position setpoint from an onboard mission item
+	 */
+	void get_next_onboard_mission_item(struct position_setpoint_s *next_pos_sp);
+
+	/**
+	 * Try to set the next position setpoint from an offboard mission item
+	 */
+	void get_next_offboard_mission_item(struct position_setpoint_s *next_pos_sp);
+
+	/**
+	 * Read a mission item from the dataman and watch out for DO_JUMPS
+	 * @return true if successful
+	 */
+	bool read_mission_item(const dm_item_t dm_item, bool is_current, int *mission_index,
+			       struct mission_item_s *new_mission_item);
+
+	/**
+	 * Report that a mission item has been reached
+	 */
+	void report_mission_item_reached();
+
+	/**
+	 * Rport the current mission item
+	 */
+	void report_current_offboard_mission_item();
+
+	/**
+	 * Report that the mission is finished if one exists or that none exists
+	 */
+	void report_mission_finished();
+
+	/**
+	 * Publish the mission result so commander and mavlink know what is going on
+	 */
+	void publish_mission_result();
+
+	control::BlockParamFloat _param_onboard_enabled;
+
+	struct mission_s _onboard_mission;
+	struct mission_s _offboard_mission;
+
+	int _current_onboard_mission_index;
+	int _current_offboard_mission_index;
+
+	orb_advert_t _mission_result_pub;
+	struct mission_result_s _mission_result;
+
+	enum {
+		MISSION_TYPE_NONE,
+		MISSION_TYPE_ONBOARD,
+		MISSION_TYPE_OFFBOARD
+	} _mission_type;
+
+	MissionFeasibilityChecker missionFeasiblityChecker; /**< class that checks if a mission is feasible */
+};
+
+#endif
diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp
new file mode 100644
index 000000000..08576750c
--- /dev/null
+++ b/src/modules/navigator/mission_block.cpp
@@ -0,0 +1,225 @@
+/****************************************************************************
+ *
+ *   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 mission_block.cpp
+ *
+ * Helper class to use mission items
+ *
+ * @author Julian Oes <julian@oes.ch>
+ */
+
+#include <sys/types.h>
+#include <string.h>
+#include <stdlib.h>
+#include <unistd.h>
+
+#include <systemlib/err.h>
+#include <geo/geo.h>
+
+#include <uORB/uORB.h>
+
+#include "navigator.h"
+#include "mission_block.h"
+
+
+MissionBlock::MissionBlock(Navigator *navigator) :
+	_waypoint_position_reached(false),
+	_waypoint_yaw_reached(false),
+	_time_first_inside_orbit(0),
+	_mission_item({0}),
+	_mission_item_valid(false),
+	_navigator_priv(navigator)
+{
+}
+
+MissionBlock::~MissionBlock()
+{
+}
+
+bool
+MissionBlock::is_mission_item_reached()
+{
+	/* don't check landed WPs */
+	if (_mission_item.nav_cmd == NAV_CMD_LAND) {
+		return false;
+	}
+	/* TODO: count turns */
+#if 0
+	if ((_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
+	     _mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
+	     _mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED) &&
+	    _mission_item.loiter_radius > 0.01f) {
+
+		return false;
+	}
+#endif
+
+	hrt_abstime now = hrt_absolute_time();
+
+	if (!_waypoint_position_reached) {
+
+		float dist = -1.0f;
+		float dist_xy = -1.0f;
+		float dist_z = -1.0f;
+
+		float altitude_amsl = _mission_item.altitude_is_relative
+				      ? _mission_item.altitude + _navigator_priv->get_home_position()->alt
+			              : _mission_item.altitude;
+
+		dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, altitude_amsl,
+				                          _navigator_priv->get_global_position()->lat,
+							  _navigator_priv->get_global_position()->lon,
+							  _navigator_priv->get_global_position()->alt,
+				&dist_xy, &dist_z);
+
+		if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF && _navigator_priv->get_vstatus()->is_rotary_wing) {
+			/* require only altitude for takeoff for multicopter */
+			if (_navigator_priv->get_global_position()->alt >
+				altitude_amsl - _navigator_priv->get_takeoff_acceptance_radius()) {
+				_waypoint_position_reached = true;
+			}
+		} else if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF) {
+			/* for takeoff mission items use the parameter for the takeoff acceptance radius */
+			if (dist >= 0.0f && dist <= _navigator_priv->get_takeoff_acceptance_radius()) {
+				_waypoint_position_reached = true;
+			}
+		} else {
+			/* for normal mission items used their acceptance radius */
+			if (dist >= 0.0f && dist <= _mission_item.acceptance_radius) {
+				_waypoint_position_reached = true;
+			}
+		}
+	}
+
+	if (_waypoint_position_reached && !_waypoint_yaw_reached) {
+
+		/* TODO: removed takeoff, why? */
+		if (_navigator_priv->get_vstatus()->is_rotary_wing && isfinite(_mission_item.yaw)) {
+
+			/* check yaw if defined only for rotary wing except takeoff */
+			float yaw_err = _wrap_pi(_mission_item.yaw - _navigator_priv->get_global_position()->yaw);
+
+			if (fabsf(yaw_err) < 0.2f) { /* TODO: get rid of magic number */
+				_waypoint_yaw_reached = true;
+			}
+
+		} else {
+			_waypoint_yaw_reached = true;
+		}
+	}
+
+	/* check if the current waypoint was reached */
+	if (_waypoint_position_reached && _waypoint_yaw_reached) {
+
+		if (_time_first_inside_orbit == 0) {
+			_time_first_inside_orbit = now;
+
+			// if (_mission_item.time_inside > 0.01f) {
+			// 	mavlink_log_info(_mavlink_fd, "#audio: waypoint reached, wait for %.1fs",
+			// 		(double)_mission_item.time_inside);
+			// }
+		}
+
+		/* check if the MAV was long enough inside the waypoint orbit */
+		if (now - _time_first_inside_orbit >= (hrt_abstime)_mission_item.time_inside * 1e6f) {
+			return true;
+		}
+	}
+	return false;
+}
+
+void
+MissionBlock::reset_mission_item_reached()
+{
+	_waypoint_position_reached = false;
+	_waypoint_yaw_reached = false;
+	_time_first_inside_orbit = 0;
+}
+
+void
+MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *item, struct position_setpoint_s *sp)
+{
+	sp->valid = true;
+	sp->lat = item->lat;
+	sp->lon = item->lon;
+	sp->alt = item->altitude_is_relative ? item->altitude + _navigator_priv->get_home_position()->alt : item->altitude;
+	sp->yaw = item->yaw;
+	sp->loiter_radius = item->loiter_radius;
+	sp->loiter_direction = item->loiter_direction;
+	sp->pitch_min = item->pitch_min;
+
+	if (item->nav_cmd == NAV_CMD_TAKEOFF) {
+		sp->type = SETPOINT_TYPE_TAKEOFF;
+
+	} else if (item->nav_cmd == NAV_CMD_LAND) {
+		sp->type = SETPOINT_TYPE_LAND;
+
+	} else if (item->nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
+		   item->nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
+		   item->nav_cmd == NAV_CMD_LOITER_UNLIMITED) {
+		sp->type = SETPOINT_TYPE_LOITER;
+
+	} else {
+		sp->type = SETPOINT_TYPE_POSITION;
+	}
+}
+
+bool
+MissionBlock::set_loiter_item(bool reuse_current_pos_sp, struct position_setpoint_triplet_s *pos_sp_triplet)
+{
+	if (_navigator_priv->get_is_in_loiter()) {
+		/* already loitering, bail out */
+		return false;
+	}
+
+	if (reuse_current_pos_sp && pos_sp_triplet->current.valid) {
+		/* leave position setpoint as is */
+	} else {
+		/* use current position */
+		pos_sp_triplet->current.lat = _navigator_priv->get_global_position()->lat;
+		pos_sp_triplet->current.lon = _navigator_priv->get_global_position()->lon;
+		pos_sp_triplet->current.alt = _navigator_priv->get_global_position()->alt;
+		pos_sp_triplet->current.yaw = NAN;	/* NAN means to use current yaw */
+	}
+	pos_sp_triplet->current.type = SETPOINT_TYPE_LOITER;
+	pos_sp_triplet->current.loiter_radius = _navigator_priv->get_loiter_radius();
+	pos_sp_triplet->current.loiter_direction = 1;
+
+	pos_sp_triplet->previous.valid = false;
+	pos_sp_triplet->current.valid = true;
+	pos_sp_triplet->next.valid = false;
+
+	_navigator_priv->set_is_in_loiter(true);
+	return true;
+}
+
diff --git a/src/modules/navigator/navigator_mission.h b/src/modules/navigator/mission_block.h
index b0f88e016..47e6fe81f 100644
--- a/src/modules/navigator/navigator_mission.h
+++ b/src/modules/navigator/mission_block.h
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *   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
@@ -31,75 +31,75 @@
  *
  ****************************************************************************/
 /**
- * @file navigator_mission.h
- * Helper class to access missions
+ * @file mission_block.h
  *
- * @author Julian Oes <joes@student.ethz.ch>
+ * Helper class to use mission items
+ *
+ * @author Julian Oes <julian@oes.ch>
  */
 
-#ifndef NAVIGATOR_MISSION_H
-#define NAVIGATOR_MISSION_H
+#ifndef NAVIGATOR_MISSION_BLOCK_H
+#define NAVIGATOR_MISSION_BLOCK_H
+
+#include <drivers/drv_hrt.h>
 
 #include <uORB/topics/mission.h>
-#include <uORB/topics/mission_result.h>
+#include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/position_setpoint_triplet.h>
 
+class Navigator;
 
-class __EXPORT Mission
+class MissionBlock
 {
 public:
 	/**
 	 * Constructor
+	 *
+	 * @param pointer to parent class
 	 */
-	Mission();
+	MissionBlock(Navigator *navigator);
 
 	/**
-	 * Destructor, also kills the sensors task.
+	 * Destructor
 	 */
-	~Mission();
+	virtual ~MissionBlock();
 
-	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_onboard_mission_allowed(bool allowed);
+	/**
+	 * Check if mission item has been reached
+	 * @return true if successfully reached
+	 */
+	bool is_mission_item_reached();
+	/**
+	 * Reset all reached flags
+	 */
+	void reset_mission_item_reached();
 
-	bool		current_mission_available();
-	bool		next_mission_available();
+	/**
+	 * Convert a mission item to a position setpoint
+	 *
+	 * @param the mission item to convert
+	 * @param the position setpoint that needs to be set
+	 */
+	void mission_item_to_position_setpoint(const mission_item_s *item, position_setpoint_s *sp);
 
-	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);
+	/**
+	 * Set a loiter item, if possible reuse the position setpoint, otherwise take the current position
+	 *
+	 * @param true if the current position setpoint should be re-used
+	 * @param the position setpoint triplet to set
+	 * @return true if setpoint has changed
+	 */
+	bool set_loiter_item(const bool reuse_current_pos_sp, position_setpoint_triplet_s *pos_sp_triplet);
 
-	void		move_to_next();
+	bool _waypoint_position_reached;
+	bool _waypoint_yaw_reached;
+	hrt_abstime _time_first_inside_orbit;
 
-	void		report_mission_item_reached();
-	void		report_current_offboard_mission_item();
-	void		publish_mission_result();
+	mission_item_s _mission_item;
+	bool _mission_item_valid;
 
 private:
-	bool		current_onboard_mission_available();
-	bool		current_offboard_mission_available();
-	bool		next_onboard_mission_available();
-	bool		next_offboard_mission_available();
-
-	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 */
-
-	bool		_onboard_mission_allowed;
-
-	enum {
-		MISSION_TYPE_NONE,
-		MISSION_TYPE_ONBOARD,
-		MISSION_TYPE_OFFBOARD,
-	} 		_current_mission_type;
-
-	int		_mission_result_pub;
-
-	struct mission_result_s _mission_result;
+	Navigator *_navigator_priv;
 };
 
 #endif
diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp
index e1a6854b2..dd7f4c801 100644
--- a/src/modules/navigator/mission_feasibility_checker.cpp
+++ b/src/modules/navigator/mission_feasibility_checker.cpp
@@ -215,11 +215,12 @@ bool MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size
 
 
 //	float slope_alt = wp_altitude + _H0 * expf(-math::max(0.0f, _flare_length - wp_distance)/_flare_constant) - _H1_virt;
+	return false;
 }
 
 void MissionFeasibilityChecker::updateNavigationCapabilities()
 {
-	int res = orb_copy(ORB_ID(navigation_capabilities), _capabilities_sub, &_nav_caps);
+	(void)orb_copy(ORB_ID(navigation_capabilities), _capabilities_sub, &_nav_caps);
 }
 
 void MissionFeasibilityChecker::init()
diff --git a/src/modules/navigator/navigator_state.h b/src/modules/navigator/mission_params.c
index 476f93414..8692328db 100644
--- a/src/modules/navigator/navigator_state.h
+++ b/src/modules/navigator/mission_params.c
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *   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
@@ -32,24 +32,38 @@
  ****************************************************************************/
 
 /**
- * @file navigator_state.h
+ * @file mission_params.c
  *
- * Navigator state
+ * Parameters for mission.
  *
- * @author Anton Babushkin <anton.babushkin@me.com>
+ * @author Julian Oes <joes@student.ethz.ch>
  */
 
-#ifndef NAVIGATOR_STATE_H_
-#define NAVIGATOR_STATE_H_
+#include <nuttx/config.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;
+#include <systemlib/param/param.h>
 
-#endif /* NAVIGATOR_STATE_H_ */
+/*
+ * Mission parameters, accessible via MAVLink
+ */
+
+/**
+ * Take-off altitude
+ *
+ * Even if first waypoint has altitude less then MIS_TAKEOFF_ALT above home position, system will climb to
+ * MIS_TAKEOFF_ALT on takeoff, then go to waypoint.
+ *
+ * @unit meters
+ * @group Mission
+ */
+PARAM_DEFINE_FLOAT(MIS_TAKEOFF_ALT, 10.0f);
+
+
+/**
+ * Enable onboard mission
+ *
+ * @min 0
+ * @max 1
+ * @group Mission
+ */
+PARAM_DEFINE_INT32(MIS_ONBOARD_EN, 0);
diff --git a/src/modules/navigator/module.mk b/src/modules/navigator/module.mk
index 6ea9dec2b..a1e109030 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,13 @@ MODULE_COMMAND	= navigator
 
 SRCS		= navigator_main.cpp \
 		  navigator_params.c \
-		  navigator_mission.cpp \
+		  navigator_mode.cpp \
+		  mission_block.cpp \
+		  mission.cpp \
+		  mission_params.c \
+		  loiter.cpp \
+		  rtl.cpp \
+		  rtl_params.c \
 		  mission_feasibility_checker.cpp \
 		  geofence.cpp \
 		  geofence_params.c
diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h
new file mode 100644
index 000000000..fe7485f56
--- /dev/null
+++ b/src/modules/navigator/navigator.h
@@ -0,0 +1,211 @@
+/***************************************************************************
+ *
+ *   Copyright (c) 2013-2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+/**
+ * @file navigator.h
+ * Helper class to access missions
+ * @author Julian Oes <julian@oes.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ */
+
+#ifndef NAVIGATOR_H
+#define NAVIGATOR_H
+
+#include <systemlib/perf_counter.h>
+
+#include <controllib/blocks.hpp>
+#include <controllib/block/BlockParam.hpp>
+
+#include <uORB/uORB.h>
+#include <uORB/topics/mission.h>
+#include <uORB/topics/vehicle_control_mode.h>
+#include <uORB/topics/position_setpoint_triplet.h>
+#include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/parameter_update.h>
+
+#include "navigator_mode.h"
+#include "mission.h"
+#include "loiter.h"
+#include "rtl.h"
+#include "geofence.h"
+
+class Navigator : public control::SuperBlock
+{
+public:
+	/**
+	 * Constructor
+	 */
+	Navigator();
+
+	/**
+	 * Destructor, also kills the navigators task.
+	 */
+	~Navigator();
+
+	/**
+	 * Start the navigator task.
+	 *
+	 * @return		OK on success.
+	 */
+	int		start();
+
+	/**
+	 * Display the navigator status.
+	 */
+	void		status();
+
+	/**
+	 * Add point to geofence
+	 */
+	void		add_fence_point(int argc, char *argv[]);
+
+	/**
+	 * Load fence from file
+	 */
+	void		load_fence_from_file(const char *filename);
+
+	/**
+	 * Setters
+	 */
+	void		set_is_in_loiter(bool is_in_loiter) { _is_in_loiter = is_in_loiter; }
+
+	/**
+	 * Getters
+	 */
+	struct vehicle_status_s*	    get_vstatus() { return &_vstatus; }
+	struct vehicle_global_position_s*   get_global_position() { return &_global_pos; }
+	struct home_position_s*		    get_home_position() { return &_home_pos; }
+	int		get_onboard_mission_sub() { return _onboard_mission_sub; }
+	int		get_offboard_mission_sub() { return _offboard_mission_sub; }
+	Geofence&	get_geofence() { return _geofence; }
+	bool		get_is_in_loiter() { return _is_in_loiter; }
+	float		get_loiter_radius() { return _param_loiter_radius.get(); }
+	float		get_takeoff_acceptance_radius() { return _param_takeoff_acceptance_radius.get(); }
+	int		get_mavlink_fd() { return _mavlink_fd; }
+
+private:
+
+	bool		_task_should_exit;		/**< if true, sensor task should exit */
+	int		_navigator_task;		/**< task handle for sensor task */
+
+	int		_mavlink_fd;			/**< the file descriptor to send messages over mavlink */
+
+	int		_global_pos_sub;		/**< global position subscription */
+	int		_home_pos_sub;			/**< home position subscription */
+	int		_vstatus_sub;			/**< vehicle status subscription */
+	int		_capabilities_sub;		/**< notification of vehicle capabilities updates */
+	int		_control_mode_sub;		/**< vehicle control mode subscription */
+	int		_onboard_mission_sub;		/**< onboard mission subscription */
+	int		_offboard_mission_sub;		/**< offboard mission subscription */
+	int		_param_update_sub;		/**< param update subscription */
+
+	orb_advert_t	_pos_sp_triplet_pub;		/**< publish position setpoint triplet */
+
+	vehicle_status_s				_vstatus;		/**< vehicle status */
+	vehicle_control_mode_s				_control_mode;		/**< vehicle control mode */
+	vehicle_global_position_s			_global_pos;		/**< global vehicle position */
+	home_position_s					_home_pos;		/**< home position for RTL */
+	mission_item_s 					_mission_item;		/**< current mission item */
+	navigation_capabilities_s			_nav_caps;		/**< navigation capabilities */
+	position_setpoint_triplet_s			_pos_sp_triplet;	/**< triplet of position setpoints */
+
+	bool 		_mission_item_valid;		/**< flags if the current mission item is valid */
+
+	perf_counter_t	_loop_perf;			/**< loop performance counter */
+
+	Geofence	_geofence;			/**< class that handles the geofence */
+	bool		_geofence_violation_warning_sent; /**< prevents spaming to mavlink */
+
+	bool		_fence_valid;			/**< flag if fence is valid */
+	bool		_inside_fence;			/**< vehicle is inside fence */
+
+	NavigatorMode	*_navigation_mode;		/**< abstract pointer to current navigation mode class */
+	Mission		_mission;			/**< class that handles the missions */
+	Loiter		_loiter;			/**< class that handles loiter */
+	RTL 		_rtl;				/**< class that handles RTL */
+
+	bool		_is_in_loiter;			/**< flags if current position SP can be used to loiter */
+	bool		_update_triplet;		/**< flags if position SP triplet needs to be published */
+
+	control::BlockParamFloat _param_loiter_radius;	/**< loiter radius for fixedwing */
+	control::BlockParamFloat _param_takeoff_acceptance_radius;	/**< acceptance for takeoff */
+	/**
+	 * Retrieve global position
+	 */
+	void		global_position_update();
+
+	/**
+	 * Retrieve home position
+	 */
+	void		home_position_update();
+
+	/**
+	 * Retreive navigation capabilities
+	 */
+	void		navigation_capabilities_update();
+
+	/**
+	 * Retrieve vehicle status
+	 */
+	void		vehicle_status_update();
+
+	/**
+	 * Retrieve vehicle control mode
+	 */
+	void		vehicle_control_mode_update();
+
+	/**
+	 * Update parameters
+	 */
+	void		params_update();
+
+	/**
+	 * Shim for calling task_main from task_create.
+	 */
+	static void	task_main_trampoline(int argc, char *argv[]);
+
+	/**
+	 * Main task.
+	 */
+	void		task_main();
+
+	/**
+	 * Translate mission item to a position setpoint.
+	 */
+	void		mission_item_to_position_setpoint(const mission_item_s *item, position_setpoint_s *sp);
+
+	/**
+	 * Publish a new position setpoint triplet for position controllers
+	 */
+	void		publish_position_setpoint_triplet();
+};
+#endif
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index c7b39ba93..459fbdc65 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -32,18 +32,18 @@
  ****************************************************************************/
 /**
  * @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>
@@ -54,42 +54,28 @@
 #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>
-#include <uORB/topics/position_setpoint_triplet.h>
 #include <uORB/topics/vehicle_status.h>
-#include <uORB/topics/vehicle_control_mode.h>
-#include <uORB/topics/parameter_update.h>
 #include <uORB/topics/mission.h>
 #include <uORB/topics/fence.h>
 #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_feasibility_checker.h"
-#include "geofence.h"
-
 
-/* oddly, ERROR is not defined for c++ */
-#ifdef ERROR
-# undef ERROR
-#endif
-static const int ERROR = -1;
+#include "navigator.h"
 
 /**
  * navigator app start / stop handling function
@@ -98,342 +84,48 @@ static const int ERROR = -1;
  */
 extern "C" __EXPORT int navigator_main(int argc, char *argv[]);
 
-class Navigator : public StateTable
-{
-public:
-	/**
-	 * Constructor
-	 */
-	Navigator();
-
-	/**
-	 * Destructor, also kills the navigators task.
-	 */
-	~Navigator();
-
-	/**
-	* Start the navigator task.
-	 *
-	 * @return		OK on success.
-	 */
-	int		start();
-
-	/**
-	 * Display the navigator status.
-	 */
-	void		status();
-
-	/**
-	 * Add point to geofence
-	 */
-	void add_fence_point(int argc, char *argv[]);
-
-	/**
-	 * Load fence from file
-	 */
-	void load_fence_from_file(const char *filename);
-
-private:
-
-	bool		_task_should_exit;		/**< if true, sensor task should exit */
-	int		_navigator_task;		/**< task handle for sensor task */
-
-	int		_mavlink_fd;
-
-	int		_global_pos_sub;		/**< global position subscription */
-	int		_home_pos_sub;			/**< home position subscription */
-	int		_vstatus_sub;			/**< vehicle status subscription */
-	int		_params_sub;			/**< notification of parameter updates */
-	int		_offboard_mission_sub;		/**< notification of offboard mission updates */
-	int 		_onboard_mission_sub;		/**< notification of onboard mission updates */
-	int		_capabilities_sub;		/**< notification of vehicle capabilities updates */
-	int		_control_mode_sub;		/**< vehicle control mode subscription */
-
-	orb_advert_t	_pos_sp_triplet_pub;		/**< publish position setpoint triplet */
-	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 */
-
-	perf_counter_t	_loop_perf;			/**< loop performance counter */
-
-	Geofence					_geofence;
-	bool						_geofence_violation_warning_sent;
-
-	bool						_fence_valid;		/**< flag if fence is valid */
-	bool						_inside_fence;		/**< vehicle is inside fence */
-
-	struct navigation_capabilities_s		_nav_caps;
-
-	class 		Mission				_mission;
-
-	bool		_mission_item_valid;		/**< current mission item valid */
-	bool		_global_pos_valid;		/**< track changes of global_position */
-	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];
-
-	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_MISSION_CHANGED,
-		EVENT_HOME_POSITION_CHANGED,
-		MAX_EVENT
-	};
-
-	/**
-	 * State machine transition table
-	 */
-	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.
-	 */
-	void		parameters_update();
-
-	/**
-	 * Retrieve global position
-	 */
-	void		global_position_update();
-
-	/**
-	 * Retrieve home position
-	 */
-	void		home_position_update();
-
-	/**
-	 * Retreive navigation capabilities
-	 */
-	void		navigation_capabilities_update();
-
-	/**
-	 * Retrieve offboard mission.
-	 */
-	void		offboard_mission_update(bool isrotaryWing);
-
-	/**
-	 * Retrieve onboard mission.
-	 */
-	void		onboard_mission_update();
-
-	/**
-	 * Retrieve vehicle status
-	 */
-	void		vehicle_status_update();
-
-	/**
-	 * Retrieve vehicle control mode
-	 */
-	void		vehicle_control_mode_update();
-
-	/**
-	 * Shim for calling task_main from task_create.
-	 */
-	static void	task_main_trampoline(int argc, char *argv[]);
-
-	/**
-	 * Main task.
-	 */
-	void		task_main();
-
-	void		publish_safepoints(unsigned points);
-
-	/**
-	 * 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();
-
-	/**
-	 * Fork for state transitions
-	 */
-	void		request_loiter_or_ready();
-	void		request_mission_if_available();
-
-	/**
-	 * Guards offboard mission
-	 */
-	bool		offboard_mission_available(unsigned relative_index);
-
-	/**
-	 * Guards onboard mission
-	 */
-	bool		onboard_mission_available(unsigned relative_index);
-
-	/**
-	 * Reset all "reached" flags.
-	 */
-	void		reset_reached();
-
-	/**
-	 * Check if current mission item has been reached.
-	 */
-	bool		check_mission_item_reached();
-
-	/**
-	 * Perform actions when current mission item reached.
-	 */
-	void		on_mission_item_reached();
-
-	/**
-	 * Move to next waypoint
-	 */
-	void		set_mission_item();
-
-	/**
-	 * Switch to next RTL state
-	 */
-	void		set_rtl_item();
-
-	/**
-	 * Set position setpoint for mission item
-	 */
-	void		position_setpoint_from_mission_item(position_setpoint_s *sp, mission_item_s *item);
-
-	/**
-	 * Helper function to get a takeoff item
-	 */
-	void		get_takeoff_setpoint(position_setpoint_s *pos_sp);
-
-	/**
-	 * Publish a new mission item triplet for position controller
-	 */
-	void		publish_position_setpoint_triplet();
-};
 
 namespace navigator
 {
 
-/* oddly, ERROR is not defined for c++ */
-#ifdef ERROR
-# undef ERROR
-#endif
-static const int ERROR = -1;
-
 Navigator	*g_navigator;
 }
 
 Navigator::Navigator() :
-
-/* state machine transition table */
-	StateTable(&myTable[0][0], NAV_STATE_MAX, MAX_EVENT),
-
+	SuperBlock(NULL, "NAV"),
 	_task_should_exit(false),
 	_navigator_task(-1),
 	_mavlink_fd(-1),
-
-/* subscriptions */
 	_global_pos_sub(-1),
 	_home_pos_sub(-1),
 	_vstatus_sub(-1),
-	_params_sub(-1),
-	_offboard_mission_sub(-1),
-	_onboard_mission_sub(-1),
 	_capabilities_sub(-1),
 	_control_mode_sub(-1),
-
-/* publications */
+	_onboard_mission_sub(-1),
+	_offboard_mission_sub(-1),
 	_pos_sp_triplet_pub(-1),
-
-/* performance counters */
+	_vstatus({}),
+	_control_mode({}),
+	_global_pos({}),
+	_home_pos({}),
+	_mission_item({}),
+	_nav_caps({}),
+	_pos_sp_triplet({}),
+	_mission_item_valid(false),
 	_loop_perf(perf_alloc(PC_ELAPSED, "navigator")),
-
+	_geofence({}),
 	_geofence_violation_warning_sent(false),
 	_fence_valid(false),
 	_inside_fence(true),
-	_mission(),
-	_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)
+	_navigation_mode(nullptr),
+	_mission(this, "MIS"),
+	_loiter(this, "LOI"),
+	_rtl(this, "RTL"),
+	_update_triplet(false),
+	_param_loiter_radius(this, "LOITER_RAD"),
+	_param_takeoff_acceptance_radius(this, "TF_ACC_RAD")
 {
-	_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();
+	updateParams();
 }
 
 Navigator::~Navigator()
@@ -462,27 +154,6 @@ Navigator::~Navigator()
 }
 
 void
-Navigator::parameters_update()
-{
-	/* read from param to clear updated flag */
-	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));
-
-	_mission.set_onboard_mission_allowed((bool)_parameter_handles.onboard_mission_enabled);
-
-	_geofence.updateParams();
-}
-
-void
 Navigator::global_position_update()
 {
 	orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos);
@@ -500,56 +171,6 @@ Navigator::navigation_capabilities_update()
 	orb_copy(ORB_ID(navigation_capabilities), _capabilities_sub, &_nav_caps);
 }
 
-
-void
-Navigator::offboard_mission_update(bool isrotaryWing)
-{
-	struct mission_s offboard_mission;
-
-	if (orb_copy(ORB_ID(offboard_mission), _offboard_mission_sub, &offboard_mission) == OK) {
-
-		/* Check mission feasibility, for now do not handle the return value,
-		 * however warnings are issued to the gcs via mavlink from inside the MissionFeasiblityChecker */
-		dm_item_t dm_current;
-
-		if (offboard_mission.dataman_id == 0) {
-			dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0;
-
-		} else {
-			dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1;
-		}
-
-		missionFeasiblityChecker.checkMissionFeasible(isrotaryWing, dm_current, (size_t)offboard_mission.count, _geofence, _home_pos.alt);
-
-		_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);
-
-	} else {
-		_mission.set_offboard_mission_count(0);
-		_mission.set_current_offboard_mission_index(0);
-	}
-
-	_mission.publish_mission_result();
-}
-
-void
-Navigator::onboard_mission_update()
-{
-	struct mission_s onboard_mission;
-
-	if (orb_copy(ORB_ID(onboard_mission), _onboard_mission_sub, &onboard_mission) == OK) {
-
-		_mission.set_onboard_mission_count(onboard_mission.count);
-		_mission.set_current_onboard_mission_index(onboard_mission.current_index);
-
-	} else {
-		_mission.set_onboard_mission_count(0);
-		_mission.set_current_onboard_mission_index(0);
-	}
-}
-
 void
 Navigator::vehicle_status_update()
 {
@@ -570,6 +191,13 @@ Navigator::vehicle_control_mode_update()
 }
 
 void
+Navigator::params_update()
+{
+	parameter_update_s param_update;
+	orb_copy(ORB_ID(parameter_update), _param_update_sub, &param_update);
+}
+
+void
 Navigator::task_main_trampoline(int argc, char *argv[])
 {
 	navigator::g_navigator->task_main();
@@ -580,16 +208,12 @@ Navigator::task_main()
 {
 	/* inform about start */
 	warnx("Initializing..");
-	fflush(stdout);
 
 	_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
 
-	mavlink_log_info(_mavlink_fd, "[navigator] started");
-
 	/* Try to load the geofence:
 	 * if /fs/microsd/etc/geofence.txt load from this file
-	 * else clear geofence data in datamanager
-	 */
+	 * else clear geofence data in datamanager */
 	struct stat buffer;
 
 	if (stat(GEOFENCE_FILENAME, &buffer) == 0) {
@@ -610,68 +234,58 @@ Navigator::task_main()
 			_mission.set_offboard_dataman_id(sys_state.offboard_waypoint_id);
 	}
 
-	/*
-	 * do subscriptions
-	 */
+	/* do subscriptions */
 	_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
-	_offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission));
-	_onboard_mission_sub = orb_subscribe(ORB_ID(onboard_mission));
 	_capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities));
 	_vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
 	_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
-	_params_sub = orb_subscribe(ORB_ID(parameter_update));
 	_home_pos_sub = orb_subscribe(ORB_ID(home_position));
+	_onboard_mission_sub = orb_subscribe(ORB_ID(onboard_mission));
+	_offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission));
+	_param_update_sub = orb_subscribe(ORB_ID(parameter_update));
 
 	/* copy all topics first time */
 	vehicle_status_update();
 	vehicle_control_mode_update();
-	parameters_update();
 	global_position_update();
 	home_position_update();
 	navigation_capabilities_update();
-	offboard_mission_update(_vstatus.is_rotary_wing);
-	onboard_mission_update();
+	params_update();
 
 	/* rate limit position updates to 50 Hz */
 	orb_set_interval(_global_pos_sub, 20);
 
-	unsigned prevState = NAV_STATE_NONE;
 	hrt_abstime mavlink_open_time = 0;
 	const hrt_abstime mavlink_open_interval = 500000;
 
 	/* wakeup source(s) */
-	struct pollfd fds[8];
+	struct pollfd fds[6];
 
 	/* Setup of loop */
-	fds[0].fd = _params_sub;
+	fds[0].fd = _global_pos_sub;
 	fds[0].events = POLLIN;
-	fds[1].fd = _global_pos_sub;
+	fds[1].fd = _home_pos_sub;
 	fds[1].events = POLLIN;
-	fds[2].fd = _home_pos_sub;
+	fds[2].fd = _capabilities_sub;
 	fds[2].events = POLLIN;
-	fds[3].fd = _capabilities_sub;
+	fds[3].fd = _vstatus_sub;
 	fds[3].events = POLLIN;
-	fds[4].fd = _offboard_mission_sub;
+	fds[4].fd = _control_mode_sub;
 	fds[4].events = POLLIN;
-	fds[5].fd = _onboard_mission_sub;
+	fds[5].fd = _param_update_sub;
 	fds[5].events = POLLIN;
-	fds[6].fd = _vstatus_sub;
-	fds[6].events = POLLIN;
-	fds[7].fd = _control_mode_sub;
-	fds[7].events = POLLIN;
 
 	while (!_task_should_exit) {
 
 		/* wait for up to 100ms for data */
 		int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
 
-		/* timed out - periodic check for _task_should_exit, etc. */
 		if (pret == 0) {
+			/* timed out - periodic check for _task_should_exit, etc. */
 			continue;
-		}
 
-		/* this is undesirable but not much we can do - might want to flag unhappy status */
-		if (pret < 0) {
+		} else if (pret < 0) {
+			/* this is undesirable but not much we can do - might want to flag unhappy status */
 			warn("poll error %d, %d", pret, errno);
 			continue;
 		}
@@ -684,162 +298,112 @@ Navigator::task_main()
 			_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
 		}
 
+		/* parameters updated */
+		if (fds[5].revents & POLLIN) {
+			params_update();
+			updateParams();
+		}
+
 		/* vehicle control mode updated */
-		if (fds[7].revents & POLLIN) {
+		if (fds[4].revents & POLLIN) {
 			vehicle_control_mode_update();
 		}
 
 		/* vehicle status updated */
-		if (fds[6].revents & POLLIN) {
+		if (fds[3].revents & POLLIN) {
 			vehicle_status_update();
-
-			/* evaluate state requested by commander */
-			if (_control_mode.flag_armed && _control_mode.flag_control_auto_enabled) {
-				/* publish position setpoint triplet on each status update if navigator active */
-				_pos_sp_triplet_updated = true;
-
-				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();
-					}
-				}
-
-				/* 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();
-					}
-				}
-
-			} else {
-				/* navigator shouldn't act */
-				dispatch(EVENT_NONE_REQUESTED);
-			}
-
-			_set_nav_state_timestamp = _vstatus.set_nav_state_timestamp;
-		}
-
-		/* parameters updated */
-		if (fds[0].revents & POLLIN) {
-			parameters_update();
-			/* note that these new parameters won't be in effect until a mission triplet is published again */
 		}
 
 		/* navigation capabilities updated */
-		if (fds[3].revents & POLLIN) {
+		if (fds[2].revents & POLLIN) {
 			navigation_capabilities_update();
 		}
 
-		/* offboard mission updated */
-		if (fds[4].revents & POLLIN) {
-			offboard_mission_update(_vstatus.is_rotary_wing);
-			// XXX 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
-			dispatch(EVENT_MISSION_CHANGED);
-		}
-
 		/* home position updated */
-		if (fds[2].revents & POLLIN) {
+		if (fds[1].revents & POLLIN) {
 			home_position_update();
-			// XXX check if home position really changed
-			dispatch(EVENT_HOME_POSITION_CHANGED);
 		}
 
 		/* global position updated */
-		if (fds[1].revents & POLLIN) {
+		if (fds[0].revents & POLLIN) {
 			global_position_update();
 
-			if (_control_mode.flag_armed && _control_mode.flag_control_auto_enabled) {
-				/* publish position setpoint triplet on each position update if navigator active */
-				_pos_sp_triplet_updated = true;
-
-				if (myState == NAV_STATE_LAND && !_global_pos_valid) {
-					/* got global position when landing, update setpoint */
-					start_land();
-				}
-
-				/* 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();
-					}
-				}
-			}
-
 			/* 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;
 			}
 		}
 
-		_global_pos_valid = _vstatus.condition_global_position_valid;
+		/* Do stuff according to navigation state set by commander */
+		switch (_vstatus.set_nav_state) {
+			case NAVIGATION_STATE_MANUAL:
+			case NAVIGATION_STATE_ACRO:
+			case NAVIGATION_STATE_ALTCTL:
+			case NAVIGATION_STATE_POSCTL:
+				_navigation_mode = nullptr;
+				_is_in_loiter = false;
+				break;
+			case NAVIGATION_STATE_AUTO_MISSION:
+				_navigation_mode = &_mission;
+				break;
+			case NAVIGATION_STATE_AUTO_LOITER:
+				_navigation_mode = &_loiter;
+				break;
+			case NAVIGATION_STATE_AUTO_RTL:
+			case NAVIGATION_STATE_AUTO_RTL_RC:
+			case NAVIGATION_STATE_AUTO_RTL_DL:
+				_navigation_mode = &_rtl;
+				break;
+			case NAVIGATION_STATE_LAND:
+			case NAVIGATION_STATE_TERMINATION:
+			default:
+				_navigation_mode = nullptr;
+				_is_in_loiter = false;
+				break;
+		}
 
-		/* publish position setpoint triplet if updated */
-		if (_pos_sp_triplet_updated) {
-			_pos_sp_triplet_updated = false;
-			publish_position_setpoint_triplet();
+		/* TODO: make list of modes and loop through it */
+		if (_navigation_mode == &_mission) {
+			_update_triplet = _mission.update(&_pos_sp_triplet);
+		} else {
+			_mission.reset();
 		}
 
-		/* notify user about state changes */
-		if (myState != prevState) {
-			mavlink_log_info(_mavlink_fd, "#audio: navigation state: %s", nav_states_str[myState]);
-			prevState = myState;
+		if (_navigation_mode == &_rtl) {
+			_update_triplet = _rtl.update(&_pos_sp_triplet);
+		} else {
+			_rtl.reset();
+		}
+
+		if (_navigation_mode == &_loiter) {
+			_update_triplet = _loiter.update(&_pos_sp_triplet);
+		} else {
+			_loiter.reset();
+		}
+
+		/* if nothing is running, set position setpoint triplet invalid */
+		if (_navigation_mode == nullptr) {
+			_pos_sp_triplet.previous.valid = false;
+			_pos_sp_triplet.current.valid = false;
+			_pos_sp_triplet.next.valid = false;
+			_update_triplet = true;
+		}
+
+		if (_update_triplet ) {
+			publish_position_setpoint_triplet();
+			_update_triplet = false;
 		}
 
 		perf_end(_loop_perf);
 	}
-
 	warnx("exiting.");
 
 	_navigator_task = -1;
@@ -870,19 +434,21 @@ Navigator::start()
 void
 Navigator::status()
 {
-	warnx("Global position: %svalid", _global_pos_valid ? "" : "in");
-
-	if (_global_pos_valid) {
-		warnx("Longitude %5.5f degrees, latitude %5.5f degrees", _global_pos.lon, _global_pos.lat);
-		warnx("Altitude %5.5f meters, altitude above home %5.5f meters",
-		      (double)_global_pos.alt, (double)(_global_pos.alt - _home_pos.alt));
-		warnx("Ground velocity in m/s, N %5.5f, E %5.5f, D %5.5f",
-		      (double)_global_pos.vel_n, (double)_global_pos.vel_e, (double)_global_pos.vel_d);
-		warnx("Compass heading in degrees %5.5f", (double)(_global_pos.yaw * M_RAD_TO_DEG_F));
-	}
+	/* TODO: add this again */
+	// warnx("Global position is %svalid", _global_pos_valid ? "" : "in");
+
+	// if (_global_pos.global_valid) {
+	// 	warnx("Longitude %5.5f degrees, latitude %5.5f degrees", _global_pos.lon, _global_pos.lat);
+	// 	warnx("Altitude %5.5f meters, altitude above home %5.5f meters",
+	// 	      (double)_global_pos.alt, (double)(_global_pos.alt - _home_pos.alt));
+	// 	warnx("Ground velocity in m/s, N %5.5f, E %5.5f, D %5.5f",
+	// 	      (double)_global_pos.vel_n, (double)_global_pos.vel_e, (double)_global_pos.vel_d);
+	// 	warnx("Compass heading in degrees %5.5f", (double)(_global_pos.yaw * M_RAD_TO_DEG_F));
+	// }
 
 	if (_fence_valid) {
 		warnx("Geofence is valid");
+		/* TODO: needed? */
 //		warnx("Vertex longitude latitude");
 //		for (unsigned i = 0; i < _fence.count; i++)
 //		warnx("%6u %9.5f %8.5f", i, (double)_fence.vertices[i].lon, (double)_fence.vertices[i].lat);
@@ -890,118 +456,36 @@ Navigator::status()
 	} else {
 		warnx("Geofence not set");
 	}
+}
+#if 0
+bool
+Navigator::start_none_on_ground()
+{
+	reset_reached();
 
-	switch (myState) {
-	case NAV_STATE_NONE:
-		warnx("State: None");
-		break;
-
-	case NAV_STATE_LOITER:
-		warnx("State: Loiter");
-		break;
-
-	case NAV_STATE_MISSION:
-		warnx("State: Mission");
-		break;
-
-	case NAV_STATE_RTL:
-		warnx("State: RTL");
-		break;
+	_pos_sp_triplet.previous.valid = false;
+	_pos_sp_triplet.current.valid = false;
+	_pos_sp_triplet.next.valid = false;
 
-	default:
-		warnx("State: Unknown");
-		break;
-	}
+	_update_triplet = true;
+	return true;
 }
 
-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},
-		/* 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},
-	},
-	{
-		/* 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_LOITER */
-		/* EVENT_NONE_REQUESTED */		{ACTION(&Navigator::start_none), NAV_STATE_NONE},
-		/* EVENT_READY_REQUESTED */		{NO_ACTION, NAV_STATE_LOITER},
-		/* 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_MISSION_CHANGED */		{NO_ACTION, NAV_STATE_LOITER},
-		/* EVENT_HOME_POSITION_CHANGED */	{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_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_MISSION_CHANGED */		{ACTION(&Navigator::start_mission), NAV_STATE_MISSION},
-		/* EVENT_HOME_POSITION_CHANGED */	{NO_ACTION, 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_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_MISSION_CHANGED */		{NO_ACTION, NAV_STATE_RTL},
-		/* EVENT_HOME_POSITION_CHANGED */	{ACTION(&Navigator::start_rtl), NAV_STATE_RTL},	// TODO need to reset rtl_state
-	},
-	{
-		/* NAV_STATE_LAND */
-		/* EVENT_NONE_REQUESTED */		{ACTION(&Navigator::start_none), NAV_STATE_NONE},
-		/* EVENT_READY_REQUESTED */		{ACTION(&Navigator::start_ready), NAV_STATE_READY},
-		/* 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_MISSION_CHANGED */		{NO_ACTION, NAV_STATE_LAND},
-		/* EVENT_HOME_POSITION_CHANGED */	{NO_ACTION, NAV_STATE_LAND},
-	},
-};
-
-void
-Navigator::start_none()
+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;
-	_mission_item_valid = false;
 
-	_reset_loiter_pos = true;
-	_do_takeoff = false;
-	_rtl_state = RTL_STATE_NONE;
-
-	_pos_sp_triplet_updated = true;
+	_update_triplet = true;
+	return true;
 }
 
-void
-Navigator::start_ready()
+bool
+Navigator::start_auto_on_ground()
 {
 	reset_reached();
 
@@ -1011,46 +495,20 @@ 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;
-	}
-
-	_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;
@@ -1058,167 +516,142 @@ 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();
+		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) {
+		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));
 
-					/* set current setpoint to takeoff */
+	mission_item_s next_mission_item;
 
-					_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;
-				}
+	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;
+	}
 
-			} else if (_mission_item.nav_cmd == NAV_CMD_LAND) {
-				/* will need takeoff after landing */
-				_need_takeoff = true;
-			}
-		}
+	/* notify user about what happened */
+	mavlink_log_info(_mavlink_fd, "#audio: heading to %s%swaypoint %d",
+		(last_wp ? "last " : "" ), (onboard ? "onboard " : ""), index);
 
-		if (_do_takeoff) {
-			mavlink_log_info(_mavlink_fd, "#audio: takeoff to %.1fm above home", (double)(_pos_sp_triplet.current.alt - _home_pos.alt));
+	_update_triplet = true;
 
-		} else {
-			if (onboard) {
-				mavlink_log_info(_mavlink_fd, "#audio: heading to onboard WP %d", index);
+	reset_reached();
 
-			} else {
-				mavlink_log_info(_mavlink_fd, "#audio: heading to offboard WP %d", index);
-			}
-		}
+	return true;
+}
 
-	} 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");
-	}
+bool
+Navigator::start_rtl()
+{
+	if (_rtl.get_current_rtl_item(&_global_pos, &_mission_item)) {
 
-	if (!_do_takeoff) {
-		mission_item_s item_next;
-		ret = _mission.get_next_mission_item(&item_next);
+		_mission_item_valid = true;
 
-		if (ret == OK) {
-			position_setpoint_from_mission_item(&_pos_sp_triplet.next, &item_next);
+		mission_item_to_position_setpoint(&_mission_item, &_pos_sp_triplet.current);
+		_pos_sp_triplet.current.valid = true;
 
-		} else {
-			/* this will fail for the last WP */
-			_pos_sp_triplet.next.valid = false;
-		}
+		reset_reached();
+
+		_update_triplet = true;
+		return true;
 	}
 
-	_pos_sp_triplet_updated = true;
+	/* if RTL doesn't work, fallback to loiter */
+	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;
+
+		mission_item_to_position_setpoint(&_mission_item, &_pos_sp_triplet.current);
+		_pos_sp_triplet.current.valid = true;
+
+		reset_reached();
 
-	/* 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;
+		_update_triplet = true;
+		return true;
 	}
 
-	_reset_loiter_pos = true;
-	set_rtl_item();
+	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;
@@ -1233,224 +666,15 @@ 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_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: 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_LOITER_TIME_LIMIT;
-			_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;
+	_update_triplet = true;
+	return 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();
-	}
-}
-
-void
-Navigator::position_setpoint_from_mission_item(position_setpoint_s *sp, mission_item_s *item)
-{
-	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 */
-			sp->yaw = get_bearing_to_next_waypoint(_pos_sp_triplet.previous.lat, _pos_sp_triplet.previous.lon, sp->lat, sp->lon);
-
-		} else {
-			/* else use current position */
-			sp->yaw = get_bearing_to_next_waypoint(_global_pos.lat, _global_pos.lon, sp->lat, sp->lon);
-		}
-		sp->loiter_radius = _parameters.loiter_radius;
-		sp->loiter_direction = 1;
-		sp->pitch_min = 0.0f;
-
-	} else {
-		sp->lat = item->lat;
-		sp->lon = item->lon;
-		sp->alt = item->altitude_is_relative ? item->altitude + _home_pos.alt : item->altitude;
-		sp->yaw = item->yaw;
-		sp->loiter_radius = item->loiter_radius;
-		sp->loiter_direction = item->loiter_direction;
-		sp->pitch_min = item->pitch_min;
-	}
-
-	if (item->nav_cmd == NAV_CMD_TAKEOFF) {
-		sp->type = SETPOINT_TYPE_TAKEOFF;
-
-	} else if (item->nav_cmd == NAV_CMD_LAND) {
-		sp->type = SETPOINT_TYPE_LAND;
-
-	} else if (item->nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
-		   item->nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
-		   item->nav_cmd == NAV_CMD_LOITER_UNLIMITED) {
-		sp->type = SETPOINT_TYPE_LOITER;
-
-	} else {
-		sp->type = SETPOINT_TYPE_NORMAL;
-	}
-}
-
 bool
 Navigator::check_mission_item_reached()
 {
@@ -1468,8 +692,8 @@ Navigator::check_mission_item_reached()
 	     _mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED) &&
 	    _mission_item.loiter_radius > 0.01f) {
 
-		return false;
-	}
+	// 	return false;
+	// }
 
 	uint64_t now = hrt_absolute_time();
 
@@ -1488,7 +712,6 @@ Navigator::check_mission_item_reached()
 			if (_global_pos.alt > _pos_sp_triplet.current.alt - acceptance_radius) {
 				_waypoint_position_reached = true;
 			}
-
 		} else {
 			float dist = -1.0f;
 			float dist_xy = -1.0f;
@@ -1511,11 +734,14 @@ 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);
 
-			if (fabsf(yaw_err) < 0.2f) { /* XXX get rid of magic number */
+			if (fabsf(yaw_err) < 0.2f) { /* TODO: get rid of magic number */
 				_waypoint_yaw_reached = true;
 			}
 
@@ -1526,24 +752,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
@@ -1554,83 +779,18 @@ Navigator::reset_reached()
 	_waypoint_yaw_reached = false;
 
 }
-
-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()) {
-			if (_mission_item.autocontinue) {
-				/* continue mission */
-				set_mission_item();
-
-			} else {
-				/* autocontinue disabled for this item */
-				request_loiter_or_ready();
-			}
-
-		} 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();
-}
-
+#endif
 void
 Navigator::publish_position_setpoint_triplet()
 {
 	/* update navigation state */
-	_pos_sp_triplet.nav_state = static_cast<nav_state_t>(myState);
+	/* TODO: set nav_state */
 
 	/* 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_mission.cpp b/src/modules/navigator/navigator_mission.cpp
deleted file mode 100644
index 49fc62785..000000000
--- a/src/modules/navigator/navigator_mission.cpp
+++ /dev/null
@@ -1,319 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-/**
- * @file navigator_mission.cpp
- * Helper class to access missions
- *
- * @author Julian Oes <joes@student.ethz.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;
-
-
-Mission::Mission() :
-
-	_offboard_dataman_id(-1),
-	_current_offboard_mission_index(0),
-	_current_onboard_mission_index(0),
-	_offboard_mission_item_count(0),
-	_onboard_mission_item_count(0),
-	_onboard_mission_allowed(false),
-	_current_mission_type(MISSION_TYPE_NONE),
-	_mission_result_pub(-1)
-{
-	memset(&_mission_result, 0, sizeof(struct mission_result_s));
-}
-
-Mission::~Mission()
-{
-
-}
-
-void
-Mission::set_offboard_dataman_id(int new_id)
-{
-	_offboard_dataman_id = new_id;
-}
-
-void
-Mission::set_current_offboard_mission_index(int new_index)
-{
-	if (new_index != -1) {
-		warnx("specifically set to %d", new_index);
-		_current_offboard_mission_index = (unsigned)new_index;
-	} else {
-
-		/* if less WPs available, reset to first WP */
-		if (_current_offboard_mission_index >= _offboard_mission_item_count) {
-			_current_offboard_mission_index = 0;
-		}
-	}
-	report_current_offboard_mission_item();
-}
-
-void
-Mission::set_current_onboard_mission_index(int new_index)
-{
-	if (new_index != -1) {
-		_current_onboard_mission_index = (unsigned)new_index;
-	} else {
-
-		/* if less WPs available, reset to first WP */
-		if (_current_onboard_mission_index >= _onboard_mission_item_count) {
-			_current_onboard_mission_index = 0;
-		}
-	}
-	// TODO: implement this for onboard missions as well
-	// 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()
-{
-	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)
-{
-	/* try onboard mission first */
-	if (current_onboard_mission_available()) {
-
-		const ssize_t len = sizeof(struct mission_item_s);
-
-		if (dm_read(DM_KEY_WAYPOINTS_ONBOARD, _current_onboard_mission_index, new_mission_item, len) != len) {
-			/* not supposed to happen unless the datamanager can't access the SD card, etc. */
-			return ERROR;
-		}
-
-		_current_mission_type = MISSION_TYPE_ONBOARD;
-		*onboard = true;
-		*index = _current_onboard_mission_index;
-
-		/* otherwise fallback to offboard */
-
-	} else if (current_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;
-		}
-
-		const ssize_t len = sizeof(struct mission_item_s);
-
-		if (dm_read(dm_current, _current_offboard_mission_index, new_mission_item, len) != len) {
-			/* not supposed to happen unless the datamanager can't access the SD card, etc. */
-			_current_mission_type = MISSION_TYPE_NONE;
-			return ERROR;
-		}
-
-		_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;
-		return ERROR;
-	}
-
-	return OK;
-}
-
-int
-Mission::get_next_mission_item(struct mission_item_s *new_mission_item)
-{
-	/* try onboard mission first */
-	if (next_onboard_mission_available()) {
-
-		const ssize_t len = sizeof(struct mission_item_s);
-
-		if (dm_read(DM_KEY_WAYPOINTS_ONBOARD, _current_onboard_mission_index + 1, new_mission_item, len) != len) {
-			/* not supposed to happen unless the datamanager can't access the SD card, etc. */
-			return ERROR;
-		}
-
-		/* 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;
-		}
-
-		const ssize_t len = sizeof(struct mission_item_s);
-
-		if (dm_read(dm_current, _current_offboard_mission_index + 1, new_mission_item, len) != len) {
-			/* not supposed to happen unless the datamanager can't access the SD card, etc. */
-			return ERROR;
-		}
-
-	} else {
-		/* happens when no more mission items can be added as a next item */
-		return ERROR;
-	}
-
-	return OK;
-}
-
-
-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;
-}
-
-bool
-Mission::next_onboard_mission_available()
-{
-	unsigned next = 0;
-
-	if (_current_mission_type != MISSION_TYPE_ONBOARD) {
-		next = 1;
-	}
-
-	return _onboard_mission_item_count > (_current_onboard_mission_index + next) && _onboard_mission_allowed;
-}
-
-bool
-Mission::next_offboard_mission_available()
-{
-	unsigned next = 0;
-
-	if (_current_mission_type != MISSION_TYPE_OFFBOARD) {
-		next = 1;
-	}
-
-	return _offboard_mission_item_count > (_current_offboard_mission_index + next);
-}
-
-void
-Mission::move_to_next()
-{
-	switch (_current_mission_type) {
-	case MISSION_TYPE_ONBOARD:
-		_current_onboard_mission_index++;
-		break;
-
-	case MISSION_TYPE_OFFBOARD:
-		_current_offboard_mission_index++;
-		break;
-
-	case MISSION_TYPE_NONE:
-	default:
-		break;
-	}
-}
-
-void
-Mission::report_mission_item_reached()
-{
-	if (_current_mission_type == MISSION_TYPE_OFFBOARD) {
-		_mission_result.mission_reached = true;
-		_mission_result.mission_index_reached = _current_offboard_mission_index;
-	}
-}
-
-void
-Mission::report_current_offboard_mission_item()
-{
-	_mission_result.index_current_mission = _current_offboard_mission_index;
-}
-
-void
-Mission::publish_mission_result()
-{
-	/* lazily publish the mission result only once available */
-	if (_mission_result_pub > 0) {
-		/* publish mission result */
-		orb_publish(ORB_ID(mission_result), _mission_result_pub, &_mission_result);
-
-	} else {
-		/* advertise and publish */
-		_mission_result_pub = orb_advertise(ORB_ID(mission_result), &_mission_result);
-	}
-	/* reset reached bool */
-	_mission_result.mission_reached = false;
-}
diff --git a/src/modules/navigator/navigator_mode.cpp b/src/modules/navigator/navigator_mode.cpp
new file mode 100644
index 000000000..c96829190
--- /dev/null
+++ b/src/modules/navigator/navigator_mode.cpp
@@ -0,0 +1,70 @@
+/****************************************************************************
+ *
+ *   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 navigator_mode.cpp
+ *
+ * Helper class for different modes in navigator
+ *
+ * @author Julian Oes <julian@oes.ch>
+ */
+
+#include "navigator_mode.h"
+
+NavigatorMode::NavigatorMode(Navigator *navigator, const char *name) :
+	SuperBlock(NULL, name),
+	_navigator(navigator),
+	_first_run(true)
+{
+	/* load initial params */
+	updateParams();
+	/* set initial mission items */
+	reset();
+}
+
+NavigatorMode::~NavigatorMode()
+{
+}
+
+void
+NavigatorMode::reset()
+{
+	_first_run = true;
+}
+
+bool
+NavigatorMode::update(struct position_setpoint_triplet_s *pos_sp_triplet)
+{
+	pos_sp_triplet->current.valid = false;
+	_first_run = false;
+	return false;
+}
diff --git a/src/modules/navigator/navigator_mode.h b/src/modules/navigator/navigator_mode.h
new file mode 100644
index 000000000..0844bb94d
--- /dev/null
+++ b/src/modules/navigator/navigator_mode.h
@@ -0,0 +1,86 @@
+/****************************************************************************
+ *
+ *   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 navigator_mode.h
+ *
+ * Helper class for different modes in navigator
+ *
+ * @author Julian Oes <julian@oes.ch>
+ */
+
+#ifndef NAVIGATOR_MODE_H
+#define NAVIGATOR_MODE_H
+
+#include <drivers/drv_hrt.h>
+
+#include <controllib/blocks.hpp>
+#include <controllib/block/BlockParam.hpp>
+
+#include <dataman/dataman.h>
+
+#include <uORB/topics/position_setpoint_triplet.h>
+
+class Navigator;
+
+class NavigatorMode : public control::SuperBlock
+{
+public:
+	/**
+	 * Constructor
+	 */
+	NavigatorMode(Navigator *navigator, const char *name);
+
+	/**
+	 * Destructor
+	 */
+	virtual ~NavigatorMode();
+
+	/**
+	 * This function is called while the mode is inactive
+	 */
+	virtual void reset();
+
+	/**
+	 * This function is called while the mode is active
+	 *
+	 * @param position setpoint triplet to set
+	 * @return true if position setpoint triplet has been changed
+	 */
+	virtual bool update(struct position_setpoint_triplet_s *pos_sp_triplet);
+
+protected:
+	Navigator *_navigator;
+	bool _first_run;
+};
+
+#endif
diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c
index 5139283b6..ca31adecc 100644
--- a/src/modules/navigator/navigator_params.c
+++ b/src/modules/navigator/navigator_params.c
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *   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
@@ -17,7 +17,7 @@
  *    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
+ * 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,
@@ -34,104 +34,33 @@
 /**
  * @file navigator_params.c
  *
- * Parameters defined by the navigator task.
+ * Parameters for navigator in general
  *
- * @author Lorenz Meier <lm@inf.ethz.ch>
- * @author Julian Oes <joes@student.ethz.ch>
- * @author Anton Babushkin <anton.babushkin@me.com>
+ * @author Julian Oes <julian@oes.ch>
  */
 
 #include <nuttx/config.h>
 
 #include <systemlib/param/param.h>
 
-/*
- * Navigator parameters, accessible via MAVLink
- */
-
-/**
- * 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).
- *
- * @unit meters
- * @min 0.0
- * @group Navigation
- */
-PARAM_DEFINE_FLOAT(NAV_ACCEPT_RAD, 10.0f);
-
 /**
- * Loiter radius (fixed wing only)
+ * Loiter radius (FW only)
  *
- * Default value of loiter radius (if not specified in mission item).
+ * Default value of loiter radius for missions, loiter, RTL, etc. (fixedwing only).
  *
  * @unit meters
  * @min 0.0
- * @group Navigation
+ * @group Mission
  */
 PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 50.0f);
 
 /**
- * Enable onboard mission
- *
- * @group Navigation
- */
-PARAM_DEFINE_INT32(NAV_ONB_MIS_EN, 0);
-
-/**
- * Take-off altitude
- *
- * Even if first waypoint has altitude less then NAV_TAKEOFF_ALT above home position, system will climb to NAV_TAKEOFF_ALT on takeoff, then go to waypoint.
- *
- * @unit meters
- * @group Navigation
- */
-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
+ * Takeoff Acceptance Radius (FW only)
  *
- * Minimum altitude above home position for going home in RTL mode.
+ * Acceptance radius for fixedwing.
  *
  * @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
+ * @min 1.0
+ * @group Mission
  */
-PARAM_DEFINE_INT32(NAV_PARACHUTE_EN, 0);
+PARAM_DEFINE_FLOAT(NAV_TF_ACC_RAD, 25.0f);
diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp
new file mode 100644
index 000000000..8888c6b62
--- /dev/null
+++ b/src/modules/navigator/rtl.cpp
@@ -0,0 +1,282 @@
+/****************************************************************************
+ *
+ *   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 "navigator.h"
+#include "rtl.h"
+
+RTL::RTL(Navigator *navigator, const char *name) :
+	NavigatorMode(navigator, name),
+	MissionBlock(navigator),
+	_rtl_state(RTL_STATE_NONE),
+	_param_return_alt(this, "RETURN_ALT"),
+	_param_descend_alt(this, "DESCEND_ALT"),
+	_param_land_delay(this, "LAND_DELAY"),
+	_param_acceptance_radius(this, "ACCEPT_RAD")
+{
+	/* load initial params */
+	updateParams();
+	/* initial reset */
+	reset();
+}
+
+RTL::~RTL()
+{
+}
+
+void
+RTL::reset()
+{
+	_first_run = true;
+	_rtl_state = RTL_STATE_NONE;
+}
+
+bool
+RTL::update(struct position_setpoint_triplet_s *pos_sp_triplet)
+{
+	bool updated = false;
+
+	if (_first_run) {
+		set_rtl_item(pos_sp_triplet);
+		updated = true;
+		_first_run = false;
+	}
+
+	if ((_rtl_state == RTL_STATE_CLIMB
+	     || _rtl_state == RTL_STATE_RETURN)
+	    && is_mission_item_reached()) {
+		advance_rtl();
+		set_rtl_item(pos_sp_triplet);
+		updated = true;
+	}
+
+	return updated;
+}
+
+void
+RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
+{
+	/* make sure we have the latest params */
+	updateParams();
+
+	/* decide where to enter the RTL procedure when we switch into it */
+	if (_rtl_state == RTL_STATE_NONE) {
+		/* for safety reasons don't go into RTL if landed */
+		if (_navigator->get_vstatus()->condition_landed) {
+			_rtl_state = RTL_STATE_FINISHED;
+			mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no RTL when landed");
+		/* if lower than return altitude, climb up first */
+		} else if (_navigator->get_global_position()->alt < _navigator->get_home_position()->alt
+			   + _param_return_alt.get()) {
+			_rtl_state = RTL_STATE_CLIMB;
+		/* otherwise go straight to return */
+		} else {
+			_rtl_state = RTL_STATE_RETURN;
+		}
+	}
+
+	/* if switching directly to return state, set altitude setpoint to current altitude */
+	if (_rtl_state == RTL_STATE_RETURN) {
+		_mission_item.altitude_is_relative = false;
+		_mission_item.altitude = _navigator->get_global_position()->alt;
+	}
+
+	switch (_rtl_state) {
+	case RTL_STATE_CLIMB: {
+
+		float climb_alt = _navigator->get_home_position()->alt + _param_return_alt.get();
+
+		_mission_item.lat = _navigator->get_global_position()->lat;
+		_mission_item.lon = _navigator->get_global_position()->lon;
+		_mission_item.altitude_is_relative = false;
+		_mission_item.altitude = climb_alt;
+		_mission_item.yaw = NAN;
+		_mission_item.loiter_radius = _navigator->get_loiter_radius();
+		_mission_item.loiter_direction = 1;
+		_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
+		_mission_item.acceptance_radius = _param_acceptance_radius.get();
+		_mission_item.time_inside = 0.0f;
+		_mission_item.pitch_min = 0.0f;
+		_mission_item.autocontinue = true;
+		_mission_item.origin = ORIGIN_ONBOARD;
+
+		_navigator->set_is_in_loiter(false);
+
+		mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: climb to %d meters above home",
+			(int)(climb_alt - _navigator->get_home_position()->alt));
+		break;
+	}
+	case RTL_STATE_RETURN: {
+
+		_mission_item.lat = _navigator->get_home_position()->lat;
+		_mission_item.lon = _navigator->get_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 */
+		// 	_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 = _navigator->get_loiter_radius();
+		_mission_item.loiter_direction = 1;
+		_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
+		_mission_item.acceptance_radius = _param_acceptance_radius.get();
+		_mission_item.time_inside = 0.0f;
+		_mission_item.pitch_min = 0.0f;
+		_mission_item.autocontinue = true;
+		_mission_item.origin = ORIGIN_ONBOARD;
+
+		_navigator->set_is_in_loiter(false);
+
+		mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: return at %d meters above home",
+			(int)(_mission_item.altitude - _navigator->get_home_position()->alt));
+		break;
+	}
+	case RTL_STATE_DESCEND: {
+
+		_mission_item.lat = _navigator->get_home_position()->lat;
+		_mission_item.lon = _navigator->get_home_position()->lon;
+		_mission_item.altitude_is_relative = false;
+		_mission_item.altitude = _navigator->get_home_position()->alt + _param_descend_alt.get();
+		_mission_item.yaw = NAN;
+		_mission_item.loiter_radius = _navigator->get_loiter_radius();
+		_mission_item.loiter_direction = 1;
+		_mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT;
+		_mission_item.acceptance_radius = _param_acceptance_radius.get();
+		_mission_item.time_inside = _param_land_delay.get() < 0.0f ? 0.0f : _param_land_delay.get();
+		_mission_item.pitch_min = 0.0f;
+		_mission_item.autocontinue = _param_land_delay.get() > -0.001f;
+		_mission_item.origin = ORIGIN_ONBOARD;
+
+		_navigator->set_is_in_loiter(true);
+
+		mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: descend to %d meters above home",
+			(int)(_mission_item.altitude - _navigator->get_home_position()->alt));
+		break;
+	}
+
+	case RTL_STATE_LAND: {
+
+		_mission_item.lat = _navigator->get_home_position()->lat;
+		_mission_item.lon = _navigator->get_home_position()->lon;
+		_mission_item.altitude_is_relative = false;
+		_mission_item.altitude = _navigator->get_home_position()->alt;
+		_mission_item.yaw = NAN;
+		_mission_item.loiter_radius = _navigator->get_loiter_radius();
+		_mission_item.loiter_direction = 1;
+		_mission_item.nav_cmd = NAV_CMD_LAND;
+		_mission_item.acceptance_radius = _param_acceptance_radius.get();
+		_mission_item.time_inside = 0.0f;
+		_mission_item.pitch_min = 0.0f;
+		_mission_item.autocontinue = true;
+		_mission_item.origin = ORIGIN_ONBOARD;
+
+		_navigator->set_is_in_loiter(false);
+
+		mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: land at home");
+		break;
+	}
+
+	case RTL_STATE_FINISHED: {
+		/* nothing to do, report fail */
+	}
+
+	default:
+		break;
+	}
+
+	if (_rtl_state == RTL_STATE_FINISHED) {
+		pos_sp_triplet->current.valid = false;
+		pos_sp_triplet->next.valid = false;
+	} else {
+		/* if not finished, convert mission item to current position setpoint and make it valid */
+		mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
+		reset_mission_item_reached();
+		pos_sp_triplet->current.valid = true;
+		pos_sp_triplet->next.valid = false;
+	}
+}
+
+void
+RTL::advance_rtl()
+{
+	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;
+	}
+}
diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h
new file mode 100644
index 000000000..9836f5064
--- /dev/null
+++ b/src/modules/navigator/rtl.h
@@ -0,0 +1,110 @@
+/***************************************************************************
+ *
+ *   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.h
+ * Helper class for RTL
+ *
+ * @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/mission.h>
+#include <uORB/topics/home_position.h>
+#include <uORB/topics/vehicle_global_position.h>
+
+#include "navigator_mode.h"
+#include "mission_block.h"
+
+class Navigator;
+
+class RTL : public NavigatorMode, MissionBlock
+{
+public:
+	/**
+	 * Constructor
+	 */
+	RTL(Navigator *navigator, const char *name);
+
+	/**
+	 * Destructor
+	 */
+	~RTL();
+
+	/**
+	 * This function is called while the mode is inactive
+	 */
+	void reset();
+
+	/**
+	 * This function is called while the mode is active
+	 *
+	 * @param position setpoint triplet that needs to be set
+	 * @return true if updated
+	 */
+	bool update(position_setpoint_triplet_s *pos_sp_triplet);
+
+
+private:
+	/**
+	 * Set the RTL item
+	 */
+	void		set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet);
+
+	/**
+	 * Move to next RTL item
+	 */
+	void		advance_rtl();
+
+	enum RTLState {
+		RTL_STATE_NONE = 0,
+		RTL_STATE_CLIMB,
+		RTL_STATE_RETURN,
+		RTL_STATE_DESCEND,
+		RTL_STATE_LAND,
+		RTL_STATE_FINISHED,
+	} _rtl_state;
+
+	control::BlockParamFloat _param_return_alt;
+	control::BlockParamFloat _param_descend_alt;
+	control::BlockParamFloat _param_land_delay;
+	control::BlockParamFloat _param_acceptance_radius;
+};
+
+#endif
diff --git a/src/modules/navigator/rtl_params.c b/src/modules/navigator/rtl_params.c
new file mode 100644
index 000000000..63724f461
--- /dev/null
+++ b/src/modules/navigator/rtl_params.c
@@ -0,0 +1,110 @@
+/****************************************************************************
+ *
+ *   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
+ */
+
+/**
+ * Loiter radius after RTL (FW only)
+ *
+ * Default value of loiter radius after RTL (fixedwing only).
+ *
+ * @unit meters
+ * @min 0.0
+ * @group RTL
+ */
+PARAM_DEFINE_FLOAT(RTL_LOITER_RAD, 50.0f);
+
+/**
+ * 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);
+
+/**
+ * RTL acceptance radius
+ *
+ * Acceptance radius for waypoints set for RTL
+ *
+ * @unit meters
+ * @min 1
+ * @max
+ * @group RTL
+ */
+PARAM_DEFINE_FLOAT(RTL_ACCEPT_RAD, 25.0f);