Merge remote-tracking branch 'upstream/navigator_new' into navigator_new_fw

5 files changed, 145 insertions, 156 deletions

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 03d3c02d1..1e5318121 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -605,7 +605,7 @@ int commander_thread_main(int argc, char *argv[])
 	memset(&armed, 0, sizeof(armed));
 
 	status.main_state = MAIN_STATE_MANUAL;
-	status.set_nav_state = NAV_STATE_INIT;
+	status.set_nav_state = NAV_STATE_NONE;
 	status.set_nav_state_timestamp = 0;
 	status.arming_state = ARMING_STATE_INIT;
 	status.hil_state = HIL_STATE_OFF;
diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c
index bc191f30d..d4e4c027b 100644
--- a/src/modules/mavlink/mavlink.c
+++ b/src/modules/mavlink/mavlink.c
@@ -220,17 +220,14 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, u
 	} else if (v_status.main_state == MAIN_STATE_AUTO) {
 		*mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED;
 		custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
-		// TODO review
 		if (control_mode.nav_state == NAV_STATE_NONE) {
 			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY;
 		} else if (control_mode.nav_state == NAV_STATE_LOITER) {
 			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER;
-		} else if (control_mode.nav_state == NAV_STATE_MISSION_LOITER) {
-			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER;
-		} else if (control_mode.nav_state == NAV_STATE_RTL_LOITER) {
-			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER;
 		} else if (control_mode.nav_state == NAV_STATE_MISSION) {
 			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION;
+		} else if (control_mode.nav_state == NAV_STATE_RTL) {
+			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL;
 		}
 	}
 	*mavlink_custom_mode = custom_mode.data;
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index 4176cee1b..982ebefcc 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -165,6 +165,8 @@ private:
 
 	class 		Mission				_mission;
 
+	bool		_reset_loiter_pos;				/**< if true then loiter position should be set to current position */
+	bool		_rtl_done;
 	bool		_waypoint_position_reached;
 	bool		_waypoint_yaw_reached;
 	uint64_t	_time_first_inside_orbit;
@@ -260,9 +262,9 @@ private:
 	void		start_none();
 	void		start_loiter();
 	void		start_mission();
-	void		start_mission_loiter();
+	void		finish_mission();
 	void		start_rtl();
-	void		start_rtl_loiter();
+	void		finish_rtl();
 
 	/**
 	 * Guards offboard mission
@@ -354,6 +356,8 @@ Navigator::Navigator() :
 	_fence_valid(false),
 	_inside_fence(true),
 	_mission(),
+	_reset_loiter_pos(true),
+	_rtl_done(false),
 	_waypoint_position_reached(false),
 	_waypoint_yaw_reached(false),
 	_time_first_inside_orbit(0),
@@ -375,7 +379,8 @@ Navigator::Navigator() :
 	memset(&_mission_result, 0, sizeof(struct mission_result_s));
 
 	/* Initialize state machine */
-	myState = NAV_STATE_INIT;
+	myState = NAV_STATE_NONE;
+	start_none();
 }
 
 Navigator::~Navigator()
@@ -496,11 +501,14 @@ Navigator::task_main_trampoline(int argc, char *argv[])
 void
 Navigator::task_main()
 {
-
 	/* inform about start */
 	warnx("Initializing..");
 	fflush(stdout);
 
+	_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
+
+	mavlink_log_info(_mavlink_fd, "[navigator] started");
+
 	_fence_valid = load_fence(GEOFENCE_MAX_VERTICES);
 
 	/*
@@ -526,7 +534,10 @@ Navigator::task_main()
 	/* rate limit position updates to 50 Hz */
 	orb_set_interval(_global_pos_sub, 20);
 
-	unsigned prevState = 0;
+	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];
@@ -565,10 +576,16 @@ Navigator::task_main()
 
 		perf_begin(_loop_perf);
 
-		/* only update vehicle status if it changed */
+		if (_mavlink_fd < 0 && hrt_absolute_time() > mavlink_open_time) {
+			/* try to reopen the mavlink log device with specified interval */
+			mavlink_open_time = hrt_abstime() + mavlink_open_interval;
+			_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
+		}
+
+		/* 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.main_state == MAIN_STATE_AUTO) {
@@ -577,23 +594,21 @@ Navigator::task_main()
 					/* RC signal available, use control switches to set mode */
 					/* RETURN switch, overrides MISSION switch */
 					if (_vstatus.return_switch == RETURN_SWITCH_RETURN) {
-						dispatch(EVENT_RTL_REQUESTED);
+						dispatch(_rtl_done ? EVENT_LOITER_REQUESTED : EVENT_RTL_REQUESTED);
 						stick_mode = true;
 					} else {
 						/* MISSION switch */
-						if (!stick_mode) {
-							if (_vstatus.mission_switch == MISSION_SWITCH_LOITER) {
+						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;
-							} 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;
 							}
+							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 */
@@ -609,7 +624,6 @@ Navigator::task_main()
 						_set_nav_state_timestamp = _vstatus.set_nav_state_timestamp;
 
 						switch (_vstatus.set_nav_state) {
-						case NAV_STATE_INIT:
 						case NAV_STATE_NONE:
 							/* nothing to do */
 							break;
@@ -619,11 +633,15 @@ Navigator::task_main()
 							break;
 
 						case NAV_STATE_MISSION:
-							dispatch(EVENT_MISSION_REQUESTED);
+							if (_mission.current_mission_available()) {
+								dispatch(EVENT_MISSION_REQUESTED);
+							} else {
+								dispatch(EVENT_LOITER_REQUESTED);
+							}
 							break;
 
 						case NAV_STATE_RTL:
-							dispatch(EVENT_RTL_REQUESTED);
+							dispatch(_rtl_done ? EVENT_LOITER_REQUESTED : EVENT_RTL_REQUESTED);
 							break;
 
 						default:
@@ -632,8 +650,8 @@ Navigator::task_main()
 						}
 
 					} else {
-						/* on first switch to AUTO try mission, if none is available fallback to loiter instead */
-						if (myState == NAV_STATE_INIT || myState == NAV_STATE_NONE) {
+						/* 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 {
@@ -647,73 +665,76 @@ Navigator::task_main()
 				/* not in AUTO */
 				dispatch(EVENT_NONE_REQUESTED);
 			}
-
-			/* XXX Hack to get mavlink output going, try opening the fd with 5Hz */
-			if (_mavlink_fd < 0) {
-				/* try to open the mavlink log device every once in a while */
-				_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
-			}
 		}
 
-		/* only update parameters if it changed */
+		/* parameters updated */
 		if (fds[0].revents & POLLIN) {
 			parameters_update();
 			/* note that these new parameters won't be in effect until a mission triplet is published again */
 		}
 
-		/* only update craft capabilities if they have changed */
+		/* navigation capabilities updated */
 		if (fds[3].revents & POLLIN) {
 			navigation_capabilities_update();
 		}
 
+		/* offboard mission updated */
 		if (fds[4].revents & POLLIN) {
 			offboard_mission_update(_vstatus.is_rotary_wing);
 			// XXX check if mission really changed 
 			dispatch(EVENT_MISSION_CHANGED);
 		}
 
+		/* onboard mission updated */
 		if (fds[5].revents & POLLIN) {
 			onboard_mission_update();
 			// XXX check if mission really changed
 			dispatch(EVENT_MISSION_CHANGED);
 		}
 
+		/* home position updated */
 		if (fds[2].revents & POLLIN) {
 			home_position_update();
 			// XXX check if home position really changed
 			dispatch(EVENT_HOME_POSITION_CHANGED);
 		}
 
-		/* only run controller if position changed */
+		/* global position updated */
 		if (fds[1].revents & POLLIN) {
 			global_position_update();
-			/* only check if waypoint has been reached in Mission or RTL mode */
-			if (mission_item_reached()) {
-
-				if (_vstatus.main_state == MAIN_STATE_AUTO &&
-				    (myState == NAV_STATE_MISSION)) {
-
-					/* advance by one mission item */
-					_mission.move_to_next();
-
-					/* if no more mission items available send this to state machine and start loiter at the last WP */
-					if (_mission.current_mission_available()) {
-						advance_mission();
+			/* only check if waypoint has been reached in MISSION */
+			if (myState == NAV_STATE_MISSION || myState == NAV_STATE_RTL) {
+				if (mission_item_reached()) {
+					if (myState == NAV_STATE_MISSION) {
+						/* advance by one mission item */
+						_mission.move_to_next();
+
+						/* if no more mission items available send this to state machine and start loiter at the last WP */
+						if (_mission.current_mission_available()) {
+							advance_mission();
+						} else {
+							dispatch(EVENT_MISSION_FINISHED);
+						}
 					} else {
+						/* RTL finished */
 						dispatch(EVENT_MISSION_FINISHED);
 					}
-				} else {
-					dispatch(EVENT_MISSION_FINISHED);
 				}
 			}
 		}
 
+
+		/* notify user about state changes */
 		if (myState != prevState) {
 			mavlink_log_info(_mavlink_fd, "[navigator] nav state %d -> %d", prevState, myState);
 			prevState = myState;
+			pub_control_mode = true;
 		}
 
-		publish_control_mode();
+		/* publish control mode if updated */
+		if (pub_control_mode) {
+			publish_control_mode();
+		}
 
 		perf_end(_loop_perf);
 	}
@@ -767,9 +788,6 @@ Navigator::status()
 	}
 
 	switch (myState) {
-		case NAV_STATE_INIT:
-			warnx("State: Init");
-			break;
 		case NAV_STATE_NONE:
 			warnx("State: None");
 			break;
@@ -779,15 +797,9 @@ Navigator::status()
 		case NAV_STATE_MISSION:
 			warnx("State: Mission");
 			break;
-		case NAV_STATE_MISSION_LOITER:
-			warnx("State: Loiter after Mission");
-			break;
 		case NAV_STATE_RTL:
 			warnx("State: RTL");
 			break;
-		case NAV_STATE_RTL_LOITER:
-			warnx("State: Loiter after RTL");
-			break;
 		default:
 			warnx("State: Unknown");
 			break;
@@ -886,16 +898,6 @@ Navigator::fence_point(int argc, char *argv[])
 
 StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = {
 	{	
-							/* STATE_INIT */
-		/* EVENT_NONE_REQUESTED */		{ACTION(&Navigator::start_none), NAV_STATE_NONE},
-		/* EVENT_LOITER_REQUESTED */		{ACTION(&Navigator::start_loiter), NAV_STATE_LOITER},
-		/* EVENT_MISSION_REQUESTED */		{ACTION(&Navigator::start_mission), NAV_STATE_MISSION},
-		/* EVENT_RTL_REQUESTED */		{ACTION(&Navigator::start_rtl), NAV_STATE_RTL},
-		/* EVENT_MISSION_FINISHED */		{NO_ACTION, NAV_STATE_INIT},
-		/* EVENT_MISSION_CHANGED */		{NO_ACTION, NAV_STATE_INIT},
-		/* EVENT_HOME_POSITION_CHANGED */	{NO_ACTION, NAV_STATE_INIT},
-	},
-	{	
 							/* STATE_NONE */
 		/* EVENT_NONE_REQUESTED */		{NO_ACTION, NAV_STATE_NONE},
 		/* EVENT_LOITER_REQUESTED */		{ACTION(&Navigator::start_loiter), NAV_STATE_LOITER},
@@ -921,39 +923,19 @@ StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = {
 		/* EVENT_LOITER_REQUESTED */		{ACTION(&Navigator::start_loiter), NAV_STATE_LOITER},
 		/* EVENT_MISSION_REQUESTED */		{NO_ACTION, NAV_STATE_MISSION},
 		/* EVENT_RTL_REQUESTED */		{ACTION(&Navigator::start_rtl), NAV_STATE_RTL},
-		/* EVENT_MISSION_FINISHED */		{ACTION(&Navigator::start_mission_loiter), NAV_STATE_MISSION_LOITER},
+		/* EVENT_MISSION_FINISHED */		{ACTION(&Navigator::finish_mission), NAV_STATE_LOITER},
 		/* EVENT_MISSION_CHANGED */		{ACTION(&Navigator::start_mission), NAV_STATE_MISSION},
 		/* EVENT_HOME_POSITION_CHANGED */	{NO_ACTION, NAV_STATE_MISSION},
 	},
 	{	
-							/* STATE_MISSION_LOITER */
-		/* EVENT_NONE_REQUESTED */		{ACTION(&Navigator::start_none), NAV_STATE_NONE},
-		/* EVENT_LOITER_REQUESTED */		{NO_ACTION, NAV_STATE_MISSION_LOITER},
-		/* EVENT_MISSION_REQUESTED */		{NO_ACTION, NAV_STATE_MISSION_LOITER},
-		/* EVENT_RTL_REQUESTED */		{ACTION(&Navigator::start_rtl), NAV_STATE_RTL},
-		/* EVENT_MISSION_FINISHED */		{NO_ACTION, NAV_STATE_MISSION_LOITER},
-		/* EVENT_MISSION_CHANGED */		{ACTION(&Navigator::start_mission), NAV_STATE_MISSION},
-		/* EVENT_HOME_POSITION_CHANGED */	{NO_ACTION, NAV_STATE_MISSION_LOITER},
-	},
-	{	
 							/* STATE_RTL */
 		/* EVENT_NONE_REQUESTED */		{ACTION(&Navigator::start_none), NAV_STATE_NONE},
 		/* 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_MISSION_FINISHED */		{ACTION(&Navigator::start_rtl_loiter), NAV_STATE_RTL_LOITER},
+		/* EVENT_MISSION_FINISHED */		{ACTION(&Navigator::finish_rtl), NAV_STATE_LOITER},
 		/* EVENT_MISSION_CHANGED */		{NO_ACTION, NAV_STATE_RTL},
-		/* EVENT_HOME_POSITION_CHANGED */	{ACTION(&Navigator::start_loiter), NAV_STATE_RTL},
-	},
-	{	
-							/* STATE_RTL_LOITER */
-		/* EVENT_NONE_REQUESTED */		{ACTION(&Navigator::start_none), NAV_STATE_NONE},
-		/* EVENT_LOITER_REQUESTED */		{NO_ACTION,  NAV_STATE_RTL_LOITER},
-		/* EVENT_MISSION_REQUESTED */		{ACTION(&Navigator::start_mission), NAV_STATE_MISSION},
-		/* EVENT_RTL_REQUESTED */		{NO_ACTION, NAV_STATE_RTL_LOITER},
-		/* EVENT_MISSION_FINISHED */		{NO_ACTION, NAV_STATE_RTL_LOITER},
-		/* EVENT_MISSION_CHANGED */		{NO_ACTION, NAV_STATE_RTL_LOITER},
-		/* EVENT_HOME_POSITION_CHANGED */	{ACTION(&Navigator::start_loiter), NAV_STATE_LOITER},
+		/* EVENT_HOME_POSITION_CHANGED */	{ACTION(&Navigator::start_rtl), NAV_STATE_RTL},
 	},
 };
 
@@ -964,33 +946,42 @@ Navigator::start_none()
 	_mission_item_triplet.current_valid = false;
 	_mission_item_triplet.next_valid = false;
 
+	_reset_loiter_pos = true;
+	_rtl_done = false;
+
 	publish_mission_item_triplet();
 }
 
 void
 Navigator::start_loiter()
 {
+	/* set loiter position if needed */
+	if (_reset_loiter_pos || !_mission_item_triplet.current_valid) {
+		_reset_loiter_pos = false;
+
+		_mission_item_triplet.current.lat = (double)_global_pos.lat / 1e7d;
+		_mission_item_triplet.current.lon = (double)_global_pos.lon / 1e7d;
+		_mission_item_triplet.current.yaw = NAN;	// NAN means to use current yaw
+
+		_mission_item_triplet.current.altitude_is_relative = false;
+		float min_alt_amsl = _parameters.min_altitude + _home_pos.altitude;
+
+		/* Use current altitude if above min altitude set by parameter */
+		if (_global_pos.alt < min_alt_amsl) {
+			_mission_item_triplet.current.altitude = min_alt_amsl;
+			mavlink_log_info(_mavlink_fd, "[navigator] loiter %.1fm higher", (double)(min_alt_amsl - _global_pos.alt));
+		} else {
+			_mission_item_triplet.current.altitude = _global_pos.alt;
+			mavlink_log_info(_mavlink_fd, "[navigator] loiter at current altitude");
+		}
+	}
+
 	_mission_item_triplet.previous_valid = false;
 	_mission_item_triplet.current_valid = true;
 	_mission_item_triplet.next_valid = false;
 
-	_mission_item_triplet.current.lat = (double)_global_pos.lat / 1e7d;
-	_mission_item_triplet.current.lon = (double)_global_pos.lon / 1e7d;
-	_mission_item_triplet.current.yaw = 0.0f;	// TODO use current yaw sp here or set to undefined?
-
 	get_loiter_item(&_mission_item_triplet.current);
 
-	float global_min_alt = _parameters.min_altitude + _home_pos.altitude;
-
-	/* Use current altitude if above min altitude set by parameter */
-	if (_global_pos.alt < global_min_alt) {
-		_mission_item_triplet.current.altitude = global_min_alt;
-		mavlink_log_info(_mavlink_fd, "[navigator] loiter %.1fm higher", (double)(global_min_alt - _global_pos.alt));
-	} else {
-		_mission_item_triplet.current.altitude = _global_pos.alt;
-		mavlink_log_info(_mavlink_fd, "[navigator] loiter here");
-	}
-
 	publish_mission_item_triplet();
 }
 
@@ -998,7 +989,9 @@ Navigator::start_loiter()
 void
 Navigator::start_mission()
 {
-	/* leave previous mission item as isas is */
+	// TODO set prev item to current position?
+	_reset_loiter_pos = true;
+	_rtl_done = false;
 
 	int ret;
 	bool onboard;
@@ -1012,9 +1005,9 @@ Navigator::start_mission()
 		_mission_item_triplet.current_valid = true;
 
 		if (onboard) {
-			mavlink_log_info(_mavlink_fd, "[navigator] heading to onboard WP %d", index);
+			mavlink_log_info(_mavlink_fd, "[navigator] mission started, onboard WP %d", index);
 		} else {
-			mavlink_log_info(_mavlink_fd, "[navigator] heading to offboard WP %d", index);
+			mavlink_log_info(_mavlink_fd, "[navigator] mission started, offboard WP %d", index);
 		}
 	} else {
 		/* since a mission is started without knowledge if there are more mission items available, this can fail */
@@ -1044,6 +1037,8 @@ Navigator::advance_mission()
 	memcpy(&_mission_item_triplet.previous, &_mission_item_triplet.current, sizeof(mission_item_s));
 	_mission_item_triplet.previous_valid = _mission_item_triplet.current_valid;
 
+	_reset_loiter_pos = true;
+
 	int ret;
 	bool onboard;
 	unsigned index;
@@ -1082,23 +1077,21 @@ Navigator::advance_mission()
 }
 
 void
-Navigator::start_mission_loiter()
+Navigator::finish_mission()
 {
-	/* make sure the current WP is valid */
-	if (!_mission_item_triplet.current_valid) {
-		warnx("ERROR: cannot switch to offboard mission loiter");
-	}
+	/* loiter at last waypoint */
+	_reset_loiter_pos = false;
 
-	get_loiter_item(&_mission_item_triplet.current);
+	mavlink_log_info(_mavlink_fd, "[navigator] mission completed");
 
-	publish_mission_item_triplet();
-
-	mavlink_log_info(_mavlink_fd, "[navigator] loiter at last WP");
+	start_loiter();
 }
 
 void
 Navigator::start_rtl()
 {
+	_reset_loiter_pos = true;
+	_rtl_done = false;
 
 	/* discard all mission item and insert RTL item */
 	_mission_item_triplet.previous_valid = false;
@@ -1108,36 +1101,29 @@ Navigator::start_rtl()
 	_mission_item_triplet.current.lat = _home_pos.lat;
 	_mission_item_triplet.current.lon = _home_pos.lon;
 	_mission_item_triplet.current.altitude = _home_pos.altitude + _parameters.min_altitude;
-	_mission_item_triplet.current.yaw = 0.0f;
+	_mission_item_triplet.current.yaw = 0.0f;	// TODO use heading to waypoint?
 	_mission_item_triplet.current.nav_cmd = NAV_CMD_RETURN_TO_LAUNCH;
 	_mission_item_triplet.current.loiter_direction = 1;
-	_mission_item_triplet.current.loiter_radius = _parameters.loiter_radius; // TODO: get rid of magic number
+	_mission_item_triplet.current.loiter_radius = _parameters.loiter_radius;
 	_mission_item_triplet.current.radius = 50.0f; // TODO: get rid of magic number
 	_mission_item_triplet.current.autocontinue = false;
 	_mission_item_triplet.current_valid = true;
 
 	publish_mission_item_triplet();
 
-	mavlink_log_info(_mavlink_fd, "[navigator] return to launch");
+	mavlink_log_info(_mavlink_fd, "[navigator] RTL started");
 }	
 
-
 void
-Navigator::start_rtl_loiter()
+Navigator::finish_rtl()
 {
-	_mission_item_triplet.previous_valid = false;
-	_mission_item_triplet.current_valid = true;
-	_mission_item_triplet.next_valid = false;
+	/* loiter at home position */
+	_reset_loiter_pos = false;
+	_rtl_done = true;
 
-	_mission_item_triplet.current.lat = _home_pos.lat;
-	_mission_item_triplet.current.lon = _home_pos.lon;
-	_mission_item_triplet.current.altitude = _home_pos.altitude + _parameters.min_altitude;
-	
-	get_loiter_item(&_mission_item_triplet.current);
-
-	publish_mission_item_triplet();
+	mavlink_log_info(_mavlink_fd, "[navigator] RTL completed");
 
-	mavlink_log_info(_mavlink_fd, "[navigator] loiter after RTL");
+	start_loiter();
 }
 
 bool
@@ -1187,8 +1173,12 @@ Navigator::mission_item_reached()
 	float dist_z = -1.0f;
 
 	// if (coordinate_frame == (int)MAV_FRAME_GLOBAL) {
+
+	// current relative or AMSL altitude depending on _mission_item_triplet.current.altitude_is_relative
+	float current_alt = _mission_item_triplet.current.altitude_is_relative ? _global_pos.relative_alt : _global_pos.alt;
+
 	dist = get_distance_to_point_global_wgs84(_mission_item_triplet.current.lat, _mission_item_triplet.current.lon, _mission_item_triplet.current.altitude,
-		                              (double)_global_pos.lat / 1e7d, (double)_global_pos.lon / 1e7d, _global_pos.alt,
+		                              (double)_global_pos.lat / 1e7d, (double)_global_pos.lon / 1e7d, current_alt,
 		                              &dist_xy, &dist_z);
 
 	// warnx("1 lat: %2.2f, lon: %2.2f, alt: %2.2f", _mission_item_triplet.current.lat, _mission_item_triplet.current.lon, _mission_item_triplet.current.altitude);
@@ -1207,13 +1197,18 @@ Navigator::mission_item_reached()
 	// 	// XXX TODO
 	// }
 
-	if (dist >= 0.0f && dist_xy <= orbit && dist_z >= 0.0f && dist_z <= vertical_switch_distance) {
-		_waypoint_position_reached = true;
+	if (_mission_item_triplet.current.nav_cmd == NAV_CMD_TAKEOFF) {
+		/* require only altitude for takeoff */
+		if (current_alt > _mission_item_triplet.current.altitude)
+			_waypoint_position_reached = true;
+	} else {
+		if (dist >= 0.0f && dist_xy <= orbit && dist_z >= 0.0f && dist_z <= vertical_switch_distance) {
+			_waypoint_position_reached = true;
+		}
 	}
 
 	/* check if required yaw reached */
-	float yaw_sp = _wrap_pi(_mission_item_triplet.current.yaw);
-	float yaw_err = _wrap_pi(yaw_sp - _global_pos.yaw);
+	float yaw_err = _wrap_pi(_mission_item_triplet.current.yaw - _global_pos.yaw);
 
 	if (fabsf(yaw_err) < 0.05f) { /* XXX get rid of magic number */
 		_waypoint_yaw_reached = true;
@@ -1223,7 +1218,7 @@ Navigator::mission_item_reached()
 	if (_waypoint_position_reached /* && _waypoint_yaw_reached */) { /* XXX what about yaw? */
 		
 		if (_time_first_inside_orbit == 0) {
-			/* XXX announcment? */
+			/* XXX announcement? */
 			_time_first_inside_orbit = now;
 		}
 		
@@ -1337,17 +1332,17 @@ Navigator::publish_control_mode()
 }
 
 bool Navigator::cmp_mission_item_equivalent(const struct mission_item_s a, const struct mission_item_s b) {
-	if (fabsf(a.altitude_is_relative - b.altitude_is_relative) < FLT_EPSILON &&
-	    fabsf(a.lat - b.lat) < FLT_EPSILON &&
-	    fabsf(a.lon - b.lon) < FLT_EPSILON &&
+	if (a.altitude_is_relative == b.altitude_is_relative &&
+	    fabs(a.lat - b.lat) < FLT_EPSILON &&
+	    fabs(a.lon - b.lon) < FLT_EPSILON &&
 	    fabsf(a.altitude - b.altitude) < FLT_EPSILON &&
 	    fabsf(a.yaw - b.yaw) < FLT_EPSILON &&
 	    fabsf(a.loiter_radius - b.loiter_radius) < FLT_EPSILON &&
-	    fabsf(a.loiter_direction - b.loiter_direction) < FLT_EPSILON &&
-	    fabsf(a.nav_cmd - b.nav_cmd) < FLT_EPSILON &&
+	    a.loiter_direction == b.loiter_direction &&
+	    a.nav_cmd == b.nav_cmd &&
 	    fabsf(a.radius - b.radius) < FLT_EPSILON &&
 	    fabsf(a.time_inside - b.time_inside) < FLT_EPSILON &&
-	    fabsf(a.autocontinue - b.autocontinue) < FLT_EPSILON) {
+	    a.autocontinue == b.autocontinue) {
 		return true;
 	} else {
 		return false;
diff --git a/src/modules/uORB/topics/home_position.h b/src/modules/uORB/topics/home_position.h
index 1cf37e29c..3e2fee84e 100644
--- a/src/modules/uORB/topics/home_position.h
+++ b/src/modules/uORB/topics/home_position.h
@@ -59,7 +59,7 @@ struct home_position_s
 {
 	uint64_t timestamp;			/**< Timestamp (microseconds since system boot)	*/
 
-	bool altitude_is_relative;
+	//bool altitude_is_relative;	// TODO what means home relative altitude? we need clear definition of reference altitude then
 	double lat;				/**< Latitude in degrees 			*/
 	double lon;				/**< Longitude in degrees			*/
 	float altitude;				/**< Altitude in meters				*/
diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h
index 5ce968f67..9d5dad9f9 100644
--- a/src/modules/uORB/topics/vehicle_control_mode.h
+++ b/src/modules/uORB/topics/vehicle_control_mode.h
@@ -62,13 +62,10 @@
  */
 
 typedef enum {
-	NAV_STATE_INIT = 0,
-	NAV_STATE_NONE,
+	NAV_STATE_NONE = 0,
 	NAV_STATE_LOITER,
 	NAV_STATE_MISSION,
-	NAV_STATE_MISSION_LOITER,
 	NAV_STATE_RTL,
-	NAV_STATE_RTL_LOITER,
 	NAV_STATE_MAX
 } nav_state_t;