Merge branch 'beta1' into beta

2 files changed, 248 insertions, 314 deletions

diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index e6b7ef93d..061baf0c5 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -84,6 +84,7 @@
 #include <sys/types.h>
 #include <sys/stat.h>
 
+#include "navigator_state.h"
 #include "navigator_mission.h"
 #include "mission_feasibility_checker.h"
 #include "geofence.h"
@@ -151,10 +152,10 @@ private:
 	int		_offboard_mission_sub;		/**< notification of offboard mission updates */
 	int 		_onboard_mission_sub;		/**< notification of onboard mission updates */
 	int		_capabilities_sub;		/**< notification of vehicle capabilities updates */
+	int		_control_mode_sub;		/**< vehicle control mode subscription */
 
 	orb_advert_t	_pos_sp_triplet_pub;			/**< publish position setpoint triplet */
 	orb_advert_t	_mission_result_pub;		/**< publish mission result topic */
-	orb_advert_t	_control_mode_pub;			/**< publish vehicle control mode topic */
 
 	struct vehicle_status_s				_vstatus;		/**< vehicle status */
 	struct vehicle_control_mode_s		_control_mode;		/**< vehicle control mode */
@@ -177,6 +178,7 @@ private:
 
 	class 		Mission				_mission;
 
+	bool		_global_pos_valid;				/**< track changes of global_position.global_valid flag */
 	bool		_reset_loiter_pos;				/**< if true then loiter position should be set to current position */
 	bool		_waypoint_position_reached;
 	bool		_waypoint_yaw_reached;
@@ -188,6 +190,8 @@ private:
 
 	uint64_t	_set_nav_state_timestamp;		/**< timestamp of last handled navigation state request */
 
+	bool		_pos_sp_triplet_updated;
+
 	char *nav_states_str[NAV_STATE_MAX];
 
 	struct {
@@ -233,8 +237,7 @@ private:
 		RTL_STATE_NONE = 0,
 		RTL_STATE_CLIMB,
 		RTL_STATE_RETURN,
-		RTL_STATE_DESCEND,
-		RTL_STATE_LAND
+		RTL_STATE_DESCEND
 	};
 
 	enum RTLState _rtl_state;
@@ -274,6 +277,10 @@ private:
 	 */
 	void		vehicle_status_update();
 
+	/**
+	 * Retrieve vehicle control mode
+	 */
+	void		vehicle_control_mode_update();
 
 	/**
 	 * Shim for calling task_main from task_create.
@@ -296,6 +303,7 @@ private:
 	void		start_mission();
 	void		start_rtl();
 	void		start_land();
+	void		start_land_home();
 
 	/**
 	 * Guards offboard mission
@@ -341,11 +349,6 @@ private:
 	 * Publish a new mission item triplet for position controller
 	 */
 	void		publish_position_setpoint_triplet();
-
-	/**
-	 * Publish vehicle_control_mode topic for controllers
-	 */
-	void		publish_control_mode();
 };
 
 namespace navigator
@@ -373,6 +376,7 @@ Navigator::Navigator() :
 	_global_pos_sub(-1),
 	_home_pos_sub(-1),
 	_vstatus_sub(-1),
+	_control_mode_sub(-1),
 	_params_sub(-1),
 	_offboard_mission_sub(-1),
 	_onboard_mission_sub(-1),
@@ -381,7 +385,6 @@ Navigator::Navigator() :
 /* publications */
 	_pos_sp_triplet_pub(-1),
 	_mission_result_pub(-1),
-	_control_mode_pub(-1),
 
 /* performance counters */
 	_loop_perf(perf_alloc(PC_ELAPSED, "navigator")),
@@ -391,6 +394,7 @@ Navigator::Navigator() :
 	_fence_valid(false),
 	_inside_fence(true),
 	_mission(),
+	_global_pos_valid(false),
 	_reset_loiter_pos(true),
 	_waypoint_position_reached(false),
 	_waypoint_yaw_reached(false),
@@ -399,6 +403,7 @@ Navigator::Navigator() :
 	_mission_item_valid(false),
 	_need_takeoff(true),
 	_do_takeoff(false),
+	_pos_sp_triplet_updated(false),
 	_geofence_violation_warning_sent(false)
 {
 	_parameter_handles.min_altitude = param_find("NAV_MIN_ALT");
@@ -541,9 +546,19 @@ Navigator::onboard_mission_update()
 void
 Navigator::vehicle_status_update()
 {
-	/* try to load initial states */
 	if (orb_copy(ORB_ID(vehicle_status), _vstatus_sub, &_vstatus) != OK) {
-		_vstatus.arming_state = ARMING_STATE_STANDBY; /* in case the commander is not be running */
+		/* in case the commander is not be running */
+		_vstatus.arming_state = ARMING_STATE_STANDBY;
+	}
+}
+
+void
+Navigator::vehicle_control_mode_update()
+{
+	if (orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode) != OK) {
+		/* in case the commander is not be running */
+		_control_mode.flag_control_auto_enabled = false;
+		_control_mode.flag_armed = false;
 	}
 }
 
@@ -589,11 +604,13 @@ Navigator::task_main()
 	_onboard_mission_sub = orb_subscribe(ORB_ID(onboard_mission));
 	_capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities));
 	_vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
+	_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
 	_params_sub = orb_subscribe(ORB_ID(parameter_update));
 	_home_pos_sub = orb_subscribe(ORB_ID(home_position));
 
 	/* copy all topics first time */
 	vehicle_status_update();
+	vehicle_control_mode_update();
 	parameters_update();
 	global_position_update();
 	home_position_update();
@@ -605,12 +622,11 @@ Navigator::task_main()
 	orb_set_interval(_global_pos_sub, 20);
 
 	unsigned prevState = NAV_STATE_NONE;
-	bool pub_control_mode = true;
 	hrt_abstime mavlink_open_time = 0;
 	const hrt_abstime mavlink_open_interval = 500000;
 
 	/* wakeup source(s) */
-	struct pollfd fds[7];
+	struct pollfd fds[8];
 
 	/* Setup of loop */
 	fds[0].fd = _params_sub;
@@ -627,6 +643,8 @@ Navigator::task_main()
 	fds[5].events = POLLIN;
 	fds[6].fd = _vstatus_sub;
 	fds[6].events = POLLIN;
+	fds[7].fd = _control_mode_sub;
+	fds[7].events = POLLIN;
 
 	while (!_task_should_exit) {
 
@@ -652,127 +670,116 @@ Navigator::task_main()
 			_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
 		}
 
+		/* vehicle control mode updated */
+		if (fds[7].revents & POLLIN) {
+			vehicle_control_mode_update();
+		}
+
 		/* vehicle status updated */
 		if (fds[6].revents & POLLIN) {
 			vehicle_status_update();
-			pub_control_mode = true;
 
 			/* evaluate state machine from commander and set the navigator mode accordingly */
-			if (_vstatus.arming_state == ARMING_STATE_ARMED || _vstatus.arming_state == ARMING_STATE_ARMED_ERROR) {
-				if (_vstatus.failsafe_state == FAILSAFE_STATE_NORMAL) {
-					if (_vstatus.main_state == MAIN_STATE_AUTO) {
-						bool stick_mode = false;
+			if (_control_mode.flag_armed && _control_mode.flag_control_auto_enabled) {
+				bool stick_mode = false;
+
+				if (!_vstatus.rc_signal_lost) {
+					/* RC signal available, use control switches to set mode */
+					/* RETURN switch, overrides MISSION switch */
+					if (_vstatus.return_switch == RETURN_SWITCH_RETURN) {
+						/* switch to RTL if not already landed after RTL and home position set */
+						if (!(_rtl_state == RTL_STATE_DESCEND && (myState == NAV_STATE_READY || myState == NAV_STATE_LAND)) &&
+								_vstatus.condition_home_position_valid) {
+							dispatch(EVENT_RTL_REQUESTED);
+						}
 
-						if (!_vstatus.rc_signal_lost) {
-							/* RC signal available, use control switches to set mode */
-							/* RETURN switch, overrides MISSION switch */
-							if (_vstatus.return_switch == RETURN_SWITCH_RETURN) {
-								if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) {
-									dispatch(EVENT_RTL_REQUESTED);
-								}
+						stick_mode = true;
 
-								stick_mode = true;
+					} else {
+						/* MISSION switch */
+						if (_vstatus.mission_switch == MISSION_SWITCH_LOITER) {
+							dispatch(EVENT_LOITER_REQUESTED);
+							stick_mode = true;
+
+						} else if (_vstatus.mission_switch == MISSION_SWITCH_MISSION) {
+							/* switch to mission only if available */
+							if (_mission.current_mission_available()) {
+								dispatch(EVENT_MISSION_REQUESTED);
 
 							} else {
-								/* MISSION switch */
-								if (_vstatus.mission_switch == MISSION_SWITCH_LOITER) {
-									dispatch(EVENT_LOITER_REQUESTED);
-									stick_mode = true;
-
-								} else if (_vstatus.mission_switch == MISSION_SWITCH_MISSION) {
-									/* switch to mission only if available */
-									if (_mission.current_mission_available()) {
-										dispatch(EVENT_MISSION_REQUESTED);
-
-									} else {
-										dispatch(EVENT_LOITER_REQUESTED);
-									}
-
-									stick_mode = true;
-								}
-
-								if (!stick_mode && _vstatus.return_switch == RETURN_SWITCH_NORMAL && myState == NAV_STATE_RTL) {
-									/* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */
-									dispatch(EVENT_LOITER_REQUESTED);
-									stick_mode = true;
-								}
+								dispatch(EVENT_LOITER_REQUESTED);
 							}
+
+							stick_mode = true;
 						}
 
-						if (!stick_mode) {
-							if (_vstatus.set_nav_state_timestamp != _set_nav_state_timestamp) {
-								/* commander requested new navigation mode, try to set it */
-								_set_nav_state_timestamp = _vstatus.set_nav_state_timestamp;
+						if (!stick_mode && _vstatus.return_switch == RETURN_SWITCH_NORMAL && myState == NAV_STATE_RTL) {
+							/* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */
+							dispatch(EVENT_LOITER_REQUESTED);
+							stick_mode = true;
+						}
+					}
+				}
 
-								switch (_vstatus.set_nav_state) {
-								case NAV_STATE_NONE:
-									/* nothing to do */
-									break;
+				if (!stick_mode) {
+					if (_vstatus.set_nav_state_timestamp != _set_nav_state_timestamp) {
+						/* commander requested new navigation mode, try to set it */
+						_set_nav_state_timestamp = _vstatus.set_nav_state_timestamp;
 
-								case NAV_STATE_LOITER:
-									dispatch(EVENT_LOITER_REQUESTED);
-									break;
+						switch (_vstatus.set_nav_state) {
+						case NAV_STATE_NONE:
+							/* nothing to do */
+							break;
 
-								case NAV_STATE_MISSION:
-									if (_mission.current_mission_available()) {
-										dispatch(EVENT_MISSION_REQUESTED);
+						case NAV_STATE_LOITER:
+							dispatch(EVENT_LOITER_REQUESTED);
+							break;
 
-									} else {
-										dispatch(EVENT_LOITER_REQUESTED);
-									}
+						case NAV_STATE_MISSION:
+							if (_mission.current_mission_available()) {
+								dispatch(EVENT_MISSION_REQUESTED);
 
-									break;
+							} else {
+								dispatch(EVENT_LOITER_REQUESTED);
+							}
 
-								case NAV_STATE_RTL:
-									if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) {
-										dispatch(EVENT_RTL_REQUESTED);
-									}
+							break;
 
-									break;
+						case NAV_STATE_RTL:
+							if (!(_rtl_state == RTL_STATE_DESCEND && (myState == NAV_STATE_READY || myState == NAV_STATE_LAND)) &&
+									_vstatus.condition_home_position_valid) {
+								dispatch(EVENT_RTL_REQUESTED);
+							}
 
-								default:
-									warnx("ERROR: Requested navigation state not supported");
-									break;
-								}
+							break;
 
-							} else {
-								/* on first switch to AUTO try mission by default, if none is available fallback to loiter */
-								if (myState == NAV_STATE_NONE) {
-									if (_mission.current_mission_available()) {
-										dispatch(EVENT_MISSION_REQUESTED);
-
-									} else {
-										dispatch(EVENT_LOITER_REQUESTED);
-									}
-								}
+						case NAV_STATE_LAND:
+							if (myState != NAV_STATE_READY) {
+								dispatch(EVENT_LAND_REQUESTED);
 							}
-						}
 
-					} else {
-						/* not in AUTO mode */
-						dispatch(EVENT_NONE_REQUESTED);
-					}
+							break;
 
-				} else if (_vstatus.failsafe_state == FAILSAFE_STATE_RTL) {
-					/* RTL on failsafe */
-					if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) {
+						default:
+							warnx("ERROR: Requested navigation state not supported");
+							break;
+						}
 
-						dispatch(EVENT_RTL_REQUESTED);
-					}
+					} else {
+						/* on first switch to AUTO try mission by default, if none is available fallback to loiter */
+						if (myState == NAV_STATE_NONE) {
+							if (_mission.current_mission_available()) {
+								dispatch(EVENT_MISSION_REQUESTED);
 
-				} else if (_vstatus.failsafe_state == FAILSAFE_STATE_LAND) {
-					/* LAND on failsafe */
-					if (myState != NAV_STATE_READY) {
-						dispatch(EVENT_LAND_REQUESTED);
+							} else {
+								dispatch(EVENT_LOITER_REQUESTED);
+							}
+						}
 					}
-
-				} else {
-					/* shouldn't act */
-					dispatch(EVENT_NONE_REQUESTED);
 				}
 
 			} else {
-				/* not armed */
+				/* navigator shouldn't act */
 				dispatch(EVENT_NONE_REQUESTED);
 			}
 		}
@@ -813,10 +820,22 @@ Navigator::task_main()
 		if (fds[1].revents & POLLIN) {
 			global_position_update();
 
-			/* only check if waypoint has been reached in MISSION and RTL modes */
-			if (myState == NAV_STATE_MISSION || myState == NAV_STATE_RTL) {
-				if (check_mission_item_reached()) {
-					on_mission_item_reached();
+			/* publish position setpoint triplet on each position update if navigator active */
+			if (_control_mode.flag_armed && _control_mode.flag_control_auto_enabled) {
+				_pos_sp_triplet_updated = true;
+
+				if (myState == NAV_STATE_LAND && _global_pos.global_valid && !_global_pos_valid) {
+					/* got global position when landing, update setpoint */
+					start_land();
+				}
+
+				_global_pos_valid = _global_pos.global_valid;
+
+				/* check if waypoint has been reached in MISSION, RTL and LAND modes */
+				if (myState == NAV_STATE_MISSION || myState == NAV_STATE_RTL || myState == NAV_STATE_LAND) {
+					if (check_mission_item_reached()) {
+						on_mission_item_reached();
+					}
 				}
 			}
 
@@ -836,16 +855,16 @@ Navigator::task_main()
 			}
 		}
 
+		/* publish position setpoint triplet if updated */
+		if (_pos_sp_triplet_updated) {
+			_pos_sp_triplet_updated = false;
+			publish_position_setpoint_triplet();
+		}
+
 		/* notify user about state changes */
 		if (myState != prevState) {
 			mavlink_log_info(_mavlink_fd, "[navigator] nav state: %s", nav_states_str[myState]);
 			prevState = myState;
-			pub_control_mode = true;
-		}
-
-		/* publish control mode if updated */
-		if (pub_control_mode) {
-			publish_control_mode();
 		}
 
 		perf_end(_loop_perf);
@@ -881,9 +900,9 @@ Navigator::start()
 void
 Navigator::status()
 {
-	warnx("Global position is %svalid", _global_pos.valid ? "" : "in");
+	warnx("Global position is %svalid", _global_pos.global_valid ? "" : "in");
 
-	if (_global_pos.valid) {
+	if (_global_pos.global_valid) {
 		warnx("Longitude %5.5f degrees, latitude %5.5f degrees", _global_pos.lon, _global_pos.lat);
 		warnx("Altitude %5.5f meters, altitude above home %5.5f meters",
 		      (double)_global_pos.alt, (double)(_global_pos.alt - _home_pos.alt));
@@ -977,7 +996,7 @@ StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = {
 		/* EVENT_LOITER_REQUESTED */		{ACTION(&Navigator::start_loiter), NAV_STATE_LOITER},
 		/* EVENT_MISSION_REQUESTED */		{ACTION(&Navigator::start_mission), NAV_STATE_MISSION},
 		/* EVENT_RTL_REQUESTED */		{NO_ACTION, NAV_STATE_RTL},
-		/* EVENT_LAND_REQUESTED */		{ACTION(&Navigator::start_land), NAV_STATE_LAND},
+		/* EVENT_LAND_REQUESTED */		{ACTION(&Navigator::start_land_home), NAV_STATE_LAND},
 		/* EVENT_MISSION_CHANGED */		{NO_ACTION, NAV_STATE_RTL},
 		/* EVENT_HOME_POSITION_CHANGED */	{ACTION(&Navigator::start_rtl), NAV_STATE_RTL},	// TODO need to reset rtl_state
 	},
@@ -1006,26 +1025,29 @@ Navigator::start_none()
 	_do_takeoff = false;
 	_rtl_state = RTL_STATE_NONE;
 
-	publish_position_setpoint_triplet();
+	_pos_sp_triplet_updated = true;
 }
 
 void
 Navigator::start_ready()
 {
 	_pos_sp_triplet.previous.valid = false;
-	_pos_sp_triplet.current.valid = false;
+	_pos_sp_triplet.current.valid = true;
 	_pos_sp_triplet.next.valid = false;
+
+	_pos_sp_triplet.current.type = SETPOINT_TYPE_IDLE;
+
 	_mission_item_valid = false;
 
 	_reset_loiter_pos = true;
 	_do_takeoff = false;
 
-	if (_rtl_state != RTL_STATE_LAND) {
-		/* allow RTL if landed not at home */
+	if (_rtl_state != RTL_STATE_DESCEND) {
+		/* reset RTL state if landed not at home */
 		_rtl_state = RTL_STATE_NONE;
 	}
 
-	publish_position_setpoint_triplet();
+	_pos_sp_triplet_updated = true;
 }
 
 void
@@ -1054,11 +1076,6 @@ Navigator::start_loiter()
 		}
 
 		_pos_sp_triplet.current.type = SETPOINT_TYPE_NORMAL;
-
-		if (_rtl_state == RTL_STATE_LAND) {
-			/* if RTL landing was interrupted, avoid landing from MIN_ALT on next RTL */
-			_rtl_state = RTL_STATE_DESCEND;
-		}
 	}
 
 	_pos_sp_triplet.current.loiter_radius = _parameters.loiter_radius;
@@ -1068,7 +1085,7 @@ Navigator::start_loiter()
 	_pos_sp_triplet.next.valid = false;
 	_mission_item_valid = false;
 
-	publish_position_setpoint_triplet();
+	_pos_sp_triplet_updated = true;
 }
 
 void
@@ -1181,7 +1198,7 @@ Navigator::set_mission_item()
 		}
 	}
 
-	publish_position_setpoint_triplet();
+	_pos_sp_triplet_updated = true;
 }
 
 void
@@ -1189,20 +1206,22 @@ Navigator::start_rtl()
 {
 	_do_takeoff = false;
 
+	/* decide if we need climb */
 	if (_rtl_state == RTL_STATE_NONE) {
 		if (_global_pos.alt < _home_pos.alt + _parameters.rtl_alt) {
 			_rtl_state = RTL_STATE_CLIMB;
 
 		} else {
 			_rtl_state = RTL_STATE_RETURN;
-
-			if (_reset_loiter_pos) {
-				_mission_item.altitude_is_relative = false;
-				_mission_item.altitude = _global_pos.alt;
-			}
 		}
 	}
 
+	/* if switching directly to return state, reset altitude setpoint */
+	if (_rtl_state == RTL_STATE_RETURN) {
+		_mission_item.altitude_is_relative = false;
+		_mission_item.altitude = _global_pos.alt;
+	}
+
 	_reset_loiter_pos = true;
 	set_rtl_item();
 }
@@ -1210,20 +1229,62 @@ Navigator::start_rtl()
 void
 Navigator::start_land()
 {
+	/* this state can be requested by commander even if no global position available,
+	 * in his case controller must perform landing without position control */
 	_do_takeoff = false;
 	_reset_loiter_pos = true;
 
-	_pos_sp_triplet.previous.valid = false;
+	memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s));
+
+	_mission_item_valid = true;
+
+	_mission_item.lat = _global_pos.lat;
+	_mission_item.lon = _global_pos.lon;
+	_mission_item.altitude_is_relative = false;
+	_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;
+
+	position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item);
+
 	_pos_sp_triplet.next.valid = false;
+}
 
-	_pos_sp_triplet.current.valid = true;
-	_pos_sp_triplet.current.type = SETPOINT_TYPE_LAND;
-	_pos_sp_triplet.current.lat = _global_pos.lat;
-	_pos_sp_triplet.current.lon = _global_pos.lon;
-	_pos_sp_triplet.current.alt = _global_pos.alt;
-	_pos_sp_triplet.current.loiter_direction = 1;
-	_pos_sp_triplet.current.loiter_radius = _parameters.loiter_radius;
-	_pos_sp_triplet.current.yaw = NAN;
+void
+Navigator::start_land_home()
+{
+	/* land to home position, should be called when hovering above home, from RTL state */
+	_do_takeoff = false;
+	_reset_loiter_pos = true;
+
+	memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s));
+
+	_mission_item_valid = true;
+
+	_mission_item.lat = _home_pos.lat;
+	_mission_item.lon = _home_pos.lon;
+	_mission_item.altitude_is_relative = false;
+	_mission_item.altitude = _home_pos.alt;
+	_mission_item.yaw = NAN;
+	_mission_item.loiter_radius = _parameters.loiter_radius;
+	_mission_item.loiter_direction = 1;
+	_mission_item.nav_cmd = NAV_CMD_LAND;
+	_mission_item.acceptance_radius = _parameters.acceptance_radius;
+	_mission_item.time_inside = 0.0f;
+	_mission_item.pitch_min = 0.0f;
+	_mission_item.autocontinue = true;
+	_mission_item.origin = ORIGIN_ONBOARD;
+
+	position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item);
+
+	_pos_sp_triplet.next.valid = false;
 }
 
 void
@@ -1285,7 +1346,7 @@ Navigator::set_rtl_item()
 
 			_pos_sp_triplet.next.valid = false;
 
-			mavlink_log_info(_mavlink_fd, "[navigator] RTL: return");
+			mavlink_log_info(_mavlink_fd, "[navigator] RTL: return at %.1fm above home", _mission_item.altitude - _home_pos.alt);
 			break;
 		}
 
@@ -1316,33 +1377,6 @@ Navigator::set_rtl_item()
 			break;
 		}
 
-	case RTL_STATE_LAND: {
-			memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s));
-
-			_mission_item_valid = true;
-
-			_mission_item.lat = _home_pos.lat;
-			_mission_item.lon = _home_pos.lon;
-			_mission_item.altitude_is_relative = false;
-			_mission_item.altitude = _home_pos.alt;
-			_mission_item.yaw = NAN;
-			_mission_item.loiter_radius = _parameters.loiter_radius;
-			_mission_item.loiter_direction = 1;
-			_mission_item.nav_cmd = NAV_CMD_LAND;
-			_mission_item.acceptance_radius = _parameters.acceptance_radius;
-			_mission_item.time_inside = 0.0f;
-			_mission_item.pitch_min = 0.0f;
-			_mission_item.autocontinue = true;
-			_mission_item.origin = ORIGIN_ONBOARD;
-
-			position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item);
-
-			_pos_sp_triplet.next.valid = false;
-
-			mavlink_log_info(_mavlink_fd, "[navigator] RTL: land");
-			break;
-		}
-
 	default: {
 			mavlink_log_critical(_mavlink_fd, "[navigator] error: unknown RTL state: %d", _rtl_state);
 			start_loiter();
@@ -1350,7 +1384,7 @@ Navigator::set_rtl_item()
 		}
 	}
 
-	publish_position_setpoint_triplet();
+	_pos_sp_triplet_updated = true;
 }
 
 void
@@ -1527,27 +1561,40 @@ Navigator::on_mission_item_reached()
 			}
 		}
 
-	} else {
-		/* RTL finished */
-		if (_rtl_state == RTL_STATE_LAND) {
-			/* landed at home position */
-			mavlink_log_info(_mavlink_fd, "[navigator] RTL completed, landed");
-			dispatch(EVENT_READY_REQUESTED);
+	} else if (myState == NAV_STATE_RTL) {
+		/* RTL completed */
+		if (_rtl_state == RTL_STATE_DESCEND) {
+			/* hovering above home position, land if needed or loiter */
+			mavlink_log_info(_mavlink_fd, "[navigator] RTL completed");
+			if (_mission_item.autocontinue) {
+				dispatch(EVENT_LAND_REQUESTED);
+
+			} else {
+				dispatch(EVENT_LOITER_REQUESTED);
+			}
 
 		} else {
 			/* next RTL step */
 			_rtl_state = (RTLState)(_rtl_state + 1);
 			set_rtl_item();
 		}
+
+	} else if (myState == NAV_STATE_LAND) {
+		/* landing completed */
+		mavlink_log_info(_mavlink_fd, "[navigator] landing completed");
+		dispatch(EVENT_READY_REQUESTED);
 	}
 }
 
 void
 Navigator::publish_position_setpoint_triplet()
 {
-	/* lazily publish the mission triplet only once available */
+	/* update navigation state */
+	_pos_sp_triplet.nav_state = static_cast<nav_state_t>(myState);
+
+	/* lazily publish the position setpoint triplet only once available */
 	if (_pos_sp_triplet_pub > 0) {
-		/* publish the mission triplet */
+		/* publish the position setpoint triplet */
 		orb_publish(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_pub, &_pos_sp_triplet);
 
 	} else {
@@ -1556,140 +1603,6 @@ Navigator::publish_position_setpoint_triplet()
 	}
 }
 
-void
-Navigator::publish_control_mode()
-{
-	/* update vehicle_control_mode topic*/
-	_control_mode.main_state = _vstatus.main_state;
-	_control_mode.nav_state = static_cast<nav_state_t>(myState);
-	_control_mode.flag_armed = _vstatus.arming_state == ARMING_STATE_ARMED || _vstatus.arming_state == ARMING_STATE_ARMED_ERROR;
-	_control_mode.flag_external_manual_override_ok = !_vstatus.is_rotary_wing;
-	_control_mode.flag_system_hil_enabled = _vstatus.hil_state == HIL_STATE_ON;
-
-	_control_mode.flag_control_offboard_enabled = false;
-	_control_mode.flag_control_termination_enabled = false;
-
-	/* set this flag when navigator has control */
-	bool navigator_enabled = false;
-
-	switch (_vstatus.failsafe_state) {
-	case FAILSAFE_STATE_NORMAL:
-		switch (_vstatus.main_state) {
-		case MAIN_STATE_MANUAL:
-			_control_mode.flag_control_manual_enabled = true;
-			_control_mode.flag_control_rates_enabled = _vstatus.is_rotary_wing;
-			_control_mode.flag_control_attitude_enabled = _vstatus.is_rotary_wing;
-			_control_mode.flag_control_altitude_enabled = false;
-			_control_mode.flag_control_climb_rate_enabled = false;
-			_control_mode.flag_control_position_enabled = false;
-			_control_mode.flag_control_velocity_enabled = false;
-			break;
-
-		case MAIN_STATE_SEATBELT:
-			_control_mode.flag_control_manual_enabled = true;
-			_control_mode.flag_control_rates_enabled = true;
-			_control_mode.flag_control_attitude_enabled = true;
-			_control_mode.flag_control_altitude_enabled = true;
-			_control_mode.flag_control_climb_rate_enabled = true;
-			_control_mode.flag_control_position_enabled = false;
-			_control_mode.flag_control_velocity_enabled = false;
-			break;
-
-		case MAIN_STATE_EASY:
-			_control_mode.flag_control_manual_enabled = true;
-			_control_mode.flag_control_rates_enabled = true;
-			_control_mode.flag_control_attitude_enabled = true;
-			_control_mode.flag_control_altitude_enabled = true;
-			_control_mode.flag_control_climb_rate_enabled = true;
-			_control_mode.flag_control_position_enabled = true;
-			_control_mode.flag_control_velocity_enabled = true;
-			break;
-
-		case MAIN_STATE_AUTO:
-			navigator_enabled = true;
-			break;
-
-		default:
-			break;
-		}
-
-		break;
-
-	case FAILSAFE_STATE_RTL:
-		navigator_enabled = true;
-		break;
-
-	case FAILSAFE_STATE_LAND:
-		navigator_enabled = true;
-		break;
-
-	case FAILSAFE_STATE_TERMINATION:
-		navigator_enabled = true;
-		/* disable all controllers on termination */
-		_control_mode.flag_control_manual_enabled = false;
-		_control_mode.flag_control_rates_enabled = false;
-		_control_mode.flag_control_attitude_enabled = false;
-		_control_mode.flag_control_position_enabled = false;
-		_control_mode.flag_control_velocity_enabled = false;
-		_control_mode.flag_control_altitude_enabled = false;
-		_control_mode.flag_control_climb_rate_enabled = false;
-		_control_mode.flag_control_termination_enabled = true;
-		break;
-
-	default:
-		break;
-	}
-
-	/* navigator has control, set control mode flags according to nav state*/
-	if (navigator_enabled) {
-		_control_mode.flag_control_manual_enabled = false;
-
-		switch (myState) {
-		case NAV_STATE_READY:
-			/* disable all controllers, armed but idle */
-			_control_mode.flag_control_rates_enabled = false;
-			_control_mode.flag_control_attitude_enabled = false;
-			_control_mode.flag_control_position_enabled = false;
-			_control_mode.flag_control_velocity_enabled = false;
-			_control_mode.flag_control_altitude_enabled = false;
-			_control_mode.flag_control_climb_rate_enabled = false;
-			break;
-
-		case NAV_STATE_LAND:
-			/* land with or without position control */
-			_control_mode.flag_control_manual_enabled = false;
-			_control_mode.flag_control_rates_enabled = true;
-			_control_mode.flag_control_attitude_enabled = true;
-			_control_mode.flag_control_position_enabled = _vstatus.condition_global_position_valid;
-			_control_mode.flag_control_velocity_enabled = _vstatus.condition_global_position_valid;
-			_control_mode.flag_control_altitude_enabled = true;
-			_control_mode.flag_control_climb_rate_enabled = true;
-			break;
-
-		default:
-			_control_mode.flag_control_rates_enabled = true;
-			_control_mode.flag_control_attitude_enabled = true;
-			_control_mode.flag_control_position_enabled = true;
-			_control_mode.flag_control_velocity_enabled = true;
-			_control_mode.flag_control_altitude_enabled = true;
-			_control_mode.flag_control_climb_rate_enabled = true;
-			break;
-		}
-	}
-
-	_control_mode.timestamp = hrt_absolute_time();
-
-	/* lazily publish the mission triplet only once available */
-	if (_control_mode_pub > 0) {
-		/* publish the mission triplet */
-		orb_publish(ORB_ID(vehicle_control_mode), _control_mode_pub, &_control_mode);
-
-	} else {
-		/* advertise and publish */
-		_control_mode_pub = orb_advertise(ORB_ID(vehicle_control_mode), &_control_mode);
-	}
-}
-
 void Navigator::add_fence_point(int argc, char *argv[])
 {
 	_geofence.addPoint(argc, argv);
diff --git a/src/modules/navigator/navigator_state.h b/src/modules/navigator/navigator_state.h
new file mode 100644
index 000000000..6a1475c9b
--- /dev/null
+++ b/src/modules/navigator/navigator_state.h
@@ -0,0 +1,21 @@
+/*
+ * navigator_state.h
+ *
+ *  Created on: 27.01.2014
+ *      Author: ton
+ */
+
+#ifndef NAVIGATOR_STATE_H_
+#define NAVIGATOR_STATE_H_
+
+typedef enum {
+	NAV_STATE_NONE = 0,
+	NAV_STATE_READY,
+	NAV_STATE_LOITER,
+	NAV_STATE_MISSION,
+	NAV_STATE_RTL,
+	NAV_STATE_LAND,
+	NAV_STATE_MAX
+} nav_state_t;
+
+#endif /* NAVIGATOR_STATE_H_ */