Merge remote-tracking branch 'px4/ekf_home_init' into navigator_cleanup_ekf_home_init

Conflicts:
	src/modules/commander/commander.cpp
	src/modules/mc_pos_control/mc_pos_control_main.cpp
	src/modules/navigator/navigator_main.cpp
	src/modules/uORB/topics/vehicle_global_position.h

3 files changed, 22 insertions, 7 deletions

diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 1ba3d70c6..be4114e73 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -189,9 +189,18 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length
 	/* If the wait until transmit flag is on, only transmit after we've received messages.
 	   Otherwise, transmit all the time. */
 	if (instance->should_transmit()) {
-	   ssize_t ret = write(uart, ch, desired);
+
+		/* check if there is space in the buffer, let it overflow else */
+		if (!ioctl(uart, FIONWRITE, (unsigned long)&buf_free)) {
+
+			if (desired > buf_free) {
+				desired = buf_free;
+			}
+		}
+
+		ssize_t ret = write(uart, ch, desired);
 		if (ret != desired) {
-			// XXX do something here, but change to using FIONWRITE and OS buf size for detection
+			warnx("TX FAIL");
 		}
 	}
 
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 22cf9d7a6..33a4fef12 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -257,6 +257,7 @@ MavlinkReceiver::handle_message_optical_flow(mavlink_message_t *msg)
 	memset(&f, 0, sizeof(f));
 
 	f.timestamp = hrt_absolute_time();
+	f.flow_timestamp = flow.time_usec;
 	f.flow_raw_x = flow.flow_x;
 	f.flow_raw_y = flow.flow_y;
 	f.flow_comp_x_m = flow.flow_comp_m_x;
@@ -751,7 +752,6 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
 		memset(&hil_global_pos, 0, sizeof(hil_global_pos));
 
 		hil_global_pos.timestamp = timestamp;
-		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;
@@ -759,6 +759,8 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
 		hil_global_pos.vel_e = hil_state.vy / 100.0f;
 		hil_global_pos.vel_d = hil_state.vz / 100.0f;
 		hil_global_pos.yaw = hil_attitude.yaw;
+		hil_global_pos.eph = 2.0f;
+		hil_global_pos.epv = 4.0f;
 
 		if (_global_pos_pub < 0) {
 			_global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &hil_global_pos);
@@ -770,19 +772,22 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
 
 	/* local position */
 	{
+		double lat = hil_state.lat * 1e-7;
+		double lon = hil_state.lon * 1e-7;
+
 		if (!_hil_local_proj_inited) {
 			_hil_local_proj_inited = true;
 			_hil_local_alt0 = hil_state.alt / 1000.0f;
-			map_projection_init(hil_state.lat, hil_state.lon);
+			map_projection_init(&_hil_local_proj_ref, hil_state.lat, hil_state.lon);
 			hil_local_pos.ref_timestamp = timestamp;
-			hil_local_pos.ref_lat = hil_state.lat;
-			hil_local_pos.ref_lon = hil_state.lon;
+			hil_local_pos.ref_lat = lat;
+			hil_local_pos.ref_lon = lon;
 			hil_local_pos.ref_alt = _hil_local_alt0;
 		}
 
 		float x;
 		float y;
-		map_projection_project(hil_state.lat * 1e-7, hil_state.lon * 1e-7, &x, &y);
+		map_projection_project(&_hil_local_proj_ref, lat, lon, &x, &y);
 		hil_local_pos.timestamp = timestamp;
 		hil_local_pos.xy_valid = true;
 		hil_local_pos.z_valid = true;
diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h
index 72ce4560f..9ab84b58a 100644
--- a/src/modules/mavlink/mavlink_receiver.h
+++ b/src/modules/mavlink/mavlink_receiver.h
@@ -142,4 +142,5 @@ private:
 	uint64_t _old_timestamp;
 	bool _hil_local_proj_inited;
 	float _hil_local_alt0;
+	struct map_projection_reference_s _hil_local_proj_ref;
 };