Merge remote-tracking branch 'upstream/master'

14 files changed, 427 insertions, 133 deletions

diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp
index f17497aa8..4ba595a87 100644
--- a/src/modules/mavlink/mavlink_ftp.cpp
+++ b/src/modules/mavlink/mavlink_ftp.cpp
@@ -801,7 +801,7 @@ MavlinkFTP::_return_request(Request *req)
 
 /// @brief Copy file (with limited space)
 int
-MavlinkFTP::_copy_file(const char *src_path, const char *dst_path, ssize_t length)
+MavlinkFTP::_copy_file(const char *src_path, const char *dst_path, size_t length)
 {
 	char buff[512];
 	int src_fd = -1, dst_fd = -1;
diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h
index bef6775a9..9693a92a9 100644
--- a/src/modules/mavlink/mavlink_ftp.h
+++ b/src/modules/mavlink/mavlink_ftp.h
@@ -142,7 +142,7 @@ private:
 	static void	_worker_trampoline(void *arg);
 	void		_process_request(Request *req);
 	void		_reply(Request *req);
-	int		_copy_file(const char *src_path, const char *dst_path, ssize_t length);
+	int		_copy_file(const char *src_path, const char *dst_path, size_t length);
 
 	ErrorCode	_workList(PayloadHeader *payload);
 	ErrorCode	_workOpen(PayloadHeader *payload, int oflag);
diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index b99719821..8eeeb5bd7 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -904,20 +904,20 @@ Mavlink::send_statustext(unsigned char severity, const char *string)
 	mavlink_logbuffer_write(&_logbuffer, &logmsg);
 }
 
-MavlinkOrbSubscription *Mavlink::add_orb_subscription(const orb_id_t topic)
+MavlinkOrbSubscription *Mavlink::add_orb_subscription(const orb_id_t topic, int instance)
 {
 	/* check if already subscribed to this topic */
 	MavlinkOrbSubscription *sub;
 
 	LL_FOREACH(_subscriptions, sub) {
-		if (sub->get_topic() == topic) {
+		if (sub->get_topic() == topic && sub->get_instance() == instance) {
 			/* already subscribed */
 			return sub;
 		}
 	}
 
 	/* add new subscription */
-	MavlinkOrbSubscription *sub_new = new MavlinkOrbSubscription(topic);
+	MavlinkOrbSubscription *sub_new = new MavlinkOrbSubscription(topic, instance);
 
 	LL_APPEND(_subscriptions, sub_new);
 
@@ -948,7 +948,6 @@ Mavlink::configure_stream(const char *stream_name, const float rate)
 				/* delete stream */
 				LL_DELETE(_streams, stream);
 				delete stream;
-				warnx("deleted stream %s", stream->get_name());
 			}
 
 			return OK;
@@ -1287,30 +1286,11 @@ Mavlink::task_main(int argc, char *argv[])
 	}
 
 	if (Mavlink::instance_exists(_device_name, this)) {
-		warnx("mavlink instance for %s already running", _device_name);
+		warnx("%s already running", _device_name);
 		return ERROR;
 	}
 
-	/* inform about mode */
-	switch (_mode) {
-	case MAVLINK_MODE_NORMAL:
-		warnx("mode: NORMAL");
-		break;
-
-	case MAVLINK_MODE_CUSTOM:
-		warnx("mode: CUSTOM");
-		break;
-
-	case MAVLINK_MODE_ONBOARD:
-		warnx("mode: ONBOARD");
-		break;
-
-	default:
-		warnx("ERROR: Unknown mode");
-		break;
-	}
-
-	warnx("data rate: %d Bytes/s, port: %s, baud: %d", _datarate, _device_name, _baudrate);
+	warnx("mode: %u, data rate: %d B/s on %s @ %dB", _mode, _datarate, _device_name, _baudrate);
 
 	/* flush stdout in case MAVLink is about to take it over */
 	fflush(stdout);
@@ -1338,7 +1318,7 @@ Mavlink::task_main(int argc, char *argv[])
 		 * marker ring buffer approach.
 		 */
 		if (OK != message_buffer_init(2 * sizeof(mavlink_message_t) + 1)) {
-			errx(1, "can't allocate message buffer, exiting");
+			errx(1, "msg buf:");
 		}
 
 		/* initialize message buffer mutex */
@@ -1405,17 +1385,23 @@ Mavlink::task_main(int argc, char *argv[])
 		configure_stream("POSITION_TARGET_GLOBAL_INT", 3.0f);
 		configure_stream("ATTITUDE_TARGET", 3.0f);
 		configure_stream("DISTANCE_SENSOR", 0.5f);
-		configure_stream("OPTICAL_FLOW", 5.0f);
+		configure_stream("OPTICAL_FLOW_RAD", 5.0f);
 		break;
 
 	case MAVLINK_MODE_ONBOARD:
 		configure_stream("SYS_STATUS", 1.0f);
 		configure_stream("ATTITUDE", 50.0f);
 		configure_stream("GLOBAL_POSITION_INT", 50.0f);
+		configure_stream("LOCAL_POSITION_NED", 30.0f);
 		configure_stream("CAMERA_CAPTURE", 2.0f);
 		configure_stream("ATTITUDE_TARGET", 10.0f);
 		configure_stream("POSITION_TARGET_GLOBAL_INT", 10.0f);
+		configure_stream("POSITION_TARGET_LOCAL_NED", 10.0f);
+		configure_stream("DISTANCE_SENSOR", 10.0f);
+		configure_stream("OPTICAL_FLOW_RAD", 10.0f);
 		configure_stream("VFR_HUD", 10.0f);
+		configure_stream("SYSTEM_TIME", 1.0f);
+		configure_stream("TIMESYNC", 10.0f);
 		break;
 
 	default:
@@ -1566,8 +1552,6 @@ Mavlink::task_main(int argc, char *argv[])
 
 	_subscriptions = nullptr;
 
-	warnx("waiting for UART receive thread");
-
 	/* wait for threads to complete */
 	pthread_join(_receive_thread, NULL);
 
@@ -1638,7 +1622,7 @@ Mavlink::start(int argc, char *argv[])
 		       SCHED_PRIORITY_DEFAULT,
 		       2800,
 		       (main_t)&Mavlink::start_helper,
-		       (const char **)argv);
+		       (char * const *)argv);
 
 	// Ensure that this shell command
 	// does not return before the instance
diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h
index ad5e5001b..baaa7bc13 100644
--- a/src/modules/mavlink/mavlink_main.h
+++ b/src/modules/mavlink/mavlink_main.h
@@ -171,7 +171,7 @@ public:
 
 	void			handle_message(const mavlink_message_t *msg);
 
-	MavlinkOrbSubscription *add_orb_subscription(const orb_id_t topic);
+	MavlinkOrbSubscription *add_orb_subscription(const orb_id_t topic, int instance=0);
 
 	int			get_instance_id();
 
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index 978aee118..4a095a765 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -342,6 +342,8 @@ private:
 	MavlinkStreamStatustext(MavlinkStreamStatustext &);
 	MavlinkStreamStatustext& operator = (const MavlinkStreamStatustext &);
 	FILE *fp = nullptr;
+	unsigned write_err_count = 0;
+	static const unsigned write_err_threshold = 5;
 
 protected:
 	explicit MavlinkStreamStatustext(Mavlink *mavlink) : MavlinkStream(mavlink)
@@ -370,10 +372,21 @@ protected:
 				/* write log messages in first instance to disk */
 				if (_mavlink->get_instance_id() == 0) {
 					if (fp) {
-						fputs(msg.text, fp);
-						fputs("\n", fp);
-						fsync(fileno(fp));
-					} else {
+						if (EOF == fputs(msg.text, fp)) {
+							write_err_count++;
+						} else {
+							write_err_count = 0;
+						}
+
+						if (write_err_count >= write_err_threshold) {
+							(void)fclose(fp);
+							fp = nullptr;
+						} else {
+							(void)fputs("\n", fp);
+							(void)fsync(fileno(fp));
+						}
+
+					} else if (write_err_count < write_err_threshold) {
 						/* string to hold the path to the log */
 						char log_file_name[32] = "";
 						char log_file_path[64] = "";
@@ -389,6 +402,10 @@ protected:
 						strftime(log_file_name, sizeof(log_file_name), "msgs_%Y_%m_%d_%H_%M_%S.txt", &tt);
 						snprintf(log_file_path, sizeof(log_file_path), "/fs/microsd/%s", log_file_name);
 						fp = fopen(log_file_path, "ab");
+
+						/* write first message */
+						fputs(msg.text, fp);
+						fputs("\n", fp);
 					}
 				}
 			}
@@ -810,9 +827,6 @@ private:
 	MavlinkOrbSubscription *_airspeed_sub;
 	uint64_t _airspeed_time;
 
-	MavlinkOrbSubscription *_sensor_combined_sub;
-	uint64_t _sensor_combined_time;
-
 	/* do not allow top copying this class */
 	MavlinkStreamVFRHUD(MavlinkStreamVFRHUD &);
 	MavlinkStreamVFRHUD& operator = (const MavlinkStreamVFRHUD &);
@@ -828,9 +842,7 @@ protected:
 		_act_sub(_mavlink->add_orb_subscription(ORB_ID(actuator_controls_0))),
 		_act_time(0),
 		_airspeed_sub(_mavlink->add_orb_subscription(ORB_ID(airspeed))),
-		_airspeed_time(0),
-		_sensor_combined_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_combined))),
-		_sensor_combined_time(0)
+		_airspeed_time(0)
 	{}
 
 	void send(const hrt_abstime t)
@@ -840,14 +852,12 @@ protected:
 		struct actuator_armed_s armed;
 		struct actuator_controls_s act;
 		struct airspeed_s airspeed;
-		struct sensor_combined_s sensor_combined;
 
 		bool updated = _att_sub->update(&_att_time, &att);
 		updated |= _pos_sub->update(&_pos_time, &pos);
 		updated |= _armed_sub->update(&_armed_time, &armed);
 		updated |= _act_sub->update(&_act_time, &act);
 		updated |= _airspeed_sub->update(&_airspeed_time, &airspeed);
-		updated |= _sensor_combined_sub->update(&_sensor_combined_time, &sensor_combined);
 
 		if (updated) {
 			mavlink_vfr_hud_t msg;
@@ -856,7 +866,7 @@ protected:
 			msg.groundspeed = sqrtf(pos.vel_n * pos.vel_n + pos.vel_e * pos.vel_e);
 			msg.heading = _wrap_2pi(att.yaw) * M_RAD_TO_DEG_F;
 			msg.throttle = armed.armed ? act.control[3] * 100.0f : 0.0f;
-			msg.alt = sensor_combined.baro_alt_meter;
+			msg.alt = pos.alt;
 			msg.climb = -pos.vel_d;
 
 			_mavlink->send_message(MAVLINK_MSG_ID_VFR_HUD, &msg);
@@ -930,6 +940,92 @@ protected:
 	}
 };
 
+class MavlinkStreamSystemTime : public MavlinkStream
+{
+public:
+	const char *get_name() const {
+		return MavlinkStreamSystemTime::get_name_static();
+	}
+
+	static const char *get_name_static() {
+		return "SYSTEM_TIME";
+	}
+
+	uint8_t get_id() {
+		return MAVLINK_MSG_ID_SYSTEM_TIME;
+	}
+
+	static MavlinkStream *new_instance(Mavlink *mavlink) {
+		return new MavlinkStreamSystemTime(mavlink);
+	}
+
+	unsigned get_size() {
+		return MAVLINK_MSG_ID_SYSTEM_TIME_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
+	}
+
+private:
+	/* do not allow top copying this class */
+	MavlinkStreamSystemTime(MavlinkStreamSystemTime &);
+	MavlinkStreamSystemTime &operator = (const MavlinkStreamSystemTime &);
+
+protected:
+	explicit MavlinkStreamSystemTime(Mavlink *mavlink) : MavlinkStream(mavlink)
+	{}
+
+	void send(const hrt_abstime t) {
+		mavlink_system_time_t msg;
+		timespec tv;
+
+		clock_gettime(CLOCK_REALTIME, &tv);
+
+		msg.time_boot_ms = hrt_absolute_time() / 1000;
+		msg.time_unix_usec = (uint64_t)tv.tv_sec * 1000000 + tv.tv_nsec / 1000;
+
+		_mavlink->send_message(MAVLINK_MSG_ID_SYSTEM_TIME, &msg);
+	}
+};
+
+class MavlinkStreamTimesync : public MavlinkStream
+{
+public:
+	const char *get_name() const {
+		return MavlinkStreamTimesync::get_name_static();
+	}
+
+	static const char *get_name_static() {
+		return "TIMESYNC";
+	}
+
+	uint8_t get_id() {
+		return MAVLINK_MSG_ID_TIMESYNC;
+	}
+
+	static MavlinkStream *new_instance(Mavlink *mavlink) {
+		return new MavlinkStreamTimesync(mavlink);
+	}
+
+	unsigned get_size() {
+		return MAVLINK_MSG_ID_TIMESYNC_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
+	}
+
+private:
+	/* do not allow top copying this class */
+	MavlinkStreamTimesync(MavlinkStreamTimesync &);
+	MavlinkStreamTimesync &operator = (const MavlinkStreamTimesync &);
+
+protected:
+	explicit MavlinkStreamTimesync(Mavlink *mavlink) : MavlinkStream(mavlink)
+	{}
+
+	void send(const hrt_abstime t) {
+		mavlink_timesync_t msg;
+
+		msg.tc1 = 0;
+		msg.ts1 = hrt_absolute_time() * 1000; // boot time in nanoseconds
+
+		_mavlink->send_message(MAVLINK_MSG_ID_TIMESYNC, &msg);
+	}
+};
 
 class MavlinkStreamGlobalPositionInt : public MavlinkStream
 {
@@ -1246,14 +1342,7 @@ protected:
 		_act_sub(nullptr),
 		_act_time(0)
 	{
-		orb_id_t act_topics[] = {
-			ORB_ID(actuator_outputs_0),
-			ORB_ID(actuator_outputs_1),
-			ORB_ID(actuator_outputs_2),
-			ORB_ID(actuator_outputs_3)
-		};
-
-		_act_sub = _mavlink->add_orb_subscription(act_topics[N]);
+		_act_sub = _mavlink->add_orb_subscription(ORB_ID(actuator_outputs), N);
 	}
 
 	void send(const hrt_abstime t)
@@ -1328,7 +1417,7 @@ protected:
 		_status_time(0),
 		_pos_sp_triplet_sub(_mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet))),
 		_pos_sp_triplet_time(0),
-		_act_sub(_mavlink->add_orb_subscription(ORB_ID(actuator_outputs_0))),
+		_act_sub(_mavlink->add_orb_subscription(ORB_ID(actuator_outputs))),
 		_act_time(0)
 	{}
 
@@ -1358,7 +1447,10 @@ protected:
 			/* scale outputs depending on system type */
 			if (system_type == MAV_TYPE_QUADROTOR ||
 				system_type == MAV_TYPE_HEXAROTOR ||
-				system_type == MAV_TYPE_OCTOROTOR) {
+				system_type == MAV_TYPE_OCTOROTOR ||
+				system_type == MAV_TYPE_VTOL_DUOROTOR ||
+				system_type == MAV_TYPE_VTOL_QUADROTOR) {
+
 				/* multirotors: set number of rotor outputs depending on type */
 
 				unsigned n;
@@ -1372,6 +1464,14 @@ protected:
 					n = 6;
 					break;
 
+				case MAV_TYPE_VTOL_DUOROTOR:
+					n = 2;
+					break;
+
+				case MAV_TYPE_VTOL_QUADROTOR:
+					n = 4;
+					break;
+
 				default:
 					n = 8;
 					break;
@@ -1482,6 +1582,7 @@ protected:
 		if (_pos_sp_triplet_sub->update(&pos_sp_triplet)) {
 			mavlink_position_target_global_int_t msg{};
 
+			msg.time_boot_ms = hrt_absolute_time()/1000;
 			msg.coordinate_frame = MAV_FRAME_GLOBAL;
 			msg.lat_int = pos_sp_triplet.current.lat * 1e7;
 			msg.lon_int = pos_sp_triplet.current.lon * 1e7;
@@ -1760,6 +1861,9 @@ protected:
 				case RC_INPUT_SOURCE_PX4IO_ST24:
 					msg.rssi |= (3 << 4);
 					break;
+				case RC_INPUT_SOURCE_UNKNOWN:
+					// do nothing
+					break;
 			}
 
 			if (rc.rc_lost) {
@@ -1834,33 +1938,32 @@ protected:
 	}
 };
 
-
-class MavlinkStreamOpticalFlow : public MavlinkStream
+class MavlinkStreamOpticalFlowRad : public MavlinkStream
 {
 public:
 	const char *get_name() const
 	{
-		return MavlinkStreamOpticalFlow::get_name_static();
+		return MavlinkStreamOpticalFlowRad::get_name_static();
 	}
 
 	static const char *get_name_static()
 	{
-		return "OPTICAL_FLOW";
+		return "OPTICAL_FLOW_RAD";
 	}
 
 	uint8_t get_id()
 	{
-		return MAVLINK_MSG_ID_OPTICAL_FLOW;
+		return MAVLINK_MSG_ID_OPTICAL_FLOW_RAD;
 	}
 
 	static MavlinkStream *new_instance(Mavlink *mavlink)
 	{
-		return new MavlinkStreamOpticalFlow(mavlink);
+		return new MavlinkStreamOpticalFlowRad(mavlink);
 	}
 
 	unsigned get_size()
 	{
-		return _flow_sub->is_published() ? (MAVLINK_MSG_ID_OPTICAL_FLOW_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0;
+		return _flow_sub->is_published() ? (MAVLINK_MSG_ID_OPTICAL_FLOW_RAD_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0;
 	}
 
 private:
@@ -1868,11 +1971,11 @@ private:
 	uint64_t _flow_time;
 
 	/* do not allow top copying this class */
-	MavlinkStreamOpticalFlow(MavlinkStreamOpticalFlow &);
-	MavlinkStreamOpticalFlow& operator = (const MavlinkStreamOpticalFlow &);
+	MavlinkStreamOpticalFlowRad(MavlinkStreamOpticalFlowRad &);
+	MavlinkStreamOpticalFlowRad& operator = (const MavlinkStreamOpticalFlowRad &);
 
 protected:
-	explicit MavlinkStreamOpticalFlow(Mavlink *mavlink) : MavlinkStream(mavlink),
+	explicit MavlinkStreamOpticalFlowRad(Mavlink *mavlink) : MavlinkStream(mavlink),
 		_flow_sub(_mavlink->add_orb_subscription(ORB_ID(optical_flow))),
 		_flow_time(0)
 	{}
@@ -1882,18 +1985,23 @@ protected:
 		struct optical_flow_s flow;
 
 		if (_flow_sub->update(&_flow_time, &flow)) {
-			mavlink_optical_flow_t msg;
+			mavlink_optical_flow_rad_t msg;
 
 			msg.time_usec = flow.timestamp;
 			msg.sensor_id = flow.sensor_id;
-			msg.flow_x = flow.flow_raw_x;
-			msg.flow_y = flow.flow_raw_y;
-			msg.flow_comp_m_x = flow.flow_comp_x_m;
-			msg.flow_comp_m_y = flow.flow_comp_y_m;
+			msg.integrated_x = flow.pixel_flow_x_integral;
+			msg.integrated_y = flow.pixel_flow_y_integral;
+			msg.integrated_xgyro = flow.gyro_x_rate_integral;
+			msg.integrated_ygyro = flow.gyro_y_rate_integral;
+			msg.integrated_zgyro = flow.gyro_z_rate_integral;
+			msg.distance = flow.ground_distance_m;
 			msg.quality = flow.quality;
-			msg.ground_distance = flow.ground_distance_m;
+			msg.integration_time_us = flow.integration_timespan;
+			msg.sensor_id = flow.sensor_id;
+			msg.time_delta_distance_us = flow.time_since_last_sonar_update;
+			msg.temperature = flow.gyro_temperature;
 
-			_mavlink->send_message(MAVLINK_MSG_ID_OPTICAL_FLOW, &msg);
+			_mavlink->send_message(MAVLINK_MSG_ID_OPTICAL_FLOW_RAD, &msg);
 		}
 	}
 };
@@ -2165,7 +2273,7 @@ protected:
 			msg.id = 0;
 			msg.orientation = 0;
 			msg.min_distance = range.minimum_distance * 100;
-			msg.max_distance = range.minimum_distance * 100;
+			msg.max_distance = range.maximum_distance * 100;
 			msg.current_distance = range.distance * 100;
 			msg.covariance = 20;
 
@@ -2185,6 +2293,8 @@ StreamListItem *streams_list[] = {
 	new StreamListItem(&MavlinkStreamAttitudeQuaternion::new_instance, &MavlinkStreamAttitudeQuaternion::get_name_static),
 	new StreamListItem(&MavlinkStreamVFRHUD::new_instance, &MavlinkStreamVFRHUD::get_name_static),
 	new StreamListItem(&MavlinkStreamGPSRawInt::new_instance, &MavlinkStreamGPSRawInt::get_name_static),
+	new StreamListItem(&MavlinkStreamSystemTime::new_instance, &MavlinkStreamSystemTime::get_name_static),
+	new StreamListItem(&MavlinkStreamTimesync::new_instance, &MavlinkStreamTimesync::get_name_static),
 	new StreamListItem(&MavlinkStreamGlobalPositionInt::new_instance, &MavlinkStreamGlobalPositionInt::get_name_static),
 	new StreamListItem(&MavlinkStreamLocalPositionNED::new_instance, &MavlinkStreamLocalPositionNED::get_name_static),
 	new StreamListItem(&MavlinkStreamViconPositionEstimate::new_instance, &MavlinkStreamViconPositionEstimate::get_name_static),
@@ -2199,7 +2309,7 @@ StreamListItem *streams_list[] = {
 	new StreamListItem(&MavlinkStreamAttitudeTarget::new_instance, &MavlinkStreamAttitudeTarget::get_name_static),
 	new StreamListItem(&MavlinkStreamRCChannelsRaw::new_instance, &MavlinkStreamRCChannelsRaw::get_name_static),
 	new StreamListItem(&MavlinkStreamManualControl::new_instance, &MavlinkStreamManualControl::get_name_static),
-	new StreamListItem(&MavlinkStreamOpticalFlow::new_instance, &MavlinkStreamOpticalFlow::get_name_static),
+	new StreamListItem(&MavlinkStreamOpticalFlowRad::new_instance, &MavlinkStreamOpticalFlowRad::get_name_static),
 	new StreamListItem(&MavlinkStreamAttitudeControls::new_instance, &MavlinkStreamAttitudeControls::get_name_static),
 	new StreamListItem(&MavlinkStreamNamedValueFloat::new_instance, &MavlinkStreamNamedValueFloat::get_name_static),
 	new StreamListItem(&MavlinkStreamCameraCapture::new_instance, &MavlinkStreamCameraCapture::get_name_static),
diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp
index a3c127cdc..442d36dfb 100644
--- a/src/modules/mavlink/mavlink_mission.cpp
+++ b/src/modules/mavlink/mavlink_mission.cpp
@@ -120,13 +120,11 @@ MavlinkMissionManager::init_offboard_mission()
 		_count = mission_state.count;
 		_current_seq = mission_state.current_seq;
 
-		warnx("offboard mission init: dataman_id=%d, count=%u, current_seq=%d", _dataman_id, _count, _current_seq);
-
 	} else {
 		_dataman_id = 0;
 		_count = 0;
 		_current_seq = 0;
-		warnx("offboard mission init: ERROR, reading mission state failed");
+		warnx("offboard mission init: ERROR");
 	}
 }
 
@@ -292,9 +290,6 @@ MavlinkMissionManager::send_mission_item_reached(uint16_t seq)
 void
 MavlinkMissionManager::send(const hrt_abstime now)
 {
-	/* update interval for slow rate limiter */
-	_slow_rate_limiter.set_interval(_interval * 10 / _mavlink->get_rate_mult());
-
 	bool updated = false;
 	orb_check(_mission_result_sub, &updated);
 
@@ -312,6 +307,12 @@ MavlinkMissionManager::send(const hrt_abstime now)
 
 		send_mission_current(_current_seq);
 
+		if (mission_result.item_do_jump_changed) {
+			/* send a mission item again if the remaining DO_JUMPs has changed */
+			send_mission_item(_transfer_partner_sysid, _transfer_partner_compid,
+					  (uint16_t)mission_result.item_changed_index);
+		}
+
 	} else {
 		if (_slow_rate_limiter.check(now)) {
 			send_mission_current(_current_seq);
@@ -811,7 +812,7 @@ MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s *
 
 	case NAV_CMD_DO_JUMP:
 		mavlink_mission_item->param1 = mission_item->do_jump_mission_index;
-		mavlink_mission_item->param2 = mission_item->do_jump_repeat_count;
+		mavlink_mission_item->param2 = mission_item->do_jump_repeat_count - mission_item->do_jump_current_count;
 		break;
 
 	default:
diff --git a/src/modules/mavlink/mavlink_orb_subscription.cpp b/src/modules/mavlink/mavlink_orb_subscription.cpp
index 734f0903a..315776e29 100644
--- a/src/modules/mavlink/mavlink_orb_subscription.cpp
+++ b/src/modules/mavlink/mavlink_orb_subscription.cpp
@@ -46,10 +46,11 @@
 
 #include "mavlink_orb_subscription.h"
 
-MavlinkOrbSubscription::MavlinkOrbSubscription(const orb_id_t topic) :
+MavlinkOrbSubscription::MavlinkOrbSubscription(const orb_id_t topic, int instance) :
 	next(nullptr),
 	_topic(topic),
-	_fd(orb_subscribe(_topic)),
+	_instance(instance),
+	_fd(orb_subscribe_multi(_topic, instance)),
 	_published(false)
 {
 }
@@ -65,6 +66,12 @@ MavlinkOrbSubscription::get_topic() const
 	return _topic;
 }
 
+int
+MavlinkOrbSubscription::get_instance() const
+{
+	return _instance;
+}
+
 bool
 MavlinkOrbSubscription::update(uint64_t *time, void* data)
 {
diff --git a/src/modules/mavlink/mavlink_orb_subscription.h b/src/modules/mavlink/mavlink_orb_subscription.h
index 7af454df6..5394e5097 100644
--- a/src/modules/mavlink/mavlink_orb_subscription.h
+++ b/src/modules/mavlink/mavlink_orb_subscription.h
@@ -50,7 +50,7 @@ class MavlinkOrbSubscription
 public:
 	MavlinkOrbSubscription *next;	///< pointer to next subscription in list
 
-	MavlinkOrbSubscription(const orb_id_t topic);
+	MavlinkOrbSubscription(const orb_id_t topic, int instance);
 	~MavlinkOrbSubscription();
 
 	/**
@@ -77,9 +77,11 @@ public:
 	 */
 	bool is_published();
 	orb_id_t get_topic() const;
+	int get_instance() const;
 
 private:
 	const orb_id_t _topic;		///< topic metadata
+	const int _instance;		///< get topic instance
 	int _fd;			///< subscription handle
 	bool _published;		///< topic was ever published
 
diff --git a/src/modules/mavlink/mavlink_parameters.cpp b/src/modules/mavlink/mavlink_parameters.cpp
index cd5f53d88..e9858b73c 100644
--- a/src/modules/mavlink/mavlink_parameters.cpp
+++ b/src/modules/mavlink/mavlink_parameters.cpp
@@ -44,7 +44,9 @@
 #include "mavlink_main.h"
 
 MavlinkParametersManager::MavlinkParametersManager(Mavlink *mavlink) : MavlinkStream(mavlink),
-	_send_all_index(-1)
+	_send_all_index(-1),
+	_rc_param_map_pub(-1),
+	_rc_param_map()
 {
 }
 
@@ -135,6 +137,43 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
 			break;
 		}
 
+	case MAVLINK_MSG_ID_PARAM_MAP_RC: {
+			/* map a rc channel to a parameter */
+			mavlink_param_map_rc_t map_rc;
+			mavlink_msg_param_map_rc_decode(msg, &map_rc);
+
+			if (map_rc.target_system == mavlink_system.sysid &&
+			    (map_rc.target_component == mavlink_system.compid ||
+			     map_rc.target_component == MAV_COMP_ID_ALL)) {
+
+				/* Copy values from msg to uorb using the parameter_rc_channel_index as index */
+				size_t i = map_rc.parameter_rc_channel_index;
+				_rc_param_map.param_index[i] = map_rc.param_index;
+				strncpy(&(_rc_param_map.param_id[i][0]), map_rc.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
+				/* enforce null termination */
+				_rc_param_map.param_id[i][MAVLINK_MSG_PARAM_MAP_RC_FIELD_PARAM_ID_LEN] = '\0';
+				_rc_param_map.scale[i] = map_rc.scale;
+				_rc_param_map.value0[i] = map_rc.param_value0;
+				_rc_param_map.value_min[i] = map_rc.param_value_min;
+				_rc_param_map.value_max[i] = map_rc.param_value_max;
+				if (map_rc.param_index == -2) { // -2 means unset map
+					_rc_param_map.valid[i] = false;
+				} else {
+					_rc_param_map.valid[i] = true;
+				}
+				_rc_param_map.timestamp = hrt_absolute_time();
+
+				if (_rc_param_map_pub < 0) {
+					_rc_param_map_pub = orb_advertise(ORB_ID(rc_parameter_map), &_rc_param_map);
+
+				} else {
+					orb_publish(ORB_ID(rc_parameter_map), _rc_param_map_pub, &_rc_param_map);
+				}
+
+			}
+			break;
+		}
+
 	default:
 		break;
 	}
diff --git a/src/modules/mavlink/mavlink_parameters.h b/src/modules/mavlink/mavlink_parameters.h
index 5576e6b84..b6736f212 100644
--- a/src/modules/mavlink/mavlink_parameters.h
+++ b/src/modules/mavlink/mavlink_parameters.h
@@ -44,6 +44,8 @@
 
 #include "mavlink_bridge_header.h"
 #include "mavlink_stream.h"
+#include <uORB/uORB.h>
+#include <uORB/topics/rc_parameter_map.h>
 
 class MavlinkParametersManager : public MavlinkStream
 {
@@ -112,4 +114,7 @@ protected:
 	void send(const hrt_abstime t);
 
 	void send_param(param_t param);
+
+	orb_advert_t _rc_param_map_pub;
+	struct rc_parameter_map_s _rc_param_map;
 };
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 8b216d262..16d0422c7 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2015 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -67,6 +67,8 @@
 
 #include <mathlib/mathlib.h>
 
+#include <conversion/rotation.h>
+
 #include <systemlib/param/param.h>
 #include <systemlib/systemlib.h>
 #include <systemlib/err.h>
@@ -89,6 +91,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
 	_mavlink(parent),
 	status{},
 	hil_local_pos{},
+	hil_land_detector{},
 	_control_mode{},
 	_global_pos_pub(-1),
 	_local_pos_pub(-1),
@@ -115,12 +118,15 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
 	_telemetry_status_pub(-1),
 	_rc_pub(-1),
 	_manual_pub(-1),
+	_land_detector_pub(-1),
 	_control_mode_sub(orb_subscribe(ORB_ID(vehicle_control_mode))),
 	_hil_frames(0),
 	_old_timestamp(0),
 	_hil_local_proj_inited(0),
 	_hil_local_alt0(0.0f),
-	_hil_local_proj_ref{}
+	_hil_local_proj_ref{},
+	_time_offset_avg_alpha(0.6),
+	_time_offset(0)
 {
 
 	// make sure the FTP server is started
@@ -143,8 +149,8 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 		handle_message_command_int(msg);
 		break;
 
-	case MAVLINK_MSG_ID_OPTICAL_FLOW:
-		handle_message_optical_flow(msg);
+	case MAVLINK_MSG_ID_OPTICAL_FLOW_RAD:
+		handle_message_optical_flow_rad(msg);
 		break;
 
 	case MAVLINK_MSG_ID_SET_MODE:
@@ -187,6 +193,14 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 		MavlinkFTP::get_server()->handle_message(_mavlink, msg);
 		break;
 
+	case MAVLINK_MSG_ID_SYSTEM_TIME:
+		handle_message_system_time(msg);
+		break;
+
+	case MAVLINK_MSG_ID_TIMESYNC:
+		handle_message_timesync(msg);
+		break;
+
 	default:
 		break;
 	}
@@ -250,7 +264,7 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg)
 		//check for MAVLINK terminate command
 		if (cmd_mavlink.command == MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN && ((int)cmd_mavlink.param1) == 3) {
 			/* This is the link shutdown command, terminate mavlink */
-			warnx("terminated by remote command");
+			warnx("terminated by remote");
 			fflush(stdout);
 			usleep(50000);
 
@@ -260,7 +274,7 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg)
 		} else {
 
 			if (msg->sysid == mavlink_system.sysid && msg->compid == mavlink_system.compid) {
-				warnx("ignoring CMD spoofed with same SYS/COMP (%d/%d) ID",
+				warnx("ignoring CMD with same SYS/COMP (%d/%d) ID",
 				      mavlink_system.sysid, mavlink_system.compid);
 				return;
 			}
@@ -306,7 +320,7 @@ MavlinkReceiver::handle_message_command_int(mavlink_message_t *msg)
 		//check for MAVLINK terminate command
 		if (cmd_mavlink.command == MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN && ((int)cmd_mavlink.param1) == 3) {
 			/* This is the link shutdown command, terminate mavlink */
-			warnx("terminated by remote command");
+			warnx("terminated by remote");
 			fflush(stdout);
 			usleep(50000);
 
@@ -316,7 +330,7 @@ MavlinkReceiver::handle_message_command_int(mavlink_message_t *msg)
 		} else {
 
 			if (msg->sysid == mavlink_system.sysid && msg->compid == mavlink_system.compid) {
-				warnx("ignoring CMD spoofed with same SYS/COMP (%d/%d) ID",
+				warnx("ignoring CMD with same SYS/COMP (%d/%d) ID",
 				      mavlink_system.sysid, mavlink_system.compid);
 				return;
 			}
@@ -351,24 +365,34 @@ MavlinkReceiver::handle_message_command_int(mavlink_message_t *msg)
 }
 
 void
-MavlinkReceiver::handle_message_optical_flow(mavlink_message_t *msg)
+MavlinkReceiver::handle_message_optical_flow_rad(mavlink_message_t *msg)
 {
 	/* optical flow */
-	mavlink_optical_flow_t flow;
-	mavlink_msg_optical_flow_decode(msg, &flow);
+	mavlink_optical_flow_rad_t flow;
+	mavlink_msg_optical_flow_rad_decode(msg, &flow);
+
+	enum Rotation flow_rot;
+	param_get(param_find("SENS_FLOW_ROT"),&flow_rot);
 
 	struct optical_flow_s f;
 	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;
-	f.flow_comp_y_m = flow.flow_comp_m_y;
-	f.ground_distance_m = flow.ground_distance;
+	f.timestamp = flow.time_usec;
+	f.integration_timespan = flow.integration_time_us;
+	f.pixel_flow_x_integral = flow.integrated_x;
+	f.pixel_flow_y_integral = flow.integrated_y;
+	f.gyro_x_rate_integral = flow.integrated_xgyro;
+	f.gyro_y_rate_integral = flow.integrated_ygyro;
+	f.gyro_z_rate_integral = flow.integrated_zgyro;
+	f.time_since_last_sonar_update = flow.time_delta_distance_us;
+	f.ground_distance_m = flow.distance;
 	f.quality = flow.quality;
 	f.sensor_id = flow.sensor_id;
+	f.gyro_temperature = flow.temperature;
+
+	/* rotate measurements according to parameter */
+	float zeroval = 0.0f;
+	rotate_3f(flow_rot, f.pixel_flow_x_integral, f.pixel_flow_y_integral, zeroval);
 
 	if (_flow_pub < 0) {
 		_flow_pub = orb_advertise(ORB_ID(optical_flow), &f);
@@ -388,13 +412,18 @@ MavlinkReceiver::handle_message_hil_optical_flow(mavlink_message_t *msg)
 	struct optical_flow_s f;
 	memset(&f, 0, sizeof(f));
 
-	f.timestamp = hrt_absolute_time();
-	f.flow_timestamp = flow.time_usec;
-	f.flow_raw_x = flow.integrated_x;
-	f.flow_raw_y = flow.integrated_y;
+	f.timestamp = hrt_absolute_time(); // XXX we rely on the system time for now and not flow.time_usec;
+	f.integration_timespan = flow.integration_time_us;
+	f.pixel_flow_x_integral = flow.integrated_x;
+	f.pixel_flow_y_integral = flow.integrated_y;
+	f.gyro_x_rate_integral = flow.integrated_xgyro;
+	f.gyro_y_rate_integral = flow.integrated_ygyro;
+	f.gyro_z_rate_integral = flow.integrated_zgyro;
+	f.time_since_last_sonar_update = flow.time_delta_distance_us;
 	f.ground_distance_m = flow.distance;
 	f.quality = flow.quality;
 	f.sensor_id = flow.sensor_id;
+	f.gyro_temperature = flow.temperature;
 
 	if (_flow_pub < 0) {
 		_flow_pub = orb_advertise(ORB_ID(optical_flow), &f);
@@ -405,7 +434,7 @@ MavlinkReceiver::handle_message_hil_optical_flow(mavlink_message_t *msg)
 
 	/* Use distance value for range finder report */
 	struct range_finder_report r;
-	memset(&r, 0, sizeof(f));
+	memset(&r, 0, sizeof(r));
 
 	r.timestamp = hrt_absolute_time();
 	r.error_count = 0;
@@ -537,12 +566,16 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
 			offboard_control_sp.ignore &=  ~(1 << i);
 			offboard_control_sp.ignore |=  (set_position_target_local_ned.type_mask & (1 << i));
 		}
+
 		offboard_control_sp.ignore &=  ~(1 << OFB_IGN_BIT_YAW);
-			offboard_control_sp.ignore |=  (set_position_target_local_ned.type_mask & (1 << 10)) <<
-					OFB_IGN_BIT_YAW;
+		if (set_position_target_local_ned.type_mask & (1 << 10)) {
+			offboard_control_sp.ignore |=  (1 << OFB_IGN_BIT_YAW);
+		}
+
 		offboard_control_sp.ignore &=  ~(1 << OFB_IGN_BIT_YAWRATE);
-			offboard_control_sp.ignore |=  (set_position_target_local_ned.type_mask & (1 << 11)) <<
-					OFB_IGN_BIT_YAWRATE;
+		if (set_position_target_local_ned.type_mask & (1 << 11)) {
+			offboard_control_sp.ignore |=  (1 << OFB_IGN_BIT_YAWRATE);
+		}
 
 		offboard_control_sp.timestamp = hrt_absolute_time();
 
@@ -676,7 +709,7 @@ MavlinkReceiver::handle_message_vision_position_estimate(mavlink_message_t *msg)
 	// Use the component ID to identify the vision sensor
 	vision_position.id = msg->compid;
 
-	vision_position.timestamp_boot = hrt_absolute_time();
+	vision_position.timestamp_boot = to_hrt(pos.usec); // Synced time
 	vision_position.timestamp_computer = pos.usec;
 	vision_position.x = pos.x;
 	vision_position.y = pos.y;
@@ -848,7 +881,7 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg)
 	manual.r = man.r / 1000.0f;
 	manual.z = man.z / 1000.0f;
 
-	warnx("pitch: %.2f, roll: %.2f, yaw: %.2f, throttle: %.2f", (double)manual.x, (double)manual.y, (double)manual.r, (double)manual.z);
+	// warnx("pitch: %.2f, roll: %.2f, yaw: %.2f, throttle: %.2f", (double)manual.x, (double)manual.y, (double)manual.r, (double)manual.z);
 
 	if (_manual_pub < 0) {
 		_manual_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual);
@@ -906,6 +939,66 @@ MavlinkReceiver::handle_message_request_data_stream(mavlink_message_t *msg)
 }
 
 void
+MavlinkReceiver::handle_message_system_time(mavlink_message_t *msg)
+{
+	mavlink_system_time_t time;
+	mavlink_msg_system_time_decode(msg, &time);
+
+	timespec tv;
+	clock_gettime(CLOCK_REALTIME, &tv);
+
+	// date -d @1234567890: Sat Feb 14 02:31:30 MSK 2009
+	bool onb_unix_valid = tv.tv_sec > PX4_EPOCH_SECS;
+	bool ofb_unix_valid = time.time_unix_usec > PX4_EPOCH_SECS * 1000ULL;
+
+	if (!onb_unix_valid && ofb_unix_valid) {
+		tv.tv_sec = time.time_unix_usec / 1000000ULL;
+		tv.tv_nsec = (time.time_unix_usec % 1000000ULL) * 1000ULL;
+		if(clock_settime(CLOCK_REALTIME, &tv)) {
+			warn("failed setting clock");
+		}
+		else {
+		warnx("[timesync] UTC time synced.");
+		}
+	}
+
+}
+
+void
+MavlinkReceiver::handle_message_timesync(mavlink_message_t *msg)
+{
+	mavlink_timesync_t tsync;
+	mavlink_msg_timesync_decode(msg, &tsync);
+
+	uint64_t now_ns = hrt_absolute_time() * 1000LL ;
+
+	if (tsync.tc1 == 0) {
+
+		mavlink_timesync_t rsync; // return timestamped sync message
+
+		rsync.tc1 = now_ns;
+		rsync.ts1 = tsync.ts1;
+
+		_mavlink->send_message(MAVLINK_MSG_ID_TIMESYNC, &rsync);
+
+		return;
+
+	} else if (tsync.tc1 > 0) {
+
+		int64_t offset_ns = (tsync.ts1 + now_ns - tsync.tc1*2)/2 ; 
+		int64_t dt = _time_offset - offset_ns;
+
+		if (dt > 10000000LL || dt < -10000000LL) { // 10 millisecond skew 
+			_time_offset = offset_ns;
+			warnx("[timesync] Hard setting offset.");
+		} else {
+			smooth_time_offset(offset_ns);
+		}
+	}
+
+}
+
+void
 MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
 {
 	mavlink_hil_sensor_t imu;
@@ -949,10 +1042,10 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
 		gyro.temperature = imu.temperature;
 
 		if (_gyro_pub < 0) {
-			_gyro_pub = orb_advertise(ORB_ID(sensor_gyro0), &gyro);
+			_gyro_pub = orb_advertise(ORB_ID(sensor_gyro), &gyro);
 
 		} else {
-			orb_publish(ORB_ID(sensor_gyro0), _gyro_pub, &gyro);
+			orb_publish(ORB_ID(sensor_gyro), _gyro_pub, &gyro);
 		}
 	}
 
@@ -971,10 +1064,10 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
 		accel.temperature = imu.temperature;
 
 		if (_accel_pub < 0) {
-			_accel_pub = orb_advertise(ORB_ID(sensor_accel0), &accel);
+			_accel_pub = orb_advertise(ORB_ID(sensor_accel), &accel);
 
 		} else {
-			orb_publish(ORB_ID(sensor_accel0), _accel_pub, &accel);
+			orb_publish(ORB_ID(sensor_accel), _accel_pub, &accel);
 		}
 	}
 
@@ -992,10 +1085,11 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
 		mag.z = imu.zmag;
 
 		if (_mag_pub < 0) {
-			_mag_pub = orb_advertise(ORB_ID(sensor_mag0), &mag);
+			/* publish to the first mag topic */
+			_mag_pub = orb_advertise(ORB_ID(sensor_mag), &mag);
 
 		} else {
-			orb_publish(ORB_ID(sensor_mag0), _mag_pub, &mag);
+			orb_publish(ORB_ID(sensor_mag), _mag_pub, &mag);
 		}
 	}
 
@@ -1010,10 +1104,10 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
 		baro.temperature = imu.temperature;
 
 		if (_baro_pub < 0) {
-			_baro_pub = orb_advertise(ORB_ID(sensor_baro0), &baro);
+			_baro_pub = orb_advertise(ORB_ID(sensor_baro), &baro);
 
 		} else {
-			orb_publish(ORB_ID(sensor_baro0), _baro_pub, &baro);
+			orb_publish(ORB_ID(sensor_baro), _baro_pub, &baro);
 		}
 	}
 
@@ -1097,7 +1191,7 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
 
 	/* print HIL sensors rate */
 	if ((timestamp - _old_timestamp) > 10000000) {
-		printf("receiving HIL sensors at %d hz\n", _hil_frames / 10);
+		// printf("receiving HIL sensors at %d hz\n", _hil_frames / 10);
 		_old_timestamp = timestamp;
 		_hil_frames = 0;
 	}
@@ -1115,7 +1209,7 @@ MavlinkReceiver::handle_message_hil_gps(mavlink_message_t *msg)
 	memset(&hil_gps, 0, sizeof(hil_gps));
 
 	hil_gps.timestamp_time = timestamp;
-	hil_gps.time_gps_usec = gps.time_usec;
+	hil_gps.time_utc_usec = gps.time_usec;
 
 	hil_gps.timestamp_position = timestamp;
 	hil_gps.lat = gps.lat;
@@ -1261,9 +1355,6 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
 		hil_local_pos.xy_global = true;
 		hil_local_pos.z_global = true;
 
-		bool landed = (float)(hil_state.alt) / 1000.0f < (_hil_local_alt0 + 0.1f); // XXX improve?
-		hil_local_pos.landed = landed;
-
 		if (_local_pos_pub < 0) {
 			_local_pos_pub = orb_advertise(ORB_ID(vehicle_local_position), &hil_local_pos);
 
@@ -1272,6 +1363,22 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
 		}
 	}
 
+	/* land detector */
+	{
+		bool landed = (float)(hil_state.alt) / 1000.0f < (_hil_local_alt0 + 0.1f); // XXX improve?
+		if(hil_land_detector.landed != landed) {
+			hil_land_detector.landed = landed;
+			hil_land_detector.timestamp = hrt_absolute_time();
+
+			if (_land_detector_pub < 0) {
+				_land_detector_pub = orb_advertise(ORB_ID(vehicle_land_detected), &hil_land_detector);
+
+			} else {
+				orb_publish(ORB_ID(vehicle_land_detected), _land_detector_pub, &hil_land_detector);
+			}
+		}
+	}
+
 	/* accelerometer */
 	{
 		struct accel_report accel;
@@ -1287,10 +1394,10 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
 		accel.temperature = 25.0f;
 
 		if (_accel_pub < 0) {
-			_accel_pub = orb_advertise(ORB_ID(sensor_accel0), &accel);
+			_accel_pub = orb_advertise(ORB_ID(sensor_accel), &accel);
 
 		} else {
-			orb_publish(ORB_ID(sensor_accel0), _accel_pub, &accel);
+			orb_publish(ORB_ID(sensor_accel), _accel_pub, &accel);
 		}
 	}
 
@@ -1377,6 +1484,23 @@ void MavlinkReceiver::print_status()
 
 }
 
+uint64_t MavlinkReceiver::to_hrt(uint64_t usec)
+{
+	return usec - (_time_offset / 1000) ;
+}
+
+
+void MavlinkReceiver::smooth_time_offset(uint64_t offset_ns)
+{
+	/* alpha = 0.6 fixed for now. The closer alpha is to 1.0,
+         * the faster the moving average updates in response to
+	 * new offset samples.
+	 */
+
+ 	_time_offset = (_time_offset_avg_alpha * offset_ns) + (1.0 - _time_offset_avg_alpha) * _time_offset;
+}
+
+
 void *MavlinkReceiver::start_helper(void *context)
 {
 	MavlinkReceiver *rcv = new MavlinkReceiver((Mavlink *)context);
diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h
index e5f2c6a73..699996860 100644
--- a/src/modules/mavlink/mavlink_receiver.h
+++ b/src/modules/mavlink/mavlink_receiver.h
@@ -50,6 +50,7 @@
 #include <uORB/topics/vehicle_gps_position.h>
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/vehicle_local_position.h>
+#include <uORB/topics/vehicle_land_detected.h>
 #include <uORB/topics/home_position.h>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/offboard_control_setpoint.h>
@@ -75,6 +76,8 @@
 
 #include "mavlink_ftp.h"
 
+#define PX4_EPOCH_SECS 1234567890ULL
+
 class Mavlink;
 
 class MavlinkReceiver
@@ -112,7 +115,7 @@ private:
 	void handle_message(mavlink_message_t *msg);
 	void handle_message_command_long(mavlink_message_t *msg);
 	void handle_message_command_int(mavlink_message_t *msg);
-	void handle_message_optical_flow(mavlink_message_t *msg);
+	void handle_message_optical_flow_rad(mavlink_message_t *msg);
 	void handle_message_hil_optical_flow(mavlink_message_t *msg);
 	void handle_message_set_mode(mavlink_message_t *msg);
 	void handle_message_vicon_position_estimate(mavlink_message_t *msg);
@@ -124,14 +127,26 @@ private:
 	void handle_message_manual_control(mavlink_message_t *msg);
 	void handle_message_heartbeat(mavlink_message_t *msg);
 	void handle_message_request_data_stream(mavlink_message_t *msg);
+	void handle_message_system_time(mavlink_message_t *msg);
+	void handle_message_timesync(mavlink_message_t *msg);
 	void handle_message_hil_sensor(mavlink_message_t *msg);
 	void handle_message_hil_gps(mavlink_message_t *msg);
 	void handle_message_hil_state_quaternion(mavlink_message_t *msg);
 
 	void *receive_thread(void *arg);
 
+	/**
+	* Convert remote nsec timestamp to local hrt time (usec)
+	*/
+	uint64_t to_hrt(uint64_t nsec);
+	/**
+	* Exponential moving average filter to smooth time offset
+	*/
+	void smooth_time_offset(uint64_t offset_ns);	
+
 	mavlink_status_t status;
 	struct vehicle_local_position_s hil_local_pos;
+	struct vehicle_land_detected_s hil_land_detector;
 	struct vehicle_control_mode_s _control_mode;
 	orb_advert_t _global_pos_pub;
 	orb_advert_t _local_pos_pub;
@@ -158,12 +173,15 @@ private:
 	orb_advert_t _telemetry_status_pub;
 	orb_advert_t _rc_pub;
 	orb_advert_t _manual_pub;
+	orb_advert_t _land_detector_pub;
 	int _control_mode_sub;
 	int _hil_frames;
 	uint64_t _old_timestamp;
 	bool _hil_local_proj_inited;
 	float _hil_local_alt0;
 	struct map_projection_reference_s _hil_local_proj_ref;
+	double _time_offset_avg_alpha;
+	uint64_t _time_offset;
 
 	/* do not allow copying this class */
 	MavlinkReceiver(const MavlinkReceiver&);
diff --git a/src/modules/mavlink/mavlink_tests/module.mk b/src/modules/mavlink/mavlink_tests/module.mk
index 1cc28cce1..b46d2bd35 100644
--- a/src/modules/mavlink/mavlink_tests/module.mk
+++ b/src/modules/mavlink/mavlink_tests/module.mk
@@ -47,4 +47,6 @@ MODULE_STACKSIZE = 5000
 
 MAXOPTIMIZATION	 = -Os
 
-EXTRACXXFLAGS	= -Weffc++ -DMAVLINK_FTP_UNIT_TEST
+EXTRACXXFLAGS	= -Weffc++ -DMAVLINK_FTP_UNIT_TEST -Wno-attributes -Wno-packed
+
+EXTRACFLAGS	= -Wno-packed
diff --git a/src/modules/mavlink/module.mk b/src/modules/mavlink/module.mk
index 91fdd6154..f9d30dcbe 100644
--- a/src/modules/mavlink/module.mk
+++ b/src/modules/mavlink/module.mk
@@ -53,4 +53,6 @@ MAXOPTIMIZATION	 = -Os
 
 MODULE_STACKSIZE = 1024
 
-EXTRACXXFLAGS	= -Weffc++
+EXTRACXXFLAGS	= -Weffc++ -Wno-attributes -Wno-packed
+
+EXTRACFLAGS	= -Wno-packed