17 files changed, 801 insertions, 378 deletions

diff --git a/src/modules/navigator/loiter.cpp b/src/modules/navigator/loiter.cpp
index 542483fb1..f827e70c9 100644
--- a/src/modules/navigator/loiter.cpp
+++ b/src/modules/navigator/loiter.cpp
@@ -36,6 +36,7 @@
  * Helper class to loiter
  *
  * @author Julian Oes <julian@oes.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
  */
 
 #include <string.h>
@@ -51,28 +52,42 @@
 #include <uORB/topics/position_setpoint_triplet.h>
 
 #include "loiter.h"
+#include "navigator.h"
 
 Loiter::Loiter(Navigator *navigator, const char *name) :
 	MissionBlock(navigator, name)
 {
 	/* load initial params */
 	updateParams();
-	/* initial reset */
-	on_inactive();
 }
 
 Loiter::~Loiter()
 {
 }
 
-bool
-Loiter::on_active(struct position_setpoint_triplet_s *pos_sp_triplet)
+void
+Loiter::on_inactive()
 {
-	/* set loiter item, don't reuse an existing position setpoint */
-	return set_loiter_item(pos_sp_triplet);
 }
 
 void
-Loiter::on_inactive()
+Loiter::on_activation()
+{
+	/* set current mission item to loiter */
+	set_loiter_item(&_mission_item);
+
+	/* convert mission item to current setpoint */
+	struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
+	pos_sp_triplet->previous.valid = false;
+	mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
+	pos_sp_triplet->next.valid = false;
+
+	_navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == SETPOINT_TYPE_LOITER);
+
+	_navigator->set_position_setpoint_triplet_updated();
+}
+
+void
+Loiter::on_active()
 {
 }
diff --git a/src/modules/navigator/loiter.h b/src/modules/navigator/loiter.h
index 65ff5c31e..37ab57a07 100644
--- a/src/modules/navigator/loiter.h
+++ b/src/modules/navigator/loiter.h
@@ -50,25 +50,15 @@
 class Loiter : public MissionBlock
 {
 public:
-	/**
-	 * Constructor
-	 */
 	Loiter(Navigator *navigator, const char *name);
 
-	/**
-	 * Destructor
-	 */
 	~Loiter();
 
-	/**
-	 * This function is called while the mode is inactive
-	 */
 	virtual void on_inactive();
 
-	/**
-	 * This function is called while the mode is active
-	 */
-	virtual bool on_active(struct position_setpoint_triplet_s *pos_sp_triplet);
+	virtual void on_activation();
+
+	virtual void on_active();
 };
 
 #endif
diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp
index 72255103b..c0e37a3ed 100644
--- a/src/modules/navigator/mission.cpp
+++ b/src/modules/navigator/mission.cpp
@@ -54,25 +54,28 @@
 #include <uORB/topics/mission.h>
 #include <uORB/topics/mission_result.h>
 
-#include "navigator.h"
 #include "mission.h"
+#include "navigator.h"
 
 Mission::Mission(Navigator *navigator, const char *name) :
 	MissionBlock(navigator, name),
 	_param_onboard_enabled(this, "ONBOARD_EN"),
+	_param_takeoff_alt(this, "TAKEOFF_ALT"),
+	_param_dist_1wp(this, "DIST_1WP"),
 	_onboard_mission({0}),
 	_offboard_mission({0}),
 	_current_onboard_mission_index(-1),
 	_current_offboard_mission_index(-1),
+	_need_takeoff(true),
+	_takeoff(false),
 	_mission_result_pub(-1),
 	_mission_result({0}),
-	_mission_type(MISSION_TYPE_NONE)
+	_mission_type(MISSION_TYPE_NONE),
+	_inited(false),
+	_dist_1wp_ok(false)
 {
 	/* load initial params */
 	updateParams();
-	/* set initial mission items */
-	on_inactive();
-
 }
 
 Mission::~Mission()
@@ -82,55 +85,71 @@ Mission::~Mission()
 void
 Mission::on_inactive()
 {
-	_first_run = true;
+	if (_inited) {
+		/* check if missions have changed so that feedback to ground station is given */
+		bool onboard_updated = false;
+		orb_check(_navigator->get_onboard_mission_sub(), &onboard_updated);
+		if (onboard_updated) {
+			update_onboard_mission();
+		}
+
+		bool offboard_updated = false;
+		orb_check(_navigator->get_offboard_mission_sub(), &offboard_updated);
+		if (offboard_updated) {
+			update_offboard_mission();
+		}
+
+	} else {
+		/* read mission topics on initialization */
+		_inited = 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();
+		update_offboard_mission();
 	}
 
-	bool offboard_updated;
-	orb_check(_navigator->get_offboard_mission_sub(), &offboard_updated);
-	if (offboard_updated) {
-		update_offboard_mission();
+	if (!_navigator->get_can_loiter_at_sp() || _navigator->get_vstatus()->condition_landed) {
+		_need_takeoff = true;
 	}
 }
 
-bool
-Mission::on_active(struct position_setpoint_triplet_s *pos_sp_triplet)
+void
+Mission::on_activation()
 {
-	bool updated = false;
+	set_mission_items();
+}
 
+void
+Mission::on_active()
+{
 	/* check if anything has changed */
-	bool onboard_updated;
+	bool onboard_updated = false;
 	orb_check(_navigator->get_onboard_mission_sub(), &onboard_updated);
 	if (onboard_updated) {
 		update_onboard_mission();
 	}
 
-	bool offboard_updated;
+	bool offboard_updated = false;
 	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;
+	if (onboard_updated || offboard_updated) {
+		set_mission_items();
 	}
 
 	/* 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;
-	}
+		set_mission_items();
 
-	return updated;
+	} else {
+		/* if waypoint position reached allow loiter on the setpoint */
+		if (_waypoint_position_reached && _mission_item.nav_cmd != NAV_CMD_IDLE) {
+			_navigator->set_can_loiter_at_sp(true);
+		}
+	}
 }
 
 void
@@ -138,9 +157,9 @@ 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;
+		if (_onboard_mission.current_seq >=0
+		&& _onboard_mission.current_seq < (int)_onboard_mission.count) {
+			_current_onboard_mission_index = _onboard_mission.current_seq;
 		} else {
 			/* if less WPs available, reset to first WP */
 			if (_current_onboard_mission_index >= (int)_onboard_mission.count) {
@@ -151,9 +170,10 @@ Mission::update_onboard_mission()
 			}
 			/* otherwise, just leave it */
 		}
+
 	} else {
 		_onboard_mission.count = 0;
-		_onboard_mission.current_index = 0;
+		_onboard_mission.current_seq = 0;
 		_current_onboard_mission_index = 0;
 	}
 }
@@ -162,15 +182,15 @@ void
 Mission::update_offboard_mission()
 {
 	if (orb_copy(ORB_ID(offboard_mission), _navigator->get_offboard_mission_sub(), &_offboard_mission) == OK) {
-
+		warnx("offboard mission updated: dataman_id=%d, count=%d, current_seq=%d", _offboard_mission.dataman_id, _offboard_mission.count, _offboard_mission.current_seq);
 		/* 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;
+		if (_offboard_mission.current_seq >= 0 && _offboard_mission.current_seq < (int)_offboard_mission.count) {
+			_current_offboard_mission_index = _offboard_mission.current_seq;
 		} else {
-			/* if less WPs available, reset to first WP */
+			/* if less items available, reset to first item */
 			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;
@@ -180,23 +200,19 @@ Mission::update_offboard_mission()
 
 		/* 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;
+		dm_item_t dm_current = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id);
 
-		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);
 
-		missionFeasiblityChecker.checkMissionFeasible(_navigator->get_vstatus()->is_rotary_wing, dm_current,
-			                                      (size_t)_offboard_mission.count,
-							      _navigator->get_geofence(),
-							      _navigator->get_home_position()->alt);
 	} else {
+		warnx("offboard mission update failed");
 		_offboard_mission.count = 0;
-		_offboard_mission.current_index = 0;
+		_offboard_mission.current_seq = 0;
 		_current_offboard_mission_index = 0;
 	}
+
 	report_current_offboard_mission_item();
 }
 
@@ -204,227 +220,375 @@ Mission::update_offboard_mission()
 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;
+	if (_takeoff) {
+		_takeoff = false;
+
+	} else {
+		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;
+		}
+	}
+}
+
+bool
+Mission::check_dist_1wp()
+{
+	if (_dist_1wp_ok) {
+		/* always return true after at least one successful check */
+		return true;
+	}
+
+	/* check if first waypoint is not too far from home */
+	if (_param_dist_1wp.get() > 0.0f) {
+		if (_navigator->get_vstatus()->condition_home_position_valid) {
+			struct mission_item_s mission_item;
+
+			/* find first waypoint (with lat/lon) item in datamanager */
+			for (unsigned i = 0; i < _offboard_mission.count; i++) {
+				if (dm_read(DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id), i,
+						&mission_item, sizeof(mission_item_s)) == sizeof(mission_item_s)) {
+
+					/* check only items with valid lat/lon */
+					if (	mission_item.nav_cmd == NAV_CMD_WAYPOINT ||
+							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 ||
+							mission_item.nav_cmd == NAV_CMD_TAKEOFF ||
+							mission_item.nav_cmd == NAV_CMD_PATHPLANNING) {
+
+						/* check distance from home to item */
+						float dist_to_1wp = get_distance_to_next_waypoint(
+								mission_item.lat, mission_item.lon,
+								_navigator->get_home_position()->lat, _navigator->get_home_position()->lon);
+
+						if (dist_to_1wp < _param_dist_1wp.get()) {
+							_dist_1wp_ok = true;
+							if (dist_to_1wp > ((_param_dist_1wp.get() * 3) / 2)) {
+								/* allow at 2/3 distance, but warn */
+								mavlink_log_critical(_navigator->get_mavlink_fd(), "Warning: First waypoint very far: %d m", (int)dist_to_1wp);
+							}
+							return true;
+
+						} else {
+							/* item is too far from home */
+							mavlink_log_critical(_navigator->get_mavlink_fd(), "Waypoint too far: %d m,[MIS_DIST_1WP=%d]", (int)dist_to_1wp, (int)_param_dist_1wp.get());
+							return false;
+						}
+					}
+
+				} else {
+					/* error reading, mission is invalid */
+					mavlink_log_info(_navigator->get_mavlink_fd(), "error reading offboard mission");
+					return false;
+				}
+			}
+
+			/* no waypoints found in mission, then we will not fly far away */
+			_dist_1wp_ok = true;
+			return true;
+
+		} else {
+			mavlink_log_info(_navigator->get_mavlink_fd(), "no home position");
+			return false;
+		}
+
+	} else {
+		return true;
 	}
 }
 
 void
-Mission::set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet)
+Mission::set_mission_items()
 {
-	set_previous_pos_setpoint(pos_sp_triplet);
+	/* make sure param is up to date */
+	updateParams();
+
+	struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
+
+	/* set previous position setpoint to current */
+	set_previous_pos_setpoint();
+
+	/* get home distance state */
+	bool home_dist_ok = check_dist_1wp();
+	/* the home dist check provides user feedback, so we initialize it to this */
+	bool user_feedback_done = !home_dist_ok;
 
 	/* try setting onboard mission item */
-	if (is_current_onboard_mission_item_set(&pos_sp_triplet->current)) {
+	if (_param_onboard_enabled.get() && read_mission_item(true, true, &_mission_item)) {
 		/* if mission type changed, notify */
 		if (_mission_type != MISSION_TYPE_ONBOARD) {
-			mavlink_log_info(_navigator->get_mavlink_fd(),
-					"#audio: onboard mission running");
+			mavlink_log_critical(_navigator->get_mavlink_fd(), "onboard mission now running");
 		}
 		_mission_type = MISSION_TYPE_ONBOARD;
-		_navigator->set_can_loiter_at_sp(false);
 
 	/* try setting offboard mission item */
-	} else if (is_current_offboard_mission_item_set(&pos_sp_triplet->current)) {
+	} else if (home_dist_ok && read_mission_item(false, true, &_mission_item)) {
 		/* if mission type changed, notify */
 		if (_mission_type != MISSION_TYPE_OFFBOARD) {
-			mavlink_log_info(_navigator->get_mavlink_fd(),
-					"#audio: offboard mission running");
+			mavlink_log_critical(_navigator->get_mavlink_fd(), "offboard mission now running");
 		}
 		_mission_type = MISSION_TYPE_OFFBOARD;
-		_navigator->set_can_loiter_at_sp(false);
+
 	} else {
+		/* no mission available or mission finished, switch to loiter */
 		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");
+			mavlink_log_critical(_navigator->get_mavlink_fd(), "mission finished, loitering");
+		} else if (!user_feedback_done) {
+			/* only tell users that we got no mission if there has not been any
+			 * better, more specific feedback yet
+			 */
+			mavlink_log_critical(_navigator->get_mavlink_fd(), "no valid mission available");
 		}
 		_mission_type = MISSION_TYPE_NONE;
-        _navigator->set_can_loiter_at_sp(pos_sp_triplet->current.valid && _waypoint_position_reached);
 
-		set_loiter_item(pos_sp_triplet);
+		/* set loiter mission item */
+		set_loiter_item(&_mission_item);
+
+		/* update position setpoint triplet  */
+		pos_sp_triplet->previous.valid = false;
+		mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
+		pos_sp_triplet->next.valid = false;
+
+		_navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == SETPOINT_TYPE_LOITER);
+
 		reset_mission_item_reached();
 		report_mission_finished();
+
+		_navigator->set_position_setpoint_triplet_updated();
+		return;
 	}
-}
 
-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;
+	/* do takeoff on first waypoint for rotary wing vehicles */
+	if (_navigator->get_vstatus()->is_rotary_wing) {
+		/* force takeoff if landed (additional protection) */
+		if (!_takeoff && _navigator->get_vstatus()->condition_landed) {
+			_need_takeoff = true;
+		}
+
+		/* new current mission item set, check if we need takeoff */
+		if (_need_takeoff && (
+				_mission_item.nav_cmd == NAV_CMD_TAKEOFF ||
+				_mission_item.nav_cmd == NAV_CMD_WAYPOINT ||
+				_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 ||
+				_mission_item.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH)) {
+			_takeoff = true;
+			_need_takeoff = false;
 		}
 	}
-	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;
+	if (_takeoff) {
+		/* do takeoff before going to setpoint */
+		/* set mission item as next position setpoint */
+		mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->next);
+
+		/* calculate takeoff altitude */
+		float takeoff_alt = _mission_item.altitude;
+		if (_mission_item.altitude_is_relative) {
+			takeoff_alt += _navigator->get_home_position()->alt;
+		}
+
+		/* perform takeoff at least to NAV_TAKEOFF_ALT above home/ground, even if first waypoint is lower */
+		if (_navigator->get_vstatus()->condition_landed) {
+			takeoff_alt = fmaxf(takeoff_alt, _navigator->get_global_position()->alt + _param_takeoff_alt.get());
+
 		} else {
-			dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1;
+			takeoff_alt = fmaxf(takeoff_alt, _navigator->get_home_position()->alt + _param_takeoff_alt.get());
 		}
-		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();
+		mavlink_log_critical(_navigator->get_mavlink_fd(), "takeoff to %.1f meters above home", (double)(takeoff_alt - _navigator->get_home_position()->alt));
 
-			report_current_offboard_mission_item();
-			memcpy(&_mission_item, &new_mission_item, sizeof(struct mission_item_s));
-			return true;
+		_mission_item.lat = _navigator->get_global_position()->lat;
+		_mission_item.lon = _navigator->get_global_position()->lon;
+		_mission_item.altitude = takeoff_alt;
+		_mission_item.altitude_is_relative = false;
+
+		mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
+
+	} else {
+		/* set current position setpoint from mission item */
+		mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
+
+		/* require takeoff after landing or idle */
+		if (pos_sp_triplet->current.type == SETPOINT_TYPE_LAND || pos_sp_triplet->current.type == SETPOINT_TYPE_IDLE) {
+			_need_takeoff = 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;
+		_navigator->set_can_loiter_at_sp(false);
+		reset_mission_item_reached();
+
+		if (_mission_type == MISSION_TYPE_OFFBOARD) {
+			report_current_offboard_mission_item();
 		}
-	}
+		// TODO: report onboard mission item somehow
 
-	/* give up */
-	next_pos_sp->valid = false;
-	return;
-}
+		/* try to read next mission item */
+		struct mission_item_s mission_item_next;
+
+		if (read_mission_item(_mission_type == MISSION_TYPE_ONBOARD, false, &mission_item_next)) {
+			/* got next mission item, update setpoint triplet */
+			mission_item_to_position_setpoint(&mission_item_next, &pos_sp_triplet->next);
 
-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;
+			/* next mission item is not available */
+			pos_sp_triplet->next.valid = false;
 		}
 	}
-	/* give up */
-	next_pos_sp->valid = false;
-	return;
+
+	_navigator->set_position_setpoint_triplet_updated();
 }
 
 bool
-Mission::read_mission_item(const dm_item_t dm_item, bool is_current, int *mission_index,
-		           struct mission_item_s *new_mission_item)
+Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s *mission_item)
 {
+	/* select onboard/offboard mission */
+	int *mission_index_ptr;
+	struct mission_s *mission;
+	dm_item_t dm_item;
+	int mission_index_next;
+
+	if (onboard) {
+		/* onboard mission */
+		mission_index_next = _current_onboard_mission_index + 1;
+		mission_index_ptr = is_current ? &_current_onboard_mission_index : &mission_index_next;
+
+		mission = &_onboard_mission;
+
+		dm_item = DM_KEY_WAYPOINTS_ONBOARD;
+
+	} else {
+		/* offboard mission */
+		mission_index_next = _current_offboard_mission_index + 1;
+		mission_index_ptr = is_current ? &_current_offboard_mission_index : &mission_index_next;
+
+		mission = &_offboard_mission;
+
+		dm_item = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id);
+	}
+
+	if (*mission_index_ptr < 0 || *mission_index_ptr >= (int)mission->count) {
+		/* mission item index out of bounds */
+		return false;
+	}
+
 	/* repeat several to get the mission item because we might have to follow multiple DO_JUMPS */
-	for (int i=0; i<10; i++) {
+	for (int i = 0; i < 10; i++) {
 		const ssize_t len = sizeof(struct mission_item_s);
 
+		/* read mission item to temp storage first to not overwrite current mission item if data damaged */
+		struct mission_item_s mission_item_tmp;
+
 		/* read mission item from datamanager */
-		if (dm_read(dm_item, *mission_index, new_mission_item, len) != len) {
+		if (dm_read(dm_item, *mission_index_ptr, &mission_item_tmp, 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");
+			                     "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) {
+		if (mission_item_tmp.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) {
+			if (mission_item_tmp.do_jump_current_count < mission_item_tmp.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)++;
+					(mission_item_tmp.do_jump_current_count)++;
 					/* save repeat count */
-					if (dm_write(dm_item, *mission_index, DM_PERSIST_IN_FLIGHT_RESET,
-						new_mission_item, len) != len) {
+					if (dm_write(dm_item, *mission_index_ptr, DM_PERSIST_IN_FLIGHT_RESET, &mission_item_tmp, 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");
+								"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;
+				*mission_index_ptr = mission_item_tmp.do_jump_mission_index;
+
 			} else {
-				mavlink_log_info(_navigator->get_mavlink_fd(),
-						 "#audio: DO JUMP repetitions completed");
+				mavlink_log_critical(_navigator->get_mavlink_fd(),
+						 "DO JUMP repetitions completed");
 				/* no more DO_JUMPS, therefore just try to continue with next mission item */
-				(*mission_index)++;
+				(*mission_index_ptr)++;
 			}
 
 		} else {
 			/* if it's not a DO_JUMP, then we were successful */
+			memcpy(mission_item, &mission_item_tmp, sizeof(struct mission_item_s));
 			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");
+			     "ERROR DO JUMP is cycling, giving up");
 	return false;
 }
 
 void
+Mission::save_offboard_mission_state()
+{
+	mission_s mission_state;
+
+	/* lock MISSION_STATE item */
+	dm_lock(DM_KEY_MISSION_STATE);
+
+	/* read current state */
+	int read_res = dm_read(DM_KEY_MISSION_STATE, 0, &mission_state, sizeof(mission_s));
+
+	if (read_res == sizeof(mission_s)) {
+		/* data read successfully, check dataman ID and items count */
+		if (mission_state.dataman_id == _offboard_mission.dataman_id && mission_state.count == _offboard_mission.count) {
+			/* navigator may modify only sequence, write modified state only if it changed */
+			if (mission_state.current_seq != _current_offboard_mission_index) {
+				if (dm_write(DM_KEY_MISSION_STATE, 0, DM_PERSIST_POWER_ON_RESET, &mission_state, sizeof(mission_s)) != sizeof(mission_s)) {
+					warnx("ERROR: can't save mission state");
+					mavlink_log_critical(_navigator->get_mavlink_fd(), "ERROR: can't save mission state");
+				}
+			}
+		}
+
+	} else {
+		/* invalid data, this must not happen and indicates error in offboard_mission publisher */
+		mission_state.dataman_id = _offboard_mission.dataman_id;
+		mission_state.count = _offboard_mission.count;
+		mission_state.current_seq = _current_offboard_mission_index;
+
+		warnx("ERROR: invalid mission state");
+		mavlink_log_critical(_navigator->get_mavlink_fd(), "ERROR: invalid mission state");
+
+		/* write modified state only if changed */
+		if (dm_write(DM_KEY_MISSION_STATE, 0, DM_PERSIST_POWER_ON_RESET, &mission_state, sizeof(mission_s)) != sizeof(mission_s)) {
+			warnx("ERROR: can't save mission state");
+			mavlink_log_critical(_navigator->get_mavlink_fd(), "ERROR: can't save mission state");
+		}
+	}
+
+	/* unlock MISSION_STATE item */
+	dm_unlock(DM_KEY_MISSION_STATE);
+}
+
+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;
+		_mission_result.reached = true;
+		_mission_result.seq_reached = _current_offboard_mission_index;
 	}
 	publish_mission_result();
 }
@@ -432,14 +596,17 @@ Mission::report_mission_item_reached()
 void
 Mission::report_current_offboard_mission_item()
 {
-	_mission_result.index_current_mission = _current_offboard_mission_index;
+	warnx("current offboard mission index: %d", _current_offboard_mission_index);
+	_mission_result.seq_current = _current_offboard_mission_index;
 	publish_mission_result();
+
+	save_offboard_mission_state();
 }
 
 void
 Mission::report_mission_finished()
 {
-	_mission_result.mission_finished = true;
+	_mission_result.finished = true;
 	publish_mission_result();
 }
 
@@ -456,6 +623,6 @@ Mission::publish_mission_result()
 		_mission_result_pub = orb_advertise(ORB_ID(mission_result), &_mission_result);
 	}
 	/* reset reached bool */
-	_mission_result.mission_reached = false;
-	_mission_result.mission_finished = false;
+	_mission_result.reached = false;
+	_mission_result.finished = false;
 }
diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h
index 6e4761946..4da6a1155 100644
--- a/src/modules/navigator/mission.h
+++ b/src/modules/navigator/mission.h
@@ -65,25 +65,15 @@ class Navigator;
 class Mission : public MissionBlock
 {
 public:
-	/**
-	 * Constructor
-	 */
 	Mission(Navigator *navigator, const char *name);
 
-	/**
-	 * Destructor
-	 */
 	virtual ~Mission();
 
-	/**
-	 * This function is called while the mode is inactive
-	 */
 	virtual void on_inactive();
 
-	/**
-	 * This function is called while the mode is active
-	 */
-	virtual bool on_active(struct position_setpoint_triplet_s *pos_sp_triplet);
+	virtual void on_activation();
+
+	virtual void on_active();
 
 private:
 	/**
@@ -102,38 +92,26 @@ private:
 	void advance_mission();
 
 	/**
-	 * Set new mission items
-	 */
-	void set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet);
-
-	/**
-	 * 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
+	 * Check distance to first waypoint (with lat/lon)
+	 * @return true only if it's not too far from home (< MIS_DIST_1WP)
 	 */
-	bool is_current_offboard_mission_item_set(struct position_setpoint_s *current_pos_sp);
+	bool check_dist_1wp();
 
 	/**
-	 * Try to set the next position setpoint from an onboard mission item
+	 * Set new mission items
 	 */
-	void get_next_onboard_mission_item(struct position_setpoint_s *next_pos_sp);
+	void set_mission_items();
 
 	/**
-	 * Try to set the next position setpoint from an offboard mission item
+	 * Read current or next mission item from the dataman and watch out for DO_JUMPS
+	 * @return true if successful
 	 */
-	void get_next_offboard_mission_item(struct position_setpoint_s *next_pos_sp);
+	bool read_mission_item(bool onboard, bool is_current, struct mission_item_s *mission_item);
 
 	/**
-	 * Read a mission item from the dataman and watch out for DO_JUMPS
-	 * @return true if successful
+	 * Save current offboard mission state to dataman
 	 */
-	bool read_mission_item(const dm_item_t dm_item, bool is_current, int *mission_index,
-			       struct mission_item_s *new_mission_item);
+	void save_offboard_mission_state();
 
 	/**
 	 * Report that a mission item has been reached
@@ -155,13 +133,17 @@ private:
 	 */
 	void publish_mission_result();
 
-	control::BlockParamFloat _param_onboard_enabled;
+	control::BlockParamInt _param_onboard_enabled;
+	control::BlockParamFloat _param_takeoff_alt;
+	control::BlockParamFloat _param_dist_1wp;
 
 	struct mission_s _onboard_mission;
 	struct mission_s _offboard_mission;
 
 	int _current_onboard_mission_index;
 	int _current_offboard_mission_index;
+	bool _need_takeoff;
+	bool _takeoff;
 
 	orb_advert_t _mission_result_pub;
 	struct mission_result_s _mission_result;
@@ -172,6 +154,9 @@ private:
 		MISSION_TYPE_OFFBOARD
 	} _mission_type;
 
+	bool _inited;
+	bool _dist_1wp_ok;
+
 	MissionFeasibilityChecker missionFeasiblityChecker; /**< class that checks if a mission is feasible */
 };
 
diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp
index 26a573544..4adf77dce 100644
--- a/src/modules/navigator/mission_block.cpp
+++ b/src/modules/navigator/mission_block.cpp
@@ -47,6 +47,7 @@
 
 #include <systemlib/err.h>
 #include <geo/geo.h>
+#include <mavlink/mavlink_log.h>
 
 #include <uORB/uORB.h>
 
@@ -56,11 +57,10 @@
 
 MissionBlock::MissionBlock(Navigator *navigator, const char *name) :
 	NavigatorMode(navigator, name),
+	_mission_item({0}),
 	_waypoint_position_reached(false),
 	_waypoint_yaw_reached(false),
-	_time_first_inside_orbit(0),
-	_mission_item({0}),
-	_mission_item_valid(false)
+	_time_first_inside_orbit(0)
 {
 }
 
@@ -71,6 +71,10 @@ MissionBlock::~MissionBlock()
 bool
 MissionBlock::is_mission_item_reached()
 {
+	if (_mission_item.nav_cmd == NAV_CMD_IDLE) {
+		return false;
+	}
+
 	if (_mission_item.nav_cmd == NAV_CMD_LAND) {
 		return _navigator->get_vstatus()->condition_landed;
 	}
@@ -84,7 +88,6 @@ MissionBlock::is_mission_item_reached()
 	hrt_abstime now = hrt_absolute_time();
 
 	if (!_waypoint_position_reached) {
-
 		float dist = -1.0f;
 		float dist_xy = -1.0f;
 		float dist_z = -1.0f;
@@ -201,44 +204,48 @@ MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *ite
 }
 
 void
-MissionBlock::set_previous_pos_setpoint(struct position_setpoint_triplet_s *pos_sp_triplet)
+MissionBlock::set_previous_pos_setpoint()
 {
-    /* reuse current setpoint as previous setpoint */
+	struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
+
     if (pos_sp_triplet->current.valid) {
         memcpy(&pos_sp_triplet->previous, &pos_sp_triplet->current, sizeof(struct position_setpoint_s));
     }
 }
 
-bool
-MissionBlock::set_loiter_item(struct position_setpoint_triplet_s *pos_sp_triplet)
+void
+MissionBlock::set_loiter_item(struct mission_item_s *item)
 {
-    /* don't change setpoint if 'can_loiter_at_sp' flag set */
-	if (!(_navigator->get_can_loiter_at_sp() && pos_sp_triplet->current.valid)) {
-		/* use current position */
-		pos_sp_triplet->current.lat = _navigator->get_global_position()->lat;
-		pos_sp_triplet->current.lon = _navigator->get_global_position()->lon;
-		pos_sp_triplet->current.alt = _navigator->get_global_position()->alt;
-		pos_sp_triplet->current.yaw = NAN;	/* NAN means to use current yaw */
-
-	    _navigator->set_can_loiter_at_sp(true);
-	}
+	if (_navigator->get_vstatus()->condition_landed) {
+		/* landed, don't takeoff, but switch to IDLE mode */
+		item->nav_cmd = NAV_CMD_IDLE;
 
-    if (pos_sp_triplet->current.type != SETPOINT_TYPE_LOITER
-            || (fabsf(pos_sp_triplet->current.loiter_radius - _navigator->get_loiter_radius()) > FLT_EPSILON)
-            || pos_sp_triplet->current.loiter_direction != 1
-            || pos_sp_triplet->previous.valid
-            || !pos_sp_triplet->current.valid
-            || pos_sp_triplet->next.valid) {
-        /* position setpoint triplet should be updated */
-        pos_sp_triplet->current.type = SETPOINT_TYPE_LOITER;
-        pos_sp_triplet->current.loiter_radius = _navigator->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;
-        return true;
-    }
+	} else {
+		item->nav_cmd = NAV_CMD_LOITER_UNLIMITED;
+
+		struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
+
+		if (_navigator->get_can_loiter_at_sp() && pos_sp_triplet->current.valid) {
+			/* use current position setpoint */
+			item->lat = pos_sp_triplet->current.lat;
+			item->lon = pos_sp_triplet->current.lon;
+			item->altitude = pos_sp_triplet->current.alt;
 
-    return false;
+		} else {
+			/* use current position */
+			item->lat = _navigator->get_global_position()->lat;
+			item->lon = _navigator->get_global_position()->lon;
+			item->altitude = _navigator->get_global_position()->alt;
+		}
+
+		item->altitude_is_relative = false;
+		item->yaw = NAN;
+		item->loiter_radius = _navigator->get_loiter_radius();
+		item->loiter_direction = 1;
+		item->acceptance_radius = _navigator->get_acceptance_radius();
+		item->time_inside = 0.0f;
+		item->pitch_min = 0.0f;
+		item->autocontinue = false;
+		item->origin = ORIGIN_ONBOARD;
+	}
 }
diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h
index f99002752..adf50bc39 100644
--- a/src/modules/navigator/mission_block.h
+++ b/src/modules/navigator/mission_block.h
@@ -64,6 +64,7 @@ public:
 	 */
 	virtual ~MissionBlock();
 
+protected:
 	/**
 	 * Check if mission item has been reached
 	 * @return true if successfully reached
@@ -85,22 +86,17 @@ public:
     /**
      * Set previous position setpoint to current setpoint
      */
-	void set_previous_pos_setpoint(struct position_setpoint_triplet_s *pos_sp_triplet);
+	void set_previous_pos_setpoint();
 
 	/**
-	 * Set a loiter item, if possible reuse the position setpoint, otherwise take the current position
-	 *
-	 * @param the position setpoint triplet to set
-	 * @return true if setpoint has changed
+	 * Set a loiter mission item, if possible reuse the position setpoint, otherwise take the current position
 	 */
-	bool set_loiter_item(position_setpoint_triplet_s *pos_sp_triplet);
+	void set_loiter_item(struct mission_item_s *item);
 
+	mission_item_s _mission_item;
 	bool _waypoint_position_reached;
 	bool _waypoint_yaw_reached;
 	hrt_abstime _time_first_inside_orbit;
-
-	mission_item_s _mission_item;
-	bool _mission_item_valid;
 };
 
 #endif
diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp
index dd7f4c801..606521f20 100644
--- a/src/modules/navigator/mission_feasibility_checker.cpp
+++ b/src/modules/navigator/mission_feasibility_checker.cpp
@@ -135,12 +135,15 @@ bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current,
 			}
 		}
 
-		if (home_alt > missionitem.altitude) {
+		/* calculate the global waypoint altitude */
+		float wp_alt = (missionitem.altitude_is_relative) ? missionitem.altitude + home_alt : missionitem.altitude;
+
+		if (home_alt > wp_alt) {
 			if (throw_error) {
-				mavlink_log_info(_mavlink_fd, "Waypoint %d below home", i);
+				mavlink_log_critical(_mavlink_fd, "Rejecting Mission: Waypoint %d below home", i);
 				return false;
 			} else	{
-				mavlink_log_info(_mavlink_fd, "#audio: warning waypoint %d below home", i);
+				mavlink_log_critical(_mavlink_fd, "Warning: Waypoint %d below home", i);
 				return true;
 			}
 		}
diff --git a/src/modules/navigator/mission_params.c b/src/modules/navigator/mission_params.c
index 8692328db..d15e1e771 100644
--- a/src/modules/navigator/mission_params.c
+++ b/src/modules/navigator/mission_params.c
@@ -58,12 +58,27 @@
  */
 PARAM_DEFINE_FLOAT(MIS_TAKEOFF_ALT, 10.0f);
 
-
 /**
- * Enable onboard mission
+ * Enable persistent onboard mission storage
+ *
+ * When enabled, missions that have been uploaded by the GCS are stored
+ * and reloaded after reboot persistently.
  *
  * @min 0
  * @max 1
  * @group Mission
  */
-PARAM_DEFINE_INT32(MIS_ONBOARD_EN, 0);
+PARAM_DEFINE_INT32(MIS_ONBOARD_EN, 1);
+
+/**
+ * Maximal horizontal distance from home to first waypoint
+ *
+ * Failsafe check to prevent running mission stored from previous flight at a new takeoff location.
+ * Set a value of zero or less to disable. The mission will not be started if the current
+ * waypoint is more distant than MIS_DIS_1WP from the current position.
+ *
+ * @min 0
+ * @max 1000
+ * @group Mission
+ */
+PARAM_DEFINE_FLOAT(MIS_DIST_1WP, 500);
diff --git a/src/modules/navigator/module.mk b/src/modules/navigator/module.mk
index a1e109030..b50198996 100644
--- a/src/modules/navigator/module.mk
+++ b/src/modules/navigator/module.mk
@@ -46,6 +46,7 @@ SRCS		= navigator_main.cpp \
 		  loiter.cpp \
 		  rtl.cpp \
 		  rtl_params.c \
+		  offboard.cpp \
 		  mission_feasibility_checker.cpp \
 		  geofence.cpp \
 		  geofence_params.c
@@ -53,3 +54,5 @@ SRCS		= navigator_main.cpp \
 INCLUDE_DIRS	 += $(MAVLINK_SRC)/include/mavlink
 
 MODULE_STACKSIZE = 1200
+
+EXTRACXXFLAGS	= -Weffc++
diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h
index 184ecd365..8edbb63b3 100644
--- a/src/modules/navigator/navigator.h
+++ b/src/modules/navigator/navigator.h
@@ -56,13 +56,14 @@
 #include "mission.h"
 #include "loiter.h"
 #include "rtl.h"
+#include "offboard.h"
 #include "geofence.h"
 
 /**
  * Number of navigation modes that need on_active/on_inactive calls
  * Currently: mission, loiter, and rtl
  */
-#define NAVIGATOR_MODE_ARRAY_SIZE 3
+#define NAVIGATOR_MODE_ARRAY_SIZE 4
 
 class Navigator : public control::SuperBlock
 {
@@ -103,15 +104,19 @@ public:
 	 * Setters
 	 */
 	void		set_can_loiter_at_sp(bool can_loiter) { _can_loiter_at_sp = can_loiter; }
+	void		set_position_setpoint_triplet_updated() { _pos_sp_triplet_updated = true; }
 
 	/**
 	 * Getters
 	 */
 	struct vehicle_status_s*	    get_vstatus() { return &_vstatus; }
+	struct vehicle_control_mode_s*	    get_control_mode() { return &_control_mode; }
 	struct vehicle_global_position_s*   get_global_position() { return &_global_pos; }
 	struct home_position_s*		    get_home_position() { return &_home_pos; }
+	struct position_setpoint_triplet_s*		    get_position_setpoint_triplet() { return &_pos_sp_triplet; }
 	int		get_onboard_mission_sub() { return _onboard_mission_sub; }
 	int		get_offboard_mission_sub() { return _offboard_mission_sub; }
+	int		get_offboard_control_sp_sub() { return _offboard_control_sp_sub; }
 	Geofence&	get_geofence() { return _geofence; }
 	bool		get_can_loiter_at_sp() { return _can_loiter_at_sp; }
 	float		get_loiter_radius() { return _param_loiter_radius.get(); }
@@ -129,6 +134,7 @@ private:
 	int		_home_pos_sub;			/**< home position subscription */
 	int		_vstatus_sub;			/**< vehicle status subscription */
 	int		_capabilities_sub;		/**< notification of vehicle capabilities updates */
+	int		_offboard_control_sp_sub;	/*** offboard control subscription */
 	int		_control_mode_sub;		/**< vehicle control mode subscription */
 	int		_onboard_mission_sub;		/**< onboard mission subscription */
 	int		_offboard_mission_sub;		/**< offboard mission subscription */
@@ -158,11 +164,12 @@ private:
 	Mission		_mission;			/**< class that handles the missions */
 	Loiter		_loiter;			/**< class that handles loiter */
 	RTL 		_rtl;				/**< class that handles RTL */
+	Offboard	_offboard;			/**< class that handles offboard */
 
 	NavigatorMode *_navigation_mode_array[NAVIGATOR_MODE_ARRAY_SIZE];	/**< array of navigation modes */
 
 	bool		_can_loiter_at_sp;			/**< flags if current position SP can be used to loiter */
-	bool		_update_triplet;		/**< flags if position SP triplet needs to be published */
+	bool		_pos_sp_triplet_updated;		/**< flags if position SP triplet needs to be published */
 
 	control::BlockParamFloat _param_loiter_radius;	/**< loiter radius for fixedwing */
 	control::BlockParamFloat _param_acceptance_radius;	/**< acceptance for takeoff */
@@ -215,5 +222,11 @@ private:
 	 * Publish a new position setpoint triplet for position controllers
 	 */
 	void		publish_position_setpoint_triplet();
+
+	/* this class has ptr data members, so it should not be copied,
+	 * consequently the copy constructors are private.
+	 */
+	Navigator(const Navigator&);
+	Navigator operator=(const Navigator&);
 };
 #endif
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index 546602741..331a9a728 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -67,6 +67,7 @@
 #include <uORB/topics/mission.h>
 #include <uORB/topics/fence.h>
 #include <uORB/topics/navigation_capabilities.h>
+#include <uORB/topics/offboard_control_setpoint.h>
 
 #include <systemlib/err.h>
 #include <systemlib/systemlib.h>
@@ -100,20 +101,22 @@ Navigator::Navigator() :
 	_home_pos_sub(-1),
 	_vstatus_sub(-1),
 	_capabilities_sub(-1),
+	_offboard_control_sp_sub(-1),
 	_control_mode_sub(-1),
 	_onboard_mission_sub(-1),
 	_offboard_mission_sub(-1),
+	_param_update_sub(-1),
 	_pos_sp_triplet_pub(-1),
-	_vstatus({}),
-	_control_mode({}),
-	_global_pos({}),
-	_home_pos({}),
-	_mission_item({}),
-	_nav_caps({}),
-	_pos_sp_triplet({}),
+	_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{},
 	_geofence_violation_warning_sent(false),
 	_fence_valid(false),
 	_inside_fence(true),
@@ -121,7 +124,9 @@ Navigator::Navigator() :
 	_mission(this, "MIS"),
 	_loiter(this, "LOI"),
 	_rtl(this, "RTL"),
-	_update_triplet(false),
+	_offboard(this, "OFF"),
+	_can_loiter_at_sp(false),
+	_pos_sp_triplet_updated(false),
 	_param_loiter_radius(this, "LOITER_RAD"),
 	_param_acceptance_radius(this, "ACC_RAD")
 {
@@ -129,6 +134,7 @@ Navigator::Navigator() :
 	_navigation_mode_array[0] = &_mission;
 	_navigation_mode_array[1] = &_loiter;
 	_navigation_mode_array[2] = &_rtl;
+	_navigation_mode_array[3] = &_offboard;
 
 	updateParams();
 }
@@ -241,6 +247,7 @@ Navigator::task_main()
 	_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));
+	_offboard_control_sp_sub = orb_subscribe(ORB_ID(offboard_control_setpoint));
 
 	/* copy all topics first time */
 	vehicle_status_update();
@@ -363,6 +370,9 @@ Navigator::task_main()
 				break;
 			case NAVIGATION_STATE_LAND:
 			case NAVIGATION_STATE_TERMINATION:
+			case NAVIGATION_STATE_OFFBOARD:
+				_navigation_mode = &_offboard;
+				break;
 			default:
 				_navigation_mode = nullptr;
 				_can_loiter_at_sp = false;
@@ -371,24 +381,21 @@ Navigator::task_main()
 
 		/* iterate through navigation modes and set active/inactive for each */
 		for(unsigned int i = 0; i < NAVIGATOR_MODE_ARRAY_SIZE; i++) {
-			if (_navigation_mode == _navigation_mode_array[i]) {
-				_update_triplet = _navigation_mode_array[i]->on_active(&_pos_sp_triplet);
-			} else {
-				_navigation_mode_array[i]->on_inactive();
-			}
+			_navigation_mode_array[i]->run(_navigation_mode == _navigation_mode_array[i]);
 		}
 
 		/* if nothing is running, set position setpoint triplet invalid */
 		if (_navigation_mode == nullptr) {
+			// TODO publish empty sp only once
 			_pos_sp_triplet.previous.valid = false;
 			_pos_sp_triplet.current.valid = false;
 			_pos_sp_triplet.next.valid = false;
-			_update_triplet = true;
+			_pos_sp_triplet_updated = true;
 		}
 
-		if (_update_triplet) {
+		if (_pos_sp_triplet_updated) {
 			publish_position_setpoint_triplet();
-			_update_triplet = false;
+			_pos_sp_triplet_updated = false;
 		}
 
 		perf_end(_loop_perf);
@@ -451,7 +458,7 @@ void
 Navigator::publish_position_setpoint_triplet()
 {
 	/* update navigation state */
-	/* TODO: set nav_state */
+	_pos_sp_triplet.nav_state = _vstatus.nav_state;
 
 	/* lazily publish the position setpoint triplet only once available */
 	if (_pos_sp_triplet_pub > 0) {
diff --git a/src/modules/navigator/navigator_mode.cpp b/src/modules/navigator/navigator_mode.cpp
index 25e767c2b..f43215665 100644
--- a/src/modules/navigator/navigator_mode.cpp
+++ b/src/modules/navigator/navigator_mode.cpp
@@ -33,12 +33,14 @@
 /**
  * @file navigator_mode.cpp
  *
- * Helper class for different modes in navigator
+ * Base class for different modes in navigator
  *
  * @author Julian Oes <julian@oes.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
  */
 
 #include "navigator_mode.h"
+#include "navigator.h"
 
 NavigatorMode::NavigatorMode(Navigator *navigator, const char *name) :
 	SuperBlock(NULL, name),
@@ -56,15 +58,38 @@ NavigatorMode::~NavigatorMode()
 }
 
 void
+NavigatorMode::run(bool active) {
+	if (active) {
+		if (_first_run) {
+			/* first run */
+			_first_run = false;
+			on_activation();
+
+		} else {
+			/* periodic updates when active */
+			on_active();
+		}
+
+	} else {
+		/* periodic updates when inactive */
+		_first_run = true;
+		on_inactive();
+	}
+}
+
+void
 NavigatorMode::on_inactive()
 {
-	_first_run = true;
 }
 
-bool
-NavigatorMode::on_active(struct position_setpoint_triplet_s *pos_sp_triplet)
+void
+NavigatorMode::on_activation()
+{
+	/* invalidate position setpoint by default */
+	_navigator->get_position_setpoint_triplet()->current.valid = false;
+}
+
+void
+NavigatorMode::on_active()
 {
-	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
index cbb53d91b..de5545dcb 100644
--- a/src/modules/navigator/navigator_mode.h
+++ b/src/modules/navigator/navigator_mode.h
@@ -33,9 +33,10 @@
 /**
  * @file navigator_mode.h
  *
- * Helper class for different modes in navigator
+ * Base class for different modes in navigator
  *
  * @author Julian Oes <julian@oes.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
  */
 
 #ifndef NAVIGATOR_MODE_H
@@ -65,22 +66,34 @@ public:
 	 */
 	virtual ~NavigatorMode();
 
+	void run(bool active);
+
 	/**
 	 * This function is called while the mode is inactive
 	 */
 	virtual void on_inactive();
 
 	/**
+	 * This function is called one time when mode become active, poos_sp_triplet must be initialized here
+	 */
+	virtual void on_activation();
+
+	/**
 	 * 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 on_active(struct position_setpoint_triplet_s *pos_sp_triplet);
+	virtual void on_active();
 
 protected:
 	Navigator *_navigator;
+
+private:
 	bool _first_run;
+
+	/* this class has ptr data members, so it should not be copied,
+	 * consequently the copy constructors are private.
+	 */
+	NavigatorMode(const NavigatorMode&);
+	NavigatorMode operator=(const NavigatorMode&);
 };
 
 #endif
diff --git a/src/modules/navigator/offboard.cpp b/src/modules/navigator/offboard.cpp
new file mode 100644
index 000000000..dcb5c6000
--- /dev/null
+++ b/src/modules/navigator/offboard.cpp
@@ -0,0 +1,129 @@
+/****************************************************************************
+ *
+ *   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 offboard.cpp
+ *
+ * Helper class for offboard commands
+ *
+ * @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 "navigator.h"
+#include "offboard.h"
+
+Offboard::Offboard(Navigator *navigator, const char *name) :
+	NavigatorMode(navigator, name),
+	_offboard_control_sp({0})
+{
+	/* load initial params */
+	updateParams();
+	/* initial reset */
+	on_inactive();
+}
+
+Offboard::~Offboard()
+{
+}
+
+void
+Offboard::on_inactive()
+{
+}
+
+void
+Offboard::on_activation()
+{
+}
+
+void
+Offboard::on_active()
+{
+	struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
+
+	bool updated;
+	orb_check(_navigator->get_offboard_control_sp_sub(), &updated);
+	if (updated) {
+		update_offboard_control_setpoint();
+	}
+
+	/* copy offboard setpoints to the corresponding topics */
+	if (_navigator->get_control_mode()->flag_control_position_enabled
+	    && _offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_POSITION) {
+		/* position control */
+		pos_sp_triplet->current.x = _offboard_control_sp.p1;
+		pos_sp_triplet->current.y = _offboard_control_sp.p2;
+		pos_sp_triplet->current.yaw = _offboard_control_sp.p3;
+		pos_sp_triplet->current.z = -_offboard_control_sp.p4;
+
+		pos_sp_triplet->current.type = SETPOINT_TYPE_OFFBOARD;
+		pos_sp_triplet->current.valid = true;
+		pos_sp_triplet->current.position_valid = true;
+
+		_navigator->set_position_setpoint_triplet_updated();
+
+	} else if (_navigator->get_control_mode()->flag_control_velocity_enabled
+	           && _offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY) {
+		/* velocity control */
+		pos_sp_triplet->current.vx = _offboard_control_sp.p2;
+		pos_sp_triplet->current.vy = _offboard_control_sp.p1;
+		pos_sp_triplet->current.yawspeed = _offboard_control_sp.p3;
+		pos_sp_triplet->current.vz = _offboard_control_sp.p4;
+
+		pos_sp_triplet->current.type = SETPOINT_TYPE_OFFBOARD;
+		pos_sp_triplet->current.valid = true;
+		pos_sp_triplet->current.velocity_valid = true;
+
+		_navigator->set_position_setpoint_triplet_updated();
+	}
+
+}
+
+
+void
+Offboard::update_offboard_control_setpoint()
+{
+	orb_copy(ORB_ID(offboard_control_setpoint), _navigator->get_offboard_control_sp_sub(), &_offboard_control_sp);
+
+}
diff --git a/src/modules/navigator/offboard.h b/src/modules/navigator/offboard.h
new file mode 100644
index 000000000..66b923bdb
--- /dev/null
+++ b/src/modules/navigator/offboard.h
@@ -0,0 +1,72 @@
+/***************************************************************************
+ *
+ *   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 offboard.h
+ *
+ * Helper class for offboard commands
+ *
+ * @author Julian Oes <julian@oes.ch>
+ */
+
+#ifndef NAVIGATOR_OFFBOARD_H
+#define NAVIGATOR_OFFBOARD_H
+
+#include <controllib/blocks.hpp>
+#include <controllib/block/BlockParam.hpp>
+
+#include <uORB/uORB.h>
+#include <uORB/topics/offboard_control_setpoint.h>
+
+#include "navigator_mode.h"
+
+class Navigator;
+
+class Offboard : public NavigatorMode
+{
+public:
+	Offboard(Navigator *navigator, const char *name);
+
+	~Offboard();
+
+	virtual void on_inactive();
+
+	virtual void on_activation();
+
+	virtual void on_active();
+private:
+	void update_offboard_control_setpoint();
+
+	struct offboard_control_setpoint_s _offboard_control_sp;
+};
+
+#endif
diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp
index 597a2c0ec..142a73409 100644
--- a/src/modules/navigator/rtl.cpp
+++ b/src/modules/navigator/rtl.cpp
@@ -75,62 +75,57 @@ RTL::~RTL()
 void
 RTL::on_inactive()
 {
-	_first_run = true;
-
 	/* reset RTL state only if setpoint moved */
 	if (!_navigator->get_can_loiter_at_sp()) {
 		_rtl_state = RTL_STATE_NONE;
 	}
 }
 
-bool
-RTL::on_active(struct position_setpoint_triplet_s *pos_sp_triplet)
+void
+RTL::on_activation()
 {
-	bool updated = false;
-
-	if (_first_run) {
-		_first_run = false;
-
-		/* 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_LANDED;
-				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 {
-				/* set altitude setpoint to current altitude */
-				_rtl_state = RTL_STATE_RETURN;
-				_mission_item.altitude_is_relative = false;
-				_mission_item.altitude = _navigator->get_global_position()->alt;
-			}
+	/* 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_LANDED;
+			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 {
+			/* set altitude setpoint to current altitude */
+			_rtl_state = RTL_STATE_RETURN;
+			_mission_item.altitude_is_relative = false;
+			_mission_item.altitude = _navigator->get_global_position()->alt;
 		}
+	}
 
-		set_rtl_item(pos_sp_triplet);
-		updated = true;
+	set_rtl_item();
+}
 
-	} else if (_rtl_state != RTL_STATE_LANDED && is_mission_item_reached()) {
+void
+RTL::on_active()
+{
+	if (_rtl_state != RTL_STATE_LANDED && is_mission_item_reached()) {
 		advance_rtl();
-		set_rtl_item(pos_sp_triplet);
-		updated = true;
+		set_rtl_item();
 	}
-
-	return updated;
 }
 
 void
-RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
+RTL::set_rtl_item()
 {
+	struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
+
 	/* make sure we have the latest params */
 	updateParams();
 
-	set_previous_pos_setpoint(pos_sp_triplet);
+	set_previous_pos_setpoint();
 	_navigator->set_can_loiter_at_sp(false);
 
 	switch (_rtl_state) {
@@ -277,11 +272,13 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
 		break;
 	}
 
+	reset_mission_item_reached();
+
 	/* 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;
+
+	_navigator->set_position_setpoint_triplet_updated();
 }
 
 void
diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h
index b4b729e89..5928f1f07 100644
--- a/src/modules/navigator/rtl.h
+++ b/src/modules/navigator/rtl.h
@@ -57,35 +57,21 @@ class Navigator;
 class RTL : public MissionBlock
 {
 public:
-	/**
-	 * Constructor
-	 */
 	RTL(Navigator *navigator, const char *name);
 
-	/**
-	 * Destructor
-	 */
 	~RTL();
 
-	/**
-	 * This function is called while the mode is inactive
-	 */
-	void on_inactive();
+	virtual void on_inactive();
 
-	/**
-	 * This function is called while the mode is active
-	 *
-	 * @param position setpoint triplet that needs to be set
-	 * @return true if updated
-	 */
-	bool on_active(position_setpoint_triplet_s *pos_sp_triplet);
+	virtual void on_activation();
 
+	virtual void on_active();
 
 private:
 	/**
 	 * Set the RTL item
 	 */
-	void		set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet);
+	void		set_rtl_item();
 
 	/**
 	 * Move to next RTL item