Merge branch 'beta1' into beta

21 files changed, 851 insertions, 773 deletions

diff --git a/src/drivers/frsky_telemetry/frsky_data.c b/src/drivers/frsky_telemetry/frsky_data.c
index e201ecbb3..cfcf91e3f 100644
--- a/src/drivers/frsky_telemetry/frsky_data.c
+++ b/src/drivers/frsky_telemetry/frsky_data.c
@@ -225,7 +225,7 @@ void frsky_send_frame2(int uart)
 	float course = 0, lat = 0, lon = 0, speed = 0, alt = 0;
 	char lat_ns = 0, lon_ew = 0;
 	int sec = 0;
-	if (global_pos.valid) {
+	if (global_pos.global_valid) {
 		time_t time_gps = global_pos.time_gps_usec / 1000000;
 		struct tm *tm_gps = gmtime(&time_gps);
 
diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c
index 08fe2b696..446cd5bee 100644
--- a/src/lib/geo/geo.c
+++ b/src/lib/geo/geo.c
@@ -72,7 +72,6 @@ __EXPORT void map_projection_init(double lat_0, double lon_0) //lat_0, lon_0 are
 	/* calculate local scale by using the relation of true distance and the distance on plane */ //TODO: this is a quick solution, there are probably easier ways to determine the scale
 
 	/* 1) calculate true distance d on sphere to a point: http://www.movable-type.co.uk/scripts/latlong.html */
-	const double r_earth = 6371000;
 
 	double lat1 = phi_1;
 	double lon1 = lambda_0;
@@ -81,7 +80,7 @@ __EXPORT void map_projection_init(double lat_0, double lon_0) //lat_0, lon_0 are
 	double lon2 = lambda_0 + 0.5 / 180 * M_PI;
 	double sin_lat_2 = sin(lat2);
 	double cos_lat_2 = cos(lat2);
-	double d = acos(sin(lat1) * sin_lat_2 + cos(lat1) * cos_lat_2 * cos(lon2 - lon1)) * r_earth;
+	double d = acos(sin(lat1) * sin_lat_2 + cos(lat1) * cos_lat_2 * cos(lon2 - lon1)) * CONSTANTS_RADIUS_OF_EARTH;
 
 	/* 2) calculate distance rho on plane */
 	double k_bar = 0;
@@ -188,8 +187,7 @@ __EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, dou
 	double a = sin(d_lat / 2.0d) * sin(d_lat / 2.0d) + sin(d_lon / 2.0d) * sin(d_lon / 2.0d) * cos(lat_now_rad) * cos(lat_next_rad);
 	double c = 2.0d * atan2(sqrt(a), sqrt(1.0d - a));
 
-	const double radius_earth = 6371000.0d;
-	return radius_earth * c; 
+	return CONSTANTS_RADIUS_OF_EARTH * c;
 }
 
 __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next)
@@ -210,7 +208,7 @@ __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, doub
 	return theta;
 }
 
-__EXPORT void get_vector_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next, float* vx, float* vy)
+__EXPORT void get_vector_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next, float* v_n, float* v_e)
 {
 	double lat_now_rad = lat_now * M_DEG_TO_RAD;
 	double lon_now_rad = lon_now * M_DEG_TO_RAD;
@@ -221,11 +219,11 @@ __EXPORT void get_vector_to_next_waypoint(double lat_now, double lon_now, double
 	double d_lon = lon_next_rad - lon_now_rad;
 
 	/* conscious mix of double and float trig function to maximize speed and efficiency */
-	*vy = CONSTANTS_RADIUS_OF_EARTH * sin(d_lon) * cos(lat_next_rad);
-	*vx = CONSTANTS_RADIUS_OF_EARTH * cos(lat_now_rad) * sin(lat_next_rad) - sin(lat_now_rad) * cos(lat_next_rad) * cos(d_lon);
+	*v_n = CONSTANTS_RADIUS_OF_EARTH * (cos(lat_now_rad) * sin(lat_next_rad) - sin(lat_now_rad) * cos(lat_next_rad) * cos(d_lon));
+	*v_e = CONSTANTS_RADIUS_OF_EARTH * sin(d_lon) * cos(lat_next_rad);
 }
 
-__EXPORT void get_vector_to_next_waypoint_fast(double lat_now, double lon_now, double lat_next, double lon_next, float* vx, float* vy)
+__EXPORT void get_vector_to_next_waypoint_fast(double lat_now, double lon_now, double lat_next, double lon_next, float* v_n, float* v_e)
 {
 	double lat_now_rad = lat_now * M_DEG_TO_RAD;
 	double lon_now_rad = lon_now * M_DEG_TO_RAD;
@@ -236,8 +234,17 @@ __EXPORT void get_vector_to_next_waypoint_fast(double lat_now, double lon_now, d
 	double d_lon = lon_next_rad - lon_now_rad;
 
 	/* conscious mix of double and float trig function to maximize speed and efficiency */
-	*vy = CONSTANTS_RADIUS_OF_EARTH * d_lon;
-	*vx = CONSTANTS_RADIUS_OF_EARTH * cos(lat_now_rad);
+	*v_n = CONSTANTS_RADIUS_OF_EARTH * d_lat;
+	*v_e = CONSTANTS_RADIUS_OF_EARTH * d_lon * cos(lat_now_rad);
+}
+
+__EXPORT void add_vector_to_global_position(double lat_now, double lon_now, float v_n, float v_e, double *lat_res, double *lon_res)
+{
+	double lat_now_rad = lat_now * M_DEG_TO_RAD;
+	double lon_now_rad = lon_now * M_DEG_TO_RAD;
+
+	*lat_res = (lat_now_rad + v_n / CONSTANTS_RADIUS_OF_EARTH) * M_RAD_TO_DEG;
+	*lon_res = (lon_now_rad + v_e / (CONSTANTS_RADIUS_OF_EARTH * cos(lat_now_rad))) * M_RAD_TO_DEG;
 }
 
 // Additional functions - @author Doug Weibel <douglas.weibel@colorado.edu>
diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h
index 5f4bce698..94afb4df0 100644
--- a/src/lib/geo/geo.h
+++ b/src/lib/geo/geo.h
@@ -115,9 +115,11 @@ __EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, dou
  */
 __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next);
 
-__EXPORT void get_vector_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next, float* vx, float* vy);
+__EXPORT void get_vector_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next, float* v_n, float* v_e);
 
-__EXPORT void get_vector_to_next_waypoint_fast(double lat_now, double lon_now, double lat_next, double lon_next, float* vx, float* vy);
+__EXPORT void get_vector_to_next_waypoint_fast(double lat_now, double lon_now, double lat_next, double lon_next, float* v_n, float* v_e);
+
+__EXPORT void add_vector_to_global_position(double lat_now, double lon_now, float v_n, float v_e, double *lat_res, double *lon_res);
 
 __EXPORT int get_distance_to_line(struct crosstrack_error_s * crosstrack_error, double lat_now, double lon_now, double lat_start, double lon_start, double lat_end, double lon_end);
 
diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp
index 83145ac72..668bac5d9 100644
--- a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp
+++ b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp
@@ -314,7 +314,7 @@ void KalmanNav::updatePublications()
 	// global position publication
 	_pos.timestamp = _pubTimeStamp;
 	_pos.time_gps_usec = _gps.timestamp_position;
-	_pos.valid = true;
+	_pos.global_valid = true;
 	_pos.lat = lat * M_RAD_TO_DEG;
 	_pos.lon = lon * M_RAD_TO_DEG;
 	_pos.alt = float(alt);
diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
index 66ec20b95..620185fb7 100755
--- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
+++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
@@ -410,7 +410,7 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 							vel(2) = gps.vel_d_m_s;
 						}
 
-					} else if (ekf_params.acc_comp == 2 && global_pos.valid && hrt_absolute_time() < global_pos.timestamp + 500000) {
+					} else if (ekf_params.acc_comp == 2 && global_pos.global_valid && hrt_absolute_time() < global_pos.timestamp + 500000) {
 						vel_valid = true;
 						if (global_pos_updated) {
 							vel_t = global_pos.timestamp;
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index f579fb52a..901f91911 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -203,6 +203,8 @@ void check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicl
 
 transition_result_t set_main_state_rc(struct vehicle_status_s *status);
 
+void set_control_mode();
+
 void print_reject_mode(const char *msg);
 
 void print_reject_arm(const char *msg);
@@ -555,10 +557,8 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 }
 
 static struct vehicle_status_s status;
-
-/* armed topic */
+static struct vehicle_control_mode_s control_mode;
 static struct actuator_armed_s armed;
-
 static struct safety_s safety;
 
 int commander_thread_main(int argc, char *argv[])
@@ -613,16 +613,9 @@ int commander_thread_main(int argc, char *argv[])
 
 	mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
 
-	/* Main state machine */
-	/* make sure we are in preflight state */
+	/* vehicle status topic */
 	memset(&status, 0, sizeof(status));
 	status.condition_landed = true;	// initialize to safe value
-
-	/* armed topic */
-	orb_advert_t armed_pub;
-	/* Initialize armed with all false */
-	memset(&armed, 0, sizeof(armed));
-
 	status.main_state = MAIN_STATE_MANUAL;
 	status.set_nav_state = NAV_STATE_NONE;
 	status.set_nav_state_timestamp = 0;
@@ -645,14 +638,20 @@ int commander_thread_main(int argc, char *argv[])
 	// XXX for now just set sensors as initialized
 	status.condition_system_sensors_initialized = true;
 
-	/* advertise to ORB */
-	status_pub = orb_advertise(ORB_ID(vehicle_status), &status);
-	/* publish current state machine */
-
-	/* publish initial state */
 	status.counter++;
 	status.timestamp = hrt_absolute_time();
-	orb_publish(ORB_ID(vehicle_status), status_pub, &status);
+
+	/* publish initial state */
+	status_pub = orb_advertise(ORB_ID(vehicle_status), &status);
+
+	/* armed topic */
+	orb_advert_t armed_pub;
+	/* Initialize armed with all false */
+	memset(&armed, 0, sizeof(armed));
+
+	/* vehicle control mode topic */
+	memset(&control_mode, 0, sizeof(control_mode));
+	orb_advert_t control_mode_pub = orb_advertise(ORB_ID(vehicle_control_mode), &control_mode);
 
 	armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed);
 
@@ -872,7 +871,7 @@ int commander_thread_main(int argc, char *argv[])
 		}
 
 		/* update condition_global_position_valid */
-		check_valid(global_position.timestamp, POSITION_TIMEOUT, global_position.valid, &(status.condition_global_position_valid), &status_changed);
+		check_valid(global_position.timestamp, POSITION_TIMEOUT, global_position.global_valid, &(status.condition_global_position_valid), &status_changed);
 
 		/* update local position estimate */
 		orb_check(local_position_sub, &updated);
@@ -1031,7 +1030,7 @@ int commander_thread_main(int argc, char *argv[])
 			if (!status.condition_home_position_valid && gps_position.fix_type >= 3 &&
 			    (gps_position.eph_m < hdop_threshold_m) && (gps_position.epv_m < vdop_threshold_m) &&
 			    (hrt_absolute_time() < gps_position.timestamp_position + POSITION_TIMEOUT) && !armed.armed
-			    && global_position.valid) {
+			    && global_position.global_valid) {
 
 				/* copy position data to uORB home message, store it locally as well */
 				home.lat = global_position.lat;
@@ -1244,8 +1243,13 @@ int commander_thread_main(int argc, char *argv[])
 
 		/* publish states (armed, control mode, vehicle status) at least with 5 Hz */
 		if (counter % (200000 / COMMANDER_MONITORING_INTERVAL) == 0 || status_changed) {
+			set_control_mode();
+			control_mode.timestamp = t1;
+			orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, &control_mode);
+
 			status.timestamp = t1;
 			orb_publish(ORB_ID(vehicle_status), status_pub, &status);
+
 			armed.timestamp = t1;
 			orb_publish(ORB_ID(actuator_armed), armed_pub, &armed);
 		}
@@ -1472,7 +1476,7 @@ check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_sta
 transition_result_t
 set_main_state_rc(struct vehicle_status_s *status)
 {
-	/* evaluate the main state machine */
+	/* set main state according to RC switches */
 	transition_result_t res = TRANSITION_DENIED;
 
 	switch (status->mode_switch) {
@@ -1531,6 +1535,102 @@ set_main_state_rc(struct vehicle_status_s *status)
 }
 
 void
+set_control_mode()
+{
+	/* set vehicle_control_mode according to main state and failsafe state */
+	control_mode.flag_armed = armed.armed;
+	control_mode.flag_external_manual_override_ok = !status.is_rotary_wing;
+	control_mode.flag_system_hil_enabled = status.hil_state == HIL_STATE_ON;
+
+	control_mode.flag_control_termination_enabled = false;
+
+	/* set this flag when navigator should act */
+	bool navigator_enabled = false;
+
+	switch (status.failsafe_state) {
+	case FAILSAFE_STATE_NORMAL:
+		switch (status.main_state) {
+		case MAIN_STATE_MANUAL:
+			control_mode.flag_control_manual_enabled = true;
+			control_mode.flag_control_auto_enabled = false;
+			control_mode.flag_control_rates_enabled = status.is_rotary_wing;
+			control_mode.flag_control_attitude_enabled = status.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_auto_enabled = false;
+			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_auto_enabled = false;
+			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;
+
+		default:
+			break;
+		}
+
+		break;
+
+	case FAILSAFE_STATE_RTL:
+		navigator_enabled = true;
+		break;
+
+	case FAILSAFE_STATE_LAND:
+		navigator_enabled = true;
+		break;
+
+	case FAILSAFE_STATE_TERMINATION:
+		/* disable all controllers on termination */
+		control_mode.flag_control_manual_enabled = false;
+		control_mode.flag_control_auto_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;
+		control_mode.flag_control_auto_enabled = true;
+		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;
+	}
+}
+
+void
 print_reject_mode(const char *msg)
 {
 	hrt_abstime t = hrt_absolute_time();
diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index c7256583a..43d0e023e 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -384,6 +384,8 @@ transition_result_t failsafe_state_transition(struct vehicle_status_s *status, f
 		case FAILSAFE_STATE_RTL:
 			/* global position and home position required for RTL */
 			if (status->condition_global_position_valid && status->condition_home_position_valid) {
+				status->set_nav_state = NAV_STATE_RTL;
+				status->set_nav_state_timestamp = hrt_absolute_time();
 				ret = TRANSITION_CHANGED;
 			}
 
@@ -392,6 +394,8 @@ transition_result_t failsafe_state_transition(struct vehicle_status_s *status, f
 		case FAILSAFE_STATE_LAND:
 			/* at least relative altitude estimate required for landing */
 			if (status->condition_local_altitude_valid || status->condition_global_position_valid) {
+				status->set_nav_state = NAV_STATE_LAND;
+				status->set_nav_state_timestamp = hrt_absolute_time();
 				ret = TRANSITION_CHANGED;
 			}
 
diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c
index 4f8091716..4d975066f 100644
--- a/src/modules/mavlink/mavlink.c
+++ b/src/modules/mavlink/mavlink.c
@@ -199,8 +199,7 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, u
 	}
 
 	/* arming state */
-	if (v_status.arming_state == ARMING_STATE_ARMED
-			|| v_status.arming_state == ARMING_STATE_ARMED_ERROR) {
+	if (armed.armed) {
 		*mavlink_base_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
 	}
 
@@ -208,28 +207,36 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, u
 	*mavlink_base_mode |= MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
 	union px4_custom_mode custom_mode;
 	custom_mode.data = 0;
-	if (v_status.main_state == MAIN_STATE_MANUAL) {
-		*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (v_status.is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0);
-		custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL;
-	} else if (v_status.main_state == MAIN_STATE_SEATBELT) {
-		*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED;
-		custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_SEATBELT;
-	} else if (v_status.main_state == MAIN_STATE_EASY) {
-		*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED;
-		custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_EASY;
-	} else if (v_status.main_state == MAIN_STATE_AUTO) {
+	if (pos_sp_triplet.nav_state == NAV_STATE_NONE) {
+		/* use main state when navigator is not active */
+		if (v_status.main_state == MAIN_STATE_MANUAL) {
+			*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (v_status.is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0);
+			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL;
+		} else if (v_status.main_state == MAIN_STATE_SEATBELT) {
+			*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED;
+			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_SEATBELT;
+		} else if (v_status.main_state == MAIN_STATE_EASY) {
+			*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED;
+			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_EASY;
+		} 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;
+			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY;
+		}
+	} else {
+		/* use navigation state when navigator is active */
 		*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;
-		if (control_mode.nav_state == NAV_STATE_NONE) {	// failsafe, shouldn't happen
-			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY;
-		} else if (control_mode.nav_state == NAV_STATE_READY) {
+		if (pos_sp_triplet.nav_state == NAV_STATE_READY) {
 			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY;
-		} else if (control_mode.nav_state == NAV_STATE_LOITER) {
+		} else if (pos_sp_triplet.nav_state == NAV_STATE_LOITER) {
 			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER;
-		} else if (control_mode.nav_state == NAV_STATE_MISSION) {
+		} else if (pos_sp_triplet.nav_state == NAV_STATE_MISSION) {
 			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION;
-		} else if (control_mode.nav_state == NAV_STATE_RTL) {
+		} else if (pos_sp_triplet.nav_state == NAV_STATE_RTL) {
 			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL;
+		} else if (pos_sp_triplet.nav_state == NAV_STATE_LAND) {
+			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LAND;
 		}
 	}
 	*mavlink_custom_mode = custom_mode.data;
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 7c23488d7..a371a499e 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -634,7 +634,7 @@ handle_message(mavlink_message_t *msg)
 				orb_publish(ORB_ID(vehicle_global_position), pub_hil_global_pos, &hil_global_pos);
 				// global position packet
 				hil_global_pos.timestamp = timestamp;
-				hil_global_pos.valid = true;
+				hil_global_pos.global_valid = true;
 				hil_global_pos.lat = hil_state.lat;
 				hil_global_pos.lon = hil_state.lon;
 				hil_global_pos.alt = hil_state.alt / 1000.0f;
diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c
index 7f6237535..69cd820a0 100644
--- a/src/modules/mavlink/orb_listener.c
+++ b/src/modules/mavlink/orb_listener.c
@@ -70,7 +70,7 @@ struct vehicle_local_position_s local_pos;
 struct home_position_s home;
 struct navigation_capabilities_s nav_cap;
 struct vehicle_status_s v_status;
-struct vehicle_control_mode_s control_mode;
+struct position_setpoint_triplet_s pos_sp_triplet;
 struct rc_channels_s rc;
 struct rc_input_values rc_raw;
 struct actuator_armed_s armed;
@@ -127,7 +127,6 @@ static void	l_vehicle_rates_setpoint(const struct listener *l);
 static void	l_home(const struct listener *l);
 static void	l_airspeed(const struct listener *l);
 static void	l_nav_cap(const struct listener *l);
-static void	l_control_mode(const struct listener *l);
 
 static const struct listener listeners[] = {
 	{l_sensor_combined,		&mavlink_subs.sensor_sub,	0},
@@ -154,7 +153,6 @@ static const struct listener listeners[] = {
 	{l_home,			&mavlink_subs.home_sub,		0},
 	{l_airspeed,			&mavlink_subs.airspeed_sub,		0},
 	{l_nav_cap,			&mavlink_subs.navigation_capabilities_sub,		0},
-	{l_control_mode,			&mavlink_subs.control_mode_sub,		0},
 };
 
 static const unsigned n_listeners = sizeof(listeners) / sizeof(listeners[0]);
@@ -315,6 +313,7 @@ l_vehicle_status(const struct listener *l)
 	/* immediately communicate state changes back to user */
 	orb_copy(ORB_ID(vehicle_status), status_sub, &v_status);
 	orb_copy(ORB_ID(actuator_armed), mavlink_subs.armed_sub, &armed);
+	orb_copy(ORB_ID(position_setpoint_triplet), mavlink_subs.position_setpoint_triplet_sub, &pos_sp_triplet);
 
 	/* enable or disable HIL */
 	if (v_status.hil_state == HIL_STATE_ON)
@@ -682,26 +681,6 @@ l_nav_cap(const struct listener *l)
 
 }
 
-void
-l_control_mode(const struct listener *l)
-{
-	orb_copy(ORB_ID(vehicle_control_mode), mavlink_subs.control_mode_sub, &control_mode);
-
-	/* translate the current syste state to mavlink state and mode */
-	uint8_t mavlink_state = 0;
-	uint8_t mavlink_base_mode = 0;
-	uint32_t mavlink_custom_mode = 0;
-	get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
-
-	/* send heartbeat */
-	mavlink_msg_heartbeat_send(chan,
-				   mavlink_system.type,
-				   MAV_AUTOPILOT_PX4,
-				   mavlink_base_mode,
-				   mavlink_custom_mode,
-				   mavlink_state);
-}
-
 static void *
 uorb_receive_thread(void *arg)
 {
@@ -777,9 +756,9 @@ uorb_receive_start(void)
 	status_sub = orb_subscribe(ORB_ID(vehicle_status));
 	orb_set_interval(status_sub, 300);		/* max 3.33 Hz updates */
 
-	/* --- CONTROL MODE --- */
-	mavlink_subs.control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
-	orb_set_interval(mavlink_subs.control_mode_sub, 300);		/* max 3.33 Hz updates */
+	/* --- POSITION SETPOINT TRIPLET --- */
+	mavlink_subs.position_setpoint_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
+	orb_set_interval(mavlink_subs.position_setpoint_triplet_sub, 0);		/* not polled, don't limit */
 
 	/* --- RC CHANNELS VALUE --- */
 	rc_sub = orb_subscribe(ORB_ID(rc_channels));
diff --git a/src/modules/mavlink/orb_topics.h b/src/modules/mavlink/orb_topics.h
index 30ba348cf..89f647bdc 100644
--- a/src/modules/mavlink/orb_topics.h
+++ b/src/modules/mavlink/orb_topics.h
@@ -94,7 +94,7 @@ struct mavlink_subscriptions {
 	int home_sub;
 	int airspeed_sub;
 	int navigation_capabilities_sub;
-	int control_mode_sub;
+	int position_setpoint_triplet_sub;
 };
 
 extern struct mavlink_subscriptions mavlink_subs;
@@ -111,8 +111,8 @@ extern struct navigation_capabilities_s nav_cap;
 /** Vehicle status */
 extern struct vehicle_status_s v_status;
 
-/** Vehicle control mode */
-extern struct vehicle_control_mode_s control_mode;
+/** Position setpoint triplet */
+extern struct position_setpoint_triplet_s pos_sp_triplet;
 
 /** RC channels */
 extern struct rc_channels_s rc;
diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp
index d3e39e3a0..fe8377a40 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -63,8 +63,7 @@
 #include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/parameter_update.h>
-#include <uORB/topics/vehicle_local_position.h>
-#include <uORB/topics/vehicle_local_position_setpoint.h>
+#include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/position_setpoint_triplet.h>
 #include <uORB/topics/vehicle_global_velocity_setpoint.h>
 #include <systemlib/param/param.h>
@@ -109,6 +108,7 @@ private:
 
 	bool	_task_should_exit;		/**< if true, task should exit */
 	int		_control_task;			/**< task handle for task */
+	int		_mavlink_fd;			/**< mavlink fd */
 
 	int		_att_sub;				/**< vehicle attitude subscription */
 	int		_att_sp_sub;			/**< vehicle attitude setpoint */
@@ -116,11 +116,11 @@ private:
 	int 	_params_sub;			/**< notification of parameter updates */
 	int 	_manual_sub;			/**< notification of manual control updates */
 	int		_arming_sub;			/**< arming status of outputs */
-	int		_local_pos_sub;			/**< vehicle local position */
+	int		_global_pos_sub;			/**< vehicle local position */
 	int		_pos_sp_triplet_sub;		/**< position setpoint triplet */
 
-	orb_advert_t	_local_pos_sp_pub;		/**< local position setpoint publication */
 	orb_advert_t	_att_sp_pub;			/**< attitude setpoint publication */
+	orb_advert_t	_pos_sp_triplet_pub;	/**< position setpoint triplet publication */
 	orb_advert_t	_global_vel_sp_pub;		/**< vehicle global velocity setpoint */
 
 	struct vehicle_attitude_s			_att;			/**< vehicle attitude */
@@ -128,8 +128,7 @@ private:
 	struct manual_control_setpoint_s	_manual;		/**< r/c channel data */
 	struct vehicle_control_mode_s		_control_mode;	/**< vehicle control mode */
 	struct actuator_armed_s				_arming;		/**< actuator arming status */
-	struct vehicle_local_position_s		_local_pos;		/**< vehicle local position */
-	struct vehicle_local_position_setpoint_s		_local_pos_sp;		/**< vehicle local position */
+	struct vehicle_global_position_s		_global_pos;	/**< vehicle global position */
 	struct position_setpoint_triplet_s		_pos_sp_triplet;	/**< vehicle global position setpoint triplet */
 	struct vehicle_global_velocity_setpoint_s	_global_vel_sp;	/**< vehicle global velocity setpoint */
 
@@ -177,9 +176,15 @@ private:
 		math::Vector<3> sp_offs_max;
 	}		_params;
 
-	math::Vector<3> _pos;
+	double _lat_sp;
+	double _lon_sp;
+	float _alt_sp;
+
+	bool _reset_lat_lon_sp;
+	bool _reset_alt_sp;
+	bool _use_global_alt;			/**< switch between global (AMSL) and barometric altitudes */
+
 	math::Vector<3> _vel;
-	math::Vector<3> _pos_sp;
 	math::Vector<3> _vel_sp;
 	math::Vector<3> _vel_prev;			/**< velocity on previous step */
 
@@ -201,6 +206,21 @@ private:
 	static float	scale_control(float ctl, float end, float dz);
 
 	/**
+	 * Reset lat/lon to current position
+	 */
+	void		reset_lat_lon_sp();
+
+	/**
+	 * Reset altitude setpoint to current altitude
+	 */
+	void		reset_alt_sp();
+
+	/**
+	 * Select between barometric and global (AMSL) altitudes
+	 */
+	void		select_alt(bool global);
+
+	/**
 	 * Shim for calling task_main from task_create.
 	 */
 	static void	task_main_trampoline(int argc, char *argv[]);
@@ -227,6 +247,7 @@ MulticopterPositionControl::MulticopterPositionControl() :
 
 	_task_should_exit(false),
 	_control_task(-1),
+	_mavlink_fd(-1),
 
 /* subscriptions */
 	_att_sub(-1),
@@ -235,21 +256,28 @@ MulticopterPositionControl::MulticopterPositionControl() :
 	_params_sub(-1),
 	_manual_sub(-1),
 	_arming_sub(-1),
-	_local_pos_sub(-1),
+	_global_pos_sub(-1),
 	_pos_sp_triplet_sub(-1),
 
 /* publications */
-	_local_pos_sp_pub(-1),
 	_att_sp_pub(-1),
-	_global_vel_sp_pub(-1)
+	_pos_sp_triplet_pub(-1),
+	_global_vel_sp_pub(-1),
+
+	_lat_sp(0.0),
+	_lon_sp(0.0),
+	_alt_sp(0.0f),
+
+	_reset_lat_lon_sp(true),
+	_reset_alt_sp(true),
+	_use_global_alt(false)
 {
 	memset(&_att, 0, sizeof(_att));
 	memset(&_att_sp, 0, sizeof(_att_sp));
 	memset(&_manual, 0, sizeof(_manual));
 	memset(&_control_mode, 0, sizeof(_control_mode));
 	memset(&_arming, 0, sizeof(_arming));
-	memset(&_local_pos, 0, sizeof(_local_pos));
-	memset(&_local_pos_sp, 0, sizeof(_local_pos_sp));
+	memset(&_global_pos, 0, sizeof(_global_pos));
 	memset(&_pos_sp_triplet, 0, sizeof(_pos_sp_triplet));
 	memset(&_global_vel_sp, 0, sizeof(_global_vel_sp));
 
@@ -261,9 +289,7 @@ MulticopterPositionControl::MulticopterPositionControl() :
 	_params.vel_ff.zero();
 	_params.sp_offs_max.zero();
 
-	_pos.zero();
 	_vel.zero();
-	_pos_sp.zero();
 	_vel_sp.zero();
 	_vel_prev.zero();
 
@@ -405,10 +431,10 @@ MulticopterPositionControl::poll_subscriptions()
 	if (updated)
 		orb_copy(ORB_ID(actuator_armed), _arming_sub, &_arming);
 
-	orb_check(_pos_sp_triplet_sub, &updated);
+	orb_check(_global_pos_sub, &updated);
 
 	if (updated)
-		orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet);
+		orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos);
 }
 
 float
@@ -432,13 +458,48 @@ MulticopterPositionControl::task_main_trampoline(int argc, char *argv[])
 }
 
 void
+MulticopterPositionControl::reset_lat_lon_sp()
+{
+	if (_reset_lat_lon_sp) {
+		_reset_lat_lon_sp = false;
+		_lat_sp = _global_pos.lat;
+		_lon_sp = _global_pos.lon;
+		mavlink_log_info(_mavlink_fd, "[mpc] reset lat/lon sp: %.7f, %.7f", _lat_sp, _lon_sp);
+	}
+}
+
+void
+MulticopterPositionControl::reset_alt_sp()
+{
+	if (_reset_alt_sp) {
+		_reset_alt_sp = false;
+		_alt_sp = _use_global_alt ? _global_pos.alt : _global_pos.baro_alt;
+		mavlink_log_info(_mavlink_fd, "[mpc] reset alt (%s) sp: %.2f", _use_global_alt ? "AMSL" : "baro", (double)_alt_sp);
+	}
+}
+
+void
+MulticopterPositionControl::select_alt(bool global)
+{
+	if (global != _use_global_alt) {
+		_use_global_alt = global;
+		if (global) {
+			/* switch from barometric to global altitude */
+			_alt_sp += _global_pos.alt - _global_pos.baro_alt;
+		} else {
+			/* switch from global to barometric altitude */
+			_alt_sp += _global_pos.baro_alt - _global_pos.alt;
+		}
+	}
+}
+
+void
 MulticopterPositionControl::task_main()
 {
 	warnx("started");
 
-	static int mavlink_fd;
-	mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
-	mavlink_log_info(mavlink_fd, "[mpc] started");
+	_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
+	mavlink_log_info(_mavlink_fd, "[mpc] started");
 
 	/*
 	 * do subscriptions
@@ -449,7 +510,7 @@ MulticopterPositionControl::task_main()
 	_params_sub = orb_subscribe(ORB_ID(parameter_update));
 	_manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
 	_arming_sub = orb_subscribe(ORB_ID(actuator_armed));
-	_local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
+	_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
 	_pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
 
 	parameters_update(true);
@@ -460,8 +521,6 @@ MulticopterPositionControl::task_main()
 	/* get an initial update for all sensor and status data */
 	poll_subscriptions();
 
-	bool reset_sp_z = true;
-	bool reset_sp_xy = true;
 	bool reset_int_z = true;
 	bool reset_int_z_manual = false;
 	bool reset_int_xy = true;
@@ -472,11 +531,6 @@ MulticopterPositionControl::task_main()
 	const float alt_ctl_dz = 0.2f;
 	const float pos_ctl_dz = 0.05f;
 
-	hrt_abstime ref_timestamp = 0;
-	int32_t ref_lat = 0.0f;
-	int32_t ref_lon = 0.0f;
-	float ref_alt = 0.0f;
-
 	math::Vector<3> sp_move_rate;
 	sp_move_rate.zero();
 	math::Vector<3> thrust_int;
@@ -488,7 +542,7 @@ MulticopterPositionControl::task_main()
 	struct pollfd fds[1];
 
 	/* Setup of loop */
-	fds[0].fd = _local_pos_sub;
+	fds[0].fd = _global_pos_sub;
 	fds[0].events = POLLIN;
 
 	while (!_task_should_exit) {
@@ -505,8 +559,6 @@ MulticopterPositionControl::task_main()
 			continue;
 		}
 
-		orb_copy(ORB_ID(vehicle_local_position), _local_pos_sub, &_local_pos);
-
 		poll_subscriptions();
 		parameters_update(false);
 
@@ -516,8 +568,8 @@ MulticopterPositionControl::task_main()
 
 		if (_control_mode.flag_armed && !was_armed) {
 			/* reset setpoints and integrals on arming */
-			reset_sp_z = true;
-			reset_sp_xy = true;
+			_reset_lat_lon_sp = true;
+			_reset_alt_sp = true;
 			reset_int_z = true;
 			reset_int_xy = true;
 		}
@@ -529,60 +581,34 @@ MulticopterPositionControl::task_main()
 		    _control_mode.flag_control_climb_rate_enabled ||
 		    _control_mode.flag_control_velocity_enabled) {
 
-			_pos(0) = _local_pos.x;
-			_pos(1) = _local_pos.y;
-			_pos(2) = _local_pos.z;
-			_vel(0) = _local_pos.vx;
-			_vel(1) = _local_pos.vy;
-			_vel(2) = _local_pos.vz;
+			_vel(0) = _global_pos.vel_n;
+			_vel(1) = _global_pos.vel_e;
+			_vel(2) = _global_pos.vel_d;
 
 			sp_move_rate.zero();
 
-			if (_local_pos.ref_timestamp != ref_timestamp) {
-				/* initialize local projection with new reference */
-				double lat_home = _local_pos.ref_lat * 1e-7;
-				double lon_home = _local_pos.ref_lon * 1e-7;
-				map_projection_init(lat_home, lon_home);
-				mavlink_log_info(mavlink_fd, "[mpc] local pos ref: %.7f, %.7f", (double)lat_home, (double)lon_home);
-
-				if (_control_mode.flag_control_manual_enabled && ref_timestamp != 0) {
-					/* correct setpoint in manual mode to stay in the same point */
-					float ref_change_x = 0.0f;
-					float ref_change_y = 0.0f;
-					map_projection_project(ref_lat, ref_lon, &ref_change_x, &ref_change_y);
-					_pos_sp(0) += ref_change_x;
-					_pos_sp(1) += ref_change_y;
-					_pos_sp(2) += _local_pos.ref_alt - ref_alt;
-				}
-
-				ref_timestamp = _local_pos.ref_timestamp;
-				ref_lat = _local_pos.ref_lat;
-				ref_lon = _local_pos.ref_lon;
-				ref_alt = _local_pos.ref_alt;
-			}
+			float alt = _global_pos.alt;
 
+			/* select control source */
 			if (_control_mode.flag_control_manual_enabled) {
+				/* select altitude source and update setpoint */
+				select_alt(_global_pos.global_valid);
+				if (!_use_global_alt) {
+					alt = _global_pos.baro_alt;
+				}
+
 				/* manual control */
 				if (_control_mode.flag_control_altitude_enabled) {
-					/* reset setpoint Z to current altitude if needed */
-					if (reset_sp_z) {
-						reset_sp_z = false;
-						_pos_sp(2) = _pos(2);
-						mavlink_log_info(mavlink_fd, "[mpc] reset alt sp: %.2f", (double) - _pos_sp(2));
-					}
+					/* reset alt setpoint to current altitude if needed */
+					reset_alt_sp();
 
 					/* move altitude setpoint with throttle stick */
 					sp_move_rate(2) = -scale_control(_manual.throttle - 0.5f, 0.5f, alt_ctl_dz);
 				}
 
 				if (_control_mode.flag_control_position_enabled) {
-					/* reset setpoint XY to current position if needed */
-					if (reset_sp_xy) {
-						reset_sp_xy = false;
-						_pos_sp(0) = _pos(0);
-						_pos_sp(1) = _pos(1);
-						mavlink_log_info(mavlink_fd, "[mpc] reset pos sp: %.2f, %.2f", (double)_pos_sp(0), (double)_pos_sp(1));
-					}
+					/* reset lat/lon setpoint to current position if needed */
+					reset_lat_lon_sp();
 
 					/* move position setpoint with roll/pitch stick */
 					sp_move_rate(0) = scale_control(-_manual.pitch / _params.rc_scale_pitch, 1.0f, pos_ctl_dz);
@@ -602,352 +628,390 @@ MulticopterPositionControl::task_main()
 				sp_move_rate = R_yaw_sp * sp_move_rate.emult(_params.vel_max);
 
 				/* move position setpoint */
-				_pos_sp += sp_move_rate * dt;
+				add_vector_to_global_position(_lat_sp, _lon_sp, sp_move_rate(0) * dt, sp_move_rate(1) * dt, &_lat_sp, &_lon_sp);
+				_alt_sp -= sp_move_rate(2) * dt;
 
 				/* check if position setpoint is too far from actual position */
 				math::Vector<3> pos_sp_offs;
 				pos_sp_offs.zero();
 
 				if (_control_mode.flag_control_position_enabled) {
-					pos_sp_offs(0) = (_pos_sp(0) - _pos(0)) / _params.sp_offs_max(0);
-					pos_sp_offs(1) = (_pos_sp(1) - _pos(1)) / _params.sp_offs_max(1);
+					get_vector_to_next_waypoint_fast(_global_pos.lat, _global_pos.lon, _lat_sp, _lon_sp, &pos_sp_offs.data[0], &pos_sp_offs.data[1]);
+					pos_sp_offs(0) /= _params.sp_offs_max(0);
+					pos_sp_offs(1) /= _params.sp_offs_max(1);
 				}
 
 				if (_control_mode.flag_control_altitude_enabled) {
-					pos_sp_offs(2) = (_pos_sp(2) - _pos(2)) / _params.sp_offs_max(2);
+					pos_sp_offs(2) = -(_alt_sp - alt) / _params.sp_offs_max(2);
 				}
 
 				float pos_sp_offs_norm = pos_sp_offs.length();
 
 				if (pos_sp_offs_norm > 1.0f) {
 					pos_sp_offs /= pos_sp_offs_norm;
-					_pos_sp = _pos + pos_sp_offs.emult(_params.sp_offs_max);
+					add_vector_to_global_position(_lat_sp, _lon_sp, pos_sp_offs(0) * _params.sp_offs_max(0), pos_sp_offs(1) * _params.sp_offs_max(1), &_lat_sp, &_lon_sp);
+					_alt_sp = alt - pos_sp_offs(2) * _params.sp_offs_max(2);
 				}
 
-			} else {
-				/* AUTO */
-				if (_pos_sp_triplet.current.valid) {
-					struct position_setpoint_s current_sp = _pos_sp_triplet.current;
+				/* fill position setpoint triplet */
+				_pos_sp_triplet.previous.valid = true;
+				_pos_sp_triplet.current.valid = true;
+				_pos_sp_triplet.next.valid = true;
+
+				_pos_sp_triplet.nav_state = NAV_STATE_NONE;
+				_pos_sp_triplet.current.type = SETPOINT_TYPE_NORMAL;
+				_pos_sp_triplet.current.lat = _lat_sp;
+				_pos_sp_triplet.current.lon = _lon_sp;
+				_pos_sp_triplet.current.alt = _alt_sp;
+				_pos_sp_triplet.current.yaw = _att_sp.yaw_body;
+				_pos_sp_triplet.current.loiter_radius = 0.0f;
+				_pos_sp_triplet.current.loiter_direction = 1.0f;
+				_pos_sp_triplet.current.pitch_min = 0.0f;
+
+				/* publish position setpoint triplet */
+				if (_pos_sp_triplet_pub > 0) {
+					orb_publish(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_pub, &_pos_sp_triplet);
 
-					_pos_sp(2) = -(current_sp.alt - ref_alt);
+				} else {
+					_pos_sp_triplet_pub = orb_advertise(ORB_ID(position_setpoint_triplet), &_pos_sp_triplet);
+				}
 
-					map_projection_project(current_sp.lat, current_sp.lon, &_pos_sp(0), &_pos_sp(1));
+			} else if (_control_mode.flag_control_auto_enabled) {
+				/* always use AMSL altitude for AUTO */
+				select_alt(true);
 
-					if (isfinite(current_sp.yaw)) {
-						_att_sp.yaw_body = current_sp.yaw;
-					}
+				/* AUTO */
+				bool updated;
+				orb_check(_pos_sp_triplet_sub, &updated);
 
+				if (updated)
+					orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet);
+
+				if (_pos_sp_triplet.current.valid) {
 					/* in case of interrupted mission don't go to waypoint but stay at current position */
-					reset_sp_xy = true;
-					reset_sp_z = true;
+					_reset_lat_lon_sp = true;
+					_reset_alt_sp = true;
+
+					_lat_sp = _pos_sp_triplet.current.lat;
+					_lon_sp = _pos_sp_triplet.current.lon;
+					_alt_sp = _pos_sp_triplet.current.alt;
 
 				} else {
 					/* no waypoint, loiter, reset position setpoint if needed */
-					if (reset_sp_xy) {
-						reset_sp_xy = false;
-						_pos_sp(0) = _pos(0);
-						_pos_sp(1) = _pos(1);
-					}
-
-					if (reset_sp_z) {
-						reset_sp_z = false;
-						_pos_sp(2) = _pos(2);
-					}
+					reset_lat_lon_sp();
+					reset_alt_sp();
 				}
+			} else {
+				/* no control, reset setpoint */
+				reset_lat_lon_sp();
+				reset_alt_sp();
 			}
 
-			/* copy resulting setpoint to vehicle_local_position_setpoint topic for logging */
-			_local_pos_sp.yaw = _att_sp.yaw_body;
-			_local_pos_sp.x = _pos_sp(0);
-			_local_pos_sp.y = _pos_sp(1);
-			_local_pos_sp.z = _pos_sp(2);
+			if (_control_mode.flag_control_auto_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == SETPOINT_TYPE_IDLE) {
+				/* idle state, don't run controller and set zero thrust */
+				R.identity();
+				memcpy(&_att_sp.R_body[0][0], R.data, sizeof(_att_sp.R_body));
+				_att_sp.R_valid = true;
+
+				_att_sp.roll_body = 0.0f;
+				_att_sp.pitch_body = 0.0f;
+				_att_sp.yaw_body = 0.0f;
+				_att_sp.thrust = 0.0f;
+
+				_att_sp.timestamp = hrt_absolute_time();
 
-			/* publish local position setpoint */
-			if (_local_pos_sp_pub > 0) {
-				orb_publish(ORB_ID(vehicle_local_position_setpoint), _local_pos_sp_pub, &_local_pos_sp);
+				/* publish attitude setpoint */
+				if (_att_sp_pub > 0) {
+					orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp);
+
+				} else {
+					_att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp);
+				}
 
 			} else {
-				_local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &_local_pos_sp);
-			}
+				/* run position & altitude controllers, calculate velocity setpoint */
+				math::Vector<3> pos_err;
+				float err_x, err_y;
+				get_vector_to_next_waypoint_fast(_global_pos.lat, _global_pos.lon, _lat_sp, _lon_sp, &pos_err.data[0], &pos_err.data[1]);
+				pos_err(2) = -(_alt_sp - alt);
 
-			/* run position & altitude controllers, calculate velocity setpoint */
-			math::Vector<3> pos_err = _pos_sp - _pos;
-			_vel_sp = pos_err.emult(_params.pos_p) + sp_move_rate.emult(_params.vel_ff);
+				_vel_sp = pos_err.emult(_params.pos_p) + sp_move_rate.emult(_params.vel_ff);
 
-			if (!_control_mode.flag_control_altitude_enabled) {
-				reset_sp_z = true;
-				_vel_sp(2) = 0.0f;
-			}
+				if (!_control_mode.flag_control_altitude_enabled) {
+					_reset_alt_sp = true;
+					_vel_sp(2) = 0.0f;
+				}
 
-			if (!_control_mode.flag_control_position_enabled) {
-				reset_sp_xy = true;
-				_vel_sp(0) = 0.0f;
-				_vel_sp(1) = 0.0f;
-			}
+				if (!_control_mode.flag_control_position_enabled) {
+					_reset_lat_lon_sp = true;
+					_vel_sp(0) = 0.0f;
+					_vel_sp(1) = 0.0f;
+				}
 
-			if (!_control_mode.flag_control_manual_enabled) {
 				/* use constant descend rate when landing, ignore altitude setpoint */
-				if (_pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) {
+				if (_control_mode.flag_control_auto_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) {
 					_vel_sp(2) = _params.land_speed;
 				}
 
-				/* limit 3D speed only in AUTO mode */
-				float vel_sp_norm = _vel_sp.edivide(_params.vel_max).length();
+				if (!_control_mode.flag_control_manual_enabled) {
+					/* limit 3D speed only in non-manual modes */
+					float vel_sp_norm = _vel_sp.edivide(_params.vel_max).length();
 
-				if (vel_sp_norm > 1.0f) {
-					_vel_sp /= vel_sp_norm;
+					if (vel_sp_norm > 1.0f) {
+						_vel_sp /= vel_sp_norm;
+					}
 				}
-			}
 
-			_global_vel_sp.vx = _vel_sp(0);
-			_global_vel_sp.vy = _vel_sp(1);
-			_global_vel_sp.vz = _vel_sp(2);
+				_global_vel_sp.vx = _vel_sp(0);
+				_global_vel_sp.vy = _vel_sp(1);
+				_global_vel_sp.vz = _vel_sp(2);
 
-			/* publish velocity setpoint */
-			if (_global_vel_sp_pub > 0) {
-				orb_publish(ORB_ID(vehicle_global_velocity_setpoint), _global_vel_sp_pub, &_global_vel_sp);
+				/* publish velocity setpoint */
+				if (_global_vel_sp_pub > 0) {
+					orb_publish(ORB_ID(vehicle_global_velocity_setpoint), _global_vel_sp_pub, &_global_vel_sp);
 
-			} else {
-				_global_vel_sp_pub = orb_advertise(ORB_ID(vehicle_global_velocity_setpoint), &_global_vel_sp);
-			}
+				} else {
+					_global_vel_sp_pub = orb_advertise(ORB_ID(vehicle_global_velocity_setpoint), &_global_vel_sp);
+				}
 
-			if (_control_mode.flag_control_climb_rate_enabled || _control_mode.flag_control_velocity_enabled) {
-				/* reset integrals if needed */
-				if (_control_mode.flag_control_climb_rate_enabled) {
-					if (reset_int_z) {
-						reset_int_z = false;
-						float i = _params.thr_min;
+				if (_control_mode.flag_control_climb_rate_enabled || _control_mode.flag_control_velocity_enabled) {
+					/* reset integrals if needed */
+					if (_control_mode.flag_control_climb_rate_enabled) {
+						if (reset_int_z) {
+							reset_int_z = false;
+							float i = _params.thr_min;
 
-						if (reset_int_z_manual) {
-							i = _manual.throttle;
+							if (reset_int_z_manual) {
+								i = _manual.throttle;
 
-							if (i < _params.thr_min) {
-								i = _params.thr_min;
+								if (i < _params.thr_min) {
+									i = _params.thr_min;
 
-							} else if (i > _params.thr_max) {
-								i = _params.thr_max;
+								} else if (i > _params.thr_max) {
+									i = _params.thr_max;
+								}
 							}
+
+							thrust_int(2) = -i;
+							mavlink_log_info(_mavlink_fd, "[mpc] reset hovering thrust: %.2f", (double)i);
 						}
 
-						thrust_int(2) = -i;
-						mavlink_log_info(mavlink_fd, "[mpc] reset hovering thrust: %.2f", (double)i);
+					} else {
+						reset_int_z = true;
 					}
 
-				} else {
-					reset_int_z = true;
-				}
+					if (_control_mode.flag_control_velocity_enabled) {
+						if (reset_int_xy) {
+							reset_int_xy = false;
+							thrust_int(0) = 0.0f;
+							thrust_int(1) = 0.0f;
+							mavlink_log_info(_mavlink_fd, "[mpc] reset xy vel integral");
+						}
 
-				if (_control_mode.flag_control_velocity_enabled) {
-					if (reset_int_xy) {
-						reset_int_xy = false;
-						thrust_int(0) = 0.0f;
-						thrust_int(1) = 0.0f;
-						mavlink_log_info(mavlink_fd, "[mpc] reset xy vel integral");
+					} else {
+						reset_int_xy = true;
 					}
 
-				} else {
-					reset_int_xy = true;
-				}
-
-				/* velocity error */
-				math::Vector<3> vel_err = _vel_sp - _vel;
+					/* velocity error */
+					math::Vector<3> vel_err = _vel_sp - _vel;
 
-				/* derivative of velocity error, not includes setpoint acceleration */
-				math::Vector<3> vel_err_d = (sp_move_rate - _vel).emult(_params.pos_p) - (_vel - _vel_prev) / dt;
-				_vel_prev = _vel;
+					/* derivative of velocity error, not includes setpoint acceleration */
+					math::Vector<3> vel_err_d = (sp_move_rate - _vel).emult(_params.pos_p) - (_vel - _vel_prev) / dt;
+					_vel_prev = _vel;
 
-				/* thrust vector in NED frame */
-				math::Vector<3> thrust_sp = vel_err.emult(_params.vel_p) + vel_err_d.emult(_params.vel_d) + thrust_int;
+					/* thrust vector in NED frame */
+					math::Vector<3> thrust_sp = vel_err.emult(_params.vel_p) + vel_err_d.emult(_params.vel_d) + thrust_int;
 
-				if (!_control_mode.flag_control_velocity_enabled) {
-					thrust_sp(0) = 0.0f;
-					thrust_sp(1) = 0.0f;
-				}
-
-				if (!_control_mode.flag_control_climb_rate_enabled) {
-					thrust_sp(2) = 0.0f;
-				}
+					if (!_control_mode.flag_control_velocity_enabled) {
+						thrust_sp(0) = 0.0f;
+						thrust_sp(1) = 0.0f;
+					}
 
-				/* limit thrust vector and check for saturation */
-				bool saturation_xy = false;
-				bool saturation_z = false;
+					if (!_control_mode.flag_control_climb_rate_enabled) {
+						thrust_sp(2) = 0.0f;
+					}
 
-				/* limit min lift */
-				float thr_min = _params.thr_min;
+					/* limit thrust vector and check for saturation */
+					bool saturation_xy = false;
+					bool saturation_z = false;
 
-				if (!_control_mode.flag_control_velocity_enabled && thr_min < 0.0f) {
-					/* don't allow downside thrust direction in manual attitude mode */
-					thr_min = 0.0f;
-				}
+					/* limit min lift */
+					float thr_min = _params.thr_min;
 
-				if (-thrust_sp(2) < thr_min) {
-					thrust_sp(2) = -thr_min;
-					saturation_z = true;
-				}
+					if (!_control_mode.flag_control_velocity_enabled && thr_min < 0.0f) {
+						/* don't allow downside thrust direction in manual attitude mode */
+						thr_min = 0.0f;
+					}
 
-				/* limit max tilt */
-				float tilt = atan2f(math::Vector<2>(thrust_sp(0), thrust_sp(1)).length(), -thrust_sp(2));
-				float tilt_max = _params.tilt_max;
-				if (!_control_mode.flag_control_manual_enabled) {
-					if (_pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) {
-						/* limit max tilt and min lift when landing */
-						tilt_max = _params.land_tilt_max;
-						if (thr_min < 0.0f)
-							thr_min = 0.0f;
+					if (-thrust_sp(2) < thr_min) {
+						thrust_sp(2) = -thr_min;
+						saturation_z = true;
 					}
-				}
 
-				if (_control_mode.flag_control_velocity_enabled) {
-					if (tilt > tilt_max && thr_min >= 0.0f) {
-						/* crop horizontal component */
-						float k = tanf(tilt_max) / tanf(tilt);
-						thrust_sp(0) *= k;
-						thrust_sp(1) *= k;
-						saturation_xy = true;
+					/* limit max tilt */
+					float tilt = atan2f(math::Vector<2>(thrust_sp(0), thrust_sp(1)).length(), -thrust_sp(2));
+					float tilt_max = _params.tilt_max;
+					if (!_control_mode.flag_control_manual_enabled) {
+						if (_pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) {
+							/* limit max tilt and min lift when landing */
+							tilt_max = _params.land_tilt_max;
+							if (thr_min < 0.0f)
+								thr_min = 0.0f;
+						}
 					}
-				} else {
-					/* thrust compensation for altitude only control mode */
-					float att_comp;
 
-					if (_att.R[2][2] > TILT_COS_MAX) {
-						att_comp = 1.0f / _att.R[2][2];
-					} else if (_att.R[2][2] > 0.0f) {
-						att_comp = ((1.0f / TILT_COS_MAX - 1.0f) / TILT_COS_MAX) * _att.R[2][2] + 1.0f;
-						saturation_z = true;
+					if (_control_mode.flag_control_velocity_enabled) {
+						if (tilt > tilt_max && thr_min >= 0.0f) {
+							/* crop horizontal component */
+							float k = tanf(tilt_max) / tanf(tilt);
+							thrust_sp(0) *= k;
+							thrust_sp(1) *= k;
+							saturation_xy = true;
+						}
 					} else {
-						att_comp = 1.0f;
-						saturation_z = true;
-					}
+						/* thrust compensation for altitude only control mode */
+						float att_comp;
 
-					thrust_sp(2) *= att_comp;
-				}
+						if (_att.R[2][2] > TILT_COS_MAX) {
+							att_comp = 1.0f / _att.R[2][2];
+						} else if (_att.R[2][2] > 0.0f) {
+							att_comp = ((1.0f / TILT_COS_MAX - 1.0f) / TILT_COS_MAX) * _att.R[2][2] + 1.0f;
+							saturation_z = true;
+						} else {
+							att_comp = 1.0f;
+							saturation_z = true;
+						}
 
-				/* limit max thrust */
-				float thrust_abs = thrust_sp.length();
+						thrust_sp(2) *= att_comp;
+					}
 
-				if (thrust_abs > _params.thr_max) {
-					if (thrust_sp(2) < 0.0f) {
-						if (-thrust_sp(2) > _params.thr_max) {
-							/* thrust Z component is too large, limit it */
-							thrust_sp(0) = 0.0f;
-							thrust_sp(1) = 0.0f;
-							thrust_sp(2) = -_params.thr_max;
-							saturation_xy = true;
-							saturation_z = true;
+					/* limit max thrust */
+					float thrust_abs = thrust_sp.length();
+
+					if (thrust_abs > _params.thr_max) {
+						if (thrust_sp(2) < 0.0f) {
+							if (-thrust_sp(2) > _params.thr_max) {
+								/* thrust Z component is too large, limit it */
+								thrust_sp(0) = 0.0f;
+								thrust_sp(1) = 0.0f;
+								thrust_sp(2) = -_params.thr_max;
+								saturation_xy = true;
+								saturation_z = true;
+
+							} else {
+								/* preserve thrust Z component and lower XY, keeping altitude is more important than position */
+								float thrust_xy_max = sqrtf(_params.thr_max * _params.thr_max - thrust_sp(2) * thrust_sp(2));
+								float thrust_xy_abs = math::Vector<2>(thrust_sp(0), thrust_sp(1)).length();
+								float k = thrust_xy_max / thrust_xy_abs;
+								thrust_sp(0) *= k;
+								thrust_sp(1) *= k;
+								saturation_xy = true;
+							}
 
 						} else {
-							/* preserve thrust Z component and lower XY, keeping altitude is more important than position */
-							float thrust_xy_max = sqrtf(_params.thr_max * _params.thr_max - thrust_sp(2) * thrust_sp(2));
-							float thrust_xy_abs = math::Vector<2>(thrust_sp(0), thrust_sp(1)).length();
-							float k = thrust_xy_max / thrust_xy_abs;
-							thrust_sp(0) *= k;
-							thrust_sp(1) *= k;
+							/* Z component is negative, going down, simply limit thrust vector */
+							float k = _params.thr_max / thrust_abs;
+							thrust_sp *= k;
 							saturation_xy = true;
+							saturation_z = true;
 						}
 
-					} else {
-						/* Z component is negative, going down, simply limit thrust vector */
-						float k = _params.thr_max / thrust_abs;
-						thrust_sp *= k;
-						saturation_xy = true;
-						saturation_z = true;
+						thrust_abs = _params.thr_max;
 					}
 
-					thrust_abs = _params.thr_max;
-				}
+					/* update integrals */
+					if (_control_mode.flag_control_velocity_enabled && !saturation_xy) {
+						thrust_int(0) += vel_err(0) * _params.vel_i(0) * dt;
+						thrust_int(1) += vel_err(1) * _params.vel_i(1) * dt;
+					}
 
-				/* update integrals */
-				if (_control_mode.flag_control_velocity_enabled && !saturation_xy) {
-					thrust_int(0) += vel_err(0) * _params.vel_i(0) * dt;
-					thrust_int(1) += vel_err(1) * _params.vel_i(1) * dt;
-				}
+					if (_control_mode.flag_control_climb_rate_enabled && !saturation_z) {
+						thrust_int(2) += vel_err(2) * _params.vel_i(2) * dt;
 
-				if (_control_mode.flag_control_climb_rate_enabled && !saturation_z) {
-					thrust_int(2) += vel_err(2) * _params.vel_i(2) * dt;
+						/* protection against flipping on ground when landing */
+						if (thrust_int(2) > 0.0f)
+							thrust_int(2) = 0.0f;
+					}
 
-					/* protection against flipping on ground when landing */
-					if (thrust_int(2) > 0.0f)
-						thrust_int(2) = 0.0f;
-				}
+					/* calculate attitude setpoint from thrust vector */
+					if (_control_mode.flag_control_velocity_enabled) {
+						/* desired body_z axis = -normalize(thrust_vector) */
+						math::Vector<3> body_x;
+						math::Vector<3> body_y;
+						math::Vector<3> body_z;
 
-				/* calculate attitude setpoint from thrust vector */
-				if (_control_mode.flag_control_velocity_enabled) {
-					/* desired body_z axis = -normalize(thrust_vector) */
-					math::Vector<3> body_x;
-					math::Vector<3> body_y;
-					math::Vector<3> body_z;
+						if (thrust_abs > SIGMA) {
+							body_z = -thrust_sp / thrust_abs;
 
-					if (thrust_abs > SIGMA) {
-						body_z = -thrust_sp / thrust_abs;
+						} else {
+							/* no thrust, set Z axis to safe value */
+							body_z.zero();
+							body_z(2) = 1.0f;
+						}
 
-					} else {
-						/* no thrust, set Z axis to safe value */
-						body_z.zero();
-						body_z(2) = 1.0f;
-					}
+						/* vector of desired yaw direction in XY plane, rotated by PI/2 */
+						math::Vector<3> y_C(-sinf(_att_sp.yaw_body), cosf(_att_sp.yaw_body), 0.0f);
 
-					/* vector of desired yaw direction in XY plane, rotated by PI/2 */
-					math::Vector<3> y_C(-sinf(_att_sp.yaw_body), cosf(_att_sp.yaw_body), 0.0f);
+						if (fabsf(body_z(2)) > SIGMA) {
+							/* desired body_x axis, orthogonal to body_z */
+							body_x = y_C % body_z;
 
-					if (fabsf(body_z(2)) > SIGMA) {
-						/* desired body_x axis, orthogonal to body_z */
-						body_x = y_C % body_z;
+							/* keep nose to front while inverted upside down */
+							if (body_z(2) < 0.0f) {
+								body_x = -body_x;
+							}
+
+							body_x.normalize();
 
-						/* keep nose to front while inverted upside down */
-						if (body_z(2) < 0.0f) {
-							body_x = -body_x;
+						} else {
+							/* desired thrust is in XY plane, set X downside to construct correct matrix,
+							 * but yaw component will not be used actually */
+							body_x.zero();
+							body_x(2) = 1.0f;
 						}
 
-						body_x.normalize();
+						/* desired body_y axis */
+						body_y = body_z % body_x;
 
-					} else {
-						/* desired thrust is in XY plane, set X downside to construct correct matrix,
-						 * but yaw component will not be used actually */
-						body_x.zero();
-						body_x(2) = 1.0f;
-					}
+						/* fill rotation matrix */
+						for (int i = 0; i < 3; i++) {
+							R(i, 0) = body_x(i);
+							R(i, 1) = body_y(i);
+							R(i, 2) = body_z(i);
+						}
 
-					/* desired body_y axis */
-					body_y = body_z % body_x;
+						/* copy rotation matrix to attitude setpoint topic */
+						memcpy(&_att_sp.R_body[0][0], R.data, sizeof(_att_sp.R_body));
+						_att_sp.R_valid = true;
 
-					/* fill rotation matrix */
-					for (int i = 0; i < 3; i++) {
-						R(i, 0) = body_x(i);
-						R(i, 1) = body_y(i);
-						R(i, 2) = body_z(i);
+						/* calculate euler angles, for logging only, must not be used for control */
+						math::Vector<3> euler = R.to_euler();
+						_att_sp.roll_body = euler(0);
+						_att_sp.pitch_body = euler(1);
+						/* yaw already used to construct rot matrix, but actual rotation matrix can have different yaw near singularity */
 					}
 
-					/* copy rotation matrix to attitude setpoint topic */
-					memcpy(&_att_sp.R_body[0][0], R.data, sizeof(_att_sp.R_body));
-					_att_sp.R_valid = true;
-
-					/* calculate euler angles, for logging only, must not be used for control */
-					math::Vector<3> euler = R.to_euler();
-					_att_sp.roll_body = euler(0);
-					_att_sp.pitch_body = euler(1);
-					/* yaw already used to construct rot matrix, but actual rotation matrix can have different yaw near singularity */
-				}
+					_att_sp.thrust = thrust_abs;
 
-				_att_sp.thrust = thrust_abs;
+					_att_sp.timestamp = hrt_absolute_time();
 
-				_att_sp.timestamp = hrt_absolute_time();
+					/* publish attitude setpoint */
+					if (_att_sp_pub > 0) {
+						orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp);
 
-				/* publish attitude setpoint */
-				if (_att_sp_pub > 0) {
-					orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp);
+					} else {
+						_att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp);
+					}
 
 				} else {
-					_att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp);
+					reset_int_z = true;
 				}
-
-			} else {
-				reset_int_z = true;
 			}
 
 		} else {
 			/* position controller disabled, reset setpoints */
-			reset_sp_z = true;
-			reset_sp_xy = true;
+			_reset_alt_sp = true;
+			_reset_lat_lon_sp = true;
 			reset_int_z = true;
 			reset_int_xy = true;
 		}
@@ -957,7 +1021,7 @@ MulticopterPositionControl::task_main()
 	}
 
 	warnx("stopped");
-	mavlink_log_info(mavlink_fd, "[mpc] stopped");
+	mavlink_log_info(_mavlink_fd, "[mpc] stopped");
 
 	_control_task = -1;
 	_exit(0);
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_ */
diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c
index af04bb0bc..e045ce4cc 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_main.c
+++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c
@@ -202,8 +202,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 	bool landed = true;
 	hrt_abstime landed_time = 0;
 
-	bool flag_armed = false;
-
 	uint32_t accel_counter = 0;
 	uint32_t baro_counter = 0;
 
@@ -329,6 +327,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 						mavlink_log_info(mavlink_fd, "[inav] baro offs: %.2f", baro_offset);
 						local_pos.z_valid = true;
 						local_pos.v_z_valid = true;
+						global_pos.baro_valid = true;
 					}
 				}
 			}
@@ -379,17 +378,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 
 			if (updated) {
 				orb_copy(ORB_ID(actuator_armed), armed_sub, &armed);
-
-				/* reset ground level on arm */
-				if (armed.armed && !flag_armed) {
-					flag_armed = armed.armed;
-					baro_offset -= z_est[0];
-					corr_baro = 0.0f;
-					local_pos.ref_alt -= z_est[0];
-					local_pos.ref_timestamp = t;
-					z_est[0] = 0.0f;
-					alt_avg = 0.0f;
-				}
 			}
 
 			/* sensor combined */
@@ -637,6 +625,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 		}
 
 		float dt = t_prev > 0 ? (t - t_prev) / 1000000.0f : 0.0f;
+		dt = fmaxf(fminf(0.02, dt), 0.005);
 		t_prev = t;
 
 		/* use GPS if it's valid and reference position initialized */
@@ -679,7 +668,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 		if (use_gps_z) {
 			float offs_corr = corr_gps[2][0] * w_z_gps_p * dt;
 			baro_offset += offs_corr;
-			baro_counter += offs_corr;
+			corr_baro += offs_corr;
 		}
 
 		/* accelerometer bias correction */
@@ -835,7 +824,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 			orb_publish(ORB_ID(vehicle_local_position), vehicle_local_position_pub, &local_pos);
 
 			/* publish global position */
-			global_pos.valid = local_pos.xy_global;
+			global_pos.global_valid = local_pos.xy_global;
 
 			if (local_pos.xy_global) {
 				double est_lat, est_lon;
@@ -855,6 +844,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 				global_pos.alt = local_pos.ref_alt - local_pos.z;
 			}
 
+			if (local_pos.z_valid) {
+				global_pos.baro_alt = baro_offset - local_pos.z;
+			}
+
 			if (local_pos.v_z_valid) {
 				global_pos.vel_d = local_pos.vz;
 			}
diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index 9bac2958e..3c218e21f 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -62,7 +62,6 @@
 
 #include <uORB/uORB.h>
 #include <uORB/topics/vehicle_status.h>
-#include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/sensor_combined.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
@@ -740,7 +739,6 @@ int sdlog2_thread_main(int argc, char *argv[])
 	/* warning! using union here to save memory, elements should be used separately! */
 	union {
 		struct vehicle_command_s cmd;
-		struct vehicle_control_mode_s control_mode;
 		struct sensor_combined_s sensor;
 		struct vehicle_attitude_s att;
 		struct vehicle_attitude_setpoint_s att_sp;
@@ -767,7 +765,6 @@ int sdlog2_thread_main(int argc, char *argv[])
 	struct {
 		int cmd_sub;
 		int status_sub;
-		int control_mode_sub;
 		int sensor_sub;
 		int att_sub;
 		int att_sp_sub;
@@ -847,12 +844,6 @@ int sdlog2_thread_main(int argc, char *argv[])
 	fds[fdsc_count].events = POLLIN;
 	fdsc_count++;
 
-	/* --- VEHICLE CONTROL MODE --- */
-	subs.control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
-	fds[fdsc_count].fd = subs.control_mode_sub;
-	fds[fdsc_count].events = POLLIN;
-	fdsc_count++;
-
 	/* --- SENSORS COMBINED --- */
 	subs.sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
 	fds[fdsc_count].fd = subs.sensor_sub;
@@ -1002,7 +993,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		/* decide use usleep() or blocking poll() */
 		bool use_sleep = sleep_delay > 0 && logging_enabled;
 
-		/* poll all topics if logging enabled or only management (first 2) if not */
+		/* poll all topics if logging enabled or only management (first 3) if not */
 		int poll_ret = poll(fds, logging_enabled ? fdsc_count : 3, use_sleep ? 0 : poll_timeout);
 
 		/* handle the poll result */
@@ -1064,11 +1055,8 @@ int sdlog2_thread_main(int argc, char *argv[])
 			/* --- VEHICLE STATUS --- */
 			if (fds[ifds++].revents & POLLIN) {
 				/* don't orb_copy, it's already done few lines above */
-				/* copy VEHICLE CONTROL MODE control mode here to construct STAT message */
-				orb_copy(ORB_ID(vehicle_control_mode), subs.control_mode_sub, &buf.control_mode);
 				log_msg.msg_type = LOG_STAT_MSG;
-				log_msg.body.log_STAT.main_state = (uint8_t) buf.control_mode.main_state;
-				log_msg.body.log_STAT.navigation_state = (uint8_t) buf.control_mode.nav_state;
+				log_msg.body.log_STAT.main_state = (uint8_t) buf_status.main_state;
 				log_msg.body.log_STAT.arming_state = (uint8_t) buf_status.arming_state;
 				log_msg.body.log_STAT.battery_remaining = buf_status.battery_remaining;
 				log_msg.body.log_STAT.battery_warning = (uint8_t) buf_status.battery_warning;
@@ -1078,7 +1066,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 
 			/* --- GPS POSITION --- */
 			if (fds[ifds++].revents & POLLIN) {
-				orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos);
+				/* don't orb_copy, it's already done few lines above */
 				log_msg.msg_type = LOG_GPS_MSG;
 				log_msg.body.log_GPS.gps_time = buf.gps_pos.time_gps_usec;
 				log_msg.body.log_GPS.fix_type = buf.gps_pos.fix_type;
@@ -1094,8 +1082,6 @@ int sdlog2_thread_main(int argc, char *argv[])
 				LOGBUFFER_WRITE_AND_COUNT(GPS);
 			}
 
-			ifds++;	// skip CONTROL MODE, already handled
-
 			/* --- SENSOR COMBINED --- */
 			if (fds[ifds++].revents & POLLIN) {
 				orb_copy(ORB_ID(sensor_combined), subs.sensor_sub, &buf.sensor);
@@ -1258,6 +1244,8 @@ int sdlog2_thread_main(int argc, char *argv[])
 				log_msg.body.log_GPOS.vel_n = buf.global_pos.vel_n;
 				log_msg.body.log_GPOS.vel_e = buf.global_pos.vel_e;
 				log_msg.body.log_GPOS.vel_d = buf.global_pos.vel_d;
+				log_msg.body.log_GPOS.baro_alt = buf.global_pos.baro_alt;
+				log_msg.body.log_GPOS.flags = (buf.global_pos.baro_valid ? 1 : 0) | (buf.global_pos.global_valid ? 2 : 0);
 				LOGBUFFER_WRITE_AND_COUNT(GPOS);
 			}
 
@@ -1265,6 +1253,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 			if (fds[ifds++].revents & POLLIN) {
 				orb_copy(ORB_ID(position_setpoint_triplet), subs.triplet_sub, &buf.triplet);
 				log_msg.msg_type = LOG_GPSP_MSG;
+				log_msg.body.log_GPSP.nav_state = buf.triplet.nav_state;
 				log_msg.body.log_GPSP.lat = (int32_t)(buf.triplet.current.lat * 1e7d);
 				log_msg.body.log_GPSP.lon = (int32_t)(buf.triplet.current.lon * 1e7d);
 				log_msg.body.log_GPSP.alt = buf.triplet.current.alt;
diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h
index 98736dd21..db87e3a6a 100644
--- a/src/modules/sdlog2/sdlog2_messages.h
+++ b/src/modules/sdlog2/sdlog2_messages.h
@@ -149,7 +149,6 @@ struct log_ATTC_s {
 #define LOG_STAT_MSG 10
 struct log_STAT_s {
 	uint8_t main_state;
-	uint8_t navigation_state;
 	uint8_t arming_state;
 	float battery_remaining;
 	uint8_t battery_warning;
@@ -205,11 +204,14 @@ struct log_GPOS_s {
 	float vel_n;
 	float vel_e;
 	float vel_d;
+	float baro_alt;
+	uint8_t flags;
 };
 
 /* --- GPSP - GLOBAL POSITION SETPOINT --- */
 #define LOG_GPSP_MSG 17
 struct log_GPSP_s {
+	uint8_t nav_state;
 	int32_t lat;
 	int32_t lon;
 	float alt;
@@ -297,14 +299,14 @@ static const struct log_format_s log_formats[] = {
 	LOG_FORMAT(LPSP, "ffff", "X,Y,Z,Yaw"),
 	LOG_FORMAT(GPS, "QBffLLfffff", "GPSTime,FixType,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog"),
 	LOG_FORMAT(ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"),
-	LOG_FORMAT(STAT, "BBBfBB", "MainState,NavState,ArmState,BatRem,BatWarn,Landed"),
+	LOG_FORMAT(STAT, "BBfBB", "MainState,ArmState,BatRem,BatWarn,Landed"),
 	LOG_FORMAT(RC, "ffffffffB", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7,Count"),
 	LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"),
 	LOG_FORMAT(AIRS, "ff", "IndSpeed,TrueSpeed"),
 	LOG_FORMAT(ARSP, "fff", "RollRateSP,PitchRateSP,YawRateSP"),
 	LOG_FORMAT(FLOW, "hhfffBB", "RawX,RawY,CompX,CompY,Dist,Q,SensID"),
-	LOG_FORMAT(GPOS, "LLffff", "Lat,Lon,Alt,VelN,VelE,VelD"),
-	LOG_FORMAT(GPSP, "LLffBfbf", "Lat,Lon,Alt,Yaw,Type,LoitR,LoitDir,PitMin"),
+	LOG_FORMAT(GPOS, "LLfffffB", "Lat,Lon,Alt,VelN,VelE,VelD,BaroAlt,Flags"),
+	LOG_FORMAT(GPSP, "BLLffBfbf", "NavState,Lat,Lon,Alt,Yaw,Type,LoitR,LoitDir,PitMin"),
 	LOG_FORMAT(ESC, "HBBBHHHHHHfH", "Counter,NumESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"),
 	LOG_FORMAT(GVSP, "fff", "VX,VY,VZ"),
 	LOG_FORMAT(BATT, "ffff", "V,VFilt,C,Discharged"),
diff --git a/src/modules/uORB/topics/position_setpoint_triplet.h b/src/modules/uORB/topics/position_setpoint_triplet.h
index 4b57833b6..cf1ddfc38 100644
--- a/src/modules/uORB/topics/position_setpoint_triplet.h
+++ b/src/modules/uORB/topics/position_setpoint_triplet.h
@@ -45,6 +45,7 @@
 #include <stdint.h>
 #include <stdbool.h>
 #include "../uORB.h"
+#include <navigator/navigator_state.h>
 
 /**
  * @addtogroup topics
@@ -53,10 +54,11 @@
 
 enum SETPOINT_TYPE
 {
-	SETPOINT_TYPE_NORMAL = 0,
-	SETPOINT_TYPE_LOITER,
-	SETPOINT_TYPE_TAKEOFF,
-	SETPOINT_TYPE_LAND,
+	SETPOINT_TYPE_NORMAL = 0,	/**< normal setpoint */
+	SETPOINT_TYPE_LOITER,		/**< loiter setpoint */
+	SETPOINT_TYPE_TAKEOFF,		/**< takeoff setpoint */
+	SETPOINT_TYPE_LAND,			/**< land setpoint, altitude must be ignored, vehicle must descend until landing */
+	SETPOINT_TYPE_IDLE,			/**< do nothing, switch off motors or keep at idle speed (MC) */
 };
 
 struct position_setpoint_s
@@ -82,6 +84,8 @@ struct position_setpoint_triplet_s
 	struct position_setpoint_s previous;
 	struct position_setpoint_s current;
 	struct position_setpoint_s next;
+
+	nav_state_t nav_state;		/**< navigation state */
 };
 
 /**
diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h
index 5aecac898..7cbb37cd8 100644
--- a/src/modules/uORB/topics/vehicle_control_mode.h
+++ b/src/modules/uORB/topics/vehicle_control_mode.h
@@ -61,23 +61,10 @@
  * Encodes the complete system state and is set by the commander app.
  */
 
-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;
-
 struct vehicle_control_mode_s
 {
 	uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */
 
-	main_state_t main_state;
-	nav_state_t nav_state;
-
 	bool flag_armed;
 
 	bool flag_external_manual_override_ok;	/**< external override non-fatal for system. Only true for fixed wing */
@@ -86,14 +73,14 @@ struct vehicle_control_mode_s
 	bool flag_system_hil_enabled;
 
 	bool flag_control_manual_enabled;		/**< true if manual input is mixed in */
-	bool flag_control_offboard_enabled;		/**< true if offboard control input is on */
+	bool flag_control_auto_enabled;			/**< true if onboard autopilot should act */
 	bool flag_control_rates_enabled;		/**< true if rates are stabilized */
 	bool flag_control_attitude_enabled;		/**< true if attitude stabilization is mixed in */
 	bool flag_control_velocity_enabled;		/**< true if horizontal velocity (implies direction) is controlled */
 	bool flag_control_position_enabled;		/**< true if position is controlled */
 	bool flag_control_altitude_enabled;		/**< true if altitude is controlled */
-	bool flag_control_climb_rate_enabled;		/**< true if climb rate is controlled */
-	bool flag_control_termination_enabled;   /**< true if flighttermination is enabled */
+	bool flag_control_climb_rate_enabled;	/**< true if climb rate is controlled */
+	bool flag_control_termination_enabled;	/**< true if flighttermination is enabled */
 };
 
 /**
diff --git a/src/modules/uORB/topics/vehicle_global_position.h b/src/modules/uORB/topics/vehicle_global_position.h
index ae771ca00..ff9e98e1c 100644
--- a/src/modules/uORB/topics/vehicle_global_position.h
+++ b/src/modules/uORB/topics/vehicle_global_position.h
@@ -61,17 +61,21 @@
  */
 struct vehicle_global_position_s
 {
-	uint64_t timestamp;		/**< time of this estimate, in microseconds since system start */
-	uint64_t time_gps_usec; 	/**< GPS timestamp in microseconds							   */
-	bool valid;			/**< true if position satisfies validity criteria of estimator */
+	uint64_t timestamp;		/**< Time of this estimate, in microseconds since system start */
 
+	bool global_valid;		/**< true if position satisfies validity criteria of estimator */
+	bool baro_valid;		/**< true if baro_alt is valid (vel_d is also valid in this case) */
+
+	uint64_t time_gps_usec; /**< GPS timestamp in microseconds					   */
 	double lat;			/**< Latitude in degrees							 	   */
 	double lon;			/**< Longitude in degrees							 	   */
-	float alt;			/**< Altitude in meters								 	   */
+	float alt;			/**< Altitude AMSL in meters						 	   */
 	float vel_n; 		/**< Ground north velocity, m/s				 			   */
 	float vel_e;		/**< Ground east velocity, m/s							   */
 	float vel_d;		/**< Ground downside velocity, m/s						   */
 	float yaw; 			/**< Yaw in radians -PI..+PI.							   */
+
+	float baro_alt;		/**< Barometric altitude (not raw baro but fused with accelerometer) */
 };
 
 /**
diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h
index a5988d3ba..1b3639e30 100644
--- a/src/modules/uORB/topics/vehicle_status.h
+++ b/src/modules/uORB/topics/vehicle_status.h
@@ -54,6 +54,8 @@
 #include <stdbool.h>
 #include "../uORB.h"
 
+#include <navigator/navigator_state.h>
+
 /**
  * @addtogroup topics @{
  */