Merge branch 'navigator_rewrite' into navigator_rewrite_offboard2_merge

12 files changed, 218 insertions, 515 deletions

diff --git a/src/modules/navigator/loiter.cpp b/src/modules/navigator/loiter.cpp
index 8bbb79d5e..542483fb1 100644
--- a/src/modules/navigator/loiter.cpp
+++ b/src/modules/navigator/loiter.cpp
@@ -53,8 +53,7 @@
 #include "loiter.h"
 
 Loiter::Loiter(Navigator *navigator, const char *name) :
-	NavigatorMode(navigator, name),
-	MissionBlock(navigator)
+	MissionBlock(navigator, name)
 {
 	/* load initial params */
 	updateParams();
@@ -70,11 +69,10 @@ bool
 Loiter::on_active(struct position_setpoint_triplet_s *pos_sp_triplet)
 {
 	/* set loiter item, don't reuse an existing position setpoint */
-	return set_loiter_item(false, pos_sp_triplet);;
+	return set_loiter_item(pos_sp_triplet);
 }
 
 void
 Loiter::on_inactive()
 {
 }
-
diff --git a/src/modules/navigator/loiter.h b/src/modules/navigator/loiter.h
index 685ae6141..65ff5c31e 100644
--- a/src/modules/navigator/loiter.h
+++ b/src/modules/navigator/loiter.h
@@ -47,7 +47,7 @@
 #include "navigator_mode.h"
 #include "mission_block.h"
 
-class Loiter : public NavigatorMode, MissionBlock
+class Loiter : public MissionBlock
 {
 public:
 	/**
diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp
index 9244063b1..72255103b 100644
--- a/src/modules/navigator/mission.cpp
+++ b/src/modules/navigator/mission.cpp
@@ -58,8 +58,7 @@
 #include "mission.h"
 
 Mission::Mission(Navigator *navigator, const char *name) :
-	NavigatorMode(navigator, name),
-	MissionBlock(navigator),
+	MissionBlock(navigator, name),
 	_param_onboard_enabled(this, "ONBOARD_EN"),
 	_onboard_mission({0}),
 	_offboard_mission({0}),
@@ -223,7 +222,7 @@ Mission::advance_mission()
 void
 Mission::set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet)
 {
-	set_previous_pos_setpoint(&pos_sp_triplet->current, &pos_sp_triplet->previous);
+	set_previous_pos_setpoint(pos_sp_triplet);
 
 	/* try setting onboard mission item */
 	if (is_current_onboard_mission_item_set(&pos_sp_triplet->current)) {
@@ -233,7 +232,7 @@ Mission::set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet)
 					"#audio: onboard mission running");
 		}
 		_mission_type = MISSION_TYPE_ONBOARD;
-		_navigator->set_is_in_loiter(false);
+		_navigator->set_can_loiter_at_sp(false);
 
 	/* try setting offboard mission item */
 	} else if (is_current_offboard_mission_item_set(&pos_sp_triplet->current)) {
@@ -243,7 +242,7 @@ Mission::set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet)
 					"#audio: offboard mission running");
 		}
 		_mission_type = MISSION_TYPE_OFFBOARD;
-		_navigator->set_is_in_loiter(false);
+		_navigator->set_can_loiter_at_sp(false);
 	} else {
 		if (_mission_type != MISSION_TYPE_NONE) {
 			mavlink_log_info(_navigator->get_mavlink_fd(),
@@ -253,24 +252,14 @@ Mission::set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet)
 					"#audio: no mission available");
 		}
 		_mission_type = MISSION_TYPE_NONE;
+        _navigator->set_can_loiter_at_sp(pos_sp_triplet->current.valid && _waypoint_position_reached);
 
-		bool use_current_pos_sp = pos_sp_triplet->current.valid && _waypoint_position_reached;
-		set_loiter_item(use_current_pos_sp, pos_sp_triplet);
+		set_loiter_item(pos_sp_triplet);
 		reset_mission_item_reached();
 		report_mission_finished();
 	}
 }
 
-void
-Mission::set_previous_pos_setpoint(const struct position_setpoint_s *current_pos_sp,
-		                   struct position_setpoint_s *previous_pos_sp)
-{
-	/* reuse current setpoint as previous setpoint */
-	if (current_pos_sp->valid) {
-		memcpy(previous_pos_sp, current_pos_sp, sizeof(struct position_setpoint_s));
-	}
-}
-
 bool
 Mission::is_current_onboard_mission_item_set(struct position_setpoint_s *current_pos_sp)
 {
diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h
index 322aaf96a..6e4761946 100644
--- a/src/modules/navigator/mission.h
+++ b/src/modules/navigator/mission.h
@@ -62,7 +62,7 @@
 
 class Navigator;
 
-class Mission : public NavigatorMode, MissionBlock
+class Mission : public MissionBlock
 {
 public:
 	/**
@@ -107,12 +107,6 @@ private:
 	void set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet);
 
 	/**
-	 * Set previous position setpoint
-	 */
-	void set_previous_pos_setpoint(const struct position_setpoint_s *current_pos_sp,
-			               struct position_setpoint_s *previous_pos_sp);
-
-	/**
 	 * Try to set the current position setpoint from an onboard mission item
 	 * @return true if mission item successfully set
 	 */
diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp
index 08576750c..9b8d3d9c7 100644
--- a/src/modules/navigator/mission_block.cpp
+++ b/src/modules/navigator/mission_block.cpp
@@ -52,13 +52,13 @@
 #include "mission_block.h"
 
 
-MissionBlock::MissionBlock(Navigator *navigator) :
+MissionBlock::MissionBlock(Navigator *navigator, const char *name) :
+	NavigatorMode(navigator, name),
 	_waypoint_position_reached(false),
 	_waypoint_yaw_reached(false),
 	_time_first_inside_orbit(0),
 	_mission_item({0}),
-	_mission_item_valid(false),
-	_navigator_priv(navigator)
+	_mission_item_valid(false)
 {
 }
 
@@ -69,20 +69,15 @@ MissionBlock::~MissionBlock()
 bool
 MissionBlock::is_mission_item_reached()
 {
-	/* don't check landed WPs */
 	if (_mission_item.nav_cmd == NAV_CMD_LAND) {
-		return false;
+		return _navigator->get_vstatus()->condition_landed;
 	}
-	/* 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) {
 
+	/* TODO: count turns */
+	if ((/*_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||*/
+	     _mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED)) {
 		return false;
 	}
-#endif
 
 	hrt_abstime now = hrt_absolute_time();
 
@@ -93,24 +88,24 @@ MissionBlock::is_mission_item_reached()
 		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 + _navigator->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,
+				                          _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 && _navigator_priv->get_vstatus()->is_rotary_wing) {
+		if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF && _navigator->get_vstatus()->is_rotary_wing) {
 			/* require only altitude for takeoff for multicopter */
-			if (_navigator_priv->get_global_position()->alt >
-				altitude_amsl - _navigator_priv->get_takeoff_acceptance_radius()) {
+			if (_navigator->get_global_position()->alt >
+				altitude_amsl - _navigator->get_acceptance_radius()) {
 				_waypoint_position_reached = true;
 			}
 		} else if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF) {
 			/* for takeoff mission items use the parameter for the takeoff acceptance radius */
-			if (dist >= 0.0f && dist <= _navigator_priv->get_takeoff_acceptance_radius()) {
+			if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius()) {
 				_waypoint_position_reached = true;
 			}
 		} else {
@@ -124,10 +119,10 @@ MissionBlock::is_mission_item_reached()
 	if (_waypoint_position_reached && !_waypoint_yaw_reached) {
 
 		/* TODO: removed takeoff, why? */
-		if (_navigator_priv->get_vstatus()->is_rotary_wing && isfinite(_mission_item.yaw)) {
+		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_priv->get_global_position()->yaw);
+			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;
@@ -172,54 +167,76 @@ MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *ite
 	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->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) {
+	switch (item->nav_cmd) {
+	case NAV_CMD_IDLE:
+		sp->type = SETPOINT_TYPE_IDLE;
+		break;
+
+	case NAV_CMD_TAKEOFF:
 		sp->type = SETPOINT_TYPE_TAKEOFF;
+		break;
 
-	} else if (item->nav_cmd == NAV_CMD_LAND) {
+	case NAV_CMD_LAND:
 		sp->type = SETPOINT_TYPE_LAND;
+		break;
 
-	} 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) {
+	case NAV_CMD_LOITER_TIME_LIMIT:
+	case NAV_CMD_LOITER_TURN_COUNT:
+	case NAV_CMD_LOITER_UNLIMITED:
 		sp->type = SETPOINT_TYPE_LOITER;
+		break;
 
-	} else {
+	default:
 		sp->type = SETPOINT_TYPE_POSITION;
+		break;
 	}
 }
 
-bool
-MissionBlock::set_loiter_item(bool reuse_current_pos_sp, struct position_setpoint_triplet_s *pos_sp_triplet)
+void
+MissionBlock::set_previous_pos_setpoint(struct position_setpoint_triplet_s *pos_sp_triplet)
 {
-	if (_navigator_priv->get_is_in_loiter()) {
-		/* already loitering, bail out */
-		return false;
-	}
+    /* reuse current setpoint as previous setpoint */
+    if (pos_sp_triplet->current.valid) {
+        memcpy(&pos_sp_triplet->previous, &pos_sp_triplet->current, sizeof(struct position_setpoint_s));
+    }
+}
 
-	if (reuse_current_pos_sp && pos_sp_triplet->current.valid) {
-		/* leave position setpoint as is */
-	} else {
+bool
+MissionBlock::set_loiter_item(struct position_setpoint_triplet_s *pos_sp_triplet)
+{
+    /* 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_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.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 = _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->set_can_loiter_at_sp(true);
+	}
 
-	_navigator_priv->set_is_in_loiter(true);
-	return true;
+    if (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
+            || !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;
+    }
+
+    return false;
 }
-
diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h
index 47e6fe81f..f99002752 100644
--- a/src/modules/navigator/mission_block.h
+++ b/src/modules/navigator/mission_block.h
@@ -47,17 +47,17 @@
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/position_setpoint_triplet.h>
 
+#include "navigator_mode.h"
+
 class Navigator;
 
-class MissionBlock
+class MissionBlock : public NavigatorMode
 {
 public:
 	/**
 	 * Constructor
-	 *
-	 * @param pointer to parent class
 	 */
-	MissionBlock(Navigator *navigator);
+	MissionBlock(Navigator *navigator, const char *name);
 
 	/**
 	 * Destructor
@@ -82,14 +82,18 @@ public:
 	 */
 	void mission_item_to_position_setpoint(const mission_item_s *item, position_setpoint_s *sp);
 
+    /**
+     * Set previous position setpoint to current setpoint
+     */
+	void set_previous_pos_setpoint(struct position_setpoint_triplet_s *pos_sp_triplet);
+
 	/**
 	 * 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 set_loiter_item(position_setpoint_triplet_s *pos_sp_triplet);
 
 	bool _waypoint_position_reached;
 	bool _waypoint_yaw_reached;
@@ -97,9 +101,6 @@ public:
 
 	mission_item_s _mission_item;
 	bool _mission_item_valid;
-
-private:
-	Navigator *_navigator_priv;
 };
 
 #endif
diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h
index e6ff213bd..b2c4929cf 100644
--- a/src/modules/navigator/navigator.h
+++ b/src/modules/navigator/navigator.h
@@ -103,7 +103,7 @@ public:
 	/**
 	 * Setters
 	 */
-	void		set_is_in_loiter(bool is_in_loiter) { _is_in_loiter = is_in_loiter; }
+	void		set_can_loiter_at_sp(bool can_loiter) { _can_loiter_at_sp = can_loiter; }
 
 	/**
 	 * Getters
@@ -117,9 +117,9 @@ public:
 	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_is_in_loiter() { return _is_in_loiter; }
+	bool		get_can_loiter_at_sp() { return _can_loiter_at_sp; }
 	float		get_loiter_radius() { return _param_loiter_radius.get(); }
-	float		get_takeoff_acceptance_radius() { return _param_takeoff_acceptance_radius.get(); }
+	float		get_acceptance_radius() { return _param_acceptance_radius.get(); }
 	int		get_mavlink_fd() { return _mavlink_fd; }
 
 private:
@@ -167,11 +167,11 @@ private:
 
 	NavigatorMode *_navigation_mode_array[NAVIGATOR_MODE_ARRAY_SIZE];	/**< array of navigation modes */
 
-	bool		_is_in_loiter;			/**< flags if current position SP can be used to loiter */
+	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 */
 
 	control::BlockParamFloat _param_loiter_radius;	/**< loiter radius for fixedwing */
-	control::BlockParamFloat _param_takeoff_acceptance_radius;	/**< acceptance for takeoff */
+	control::BlockParamFloat _param_acceptance_radius;	/**< acceptance for takeoff */
 	/**
 	 * Retrieve global position
 	 */
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index 8ba835a87..c8420c0e4 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -126,7 +126,7 @@ Navigator::Navigator() :
 	_offboard(this, "OFF"),
 	_update_triplet(false),
 	_param_loiter_radius(this, "LOITER_RAD"),
-	_param_takeoff_acceptance_radius(this, "TF_ACC_RAD")
+	_param_acceptance_radius(this, "ACC_RAD")
 {
 	/* Create a list of our possible navigation types */
 	_navigation_mode_array[0] = &_mission;
@@ -352,7 +352,7 @@ Navigator::task_main()
 			case NAVIGATION_STATE_ALTCTL:
 			case NAVIGATION_STATE_POSCTL:
 				_navigation_mode = nullptr;
-				_is_in_loiter = false;
+				_can_loiter_at_sp = false;
 				break;
 			case NAVIGATION_STATE_AUTO_MISSION:
 				_navigation_mode = &_mission;
@@ -373,7 +373,7 @@ Navigator::task_main()
 				break;
 			default:
 				_navigation_mode = nullptr;
-				_is_in_loiter = false;
+				_can_loiter_at_sp = false;
 				break;
 		}
 
@@ -394,7 +394,7 @@ Navigator::task_main()
 			_update_triplet = true;
 		}
 
-		if (_update_triplet ) {
+		if (_update_triplet) {
 			publish_position_setpoint_triplet();
 			_update_triplet = false;
 		}
@@ -454,329 +454,7 @@ Navigator::status()
 		warnx("Geofence not set");
 	}
 }
-#if 0
-bool
-Navigator::start_none_on_ground()
-{
-	reset_reached();
-
-	_pos_sp_triplet.previous.valid = false;
-	_pos_sp_triplet.current.valid = false;
-	_pos_sp_triplet.next.valid = false;
-
-	_update_triplet = true;
-	return true;
-}
-
-bool
-Navigator::start_none_in_air()
-{
-	reset_reached();
-
-	_pos_sp_triplet.previous.valid = false;
-	_pos_sp_triplet.current.valid = false;
-	_pos_sp_triplet.next.valid = false;
-
-	_update_triplet = true;
-	return true;
-}
-
-bool
-Navigator::start_auto_on_ground()
-{
-	reset_reached();
-
-	_pos_sp_triplet.previous.valid = false;
-	_pos_sp_triplet.current.valid = true;
-	_pos_sp_triplet.next.valid = false;
-
-	_pos_sp_triplet.current.type = SETPOINT_TYPE_IDLE;
-
-	_update_triplet = true;
-	return true;
-}
-
-bool
-Navigator::start_loiter()
-{
-	/* if no existing item available, use current position */
-	if (!(_pos_sp_triplet.current.valid && _waypoint_position_reached)) {
-
-		_pos_sp_triplet.current.lat = _global_pos.lat;
-		_pos_sp_triplet.current.lon = _global_pos.lon;
-		_pos_sp_triplet.current.yaw = NAN;	// NAN means to use current yaw
-		_pos_sp_triplet.current.alt = _global_pos.alt;
-	}
-	_pos_sp_triplet.current.type = SETPOINT_TYPE_LOITER;
-	_pos_sp_triplet.current.loiter_radius = _parameters.loiter_radius;
-	_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;
-
-	mavlink_log_info(_mavlink_fd, "#audio: loiter at current altitude");
-
-	_update_triplet = true;
-	return true;
-}
-
-bool
-Navigator::start_mission()
-{
-	/* start fresh */
-	_pos_sp_triplet.previous.valid = false;
-	_pos_sp_triplet.current.valid = false;
-	_pos_sp_triplet.next.valid = false;
-
-	return set_mission_items();
-}
-
-bool
-Navigator::advance_mission()
-{
-	/* tell mission to move by one */
-	_mission.move_to_next();
-
-	/* now try to set the new mission items, if it fails, it will dispatch loiter */
-	return set_mission_items();
-}
-
-bool
-Navigator::set_mission_items()
-{
-	if (_pos_sp_triplet.current.valid) {
-		memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s));
-		_pos_sp_triplet.previous.valid = true;
-	}
-
-	bool onboard;
-	int index;
-
-	/* if we fail to set the current mission, continue to loiter */
-	if (!_mission.get_current_mission_item(&_mission_item, &onboard, &index)) {
-
-		return false;
-	}
-
-	/* if we got an RTL mission item, switch to RTL mode and give up */
-	if (_mission_item.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) {
-		return false;
-	}
-
-	_mission_item_valid = true;
-
-	/* convert the current mission item and set it valid */
-	mission_item_to_position_setpoint(&_mission_item, &_pos_sp_triplet.current);
-	_pos_sp_triplet.current.valid = true;
-
-
-	mission_item_s next_mission_item;
-
-	bool last_wp = false;
-	/* now try to set the next mission item as well, if there is no more next
-	 * this means we're heading to the last waypoint */
-	if (_mission.get_next_mission_item(&next_mission_item)) {
-		/* convert the next mission item and set it valid */
-		mission_item_to_position_setpoint(&next_mission_item, &_pos_sp_triplet.next);
-		_pos_sp_triplet.next.valid = true;
-	} else {
-		last_wp = true;
-	}
-
-	/* notify user about what happened */
-	mavlink_log_info(_mavlink_fd, "#audio: heading to %s%swaypoint %d",
-		(last_wp ? "last " : "" ), (onboard ? "onboard " : ""), index);
-
-	_update_triplet = true;
-
-	reset_reached();
-
-	return true;
-}
-
-bool
-Navigator::start_rtl()
-{
-	if (_rtl.get_current_rtl_item(&_global_pos, &_mission_item)) {
-
-		_mission_item_valid = true;
-
-		mission_item_to_position_setpoint(&_mission_item, &_pos_sp_triplet.current);
-		_pos_sp_triplet.current.valid = true;
-
-		reset_reached();
-
-		_update_triplet = true;
-		return true;
-	}
-
-	/* if RTL doesn't work, fallback to loiter */
-	return false;
-}
-
-bool
-Navigator::advance_rtl()
-{
-	/* tell mission to move by one */
-	_rtl.move_to_next();
 
-	/* now try to set the new mission items, if it fails, it will dispatch loiter */
-	if (_rtl.get_current_rtl_item(&_global_pos, &_mission_item)) {
-
-		_mission_item_valid = true;
-
-		mission_item_to_position_setpoint(&_mission_item, &_pos_sp_triplet.current);
-		_pos_sp_triplet.current.valid = true;
-
-		reset_reached();
-
-		_update_triplet = true;
-		return true;
-	}
-
-	return false;
-}
-
-bool
-Navigator::start_land()
-{
-	/* TODO: verify/test */
-
-	reset_reached();
-
-	/* this state can be requested by commander even if no global position available,
-	 * in his case controller must perform landing without position control */
-
-	memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s));
-
-	_mission_item.lat = _global_pos.lat;
-	_mission_item.lon = _global_pos.lon;
-	_mission_item.altitude_is_relative = false;
-	_mission_item.altitude = _global_pos.alt;
-	_mission_item.yaw = NAN;
-	_mission_item.loiter_radius = _parameters.loiter_radius;
-	_mission_item.loiter_direction = 1;
-	_mission_item.nav_cmd = NAV_CMD_LAND;
-	_mission_item.acceptance_radius = _parameters.acceptance_radius;
-	_mission_item.time_inside = 0.0f;
-	_mission_item.pitch_min = 0.0f;
-	_mission_item.autocontinue = true;
-	_mission_item.origin = ORIGIN_ONBOARD;
-
-	_mission_item_valid = true;
-
-	mission_item_to_position_setpoint(&_mission_item, &_pos_sp_triplet.current);
-
-	_pos_sp_triplet.next.valid = false;
-
-	_update_triplet = true;
-	return true;
-}
-bool
-Navigator::check_mission_item_reached()
-{
-	/* only check if there is actually a mission item to check */
-	if (!_mission_item_valid) {
-		return false;
-	}
-
-	if (_mission_item.nav_cmd == NAV_CMD_LAND) {
-		return _vstatus.condition_landed;
-	}
-
-	/* XXX TODO count turns */
-	if ((_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
-	     _mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED) &&
-	    _mission_item.loiter_radius > 0.01f) {
-
-	// 	return false;
-	// }
-
-	uint64_t now = hrt_absolute_time();
-
-	if (!_waypoint_position_reached) {
-		float acceptance_radius;
-
-		if (_mission_item.nav_cmd == NAV_CMD_WAYPOINT && _mission_item.acceptance_radius > 0.01f) {
-			acceptance_radius = _mission_item.acceptance_radius;
-
-		} else {
-			acceptance_radius = _parameters.acceptance_radius;
-		}
-
-		if (_do_takeoff) {
-			/* require only altitude for takeoff */
-			if (_global_pos.alt > _pos_sp_triplet.current.alt - acceptance_radius) {
-				_waypoint_position_reached = true;
-			}
-		} else {
-			float dist = -1.0f;
-			float dist_xy = -1.0f;
-			float dist_z = -1.0f;
-
-			/* calculate AMSL altitude for this waypoint */
-			float wp_alt_amsl = _mission_item.altitude;
-
-			if (_mission_item.altitude_is_relative)
-				wp_alt_amsl += _home_pos.alt;
-
-			dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, wp_alt_amsl,
-					(double)_global_pos.lat, (double)_global_pos.lon, _global_pos.alt,
-					&dist_xy, &dist_z);
-
-			if (dist >= 0.0f && dist <= acceptance_radius) {
-				_waypoint_position_reached = true;
-			}
-		}
-	}
-
-	if (_waypoint_position_reached && !_waypoint_yaw_reached) {
-
-		/* TODO: removed takeoff, why? */
-		if (_vstatus.is_rotary_wing && isfinite(_mission_item.yaw)) {
-
-			/* check yaw if defined only for rotary wing except takeoff */
-			float yaw_err = _wrap_pi(_mission_item.yaw - _global_pos.yaw);
-
-			if (fabsf(yaw_err) < 0.2f) { /* 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 >= (uint64_t)_mission_item.time_inside * 1e6)
-		    || _mission_item.nav_cmd == NAV_CMD_TAKEOFF) {
-			return true;
-		}
-	}
-	return false;
-}
-
-void
-Navigator::reset_reached()
-{
-	_time_first_inside_orbit = 0;
-	_waypoint_position_reached = false;
-	_waypoint_yaw_reached = false;
-
-}
-#endif
 void
 Navigator::publish_position_setpoint_triplet()
 {
diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c
index ca31adecc..084afe340 100644
--- a/src/modules/navigator/navigator_params.c
+++ b/src/modules/navigator/navigator_params.c
@@ -55,12 +55,12 @@
 PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 50.0f);
 
 /**
- * Takeoff Acceptance Radius (FW only)
+ * Acceptance Radius
  *
- * Acceptance radius for fixedwing.
+ * Default acceptance radius, overridden by acceptance radius of waypoint if set.
  *
  * @unit meters
  * @min 1.0
  * @group Mission
  */
-PARAM_DEFINE_FLOAT(NAV_TF_ACC_RAD, 25.0f);
+PARAM_DEFINE_FLOAT(NAV_ACC_RAD, 25.0f);
diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp
index c1b1d3f09..043f773a4 100644
--- a/src/modules/navigator/rtl.cpp
+++ b/src/modules/navigator/rtl.cpp
@@ -44,6 +44,7 @@
 
 #include <mavlink/mavlink_log.h>
 #include <systemlib/err.h>
+#include <geo/geo.h>
 
 #include <uORB/uORB.h>
 #include <uORB/topics/mission.h>
@@ -52,14 +53,14 @@
 #include "navigator.h"
 #include "rtl.h"
 
+#define DELAY_SIGMA	0.01f
+
 RTL::RTL(Navigator *navigator, const char *name) :
-	NavigatorMode(navigator, name),
-	MissionBlock(navigator),
+	MissionBlock(navigator, name),
 	_rtl_state(RTL_STATE_NONE),
 	_param_return_alt(this, "RETURN_ALT"),
 	_param_descend_alt(this, "DESCEND_ALT"),
-	_param_land_delay(this, "LAND_DELAY"),
-	_param_acceptance_radius(this, "ACCEPT_RAD")
+	_param_land_delay(this, "LAND_DELAY")
 {
 	/* load initial params */
 	updateParams();
@@ -75,7 +76,11 @@ void
 RTL::on_inactive()
 {
 	_first_run = true;
-	_rtl_state = RTL_STATE_NONE;
+
+	/* reset RTL state only if setpoint moved */
+	if (!_navigator->get_can_loiter_at_sp()) {
+		_rtl_state = RTL_STATE_NONE;
+	}
 }
 
 bool
@@ -84,14 +89,33 @@ RTL::on_active(struct position_setpoint_triplet_s *pos_sp_triplet)
 	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;
+			}
+		}
+
 		set_rtl_item(pos_sp_triplet);
 		updated = true;
-		_first_run = false;
-	}
 
-	if ((_rtl_state == RTL_STATE_CLIMB
-	     || _rtl_state == RTL_STATE_RETURN)
-	    && is_mission_item_reached()) {
+	} else if (_rtl_state != RTL_STATE_LANDED && is_mission_item_reached()) {
 		advance_rtl();
 		set_rtl_item(pos_sp_triplet);
 		updated = true;
@@ -106,31 +130,11 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
 	/* make sure we have the latest params */
 	updateParams();
 
-	/* decide where to enter the RTL procedure when we switch into it */
-	if (_rtl_state == RTL_STATE_NONE) {
-		/* for safety reasons don't go into RTL if landed */
-		if (_navigator->get_vstatus()->condition_landed) {
-			_rtl_state = RTL_STATE_FINISHED;
-			mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no RTL when landed");
-		/* if lower than return altitude, climb up first */
-		} else if (_navigator->get_global_position()->alt < _navigator->get_home_position()->alt
-			   + _param_return_alt.get()) {
-			_rtl_state = RTL_STATE_CLIMB;
-		/* otherwise go straight to return */
-		} else {
-			_rtl_state = RTL_STATE_RETURN;
-		}
-	}
-
-	/* if switching directly to return state, set altitude setpoint to current altitude */
-	if (_rtl_state == RTL_STATE_RETURN) {
-		_mission_item.altitude_is_relative = false;
-		_mission_item.altitude = _navigator->get_global_position()->alt;
-	}
+	set_previous_pos_setpoint(pos_sp_triplet);
+	_navigator->set_can_loiter_at_sp(false);
 
 	switch (_rtl_state) {
 	case RTL_STATE_CLIMB: {
-
 		float climb_alt = _navigator->get_home_position()->alt + _param_return_alt.get();
 
 		_mission_item.lat = _navigator->get_global_position()->lat;
@@ -141,50 +145,49 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
 		_mission_item.loiter_radius = _navigator->get_loiter_radius();
 		_mission_item.loiter_direction = 1;
 		_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
-		_mission_item.acceptance_radius = _param_acceptance_radius.get();
+		_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
 		_mission_item.time_inside = 0.0f;
 		_mission_item.pitch_min = 0.0f;
 		_mission_item.autocontinue = true;
 		_mission_item.origin = ORIGIN_ONBOARD;
 
-		_navigator->set_is_in_loiter(false);
-
 		mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: climb to %d meters above home",
 			(int)(climb_alt - _navigator->get_home_position()->alt));
 		break;
 	}
-	case RTL_STATE_RETURN: {
 
+	case RTL_STATE_RETURN: {
 		_mission_item.lat = _navigator->get_home_position()->lat;
 		_mission_item.lon = _navigator->get_home_position()->lon;
-
-		/* TODO: add this again */
-		// don't change altitude
-		// if (_pos_sp_triplet.previous.valid) {
-		// 	/* if previous setpoint is valid then use it to calculate heading to home */
-		// 	_mission_item.yaw = get_bearing_to_next_waypoint(_pos_sp_triplet.previous.lat, _pos_sp_triplet.previous.lon, _mission_item.lat, _mission_item.lon);
-
-		// } else {
-		// 	/* else use current position */
-		// 	_mission_item.yaw = get_bearing_to_next_waypoint(_global_pos.lat, _global_pos.lon, _mission_item.lat, _mission_item.lon);
-		// }
+		 // don't change altitude
+
+		 if (pos_sp_triplet->previous.valid) {
+		 	/* if previous setpoint is valid then use it to calculate heading to home */
+		 	_mission_item.yaw = get_bearing_to_next_waypoint(
+		 	        pos_sp_triplet->previous.lat, pos_sp_triplet->previous.lon,
+		 	        _mission_item.lat, _mission_item.lon);
+
+		 } else {
+		 	/* else use current position */
+		 	_mission_item.yaw = get_bearing_to_next_waypoint(
+		 	        _navigator->get_global_position()->lat, _navigator->get_global_position()->lon,
+		 	        _mission_item.lat, _mission_item.lon);
+		 }
 		_mission_item.loiter_radius = _navigator->get_loiter_radius();
 		_mission_item.loiter_direction = 1;
 		_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
-		_mission_item.acceptance_radius = _param_acceptance_radius.get();
+		_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
 		_mission_item.time_inside = 0.0f;
 		_mission_item.pitch_min = 0.0f;
 		_mission_item.autocontinue = true;
 		_mission_item.origin = ORIGIN_ONBOARD;
 
-		_navigator->set_is_in_loiter(false);
-
 		mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: return at %d meters above home",
 			(int)(_mission_item.altitude - _navigator->get_home_position()->alt));
 		break;
 	}
-	case RTL_STATE_DESCEND: {
 
+	case RTL_STATE_DESCEND: {
 		_mission_item.lat = _navigator->get_home_position()->lat;
 		_mission_item.lon = _navigator->get_home_position()->lon;
 		_mission_item.altitude_is_relative = false;
@@ -193,59 +196,92 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet)
 		_mission_item.loiter_radius = _navigator->get_loiter_radius();
 		_mission_item.loiter_direction = 1;
 		_mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT;
-		_mission_item.acceptance_radius = _param_acceptance_radius.get();
-		_mission_item.time_inside = _param_land_delay.get() < 0.0f ? 0.0f : _param_land_delay.get();
+		_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
+		_mission_item.time_inside = 0.0f;
 		_mission_item.pitch_min = 0.0f;
-		_mission_item.autocontinue = _param_land_delay.get() > -0.001f;
+		_mission_item.autocontinue = false;
 		_mission_item.origin = ORIGIN_ONBOARD;
 
-		_navigator->set_is_in_loiter(true);
-
 		mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: descend to %d meters above home",
 			(int)(_mission_item.altitude - _navigator->get_home_position()->alt));
 		break;
 	}
 
-	case RTL_STATE_LAND: {
+	case RTL_STATE_LOITER: {
+		bool autoland = _param_land_delay.get() > -DELAY_SIGMA;
 
 		_mission_item.lat = _navigator->get_home_position()->lat;
 		_mission_item.lon = _navigator->get_home_position()->lon;
 		_mission_item.altitude_is_relative = false;
+		_mission_item.altitude = _navigator->get_home_position()->alt + _param_descend_alt.get();
+		_mission_item.yaw = NAN;
+		_mission_item.loiter_radius = _navigator->get_loiter_radius();
+		_mission_item.loiter_direction = 1;
+		_mission_item.nav_cmd = autoland ? NAV_CMD_LOITER_TIME_LIMIT : NAV_CMD_LOITER_UNLIMITED;
+		_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
+		_mission_item.time_inside = _param_land_delay.get() < 0.0f ? 0.0f : _param_land_delay.get();
+		_mission_item.pitch_min = 0.0f;
+		_mission_item.autocontinue = autoland;
+		_mission_item.origin = ORIGIN_ONBOARD;
+
+		_navigator->set_can_loiter_at_sp(true);
+
+		if (autoland) {
+			mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: loiter %.1fs", _mission_item.time_inside);
+
+		} else {
+			mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: completed, loiter");
+		}
+		break;
+	}
+
+	case RTL_STATE_LAND: {
+		_mission_item.lat = _navigator->get_home_position()->lat;
+		_mission_item.lon = _navigator->get_home_position()->lon;
+		_mission_item.altitude_is_relative = false;
 		_mission_item.altitude = _navigator->get_home_position()->alt;
 		_mission_item.yaw = NAN;
 		_mission_item.loiter_radius = _navigator->get_loiter_radius();
 		_mission_item.loiter_direction = 1;
 		_mission_item.nav_cmd = NAV_CMD_LAND;
-		_mission_item.acceptance_radius = _param_acceptance_radius.get();
+		_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
 		_mission_item.time_inside = 0.0f;
 		_mission_item.pitch_min = 0.0f;
 		_mission_item.autocontinue = true;
 		_mission_item.origin = ORIGIN_ONBOARD;
 
-		_navigator->set_is_in_loiter(false);
-
 		mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: land at home");
 		break;
 	}
 
-	case RTL_STATE_FINISHED: {
-		/* nothing to do, report fail */
+	case RTL_STATE_LANDED: {
+		_mission_item.lat = _navigator->get_home_position()->lat;
+		_mission_item.lon = _navigator->get_home_position()->lon;
+		_mission_item.altitude_is_relative = false;
+		_mission_item.altitude = _navigator->get_home_position()->alt;
+		_mission_item.yaw = NAN;
+		_mission_item.loiter_radius = _navigator->get_loiter_radius();
+		_mission_item.loiter_direction = 1;
+		_mission_item.nav_cmd = NAV_CMD_IDLE;
+		_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
+		_mission_item.time_inside = 0.0f;
+		_mission_item.pitch_min = 0.0f;
+		_mission_item.autocontinue = true;
+		_mission_item.origin = ORIGIN_ONBOARD;
+
+		mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: completed, landed");
+		break;
 	}
 
 	default:
 		break;
 	}
 
-	if (_rtl_state == RTL_STATE_FINISHED) {
-		pos_sp_triplet->current.valid = false;
-		pos_sp_triplet->next.valid = false;
-	} else {
-		/* if not finished, convert mission item to current position setpoint and make it valid */
-		mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
-		reset_mission_item_reached();
-		pos_sp_triplet->current.valid = true;
-		pos_sp_triplet->next.valid = false;
-	}
+	/* convert mission item to current position setpoint and make it valid */
+	mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
+	reset_mission_item_reached();
+	pos_sp_triplet->current.valid = true;
+	pos_sp_triplet->next.valid = false;
 }
 
 void
@@ -262,18 +298,20 @@ RTL::advance_rtl()
 
 	case RTL_STATE_DESCEND:
 		/* only go to land if autoland is enabled */
-		if (_param_land_delay.get() < 0) {
-			_rtl_state = RTL_STATE_FINISHED;
+		if (_param_land_delay.get() < -DELAY_SIGMA || _param_land_delay.get() > DELAY_SIGMA) {
+			_rtl_state = RTL_STATE_LOITER;
+
 		} else {
 			_rtl_state = RTL_STATE_LAND;
 		}
 		break;
 
-	case RTL_STATE_LAND:
-		_rtl_state = RTL_STATE_FINISHED;
+	case RTL_STATE_LOITER:
+		_rtl_state = RTL_STATE_LAND;
 		break;
 
-	case RTL_STATE_FINISHED:
+	case RTL_STATE_LAND:
+		_rtl_state = RTL_STATE_LANDED;
 		break;
 
 	default:
diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h
index bcccf7454..b4b729e89 100644
--- a/src/modules/navigator/rtl.h
+++ b/src/modules/navigator/rtl.h
@@ -54,7 +54,7 @@
 
 class Navigator;
 
-class RTL : public NavigatorMode, MissionBlock
+class RTL : public MissionBlock
 {
 public:
 	/**
@@ -97,14 +97,14 @@ private:
 		RTL_STATE_CLIMB,
 		RTL_STATE_RETURN,
 		RTL_STATE_DESCEND,
+		RTL_STATE_LOITER,
 		RTL_STATE_LAND,
-		RTL_STATE_FINISHED,
+		RTL_STATE_LANDED,
 	} _rtl_state;
 
 	control::BlockParamFloat _param_return_alt;
 	control::BlockParamFloat _param_descend_alt;
 	control::BlockParamFloat _param_land_delay;
-	control::BlockParamFloat _param_acceptance_radius;
 };
 
 #endif
diff --git a/src/modules/navigator/rtl_params.c b/src/modules/navigator/rtl_params.c
index 63724f461..bfe6ce7e1 100644
--- a/src/modules/navigator/rtl_params.c
+++ b/src/modules/navigator/rtl_params.c
@@ -96,15 +96,3 @@ PARAM_DEFINE_FLOAT(RTL_DESCEND_ALT, 20);
  * @group RTL
  */
 PARAM_DEFINE_FLOAT(RTL_LAND_DELAY, -1.0f);
-
-/**
- * RTL acceptance radius
- *
- * Acceptance radius for waypoints set for RTL
- *
- * @unit meters
- * @min 1
- * @max
- * @group RTL
- */
-PARAM_DEFINE_FLOAT(RTL_ACCEPT_RAD, 25.0f);