vehicle_local_position topic updated, position_estimator_inav and commander fixes, only altitude estimate required for SETBELT now.

10 files changed, 195 insertions, 213 deletions

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index d40f6675b..ab7d2e6cf 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -551,7 +551,7 @@ int commander_thread_main(int argc, char *argv[])
 	memset(&control_mode, 0, sizeof(control_mode));
 
 	status.main_state = MAIN_STATE_MANUAL;
-	status.navigation_state = NAVIGATION_STATE_STANDBY;
+	status.navigation_state = NAVIGATION_STATE_DIRECT;
 	status.arming_state = ARMING_STATE_INIT;
 	status.hil_state = HIL_STATE_OFF;
 
@@ -812,8 +812,9 @@ int commander_thread_main(int argc, char *argv[])
 			orb_copy(ORB_ID(vehicle_local_position), local_position_sub, &local_position);
 		}
 
-		/* update condition_local_position_valid */
-		check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.valid, &(status.condition_local_position_valid), &status_changed);
+		/* update condition_local_position_valid and condition_local_altitude_valid */
+		check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.xy_valid, &(status.condition_local_position_valid), &status_changed);
+		check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.z_valid, &(status.condition_local_altitude_valid), &status_changed);
 
 		/* update battery status */
 		orb_check(battery_sub, &updated);
@@ -1512,68 +1513,61 @@ check_navigation_state_machine(struct vehicle_status_s *current_status, struct v
 {
 	transition_result_t res = TRANSITION_DENIED;
 
-	if (current_status->arming_state == ARMING_STATE_ARMED || current_status->arming_state == ARMING_STATE_ARMED_ERROR) {
-		/* ARMED */
-		switch (current_status->main_state) {
-		case MAIN_STATE_MANUAL:
-			res = navigation_state_transition(current_status, current_status->is_rotary_wing ? NAVIGATION_STATE_STABILIZE : NAVIGATION_STATE_DIRECT, control_mode);
-			break;
+	switch (current_status->main_state) {
+	case MAIN_STATE_MANUAL:
+		res = navigation_state_transition(current_status, current_status->is_rotary_wing ? NAVIGATION_STATE_STABILIZE : NAVIGATION_STATE_DIRECT, control_mode);
+		break;
 
-		case MAIN_STATE_SEATBELT:
-			res = navigation_state_transition(current_status, NAVIGATION_STATE_ALTHOLD, control_mode);
-			break;
+	case MAIN_STATE_SEATBELT:
+		res = navigation_state_transition(current_status, NAVIGATION_STATE_ALTHOLD, control_mode);
+		break;
 
-		case MAIN_STATE_EASY:
-			res = navigation_state_transition(current_status, NAVIGATION_STATE_VECTOR, control_mode);
-			break;
+	case MAIN_STATE_EASY:
+		res = navigation_state_transition(current_status, NAVIGATION_STATE_VECTOR, control_mode);
+		break;
 
-		case MAIN_STATE_AUTO:
-			if (current_status->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) {
-				/* don't act while taking off */
-				res = TRANSITION_NOT_CHANGED;
+	case MAIN_STATE_AUTO:
+		if (current_status->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) {
+			/* don't act while taking off */
+			res = TRANSITION_NOT_CHANGED;
+
+		} else {
+			if (current_status->condition_landed) {
+				/* if landed: transitions only to AUTO_READY are allowed */
+				res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_READY, control_mode);
+				// TRANSITION_DENIED is not possible here
+				break;
 
 			} else {
-				if (current_status->condition_landed) {
-					/* if landed: transitions only to AUTO_READY are allowed */
-					res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_READY, control_mode);
-					// TRANSITION_DENIED is not possible here
-					break;
+				/* if not landed: */
+				if (current_status->rc_signal_found_once && !current_status->rc_signal_lost) {
+					/* act depending on switches when manual control enabled */
+					if (current_status->return_switch == RETURN_SWITCH_RETURN) {
+						/* RTL */
+						res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_RTL, control_mode);
 
-				} else {
-					/* if not landed: */
-					if (current_status->rc_signal_found_once && !current_status->rc_signal_lost) {
-						/* act depending on switches when manual control enabled */
-						if (current_status->return_switch == RETURN_SWITCH_RETURN) {
-							/* RTL */
-							res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_RTL, control_mode);
+					} else {
+						if (current_status->mission_switch == MISSION_SWITCH_MISSION) {
+							/* MISSION */
+							res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_MISSION, control_mode);
 
 						} else {
-							if (current_status->mission_switch == MISSION_SWITCH_MISSION) {
-								/* MISSION */
-								res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_MISSION, control_mode);
-
-							} else {
-								/* LOITER */
-								res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode);
-							}
+							/* LOITER */
+							res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode);
 						}
-
-					} else {
-						/* always switch to loiter in air when no RC control */
-						res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode);
 					}
+
+				} else {
+					/* always switch to loiter in air when no RC control */
+					res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode);
 				}
 			}
-
-			break;
-
-		default:
-			break;
 		}
 
-	} else {
-		/* DISARMED */
-		res = navigation_state_transition(current_status, NAVIGATION_STATE_STANDBY, control_mode);
+		break;
+
+	default:
+		break;
 	}
 
 	return res;
diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index f313adce4..76c7eaf92 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -217,9 +217,8 @@ main_state_transition(struct vehicle_status_s *current_state, main_state_t new_m
 
 		case MAIN_STATE_SEATBELT:
 
-			/* need position estimate */
-			// TODO only need altitude estimate really
-			if (current_state->condition_local_position_valid) {
+			/* need altitude estimate */
+			if (current_state->condition_local_altitude_valid) {
 				ret = TRANSITION_CHANGED;
 			}
 
@@ -227,7 +226,7 @@ main_state_transition(struct vehicle_status_s *current_state, main_state_t new_m
 
 		case MAIN_STATE_EASY:
 
-			/* need position estimate */
+			/* need local position estimate */
 			if (current_state->condition_local_position_valid) {
 				ret = TRANSITION_CHANGED;
 			}
@@ -236,8 +235,8 @@ main_state_transition(struct vehicle_status_s *current_state, main_state_t new_m
 
 		case MAIN_STATE_AUTO:
 
-			/* need position estimate */
-			if (current_state->condition_local_position_valid) {
+			/* need global position estimate */
+			if (current_state->condition_global_position_valid) {
 				ret = TRANSITION_CHANGED;
 			}
 
@@ -277,17 +276,6 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_
 	} else {
 
 		switch (new_navigation_state) {
-		case NAVIGATION_STATE_STANDBY:
-			ret = TRANSITION_CHANGED;
-			control_mode->flag_control_rates_enabled = false;
-			control_mode->flag_control_attitude_enabled = false;
-			control_mode->flag_control_velocity_enabled = false;
-			control_mode->flag_control_position_enabled = false;
-			control_mode->flag_control_altitude_enabled = false;
-			control_mode->flag_control_climb_rate_enabled = false;
-			control_mode->flag_control_manual_enabled = false;
-			break;
-
 		case NAVIGATION_STATE_DIRECT:
 			ret = TRANSITION_CHANGED;
 			control_mode->flag_control_rates_enabled = true;
@@ -394,9 +382,8 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_
 
 		case NAVIGATION_STATE_AUTO_LAND:
 
-			/* deny transitions from landed states */
-			if (current_status->navigation_state != NAVIGATION_STATE_STANDBY &&
-			    current_status->navigation_state != NAVIGATION_STATE_AUTO_READY) {
+			/* deny transitions from landed state */
+			if (current_status->navigation_state != NAVIGATION_STATE_AUTO_READY) {
 				ret = TRANSITION_CHANGED;
 				control_mode->flag_control_rates_enabled = true;
 				control_mode->flag_control_attitude_enabled = true;
diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c
index 1cb470240..80ce33cda 100644
--- a/src/modules/multirotor_pos_control/multirotor_pos_control.c
+++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c
@@ -219,7 +219,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
 	const float pos_ctl_dz = 0.05f;
 	float home_alt = 0.0f;
 	hrt_abstime home_alt_t = 0;
-	uint64_t local_home_timestamp = 0;
+	uint64_t local_ref_timestamp = 0;
 
 	PID_t xy_pos_pids[2];
 	PID_t xy_vel_pids[2];
@@ -316,11 +316,11 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
 			} else {
 				/* non-manual mode, project global setpoints to local frame */
 				//orb_copy(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_sub, &local_pos_sp);
-				if (local_pos.home_timestamp != local_home_timestamp) {
-					local_home_timestamp = local_pos.home_timestamp;
+				if (local_pos.ref_timestamp != local_ref_timestamp) {
+					local_ref_timestamp = local_pos.ref_timestamp;
 					/* init local projection using local position home */
-					double lat_home = local_pos.home_lat * 1e-7;
-					double lon_home = local_pos.home_lon * 1e-7;
+					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);
 					warnx("local pos home: lat = %.10f, lon = %.10f", lat_home, lon_home);
 					mavlink_log_info(mavlink_fd, "local pos home: %.7f, %.7f", lat_home, lon_home);
@@ -338,7 +338,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
 						local_pos_sp.z = -global_pos_sp.altitude;
 
 					} else {
-						local_pos_sp.z = local_pos.home_alt - global_pos_sp.altitude;
+						local_pos_sp.z = local_pos.ref_alt - global_pos_sp.altitude;
 					}
 
 					warnx("new setpoint: lat = %.10f, lon = %.10f, x = %.2f, y = %.2f", sp_lat, sp_lon, local_pos_sp.x, local_pos_sp.y);
@@ -355,14 +355,14 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
 
 			/* manual control - move setpoints */
 			if (control_mode.flag_control_manual_enabled) {
-				if (local_pos.home_timestamp != home_alt_t) {
+				if (local_pos.ref_timestamp != home_alt_t) {
 					if (home_alt_t != 0) {
 						/* home alt changed, don't follow large ground level changes in manual flight */
-						local_pos_sp.z += local_pos.home_alt - home_alt;
+						local_pos_sp.z += local_pos.ref_alt - home_alt;
 					}
 
-					home_alt_t = local_pos.home_timestamp;
-					home_alt = local_pos.home_alt;
+					home_alt_t = local_pos.ref_timestamp;
+					home_alt = local_pos.ref_alt;
 				}
 
 				if (control_mode.flag_control_altitude_enabled) {
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 0e7e0ef5d..81f938719 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_main.c
+++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c
@@ -76,6 +76,7 @@ static bool thread_running = false; /**< Deamon status flag */
 static int position_estimator_inav_task; /**< Handle of deamon task / thread */
 static bool verbose_mode = false;
 static hrt_abstime gps_timeout = 1000000;	// GPS timeout = 1s
+static hrt_abstime flow_timeout = 1000000;	// optical flow timeout = 1s
 
 __EXPORT int position_estimator_inav_main(int argc, char *argv[]);
 
@@ -169,10 +170,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 	int baro_init_num = 200;
 	float baro_alt0 = 0.0f; /* to determine while start up */
 
-	double lat_current = 0.0f; //[°] --> 47.0
-	double lon_current = 0.0f; //[°] --> 8.5
-	double alt_current = 0.0f; //[m] above MSL
-
 	uint32_t accel_counter = 0;
 	uint32_t baro_counter = 0;
 
@@ -216,21 +213,17 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 	/* first parameters update */
 	parameters_update(&pos_inav_param_handles, &params);
 
-	struct pollfd fds_init[2] = {
+	struct pollfd fds_init[1] = {
 		{ .fd = sensor_combined_sub, .events = POLLIN },
-		{ .fd = vehicle_gps_position_sub, .events = POLLIN }
 	};
 
-	/* wait for initial sensors values: baro, GPS fix, only then can we initialize the projection */
-	bool wait_gps = params.use_gps;
+	/* wait for initial baro value */
 	bool wait_baro = true;
-	hrt_abstime wait_gps_start = 0;
-	const hrt_abstime wait_gps_delay = 5000000;	// wait for 5s after 3D fix
 
 	thread_running = true;
 
-	while ((wait_gps || wait_baro) && !thread_should_exit) {
-		int ret = poll(fds_init, params.use_gps ? 2 : 1, 1000);
+	while (wait_baro && !thread_should_exit) {
+		int ret = poll(fds_init, 1, 1000);
 
 		if (ret < 0) {
 			/* poll error */
@@ -253,43 +246,21 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 						baro_alt0 /= (float) baro_init_cnt;
 						warnx("init baro: alt = %.3f", baro_alt0);
 						mavlink_log_info(mavlink_fd, "[inav] init baro: alt = %.3f", baro_alt0);
-						local_pos.home_alt = baro_alt0;
-						local_pos.home_timestamp = hrt_absolute_time();
-					}
-				}
-			}
-
-			if (params.use_gps && (fds_init[1].revents & POLLIN)) {
-				orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps);
-
-				if (wait_gps && gps.fix_type >= 3) {
-					hrt_abstime t = hrt_absolute_time();
-
-					if (wait_gps_start == 0) {
-						wait_gps_start = t;
-
-					} else if (t - wait_gps_start > wait_gps_delay) {
-						wait_gps = false;
-						/* get GPS position for first initialization */
-						lat_current = gps.lat * 1e-7;
-						lon_current = gps.lon * 1e-7;
-						alt_current = gps.alt * 1e-3;
-
-						local_pos.home_lat = lat_current * 1e7;
-						local_pos.home_lon = lon_current * 1e7;
-						local_pos.home_hdg = 0.0f;
-						local_pos.home_timestamp = t;
-
-						/* initialize coordinates */
-						map_projection_init(lat_current, lon_current);
-						warnx("init GPS: lat = %.10f,  lon = %.10f", lat_current, lon_current);
-						mavlink_log_info(mavlink_fd, "[inav] init GPS: %.7f, %.7f", lat_current, lon_current);
+						local_pos.ref_alt = baro_alt0;
+						local_pos.ref_timestamp = hrt_absolute_time();
+						local_pos.z_valid = true;
+						local_pos.v_z_valid = true;
+						local_pos.global_z = true;
 					}
 				}
 			}
 		}
 	}
 
+	bool ref_xy_inited = false;
+	hrt_abstime ref_xy_init_start = 0;
+	const hrt_abstime ref_xy_init_delay = 5000000;	// wait for 5s after 3D fix
+
 	hrt_abstime t_prev = 0;
 
 	uint16_t accel_updates = 0;
@@ -337,7 +308,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 	}
 
 	while (!thread_should_exit) {
-		int ret = poll(fds, params.use_gps ? 6 : 5, 10); // wait maximal this 10 ms = 100 Hz minimum rate
+		int ret = poll(fds, 6, 10); // wait maximal this 10 ms = 100 Hz minimum rate
 		hrt_abstime t = hrt_absolute_time();
 
 		if (ret < 0) {
@@ -428,8 +399,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 								baro_alt0 += sonar_corr;
 								warnx("new home: alt = %.3f", baro_alt0);
 								mavlink_log_info(mavlink_fd, "[inav] new home: alt = %.3f", baro_alt0);
-								local_pos.home_alt = baro_alt0;
-								local_pos.home_timestamp = hrt_absolute_time();
+								local_pos.ref_alt = baro_alt0;
+								local_pos.ref_timestamp = hrt_absolute_time();
 								z_est[0] += sonar_corr;
 								sonar_corr = 0.0f;
 								sonar_corr_filtered = 0.0f;
@@ -444,29 +415,57 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 				flow_updates++;
 			}
 
-			if (params.use_gps && (fds[5].revents & POLLIN)) {
+			if (fds[5].revents & POLLIN) {
 				/* vehicle GPS position */
 				orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps);
 
 				if (gps.fix_type >= 3) {
-					/* project GPS lat lon to plane */
-					float gps_proj[2];
-					map_projection_project(gps.lat * 1e-7, gps.lon * 1e-7, &(gps_proj[0]), &(gps_proj[1]));
-					gps_corr[0][0] = gps_proj[0] - x_est[0];
-					gps_corr[1][0] = gps_proj[1] - y_est[0];
-
-					if (gps.vel_ned_valid) {
-						gps_corr[0][1] = gps.vel_n_m_s - x_est[1];
-						gps_corr[1][1] = gps.vel_e_m_s - y_est[1];
+					/* initialize reference position if needed */
+					if (ref_xy_inited) {
+						/* project GPS lat lon to plane */
+						float gps_proj[2];
+						map_projection_project(gps.lat * 1e-7, gps.lon * 1e-7, &(gps_proj[0]), &(gps_proj[1]));
+						/* calculate correction for position */
+						gps_corr[0][0] = gps_proj[0] - x_est[0];
+						gps_corr[1][0] = gps_proj[1] - y_est[0];
+
+						/* calculate correction for velocity */
+						if (gps.vel_ned_valid) {
+							gps_corr[0][1] = gps.vel_n_m_s - x_est[1];
+							gps_corr[1][1] = gps.vel_e_m_s - y_est[1];
+
+						} else {
+							gps_corr[0][1] = 0.0f;
+							gps_corr[1][1] = 0.0f;
+						}
 
 					} else {
-						gps_corr[0][1] = 0.0f;
-						gps_corr[1][1] = 0.0f;
+						hrt_abstime t = hrt_absolute_time();
+
+						if (ref_xy_init_start == 0) {
+							ref_xy_init_start = t;
+
+						} else if (t > ref_xy_init_start + ref_xy_init_delay) {
+							ref_xy_inited = true;
+							/* reference GPS position */
+							double lat = gps.lat * 1e-7;
+							double lon = gps.lon * 1e-7;
+
+							local_pos.ref_lat = gps.lat;
+							local_pos.ref_lon = gps.lon;
+							local_pos.ref_timestamp = t;
+
+							/* initialize projection */
+							map_projection_init(lat, lon);
+							warnx("init GPS: lat = %.10f,  lon = %.10f", lat, lon);
+							mavlink_log_info(mavlink_fd, "[inav] init GPS: %.7f, %.7f", lat, lon);
+						}
 					}
 
 					gps_updates++;
 
 				} else {
+					/* no GPS lock */
 					memset(gps_corr, 0, sizeof(gps_corr));
 				}
 			}
@@ -490,10 +489,14 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 		inertial_filter_correct(sonar_corr, dt, z_est, 0, params.w_alt_sonar);
 		inertial_filter_correct(accel_corr[2], dt, z_est, 2, params.w_alt_acc);
 
-		/* dont't try to estimate position when no any position source available */
-		bool can_estimate_pos = params.use_gps && gps.fix_type >= 3 && t - gps.timestamp_position < gps_timeout;
+		bool gps_valid = ref_xy_inited && gps.fix_type >= 3 && t < gps.timestamp_position + gps_timeout;
+		bool flow_valid = false;	// TODO implement opt flow
+
+		/* try to estimate xy even if no absolute position source available,
+		 * if using optical flow velocity will be correct in this case */
+		bool can_estimate_xy = gps_valid || flow_valid;
 
-		if (can_estimate_pos) {
+		if (can_estimate_xy) {
 			/* inertial filter prediction for position */
 			inertial_filter_predict(dt, x_est);
 			inertial_filter_predict(dt, y_est);
@@ -502,12 +505,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 			inertial_filter_correct(accel_corr[0], dt, x_est, 2, params.w_pos_acc);
 			inertial_filter_correct(accel_corr[1], dt, y_est, 2, params.w_pos_acc);
 
-			inertial_filter_correct(gps_corr[0][0], dt, x_est, 0, params.w_pos_gps_p);
-			inertial_filter_correct(gps_corr[1][0], dt, y_est, 0, params.w_pos_gps_p);
-
-			if (gps.vel_ned_valid && t - gps.timestamp_velocity < gps_timeout) {
-				inertial_filter_correct(gps_corr[0][1], dt, x_est, 1, params.w_pos_gps_v);
-				inertial_filter_correct(gps_corr[1][1], dt, y_est, 1, params.w_pos_gps_v);
+			if (gps_valid) {
+				inertial_filter_correct(gps_corr[0][0], dt, x_est, 0, params.w_pos_gps_p);
+				inertial_filter_correct(gps_corr[1][0], dt, y_est, 0, params.w_pos_gps_p);
+				if (gps.vel_ned_valid && t < gps.timestamp_velocity + gps_timeout) {
+					inertial_filter_correct(gps_corr[0][1], dt, x_est, 1, params.w_pos_gps_v);
+					inertial_filter_correct(gps_corr[1][1], dt, y_est, 1, params.w_pos_gps_v);
+				}
 			}
 		}
 
@@ -533,43 +537,48 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 
 		if (t - pub_last > pub_interval) {
 			pub_last = t;
+			/* publish local position */
 			local_pos.timestamp = t;
-			local_pos.valid = can_estimate_pos;
+			local_pos.xy_valid = can_estimate_xy && gps_valid;
+			local_pos.v_xy_valid = can_estimate_xy;
+			local_pos.global_xy = local_pos.xy_valid && gps_valid;	// will make sense when local position sources (e.g. vicon) will be implemented
 			local_pos.x = x_est[0];
 			local_pos.vx = x_est[1];
 			local_pos.y = y_est[0];
 			local_pos.vy = y_est[1];
 			local_pos.z = z_est[0];
 			local_pos.vz = z_est[1];
-			local_pos.absolute_alt = local_pos.home_alt - local_pos.z;
-			local_pos.hdg = att.yaw;
-
-			if ((isfinite(local_pos.x)) && (isfinite(local_pos.vx))
-			    && (isfinite(local_pos.y))
-			    && (isfinite(local_pos.vy))
-			    && (isfinite(local_pos.z))
-			    && (isfinite(local_pos.vz))) {
-				orb_publish(ORB_ID(vehicle_local_position), vehicle_local_position_pub, &local_pos);
-
-				if (params.use_gps) {
-					double est_lat, est_lon;
-					map_projection_reproject(local_pos.x, local_pos.y, &est_lat, &est_lon);
-
-					global_pos.valid = local_pos.valid;
-					global_pos.timestamp = t;
-					global_pos.time_gps_usec = gps.time_gps_usec;
-					global_pos.lat = (int32_t)(est_lat * 1e7);
-					global_pos.lon = (int32_t)(est_lon * 1e7);
-					global_pos.alt = local_pos.home_alt - local_pos.z;
-					global_pos.relative_alt = -local_pos.z;
-					global_pos.vx = local_pos.vx;
-					global_pos.vy = local_pos.vy;
-					global_pos.vz = local_pos.vz;
-					global_pos.hdg = local_pos.hdg;
-
-					orb_publish(ORB_ID(vehicle_global_position), vehicle_global_position_pub, &global_pos);
-				}
+			local_pos.landed = false;	// TODO
+
+			orb_publish(ORB_ID(vehicle_local_position), vehicle_local_position_pub, &local_pos);
+
+			/* publish global position */
+			global_pos.valid = local_pos.global_xy;
+			if (local_pos.global_xy) {
+				double est_lat, est_lon;
+				map_projection_reproject(local_pos.x, local_pos.y, &est_lat, &est_lon);
+				global_pos.lat = (int32_t)(est_lat * 1e7);
+				global_pos.lon = (int32_t)(est_lon * 1e7);
+				global_pos.time_gps_usec = gps.time_gps_usec;
+			}
+			/* set valid values even if position is not valid */
+			if (local_pos.v_xy_valid) {
+				global_pos.vx = local_pos.vx;
+				global_pos.vy = local_pos.vy;
+				global_pos.hdg = atan2f(local_pos.vy, local_pos.vx);
+			}
+			if (local_pos.z_valid) {
+				global_pos.relative_alt = -local_pos.z;
 			}
+			if (local_pos.global_z) {
+				global_pos.alt = local_pos.ref_alt - local_pos.z;
+			}
+			if (local_pos.v_z_valid) {
+				global_pos.vz = local_pos.vz;
+			}
+			global_pos.timestamp = t;
+
+			orb_publish(ORB_ID(vehicle_global_position), vehicle_global_position_pub, &global_pos);
 		}
 	}
 
diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.c b/src/modules/position_estimator_inav/position_estimator_inav_params.c
index 70248b3b7..801e20781 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_params.c
+++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c
@@ -40,7 +40,6 @@
 
 #include "position_estimator_inav_params.h"
 
-PARAM_DEFINE_INT32(INAV_USE_GPS, 1);
 PARAM_DEFINE_FLOAT(INAV_W_ALT_BARO, 1.0f);
 PARAM_DEFINE_FLOAT(INAV_W_ALT_ACC, 50.0f);
 PARAM_DEFINE_FLOAT(INAV_W_ALT_SONAR, 3.0f);
@@ -55,7 +54,6 @@ PARAM_DEFINE_FLOAT(INAV_SONAR_ERR, 0.5f);
 
 int parameters_init(struct position_estimator_inav_param_handles *h)
 {
-	h->use_gps = param_find("INAV_USE_GPS");
 	h->w_alt_baro = param_find("INAV_W_ALT_BARO");
 	h->w_alt_acc = param_find("INAV_W_ALT_ACC");
 	h->w_alt_sonar = param_find("INAV_W_ALT_SONAR");
@@ -73,7 +71,6 @@ int parameters_init(struct position_estimator_inav_param_handles *h)
 
 int parameters_update(const struct position_estimator_inav_param_handles *h, struct position_estimator_inav_params *p)
 {
-	param_get(h->use_gps, &(p->use_gps));
 	param_get(h->w_alt_baro, &(p->w_alt_baro));
 	param_get(h->w_alt_acc, &(p->w_alt_acc));
 	param_get(h->w_alt_sonar, &(p->w_alt_sonar));
diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.h b/src/modules/position_estimator_inav/position_estimator_inav_params.h
index 1e70a3c48..ed6f61e04 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_params.h
+++ b/src/modules/position_estimator_inav/position_estimator_inav_params.h
@@ -41,7 +41,6 @@
 #include <systemlib/param/param.h>
 
 struct position_estimator_inav_params {
-	int use_gps;
 	float w_alt_baro;
 	float w_alt_acc;
 	float w_alt_sonar;
@@ -56,7 +55,6 @@ struct position_estimator_inav_params {
 };
 
 struct position_estimator_inav_param_handles {
-	param_t use_gps;
 	param_t w_alt_baro;
 	param_t w_alt_acc;
 	param_t w_alt_sonar;
diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index 30dc7df9e..7f8648d95 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -1042,10 +1042,9 @@ int sdlog2_thread_main(int argc, char *argv[])
 				log_msg.body.log_LPOS.vx = buf.local_pos.vx;
 				log_msg.body.log_LPOS.vy = buf.local_pos.vy;
 				log_msg.body.log_LPOS.vz = buf.local_pos.vz;
-				log_msg.body.log_LPOS.hdg = buf.local_pos.hdg;
-				log_msg.body.log_LPOS.home_lat = buf.local_pos.home_lat;
-				log_msg.body.log_LPOS.home_lon = buf.local_pos.home_lon;
-				log_msg.body.log_LPOS.home_alt = buf.local_pos.home_alt;
+				log_msg.body.log_LPOS.ref_lat = buf.local_pos.ref_lat;
+				log_msg.body.log_LPOS.ref_lon = buf.local_pos.ref_lon;
+				log_msg.body.log_LPOS.ref_alt = buf.local_pos.ref_alt;
 				LOGBUFFER_WRITE_AND_COUNT(LPOS);
 			}
 
diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h
index 38e2596b2..dd98f65a9 100644
--- a/src/modules/sdlog2/sdlog2_messages.h
+++ b/src/modules/sdlog2/sdlog2_messages.h
@@ -106,10 +106,9 @@ struct log_LPOS_s {
 	float vx;
 	float vy;
 	float vz;
-	float hdg;
-	int32_t home_lat;
-	int32_t home_lon;
-	float home_alt;
+	int32_t ref_lat;
+	int32_t ref_lon;
+	float ref_alt;
 };
 
 /* --- LPSP - LOCAL POSITION SETPOINT --- */
@@ -260,7 +259,7 @@ static const struct log_format_s log_formats[] = {
 	LOG_FORMAT(ATSP, "ffff", "RollSP,PitchSP,YawSP,ThrustSP"),
 	LOG_FORMAT(IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"),
 	LOG_FORMAT(SENS, "ffff", "BaroPres,BaroAlt,BaroTemp,DiffPres"),
-	LOG_FORMAT(LPOS, "fffffffLLf", "X,Y,Z,VX,VY,VZ,Heading,HomeLat,HomeLon,HomeAlt"),
+	LOG_FORMAT(LPOS, "fffffffLLf", "X,Y,Z,VX,VY,VZ,RefLat,RefLon,RefAlt"),
 	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"),
diff --git a/src/modules/uORB/topics/vehicle_local_position.h b/src/modules/uORB/topics/vehicle_local_position.h
index 9d3b4468c..26e8f335b 100644
--- a/src/modules/uORB/topics/vehicle_local_position.h
+++ b/src/modules/uORB/topics/vehicle_local_position.h
@@ -54,27 +54,26 @@
  */
 struct vehicle_local_position_s
 {
-	uint64_t timestamp;			/**< time of this estimate, in microseconds since system start */
-	bool valid;				/**< true if position satisfies validity criteria of estimator */
-
-	float x;				/**< X positin in meters in NED earth-fixed frame */
-	float y;				/**< X positin in meters in NED earth-fixed frame */
-	float z;				/**< Z positin in meters in NED earth-fixed frame (negative altitude) */
-	float absolute_alt;			/**< Altitude as defined by pressure / GPS, 			LOGME */
-	float vx; 				/**< Ground X Speed (Latitude), m/s in NED				LOGME */
-	float vy;				/**< Ground Y Speed (Longitude), m/s in NED				LOGME */
-	float vz;				/**< Ground Z Speed (Altitude), m/s	in NED				LOGME */
-	float hdg; 				/**< Compass heading in radians -PI..+PI.					  */
-
-	// TODO Add covariances here
-
+	uint64_t timestamp;		/**< Time of this estimate, in microseconds since system start */
+	bool xy_valid;			/**< true if x and y are valid */
+	bool z_valid;			/**< true if z is valid */
+	bool v_xy_valid;		/**< true if vy and vy are valid */
+	bool v_z_valid;			/**< true if vz is valid */
+	/* Position in local NED frame */
+	float x;				/**< X position in meters in NED earth-fixed frame */
+	float y;				/**< X position in meters in NED earth-fixed frame */
+	float z;				/**< Z position in meters in NED earth-fixed frame (negative altitude) */
+	/* Velocity in NED frame */
+	float vx; 				/**< Ground X Speed (Latitude), m/s in NED */
+	float vy;				/**< Ground Y Speed (Longitude), m/s in NED */
+	float vz;				/**< Ground Z Speed (Altitude), m/s	in NED */
 	/* Reference position in GPS / WGS84 frame */
-	uint64_t home_timestamp;/**< Time when home position was set						  */
-	int32_t home_lat;		/**< Latitude in 1E7 degrees							LOGME */
-	int32_t home_lon;		/**< Longitude in 1E7 degrees							LOGME */
-	float home_alt;			/**< Altitude in meters									LOGME */
-	float home_hdg; 		/**< Compass heading in radians -PI..+PI.					  */
-
+	bool global_xy;			/**< true if position (x, y) is valid and has valid global reference (ref_lat, ref_lon) */
+	bool global_z;			/**< true if z is valid and has valid global reference (ref_alt) */
+	uint64_t ref_timestamp;	/**< Time when reference position was set */
+	int32_t ref_lat;		/**< Reference point latitude in 1E7 degrees */
+	int32_t ref_lon;		/**< Reference point longitude in 1E7 degrees */
+	float ref_alt;			/**< Reference altitude AMSL in meters, MUST be set to current (not at reference point!) ground level */
 	bool landed;			/**< true if vehicle is landed */
 };
 
diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h
index 6690fb2be..4317e07f2 100644
--- a/src/modules/uORB/topics/vehicle_status.h
+++ b/src/modules/uORB/topics/vehicle_status.h
@@ -68,8 +68,7 @@ typedef enum {
 
 /* navigation state machine */
 typedef enum {
-	NAVIGATION_STATE_STANDBY = 0,		// standby state, disarmed
-	NAVIGATION_STATE_DIRECT,		// true manual control, no any stabilization
+	NAVIGATION_STATE_DIRECT = 0,		// true manual control, no any stabilization
 	NAVIGATION_STATE_STABILIZE,		// attitude stabilization
 	NAVIGATION_STATE_ALTHOLD,		// attitude + altitude stabilization
 	NAVIGATION_STATE_VECTOR,		// attitude + altitude + position stabilization
@@ -203,6 +202,7 @@ struct vehicle_status_s
 	bool condition_launch_position_valid;		/**< indicates a valid launch position */
 	bool condition_home_position_valid;		/**< indicates a valid home position (a valid home position is not always a valid launch) */
 	bool condition_local_position_valid;
+	bool condition_local_altitude_valid;
 	bool condition_airspeed_valid;			/**< set to true by the commander app if there is a valid airspeed measurement available */
 	bool condition_landed;					/**< true if vehicle is landed, always true if disarmed */