navigator: new class structure, loiter and mission working

13 files changed, 564 insertions, 242 deletions

diff --git a/src/modules/navigator/loiter.cpp b/src/modules/navigator/loiter.cpp
index 6da08f072..035d035e1 100644
--- a/src/modules/navigator/loiter.cpp
+++ b/src/modules/navigator/loiter.cpp
@@ -53,7 +53,8 @@
 #include "loiter.h"
 
 Loiter::Loiter(Navigator *navigator, const char *name) :
-	Mission(navigator, name)
+	NavigatorMode(navigator, name),
+	MissionBlock(navigator)
 {
 	/* load initial params */
 	updateParams();
diff --git a/src/modules/navigator/loiter.h b/src/modules/navigator/loiter.h
index 4ae265e44..a83b53f43 100644
--- a/src/modules/navigator/loiter.h
+++ b/src/modules/navigator/loiter.h
@@ -44,11 +44,10 @@
 #include <controllib/blocks.hpp>
 #include <controllib/block/BlockParam.hpp>
 
-#include "mission.h"
+#include "navigator_mode.h"
+#include "mission_block.h"
 
-class Navigator;
-
-class Loiter : public Mission
+class Loiter : public NavigatorMode, MissionBlock
 {
 public:
 	/**
@@ -59,11 +58,17 @@ public:
 	/**
 	 * Destructor
 	 */
-	virtual ~Loiter();
+	~Loiter();
 
-	virtual bool update(struct position_setpoint_triplet_s *pos_sp_triplet);
+	/**
+	 * This function is called while the mode is inactive
+	 */
+	bool update(struct position_setpoint_triplet_s *pos_sp_triplet);
 
-	virtual void reset();
+	/**
+	 * 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
index 839c4c960..167f24ca6 100644
--- a/src/modules/navigator/mission.cpp
+++ b/src/modules/navigator/mission.cpp
@@ -56,18 +56,15 @@
 #include "navigator.h"
 #include "mission.h"
 
-
 Mission::Mission(Navigator *navigator, const char *name) :
-	SuperBlock(NULL, name),
-	_navigator(navigator),
-	_first_run(true),
+	NavigatorMode(navigator, name),
+	MissionBlock(navigator),
 	_param_onboard_enabled(this, "ONBOARD_EN"),
 	_param_loiter_radius(this, "LOITER_RAD"),
 	_onboard_mission({0}),
 	_offboard_mission({0}),
 	_current_onboard_mission_index(-1),
 	_current_offboard_mission_index(-1),
-	_mission_item({0}),
 	_mission_result_pub(-1),
 	_mission_result({0}),
 	_mission_type(MISSION_TYPE_NONE)
@@ -92,14 +89,23 @@ Mission::reset()
 bool
 Mission::update(struct position_setpoint_triplet_s *pos_sp_triplet)
 {
+	bool updated;
+
 	/* check if anything has changed */
-	bool onboard_updated = is_onboard_mission_updated();
-	bool offboard_updated = is_offboard_mission_updated();
+	bool onboard_updated;
+	orb_check(_navigator->get_onboard_mission_sub(), &onboard_updated);
+	if (onboard_updated) {
+		update_onboard_mission();
+	}
 
-	bool updated = false;
+	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) {
+	if (onboard_updated || offboard_updated || _first_run) {
 		set_mission_items(pos_sp_triplet);
 		updated = true;
 		_first_run = false;
@@ -115,164 +121,9 @@ Mission::update(struct position_setpoint_triplet_s *pos_sp_triplet)
 	return updated;
 }
 
-bool
-Mission::is_mission_item_reached()
-{
-	/* TODO: count turns */
-#if 0
-	if ((_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
-	     _mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
-	     _mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED) &&
-	    _mission_item.loiter_radius > 0.01f) {
-
-		return false;
-	}
-#endif
-
-	hrt_abstime now = hrt_absolute_time();
-
-	if (!_waypoint_position_reached) {
-
-		float dist = -1.0f;
-		float dist_xy = -1.0f;
-		float dist_z = -1.0f;
-
-		float altitude_amsl = _mission_item.altitude_is_relative
-				      ? _mission_item.altitude + _navigator->get_home_position()->alt
-			              : _mission_item.altitude;
-
-		dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, altitude_amsl,
-				                          _navigator->get_global_position()->lat,
-							  _navigator->get_global_position()->lon,
-							  _navigator->get_global_position()->alt,
-				&dist_xy, &dist_z);
-
-		if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF) {
-
-			/* require only altitude for takeoff */
-			if (_navigator->get_global_position()->alt > altitude_amsl - _mission_item.acceptance_radius) {
-				_waypoint_position_reached = true;
-			}
-		} else {
-			if (dist >= 0.0f && dist <= _mission_item.acceptance_radius) {
-				_waypoint_position_reached = true;
-			}
-		}
-	}
-
-	if (_waypoint_position_reached && !_waypoint_yaw_reached) {
-
-		/* TODO: removed takeoff, why? */
-		if (_navigator->get_vstatus()->is_rotary_wing && isfinite(_mission_item.yaw)) {
-
-			/* check yaw if defined only for rotary wing except takeoff */
-			float yaw_err = _wrap_pi(_mission_item.yaw - _navigator->get_global_position()->yaw);
-
-			if (fabsf(yaw_err) < 0.2f) { /* TODO: get rid of magic number */
-				_waypoint_yaw_reached = true;
-			}
-
-		} else {
-			_waypoint_yaw_reached = true;
-		}
-	}
-
-	/* check if the current waypoint was reached */
-	if (_waypoint_position_reached && _waypoint_yaw_reached) {
-
-		if (_time_first_inside_orbit == 0) {
-			_time_first_inside_orbit = now;
-
-			// if (_mission_item.time_inside > 0.01f) {
-			// 	mavlink_log_info(_mavlink_fd, "#audio: waypoint reached, wait for %.1fs",
-			// 		(double)_mission_item.time_inside);
-			// }
-		}
-
-		/* check if the MAV was long enough inside the waypoint orbit */
-		if (now - _time_first_inside_orbit >= (hrt_abstime)_mission_item.time_inside * 1e6f) {
-			return true;
-		}
-	}
-	return false;
-}
-
 void
-Mission::reset_mission_item_reached()
-{
-	_waypoint_position_reached = false;
-	_waypoint_yaw_reached = false;
-	_time_first_inside_orbit = 0;
-}
-
-void
-Mission::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->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
-Mission::set_loiter_item(bool reuse_current_pos_sp, struct position_setpoint_triplet_s *pos_sp_triplet)
+Mission::update_onboard_mission()
 {
-	if (_navigator->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->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 */
-	}
-	pos_sp_triplet->current.type = SETPOINT_TYPE_LOITER;
-	pos_sp_triplet->current.loiter_radius = _param_loiter_radius.get();
-	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->set_is_in_loiter(true);
-	return true;
-}
-
-
-bool
-Mission::is_onboard_mission_updated()
-{
-	bool updated;
-	orb_check(_navigator->get_onboard_mission_sub(), &updated);
-
-	if (!updated && !_first_run) {
-		return false;
-	}
-
 	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
@@ -293,18 +144,11 @@ Mission::is_onboard_mission_updated()
 		_onboard_mission.current_index = 0;
 		_current_onboard_mission_index = 0;
 	}
-	return true;
 }
 
-bool
-Mission::is_offboard_mission_updated()
+void
+Mission::update_offboard_mission()
 {
-	bool updated;
-	orb_check(_navigator->get_offboard_mission_sub(), &updated);
-
-	if (!updated && !_first_run) {
-		return false;
-	}
 	if (orb_copy(ORB_ID(offboard_mission), _navigator->get_offboard_mission_sub(), &_offboard_mission) == OK) {
 
 		/* determine current index */
@@ -341,7 +185,6 @@ Mission::is_offboard_mission_updated()
 		_offboard_mission.current_index = 0;
 		_current_offboard_mission_index = 0;
 	}
-	return true;
 }
 
 
@@ -381,7 +224,6 @@ Mission::set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet)
 		_mission_type = MISSION_TYPE_NONE;
 
 		bool use_current_pos_sp = pos_sp_triplet->current.valid && _waypoint_position_reached;
-		reset_mission_item_reached();
 		set_loiter_item(use_current_pos_sp, pos_sp_triplet);
 	}
 }
diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h
index 4d0083d85..ad8cb467c 100644
--- a/src/modules/navigator/mission.h
+++ b/src/modules/navigator/mission.h
@@ -33,7 +33,7 @@
 /**
  * @file mission.h
  *
- * Helper class to access missions
+ * Navigator mode to access missions
  *
  * @author Julian Oes <julian@oes.ch>
  */
@@ -50,16 +50,19 @@
 
 #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 control::SuperBlock
+class Mission : public NavigatorMode, MissionBlock
 {
 public:
 	/**
@@ -82,42 +85,16 @@ public:
 	 */
 	virtual bool update(struct position_setpoint_triplet_s *pos_sp_triplet);
 
-protected:
-	/**
-	 * Check if mission item has been reached
-	 * @return true if successfully reached
-	 */
-	bool is_mission_item_reached();
-	/**
-	 * Reset all reached flags
-	 */
-	void reset_mission_item_reached();
-
-	/**
-	 * Convert a mission item to a position setpoint
-	 */
-	void mission_item_to_position_setpoint(const struct mission_item_s *item, struct position_setpoint_s *sp);
-
-	/**
-	 * Set a loiter item, if possible reuse the position setpoint, otherwise take the current position
-	 * @return true if setpoint has changed
-	 */
-	bool set_loiter_item(bool reuse_current_pos_sp, struct position_setpoint_triplet_s *pos_sp_triplet);
-
-	class Navigator *_navigator;
-
 private:
 	/**
 	 * Update onboard mission topic
-	 * @return true if onboard mission has been updated
 	 */
-	bool is_onboard_mission_updated();
+	void update_onboard_mission();
 
 	/**
 	 * Update offboard mission topic
-	 * @return true if offboard mission has been updated
 	 */
-	bool is_offboard_mission_updated();
+	void update_offboard_mission();
 
 	/**
 	 * Move on to next mission item or switch to loiter
@@ -179,12 +156,6 @@ private:
 	 */
 	void publish_mission_result();
 
-	bool _waypoint_position_reached;
-	bool _waypoint_yaw_reached;
-	hrt_abstime _time_first_inside_orbit;
-
-	bool _first_run;
-
 	control::BlockParamFloat _param_onboard_enabled;
 	control::BlockParamFloat _param_loiter_radius;
 
@@ -194,8 +165,6 @@ private:
 	int _current_onboard_mission_index;
 	int _current_offboard_mission_index;
 
-	struct mission_item_s _mission_item;
-
 	orb_advert_t _mission_result_pub;
 	struct mission_result_s _mission_result;
 
@@ -206,7 +175,6 @@ private:
 	} _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..92090d995
--- /dev/null
+++ b/src/modules/navigator/mission_block.cpp
@@ -0,0 +1,215 @@
+/****************************************************************************
+ *
+ *   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()
+{
+	/* 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) {
+
+			/* require only altitude for takeoff */
+			if (_navigator_priv->get_global_position()->alt > altitude_amsl - _mission_item.acceptance_radius) {
+				_waypoint_position_reached = true;
+			}
+		} else {
+			if (dist >= 0.0f && dist <= _mission_item.acceptance_radius) {
+				_waypoint_position_reached = true;
+			}
+		}
+	}
+
+	if (_waypoint_position_reached && !_waypoint_yaw_reached) {
+
+		/* TODO: removed takeoff, why? */
+		if (_navigator_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/mission_block.h b/src/modules/navigator/mission_block.h
new file mode 100644
index 000000000..47e6fe81f
--- /dev/null
+++ b/src/modules/navigator/mission_block.h
@@ -0,0 +1,105 @@
+/****************************************************************************
+ *
+ *   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.h
+ *
+ * Helper class to use mission items
+ *
+ * @author Julian Oes <julian@oes.ch>
+ */
+
+#ifndef NAVIGATOR_MISSION_BLOCK_H
+#define NAVIGATOR_MISSION_BLOCK_H
+
+#include <drivers/drv_hrt.h>
+
+#include <uORB/topics/mission.h>
+#include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/position_setpoint_triplet.h>
+
+class Navigator;
+
+class MissionBlock
+{
+public:
+	/**
+	 * Constructor
+	 *
+	 * @param pointer to parent class
+	 */
+	MissionBlock(Navigator *navigator);
+
+	/**
+	 * Destructor
+	 */
+	virtual ~MissionBlock();
+
+	/**
+	 * 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();
+
+	/**
+	 * 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);
+
+	/**
+	 * 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);
+
+	bool _waypoint_position_reached;
+	bool _waypoint_yaw_reached;
+	hrt_abstime _time_first_inside_orbit;
+
+	mission_item_s _mission_item;
+	bool _mission_item_valid;
+
+private:
+	Navigator *_navigator_priv;
+};
+
+#endif
diff --git a/src/modules/navigator/module.mk b/src/modules/navigator/module.mk
index f4243c5b8..c7cba64cc 100644
--- a/src/modules/navigator/module.mk
+++ b/src/modules/navigator/module.mk
@@ -38,6 +38,8 @@
 MODULE_COMMAND	= navigator
 
 SRCS		= navigator_main.cpp \
+		  navigator_mode.cpp \
+		  mission_block.cpp \
 		  mission.cpp \
 		  mission_params.c \
 		  loiter.cpp \
diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h
index 0c551bb41..476e9a36c 100644
--- a/src/modules/navigator/navigator.h
+++ b/src/modules/navigator/navigator.h
@@ -48,6 +48,7 @@
 #include <uORB/topics/position_setpoint_triplet.h>
 #include <uORB/topics/vehicle_global_position.h>
 
+#include "navigator_mode.h"
 #include "mission.h"
 #include "loiter.h"
 #include "rtl.h"
@@ -104,6 +105,8 @@ public:
 	Geofence&	get_geofence() { return _geofence; }
 	bool		get_is_in_loiter() { return _is_in_loiter; }
 
+	float		get_loiter_radius() { return 50.0f; }; /* TODO: make param*/
+
 private:
 
 	bool		_task_should_exit;		/**< if true, sensor task should exit */
@@ -139,6 +142,7 @@ private:
 	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 */
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index 44c1075c1..fe1f4893e 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -118,6 +118,7 @@ Navigator::Navigator() :
 	_geofence_violation_warning_sent(false),
 	_fence_valid(false),
 	_inside_fence(true),
+	_navigation_mode(nullptr),
 	_mission(this, "MIS"),
 	_loiter(this, "LOI"),
 	_rtl(this, "RTL"),
@@ -321,32 +322,47 @@ Navigator::task_main()
 			case NAVIGATION_STATE_ACRO:
 			case NAVIGATION_STATE_ALTCTL:
 			case NAVIGATION_STATE_POSCTL:
-				_mission.reset();
-				_loiter.reset();
-				_rtl.reset();
+				_navigation_mode = nullptr;
 				_is_in_loiter = false;
 				break;
 			case NAVIGATION_STATE_AUTO_MISSION:
-				_update_triplet = _mission.update(&_pos_sp_triplet);
+				_navigation_mode = &_mission;
 				break;
 			case NAVIGATION_STATE_AUTO_LOITER:
-				_update_triplet = _loiter.update(&_pos_sp_triplet);
+				_navigation_mode = &_loiter;
 				break;
 			case NAVIGATION_STATE_AUTO_RTL:
 			case NAVIGATION_STATE_AUTO_RTL_RC:
 			case NAVIGATION_STATE_AUTO_RTL_DL:
-				_update_triplet = _rtl.update(&_pos_sp_triplet);
+				_navigation_mode = &_rtl;
 				break;
 			case NAVIGATION_STATE_LAND:
 			case NAVIGATION_STATE_TERMINATION:
 			default:
-				_mission.reset();
-				_loiter.reset();
-				_rtl.reset();
+				_navigation_mode = nullptr;
 				_is_in_loiter = false;
 				break;
 		}
 
+		/* TODO: make list of modes and loop through it */
+		if (_navigation_mode == &_mission) {
+			_update_triplet = _mission.update(&_pos_sp_triplet);
+		} else {
+			_mission.reset();
+		}
+
+		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 (_update_triplet ) {
 			publish_position_setpoint_triplet();
 			_update_triplet = 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/rtl.cpp b/src/modules/navigator/rtl.cpp
index d4e609584..774aa802d 100644
--- a/src/modules/navigator/rtl.cpp
+++ b/src/modules/navigator/rtl.cpp
@@ -52,13 +52,13 @@
 #include "rtl.h"
 
 RTL::RTL(Navigator *navigator, const char *name) :
-	Mission(navigator, name),
+	NavigatorMode(navigator, name),
+	MissionBlock(navigator),
 	_mavlink_fd(-1),
 	_rtl_state(RTL_STATE_NONE),
 	_home_position({}),
 	_loiter_radius(50),
 	_acceptance_radius(50),
-	_param_loiter_rad(this, "LOITER_RAD"),
 	_param_return_alt(this, "RETURN_ALT"),
 	_param_descend_alt(this, "DESCEND_ALT"),
 	_param_land_delay(this, "LAND_DELAY")
diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h
index d84fd1a6f..9b32fbb0c 100644
--- a/src/modules/navigator/rtl.h
+++ b/src/modules/navigator/rtl.h
@@ -31,8 +31,9 @@
  *
  ****************************************************************************/
 /**
- * @file navigator_mission.h
- * Helper class to access missions
+ * @file navigator_rtl.h
+ * Helper class for RTL
+ *
  * @author Julian Oes <julian@oes.ch>
  * @author Anton Babushkin <anton.babushkin@me.com>
  */
@@ -48,11 +49,12 @@
 #include <uORB/topics/home_position.h>
 #include <uORB/topics/vehicle_global_position.h>
 
-#include "mission.h"
+#include "navigator_mode.h"
+#include "mission_block.h"
 
 class Navigator;
 
-class RTL : public Mission
+class RTL : public NavigatorMode, MissionBlock
 {
 public:
 	/**
@@ -63,11 +65,18 @@ public:
 	/**
 	 * Destructor
 	 */
-	virtual ~RTL();
+	~RTL();
+
+	/**
+	 * This function is called while the mode is inactive
+	 */
+	void reset();
 
-	virtual bool update(struct position_setpoint_triplet_s *pos_sp_triplet);
+	/**
+	 * This function is called while the mode is active
+	 */
+	bool update(struct position_setpoint_triplet_s *pos_sp_triplet);
 
-	virtual void reset();
 
 private:
 	void		set_home_position(const home_position_s *home_position);
@@ -92,7 +101,6 @@ private:
 	float		_loiter_radius;
 	float		_acceptance_radius;
 
-	control::BlockParamFloat _param_loiter_rad;
 	control::BlockParamFloat _param_return_alt;
 	control::BlockParamFloat _param_descend_alt;
 	control::BlockParamFloat _param_land_delay;