mavlink : add advanced timesync algorithm

msg/CMakeLists.txt

@@ -103,7 +103,7 @@ set(msg_files
 	tecs_status.msg
 	telemetry_status.msg
 	test_motor.msg
-	time_offset.msg
+	timesync_status.msg
 	transponder_report.msg
 	tune_control.msg
 	uavcan_parameter_request.msg

msg/time_offset.msg

@@ -1 +0,0 @@
-uint64 offset_ns	# time offset between companion system and PX4, in nanoseconds

msg/timesync_status.msg

@@ -0,0 +1,4 @@
+uint64 remote_timestamp		# remote system timestamp (microseconds)
+int64 observed_offset		# raw time offset directly observed from this timesync packet (microseconds)
+int64 estimated_offset		# smoothed time offset between companion system and PX4 (microseconds)
+uint32 round_trip_time		# round trip time of this timesync packet (microseconds)

src/modules/mavlink/CMakeLists.txt

@@ -60,6 +60,7 @@ px4_add_module(
 		mavlink_simple_analyzer.cpp
 		mavlink_stream.cpp
 		mavlink_ulog.cpp
+		mavlink_timesync.cpp
 	DEPENDS
 		git_mavlink_v2
 		conversion

src/modules/mavlink/mavlink_receiver.cpp

@@ -96,6 +96,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
 	_parameters_manager(parent),
 	_mavlink_ftp(parent),
 	_mavlink_log_handler(parent),
+	_mavlink_timesync(parent),
 	_status{},
 	_hil_local_pos{},
 	_hil_land_detector{},
@@ -128,7 +129,6 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
 	_manual_pub(nullptr),
 	_obstacle_distance_pub(nullptr),
 	_land_detector_pub(nullptr),
-	_time_offset_pub(nullptr),
 	_follow_target_pub(nullptr),
 	_landing_target_pose_pub(nullptr),
 	_transponder_report_pub(nullptr),
@@ -147,8 +147,6 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
 	_hil_local_alt0(0.0f),
 	_hil_local_proj_ref{},
 	_offboard_control_mode{},
-	_time_offset_avg_alpha(0.8),
-	_time_offset(0),
 	_orb_class_instance(-1),
 	_mom_switch_pos{},
 	_mom_switch_state(0),
@@ -285,14 +283,6 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 		handle_message_heartbeat(msg);
 		break;
 
-	case MAVLINK_MSG_ID_SYSTEM_TIME:
-		handle_message_system_time(msg);
-		break;
-
-	case MAVLINK_MSG_ID_TIMESYNC:
-		handle_message_timesync(msg);
-		break;
-
 	case MAVLINK_MSG_ID_DISTANCE_SENSOR:
 		handle_message_distance_sensor(msg);
 		break;
@@ -812,7 +802,7 @@ MavlinkReceiver::handle_message_att_pos_mocap(mavlink_message_t *msg)
 	// Use the component ID to identify the mocap system
 	att_pos_mocap.id = msg->compid;
 
-	att_pos_mocap.timestamp = sync_stamp(mocap.time_usec);
+	att_pos_mocap.timestamp = _mavlink_timesync.sync_stamp(mocap.time_usec);
 	att_pos_mocap.timestamp_received = hrt_absolute_time();
 
 	att_pos_mocap.q[0] = mocap.q[0];
@@ -1106,7 +1096,7 @@ MavlinkReceiver::handle_message_attitude_quaternion_cov(mavlink_message_t *msg)
 
 	struct vehicle_attitude_s vision_attitude = {};
 
-	vision_attitude.timestamp = sync_stamp(att.time_usec);
+	vision_attitude.timestamp = _mavlink_timesync.sync_stamp(att.time_usec);
 
 	vision_attitude.q[0] = att.q[0];
 	vision_attitude.q[1] = att.q[1];
@@ -1132,7 +1122,7 @@ MavlinkReceiver::handle_message_local_position_ned_cov(mavlink_message_t *msg)
 
 	struct vehicle_local_position_s vision_position = {};
 
-	vision_position.timestamp = sync_stamp(pos.time_usec);
+	vision_position.timestamp = _mavlink_timesync.sync_stamp(pos.time_usec);
 
 	vision_position.xy_valid = true;
 	vision_position.z_valid = true;
@@ -1192,7 +1182,7 @@ MavlinkReceiver::handle_message_vision_position_estimate(mavlink_message_t *msg)
 
 	struct vehicle_local_position_s vision_position = {};
 
-	vision_position.timestamp = sync_stamp(pos.usec);
+	vision_position.timestamp = _mavlink_timesync.sync_stamp(pos.usec);
 	vision_position.x = pos.x;
 	vision_position.y = pos.y;
 	vision_position.z = pos.z;
@@ -1204,7 +1194,7 @@ MavlinkReceiver::handle_message_vision_position_estimate(mavlink_message_t *msg)
 
 	struct vehicle_attitude_s vision_attitude = {};
 
-	vision_attitude.timestamp = sync_stamp(pos.usec);
+	vision_attitude.timestamp = _mavlink_timesync.sync_stamp(pos.usec);
 
 	matrix::Quatf q(matrix::Eulerf(pos.roll, pos.pitch, pos.yaw));
 	q.copyTo(vision_attitude.q);
@@ -1793,80 +1783,6 @@ MavlinkReceiver::get_message_interval(int msgId)
 	mavlink_msg_message_interval_send(_mavlink->get_channel(), msgId, interval);
 }
 
-void
-MavlinkReceiver::handle_message_system_time(mavlink_message_t *msg)
-{
-	mavlink_system_time_t time;
-	mavlink_msg_system_time_decode(msg, &time);
-
-	timespec tv = {};
-	px4_clock_gettime(CLOCK_REALTIME, &tv);
-
-	// date -d @1234567890: Sat Feb 14 02:31:30 MSK 2009
-	bool onb_unix_valid = (unsigned long long)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 (px4_clock_settime(CLOCK_REALTIME, &tv)) {
-			PX4_ERR("failed setting clock");
-		}
-	}
-
-}
-
-void
-MavlinkReceiver::handle_message_timesync(mavlink_message_t *msg)
-{
-	mavlink_timesync_t tsync = {};
-	mavlink_msg_timesync_decode(msg, &tsync);
-
-	struct time_offset_s tsync_offset = {};
-
-	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_msg_timesync_send_struct(_mavlink->get_channel(), &rsync);
-
-		return;
-
-	} else if (tsync.tc1 > 0) {
-
-		int64_t offset_ns = (int64_t)(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;
-
-			// Provide a warning only if not syncing initially
-			if (_time_offset != 0) {
-				PX4_ERR("[timesync] Hard setting offset.");
-			}
-
-		} else {
-			smooth_time_offset(offset_ns);
-		}
-	}
-
-	tsync_offset.offset_ns = _time_offset ;
-
-	if (_time_offset_pub == nullptr) {
-		_time_offset_pub = orb_advertise(ORB_ID(time_offset), &tsync_offset);
-
-	} else {
-		orb_publish(ORB_ID(time_offset), _time_offset_pub, &tsync_offset);
-	}
-
-}
-
 void
 MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
 {
@@ -2074,7 +1990,7 @@ void MavlinkReceiver::handle_message_landing_target(mavlink_message_t *msg)
 
 	if (landing_target.position_valid && landing_target.frame == MAV_FRAME_LOCAL_NED) {
 		landing_target_pose_s landing_target_pose = {};
-		landing_target_pose.timestamp = sync_stamp(landing_target.time_usec);
+		landing_target_pose.timestamp = _mavlink_timesync.sync_stamp(landing_target.time_usec);
 		landing_target_pose.abs_pos_valid = true;
 		landing_target_pose.x_abs = landing_target.x;
 		landing_target_pose.y_abs = landing_target.y;
@@ -2547,6 +2463,9 @@ MavlinkReceiver::receive_thread(void *arg)
 						/* handle packet with log component */
 						_mavlink_log_handler.handle_message(&msg);
 
+						/* handle packet with timesync component */
+						_mavlink_timesync.handle_message(&msg);
+
 						/* handle packet with parent object */
 						_mavlink->handle_message(&msg);
 					}
@@ -2587,28 +2506,6 @@ void MavlinkReceiver::print_status()
 
 }
 
-uint64_t MavlinkReceiver::sync_stamp(uint64_t usec)
-{
-
-	if (_time_offset != 0) {
-		return usec + (_time_offset / 1000) ;
-
-	} else {
-		return hrt_absolute_time();
-	}
-}
-
-
-void MavlinkReceiver::smooth_time_offset(int64_t offset_ns)
-{
-	/* 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)
 {
 

src/modules/mavlink/mavlink_receiver.h

@@ -71,7 +71,6 @@
 #include <uORB/topics/debug_vect.h>
 #include <uORB/topics/airspeed.h>
 #include <uORB/topics/battery_status.h>
-#include <uORB/topics/time_offset.h>
 #include <uORB/topics/distance_sensor.h>
 #include <uORB/topics/follow_target.h>
 #include <uORB/topics/landing_target_pose.h>
@@ -84,8 +83,7 @@
 #include "mavlink_parameters.h"
 #include "mavlink_ftp.h"
 #include "mavlink_log_handler.h"
-
+#include "mavlink_timesync.h"
-#define PX4_EPOCH_SECS 1234567890ULL
 
 class Mavlink;
 
@@ -143,8 +141,6 @@ private:
 	void handle_message_rc_channels_override(mavlink_message_t *msg);
 	void handle_message_heartbeat(mavlink_message_t *msg);
 	void handle_message_ping(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);
@@ -178,17 +174,6 @@ private:
 	int set_message_interval(int msgId, float interval, int data_rate = -1);
 	void get_message_interval(int msgId);
 
-	/**
-	 * Convert remote timestamp to local hrt time (usec)
-	 * Use timesync if available, monotonic boot time otherwise
-	 */
-	uint64_t sync_stamp(uint64_t usec);
-
-	/**
-	 * Exponential moving average filter to smooth time offset
-	 */
-	void smooth_time_offset(int64_t offset_ns);
-
 	/**
 	 * Decode a switch position from a bitfield
 	 */
@@ -209,6 +194,7 @@ private:
 	MavlinkParametersManager	_parameters_manager;
 	MavlinkFTP			_mavlink_ftp;
 	MavlinkLogHandler		_mavlink_log_handler;
+	MavlinkTimesync		_mavlink_timesync;
 
 	mavlink_status_t _status; ///< receiver status, used for mavlink_parse_char()
 	struct vehicle_local_position_s _hil_local_pos;
@@ -242,7 +228,6 @@ private:
 	orb_advert_t _manual_pub;
 	orb_advert_t _obstacle_distance_pub;
 	orb_advert_t _land_detector_pub;
-	orb_advert_t _time_offset_pub;
 	orb_advert_t _follow_target_pub;
 	orb_advert_t _landing_target_pose_pub;
 	orb_advert_t _transponder_report_pub;
@@ -262,8 +247,6 @@ private:
 	float _hil_local_alt0;
 	struct map_projection_reference_s _hil_local_proj_ref;
 	struct offboard_control_mode_s _offboard_control_mode;
-	double _time_offset_avg_alpha;
-	int64_t _time_offset;
 	int	_orb_class_instance;
 
 	static constexpr unsigned MOM_SWITCH_COUNT = 8;

src/modules/mavlink/mavlink_timesync.cpp

@@ -0,0 +1,256 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2018 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
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file mavlink_timesync.cpp
+ * Mavlink timesync implementation.
+ *
+ * @author Mohammed Kabir <mhkabir98@gmail.com>
+ */
+
+#include "mavlink_timesync.h"
+#include "mavlink_main.h"
+
+MavlinkTimesync::MavlinkTimesync(Mavlink *mavlink) :
+	_timesync_status_pub(nullptr),
+	_sequence(0),
+	_time_offset(0.0),
+	_time_skew(0.0),
+	_filter_alpha(ALPHA_GAIN_INITIAL),
+	_filter_beta(BETA_GAIN_INITIAL),
+	_high_deviation_count(0),
+	_high_rtt_count(0),
+	_mavlink(mavlink)
+{
+}
+MavlinkTimesync::~MavlinkTimesync()
+{
+	if (_timesync_status_pub) {
+		orb_unadvertise(_timesync_status_pub);
+	}
+}
+
+void
+MavlinkTimesync::handle_message(const mavlink_message_t *msg)
+{
+	switch (msg->msgid) {
+	case MAVLINK_MSG_ID_TIMESYNC: {
+
+			mavlink_timesync_t tsync = {};
+			mavlink_msg_timesync_decode(msg, &tsync);
+
+			const uint64_t now = hrt_absolute_time();
+
+			if (tsync.tc1 == 0) {			// Message originating from remote system, timestamp and return it
+
+				mavlink_timesync_t rsync;
+
+				rsync.tc1 = now * 1000ULL;
+				rsync.ts1 = tsync.ts1;
+
+				mavlink_msg_timesync_send_struct(_mavlink->get_channel(), &rsync);
+
+				return;
+
+			} else if (tsync.tc1 > 0) {		// Message originating from this system, compute time offset from it
+
+				// Calculate time offset between this system and the remote system, assuming RTT for
+				// the timesync packet is roughly equal both ways.
+				int64_t offset_us = (int64_t)((tsync.ts1 / 1000ULL) + now - (tsync.tc1 / 1000ULL) * 2) / 2 ;
+
+				// Calculate the round trip time (RTT) it took the timesync packet to bounce back to us from remote system
+				uint64_t rtt_us = now - (tsync.ts1 / 1000ULL);
+
+				// Calculate the difference of this sample from the current estimate
+				uint64_t deviation = llabs((int64_t)_time_offset - offset_us);
+
+				if (rtt_us < MAX_RTT_SAMPLE) {	// Only use samples with low RTT
+
+					if (sync_converged() && (deviation > MAX_DEVIATION_SAMPLE)) {
+
+						// Increment the counter if we have a good estimate and are getting samples far from the estimate
+						_high_deviation_count++;
+
+						// We reset the filter if we received 5 consecutive samples which violate our present estimate.
+						// This is most likely due to a time jump on the offboard system.
+						if (_high_deviation_count > MAX_CONSECUTIVE_HIGH_DEVIATION) {
+							PX4_ERR("[timesync] Time jump detected. Resetting time synchroniser.");
+							// Reset the filter
+							reset_filter();
+						}
+
+					} else {
+
+						// Filter gain scheduling
+						if (!sync_converged()) {
+							// Interpolate with a sigmoid function
+							float progress = ((float)_sequence) / CONVERGENCE_WINDOW;
+							float p = 1.0f - expf(0.5f * (1.0f - 1.0f / (1.0f - progress)));
+							_filter_alpha = p * (float)ALPHA_GAIN_FINAL + (1.0f - p) * (float)ALPHA_GAIN_INITIAL;
+							_filter_beta = p * (float)BETA_GAIN_FINAL + (1.0f - p) * (float)BETA_GAIN_INITIAL;
+
+						} else {
+							_filter_alpha = ALPHA_GAIN_FINAL;
+							_filter_beta = BETA_GAIN_FINAL;
+						}
+
+						// Perform filter update
+						add_sample(offset_us);
+
+						// Increment sequence counter after filter update
+						_sequence++;
+
+						// Reset high deviation count after filter update
+						_high_deviation_count = 0;
+
+						// Reset high RTT count after filter update
+						_high_rtt_count = 0;
+					}
+
+				} else {
+					// Increment counter if round trip time is too high for accurate timesync
+					_high_rtt_count++;
+
+					if (_high_rtt_count > MAX_CONSECUTIVE_HIGH_RTT) {
+						PX4_WARN("[timesync] RTT too high for timesync: %llu ms", rtt_us / 1000ULL);
+						// Reset counter to rate-limit warnings
+						_high_rtt_count = 0;
+					}
+
+				}
+
+				// Publish status message
+				struct timesync_status_s tsync_status = {};
+
+				tsync_status.timestamp = hrt_absolute_time();
+				tsync_status.remote_timestamp = tsync.tc1 / 1000ULL;
+				tsync_status.observed_offset = offset_us;
+				tsync_status.estimated_offset = (int64_t)_time_offset;
+				tsync_status.round_trip_time = rtt_us;
+
+				if (_timesync_status_pub == nullptr) {
+					int instance;
+					_timesync_status_pub = orb_advertise_multi(ORB_ID(timesync_status), &tsync_status, &instance, ORB_PRIO_DEFAULT);
+
+				} else {
+					orb_publish(ORB_ID(timesync_status), _timesync_status_pub, &tsync_status);
+				}
+
+			}
+
+			break;
+		}
+
+	case MAVLINK_MSG_ID_SYSTEM_TIME: {
+
+			mavlink_system_time_t time;
+			mavlink_msg_system_time_decode(msg, &time);
+
+			timespec tv = {};
+			px4_clock_gettime(CLOCK_REALTIME, &tv);
+
+			// date -d @1234567890: Sat Feb 14 02:31:30 MSK 2009
+			bool onb_unix_valid = (unsigned long long)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 (px4_clock_settime(CLOCK_REALTIME, &tv)) {
+					PX4_ERR("[timesync] Failed setting realtime clock");
+				}
+			}
+
+			break;
+		}
+
+	default:
+		break;
+	}
+}
+
+uint64_t
+MavlinkTimesync::sync_stamp(uint64_t usec)
+{
+	// Only return synchronised stamp if we have converged to a good value
+	if (sync_converged()) {
+		return usec + (int64_t)_time_offset;
+
+	} else {
+		return hrt_absolute_time();
+	}
+}
+
+bool
+MavlinkTimesync::sync_converged()
+{
+	return _sequence >= CONVERGENCE_WINDOW;
+}
+
+void
+MavlinkTimesync::add_sample(int64_t offset_us)
+{
+	/* Online exponential smoothing filter. The derivative of the estimate is also
+	 * estimated in order to produce an estimate without steady state lag:
+	 * https://en.wikipedia.org/wiki/Exponential_smoothing#Double_exponential_smoothing
+	 */
+
+	double time_offset_prev = _time_offset;
+
+	if (_sequence == 0) {			// First offset sample
+		_time_offset = offset_us;
+
+	} else {
+		// Update the clock offset estimate
+		_time_offset = _filter_alpha * offset_us + (1.0 - _filter_alpha) * (_time_offset + _time_skew);
+
+		// Update the clock skew estimate
+		_time_skew = _filter_beta * (_time_offset - time_offset_prev) + (1.0 - _filter_beta) * _time_skew;
+	}
+
+}
+
+void
+MavlinkTimesync::reset_filter()
+{
+	// Do a full reset of all statistics and parameters
+	_sequence = 0;
+	_time_offset = 0.0;
+	_time_skew = 0.0;
+	_filter_alpha = ALPHA_GAIN_INITIAL;
+	_filter_beta = BETA_GAIN_INITIAL;
+	_high_deviation_count = 0;
+	_high_rtt_count = 0;
+
+}

src/modules/mavlink/mavlink_timesync.h

@@ -0,0 +1,150 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2018 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
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file mavlink_timesync.h
+ * Mavlink time synchroniser definition.
+ *
+ * @author Mohammed Kabir <mhkabir98@gmail.com>
+ */
+
+#pragma once
+
+#include "mavlink_bridge_header.h"
+
+#include <uORB/uORB.h>
+#include <uORB/topics/timesync_status.h>
+
+#include <drivers/drv_hrt.h>
+
+#include <math.h>
+#include <float.h>
+
+#define PX4_EPOCH_SECS 1234567890ULL
+
+// Filter gains
+//
+// Alpha : Used to smooth the overall clock offset estimate. Smaller values will lead
+// to a smoother estimate, but track time drift more slowly, introducing a bias
+// in the estimate. Larger values will cause low-amplitude oscillations.
+//
+// Beta : Used to smooth the clock skew estimate. Smaller values will lead to a
+// tighter estimation of the skew (derivative), but will negatively affect how fast the
+// filter reacts to clock skewing (e.g cause by temperature changes to the oscillator).
+// Larger values will cause large-amplitude oscillations.
+#define ALPHA_GAIN_INITIAL 				0.05
+#define BETA_GAIN_INITIAL 				0.05
+#define ALPHA_GAIN_FINAL 				0.003
+#define BETA_GAIN_FINAL 				0.003
+
+// Filter gain scheduling
+//
+// The filter interpolates between the INITIAL and FINAL gains while the number of
+// exhanged timesync packets is less than CONVERGENCE_WINDOW. A lower value will
+// allow the timesync to converge faster, but with potentially less accurate initial
+// offset and skew estimates.
+#define CONVERGENCE_WINDOW 				500
+
+// Outlier rejection and filter reset
+//
+// Samples with round-trip time higher than MAX_RTT_SAMPLE are not used to update the filter.
+// More than MAX_CONSECUTIVE_HIGH_RTT number of such events in a row will throw a warning
+// but not reset the filter.
+// Samples whose calculated clock offset is more than MAX_DEVIATION_SAMPLE off from the current
+// estimate are not used to update the filter. More than MAX_CONSECUTIVE_HIGH_DEVIATION number
+// of such events in a row will reset the filter. This usually happens only due to a time jump
+// on the remote system.
+// TODO : automatically determine these using ping statistics?
+#define MAX_RTT_SAMPLE 					10000ULL 	// 10ms
+#define MAX_DEVIATION_SAMPLE 			100000ULL 	// 100ms
+#define MAX_CONSECUTIVE_HIGH_RTT 		5
+#define MAX_CONSECUTIVE_HIGH_DEVIATION 	5
+
+class Mavlink;
+
+class MavlinkTimesync
+{
+public:
+	explicit MavlinkTimesync(Mavlink *mavlink);
+	~MavlinkTimesync();
+
+	void handle_message(const mavlink_message_t *msg);
+
+	/**
+	 * Convert remote timestamp to local hrt time (usec)
+	 * Use synchronised time if available, monotonic boot time otherwise
+	 */
+	uint64_t sync_stamp(uint64_t usec);
+
+private:
+
+	/* do not allow top copying this class */
+	MavlinkTimesync(MavlinkTimesync &);
+	MavlinkTimesync &operator = (const MavlinkTimesync &);
+
+protected:
+
+	/**
+	 * Online exponential filter to smooth time offset
+	 */
+	void add_sample(int64_t offset_us);
+
+	/**
+	 * Return true if the timesync algorithm converged to a good estimate,
+	 * return false otherwise
+	 */
+	bool sync_converged();
+
+	/**
+	 * Reset the exponential filter and its states
+	 */
+	void reset_filter();
+
+	orb_advert_t _timesync_status_pub;
+
+	uint32_t _sequence;
+
+	// Timesync statistics
+	double _time_offset;
+	double _time_skew;
+
+	// Filter parameters
+	double _filter_alpha;
+	double _filter_beta;
+
+	// Outlier rejection and filter reset
+	uint32_t _high_deviation_count;
+	uint32_t _high_rtt_count;
+
+	Mavlink *_mavlink;
+};