[WIP]: GPS clock set time updates

- remove drivers/gps set clock callback - drivers/gps try to set sensor_gps timestamp_sample as closely as possible - move mavlink timesync to new lib/timesync library - sensors/vehicle_gps_position consume all sensor_gps sources and determine time GPS utc time offset using lib/timesync - once time offset is stable update system clock if sufficiently different
2026-05-28 11:00:04 +08:00 · 2022-01-28 13:48:35 -05:00
39 changed files with 599 additions and 428 deletions
@@ -18,6 +18,7 @@ CONFIG_ARMV7M_USEBASEPRI=y
 CONFIG_ARM_MPU_EARLY_RESET=y
 CONFIG_BOARD_LOOPSPERMSEC=2000
 CONFIG_C99_BOOL8=y
+CONFIG_CLOCK_MONOTONIC=y
 CONFIG_DEBUG_FULLOPT=y
 CONFIG_DEBUG_SYMBOLS=y
 CONFIG_DEFAULT_SMALL=y
@@ -18,6 +18,7 @@ CONFIG_ARMV7M_USEBASEPRI=y
 CONFIG_ARM_MPU_EARLY_RESET=y
 CONFIG_BOARD_LOOPSPERMSEC=2000
 CONFIG_C99_BOOL8=y
+CONFIG_CLOCK_MONOTONIC=y
 CONFIG_DEBUG_FULLOPT=y
 CONFIG_DEBUG_SYMBOLS=y
 CONFIG_DEFAULT_SMALL=y
@@ -1,6 +1,7 @@
 # GPS position in WGS84 coordinates.
-# the field 'timestamp' is for the position & velocity (microseconds)
+# the field 'timestamp_sample' is for the position & velocity (microseconds)
 uint64 timestamp		# time since system start (microseconds)
+uint64 timestamp_sample

 uint32 device_id                # unique device ID for the sensor that does not change between power cycles

@@ -31,7 +32,6 @@ float32 vel_d_m_s		# GPS Down velocity, (metres/sec)
 float32 cog_rad			# Course over ground (NOT heading, but direction of movement), -PI..PI, (radians)
 bool vel_ned_valid		# True if NED velocity is valid

-int32 timestamp_time_relative	# timestamp + timestamp_time_relative = Time of the UTC timestamp since system start, (microseconds)
 uint64 time_utc_usec		# Timestamp (microseconds, UTC), this is the timestamp which comes from the gps module. It might be unavailable right after cold start, indicated by a value of 0

 uint8 satellites_used		# Number of satellites used
@@ -288,16 +288,16 @@ private:

 	/** Timesync Status msg Setters **/
@[if version.parse(fastrtps_version) <= version.parse('1.7.2') or not ros2_distro]@
-	inline void setMsgSourceProtocol(timesync_status_msg_t *msg, const uint8_t &source_protocol) { msg->source_protocol_() = source_protocol; }
+	inline void setMsgSourceProtocol(timesync_status_msg_t *msg, const uint8_t &source_protocol) { msg->source_() = source_protocol; }
 	inline void setMsgRemoteTimeStamp(timesync_status_msg_t *msg, const uint64_t &remote_timestamp) { msg->remote_timestamp_() = remote_timestamp; }
 	inline void setMsgObservedOffset(timesync_status_msg_t *msg, const int64_t &observed_offset) { msg->observed_offset_() = observed_offset; }
 	inline void setMsgEstimatedOffset(timesync_status_msg_t *msg, const int64_t &estimated_offset) { msg->estimated_offset_() = estimated_offset; }
-	inline void setMsgRoundTripTime(timesync_status_msg_t *msg, const uint32_t &round_trip_time) { msg->round_trip_time_() = round_trip_time; }
+	//inline void setMsgRoundTripTime(timesync_status_msg_t *msg, const uint32_t &round_trip_time) { msg->round_trip_time_() = round_trip_time; }
@[elif ros2_distro]@
-	inline void setMsgSourceProtocol(timesync_status_msg_t *msg, const uint8_t &source_protocol) { msg->source_protocol() = source_protocol; }
+	inline void setMsgSourceProtocol(timesync_status_msg_t *msg, const uint8_t &source_protocol) { msg->source() = source_protocol; }
 	inline void setMsgRemoteTimeStamp(timesync_status_msg_t *msg, const uint64_t &remote_timestamp) { msg->remote_timestamp() = remote_timestamp; }
 	inline void setMsgObservedOffset(timesync_status_msg_t *msg, const int64_t &observed_offset) { msg->observed_offset() = observed_offset; }
 	inline void setMsgEstimatedOffset(timesync_status_msg_t *msg, const int64_t &estimated_offset) { msg->estimated_offset() = estimated_offset; }
-	inline void setMsgRoundTripTime(timesync_status_msg_t *msg, const uint32_t &round_trip_time) { msg->round_trip_time() = round_trip_time; }
+	//inline void setMsgRoundTripTime(timesync_status_msg_t *msg, const uint32_t &round_trip_time) { msg->round_trip_time() = round_trip_time; }
@[end if]@
 };
@@ -1,10 +1,17 @@
 uint64 timestamp			# time since system start (microseconds)

-uint8 SOURCE_PROTOCOL_MAVLINK = 0
-uint8 SOURCE_PROTOCOL_RTPS = 1
-uint8 source_protocol			# timesync source. Source can be MAVLink or the microRTPS bridge
+uint8 SOURCE_UNKNOWN          = 0
+uint8 SOURCE_PROTOCOL_MAVLINK = 1
+uint8 SOURCE_PROTOCOL_RTPS    = 2
+uint8 SOURCE_GPS1             = 3
+uint8 SOURCE_GPS2             = 4
+uint8 source                            # timesync source

-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)
+
+uint64 deviation
+
+int32 sequence
+
+bool converged
@@ -1,6 +1,7 @@
 # GPS position in WGS84 coordinates.
 # the field 'timestamp' is for the position & velocity (microseconds)
 uint64 timestamp		# time since system start (microseconds)
+uint64 timestamp_sample

 int32 lat			# Latitude in 1E-7 degrees
 int32 lon			# Longitude in 1E-7 degrees
@@ -17,6 +18,8 @@ float32 epv			# GPS vertical position accuracy (metres)
 float32 hdop			# Horizontal dilution of precision
 float32 vdop			# Vertical dilution of precision

+float32 pdop
+
 int32 noise_per_ms		# GPS noise per millisecond
 int32 jamming_indicator		# indicates jamming is occurring
 uint8 jamming_state		# indicates whether jamming has been detected or suspected by the receivers. O: Unknown, 1: OK, 2: Warning, 3: Critical
@@ -28,7 +31,6 @@ float32 vel_d_m_s		# GPS Down velocity, (metres/sec)
 float32 cog_rad			# Course over ground (NOT heading, but direction of movement), -PI..PI, (radians)
 bool vel_ned_valid		# True if NED velocity is valid

-int32 timestamp_time_relative	# timestamp + timestamp_time_relative = Time of the UTC timestamp since system start, (microseconds)
 uint64 time_utc_usec		# Timestamp (microseconds, UTC), this is the timestamp which comes from the gps module. It might be unavailable right after cold start, indicated by a value of 0

 uint8 satellites_used		# Number of satellites used
@@ -72,9 +72,9 @@ static constexpr wq_config_t INS1{"wq:INS1", 6000, -15};
 static constexpr wq_config_t INS2{"wq:INS2", 6000, -16};
 static constexpr wq_config_t INS3{"wq:INS3", 6000, -17};

-static constexpr wq_config_t hp_default{"wq:hp_default", 1900, -18};
+static constexpr wq_config_t hp_default{"wq:hp_default", 1900, -19};

-static constexpr wq_config_t uavcan{"wq:uavcan", 3624, -19};
+static constexpr wq_config_t uavcan{"wq:uavcan", 3624, -20};

 static constexpr wq_config_t ttyS0{"wq:ttyS0", 1632, -21};
 static constexpr wq_config_t ttyS1{"wq:ttyS1", 1632, -22};
@@ -101,11 +101,11 @@ typedef struct {
 #endif

 // PX4 work queue starting high priority
-#define PX4_WQ_HP_BASE (SCHED_PRIORITY_MAX - 15)
+#define PX4_WQ_HP_BASE (SCHED_PRIORITY_MAX - 19)

 // Fast drivers - they need to run as quickly as possible to minimize control
 // latency.
-#define SCHED_PRIORITY_FAST_DRIVER		(SCHED_PRIORITY_MAX - 0)
+#define SCHED_PRIORITY_FAST_DRIVER		(PX4_WQ_HP_BASE + 1)

 // Actuator outputs should run as soon as the rate controller publishes
 // the actuator controls topic
@@ -136,7 +136,7 @@ typedef struct {

 // Slow drivers should run at a rate where they do not impact the overall
 // system execution
-#define SCHED_PRIORITY_SLOW_DRIVER		(PX4_WQ_HP_BASE - 35)
+#define SCHED_PRIORITY_SLOW_DRIVER		(PX4_WQ_HP_BASE - 18)

 // The navigation system needs to execute regularly but has no realtime needs
 #define SCHED_PRIORITY_NAVIGATION		(SCHED_PRIORITY_DEFAULT + 5)
@@ -578,12 +578,27 @@ ts_to_abstime(const struct timespec *ts)
 /**
 * Convert absolute time to a timespec.
 */
-void
-abstime_to_ts(struct timespec *ts, hrt_abstime abstime)
+struct timespec abstime_to_ts(hrt_abstime abstime)
 {
-	ts->tv_sec = abstime / 1000000;
-	abstime -= ts->tv_sec * 1000000;
-	ts->tv_nsec = abstime * 1000;
+	// get offset between hrt and system CLOCK_MONOTONIC
+	struct timespec system_time;
+	irqstate_t flags = enter_critical_section();
+	const hrt_abstime now_us = hrt_absolute_time();
+	clock_gettime(CLOCK_REALTIME, &system_time);
+	leave_critical_section(flags);
+
+	// adjust input abstime with offset to CLOCK_MONOTONIC
+	int64_t system_time_us = (system_time.tv_sec * 1000000) + (system_time.tv_nsec / 1000);
+	int64_t offset_us = system_time_us - now_us;
+
+	uint64_t ts_abstime = abstime + offset_us;
+
+	struct timespec ts;
+	ts.tv_sec = ts_abstime / 1000000;
+	ts_abstime -= ts.tv_sec * 1000000;
+	ts.tv_nsec = ts_abstime * 1000; // microseconds -> nanoseconds
+
+	return ts;
 }

 /**
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2016-2017 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2016-2021 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
@@ -490,14 +490,12 @@ static int
 hrt_tim_isr(int irq, void *context, void *arg)
 {
 	/* grab the timer for latency tracking purposes */
-
 	latency_actual = rCNT;

 	/* copy interrupt status */
 	uint32_t status = rSTATUS;

 	/* ack the interrupts we just read */
-
 	rSTATUS = status;

 #ifdef HRT_PPM_CHANNEL
@@ -579,23 +577,46 @@ hrt_absolute_time(void)
 hrt_abstime
 ts_to_abstime(const struct timespec *ts)
 {
-	hrt_abstime	result;
+	// get offset between hrt and system CLOCK_MONOTONIC
+	struct timespec system_time;
+	irqstate_t flags = enter_critical_section();
+	const hrt_abstime now_us = hrt_absolute_time();
+	clock_gettime(CLOCK_REALTIME, &system_time);
+	leave_critical_section(flags);

-	result = (hrt_abstime)(ts->tv_sec) * 1000000;
-	result += ts->tv_nsec / 1000;
+	// convert input ts to hrt with offset to system CLOCK_MONOTONIC
+	int64_t ts_us = (ts->tv_sec * 1000000) + (ts->tv_nsec / 1000);
+	int64_t system_time_us = (system_time.tv_sec * 1000000) + (system_time.tv_nsec / 1000);

-	return result;
+	int64_t offset_us = system_time_us - now_us;
+
+	return ts_us - offset_us;
 }

 /**
 * Convert absolute time to a timespec.
 */
-void
-abstime_to_ts(struct timespec *ts, hrt_abstime abstime)
+struct timespec abstime_to_ts(hrt_abstime abstime)
 {
-	ts->tv_sec = abstime / 1000000;
-	abstime -= ts->tv_sec * 1000000;
-	ts->tv_nsec = abstime * 1000;
+	// get offset between hrt and system CLOCK_MONOTONIC
+	struct timespec system_time;
+	irqstate_t flags = enter_critical_section();
+	const hrt_abstime now_us = hrt_absolute_time();
+	clock_gettime(CLOCK_REALTIME, &system_time);
+	leave_critical_section(flags);
+
+	// adjust input abstime with offset to CLOCK_MONOTONIC
+	int64_t system_time_us = (system_time.tv_sec * 1000000) + (system_time.tv_nsec / 1000);
+	int64_t offset_us = system_time_us - now_us;
+
+	uint64_t ts_abstime = abstime + offset_us;
+
+	struct timespec ts;
+	ts.tv_sec = ts_abstime / 1000000;
+	ts_abstime -= ts.tv_sec * 1000000;
+	ts.tv_nsec = ts_abstime * 1000; // microseconds -> nanoseconds
+
+	return ts;
 }

 /**
@@ -626,12 +626,27 @@ ts_to_abstime(const struct timespec *ts)
 /**
 * Convert absolute time to a timespec.
 */
-void
-abstime_to_ts(struct timespec *ts, hrt_abstime abstime)
+struct timespec abstime_to_ts(hrt_abstime abstime)
 {
-	ts->tv_sec = abstime / 1000000;
-	abstime -= ts->tv_sec * 1000000;
-	ts->tv_nsec = abstime * 1000;
+	// get offset between hrt and system CLOCK_MONOTONIC
+	struct timespec system_time;
+	irqstate_t flags = enter_critical_section();
+	const hrt_abstime now_us = hrt_absolute_time();
+	clock_gettime(CLOCK_REALTIME, &system_time);
+	leave_critical_section(flags);
+
+	// adjust input abstime with offset to CLOCK_MONOTONIC
+	int64_t system_time_us = (system_time.tv_sec * 1000000) + (system_time.tv_nsec / 1000);
+	int64_t offset_us = system_time_us - now_us;
+
+	uint64_t ts_abstime = abstime + offset_us;
+
+	struct timespec ts;
+	ts.tv_sec = ts_abstime / 1000000;
+	ts_abstime -= ts.tv_sec * 1000000;
+	ts.tv_nsec = ts_abstime * 1000; // microseconds -> nanoseconds
+
+	return ts;
 }

 /**
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2022 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
@@ -605,13 +605,11 @@ error:
 static int
 hrt_tim_isr(int irq, void *context, void *arg)
 {
-	uint32_t status;
-
 	/* grab the timer for latency tracking purposes */
 	latency_actual = rCNT;

 	/* copy interrupt status */
-	status = rSR;
+	uint32_t status = rSR;

 	/* ack the interrupts we just read */
 	rSR = ~status;
@@ -700,23 +698,47 @@ hrt_absolute_time(void)
 hrt_abstime
 ts_to_abstime(const struct timespec *ts)
 {
-	hrt_abstime	result;
+	// get offset between hrt and system CLOCK_MONOTONIC
+	struct timespec system_time;

-	result = (hrt_abstime)(ts->tv_sec) * 1000000;
-	result += ts->tv_nsec / 1000;
+	irqstate_t flags = enter_critical_section();
+	const hrt_abstime now_us = hrt_absolute_time();
+	clock_gettime(CLOCK_REALTIME, &system_time);
+	leave_critical_section(flags);

-	return result;
+	int64_t system_time_us = (system_time.tv_sec * 1000000) + (system_time.tv_nsec / 1000);
+	int64_t offset_us = system_time_us - now_us;
+
+	// convert input ts to hrt with offset to system CLOCK_REALTIME
+	uint64_t ts_us = (ts->tv_sec * 1000000) + (ts->tv_nsec / 1000);
+	return ts_us - offset_us;
 }

 /**
 * Convert absolute time to a timespec.
 */
-void
-abstime_to_ts(struct timespec *ts, hrt_abstime abstime)
+struct timespec abstime_to_ts(hrt_abstime abstime)
 {
-	ts->tv_sec = abstime / 1000000;
-	abstime -= ts->tv_sec * 1000000;
-	ts->tv_nsec = abstime * 1000;
+	// get offset between hrt and system CLOCK_MONOTONIC
+	struct timespec system_time;
+
+	irqstate_t flags = enter_critical_section();
+	const hrt_abstime now_us = hrt_absolute_time();
+	clock_gettime(CLOCK_REALTIME, &system_time);
+	leave_critical_section(flags);
+
+	// adjust input abstime with offset to CLOCK_MONOTONIC
+	int64_t system_time_us = (system_time.tv_sec * 1000000) + (system_time.tv_nsec / 1000);
+	int64_t offset_us = system_time_us - now_us;
+
+	uint64_t ts_abstime = abstime + offset_us;
+
+	struct timespec ts;
+	ts.tv_sec = ts_abstime / 1000000;
+	ts_abstime -= ts.tv_sec * 1000000;
+	ts.tv_nsec = ts_abstime * 1000; // microseconds -> nanoseconds
+
+	return ts;
 }

 /**
@@ -731,7 +753,7 @@ hrt_store_absolute_time(volatile hrt_abstime *t)
 }

 /**
- * Initialise the high-resolution timing module.
+ * Initialize the high-resolution timing module.
 */
 void
 hrt_init(void)
@@ -786,11 +808,11 @@ hrt_call_internal(struct hrt_call *entry, hrt_abstime deadline, hrt_abstime inte
 	irqstate_t flags = px4_enter_critical_section();

 	/* if the entry is currently queued, remove it */
-	/* note that we are using a potentially uninitialised
+	/* note that we are using a potentially uninitialized
 	   entry->link here, but it is safe as sq_rem() doesn't
 	   dereference the passed node unless it is found in the
 	   list. So we potentially waste a bit of time searching the
-	   queue for the uninitialised entry->link but we don't do
+	   queue for the uninitialized entry->link but we don't do
 	   anything actually unsafe.
 	*/
 	if (entry->deadline != 0) {
@@ -484,11 +484,13 @@ hrt_call_invoke()
 	hrt_unlock();
 }

-void abstime_to_ts(struct timespec *ts, hrt_abstime abstime)
+struct timespec abstime_to_ts(hrt_abstime abstime)
 {
-	ts->tv_sec = abstime / 1000000;
-	abstime -= ts->tv_sec * 1000000;
-	ts->tv_nsec = abstime * 1000;
+	struct timespec ts {};
+	ts.tv_sec = abstime / 1000000;
+	abstime -= ts.tv_sec * 1000000;
+	ts.tv_nsec = abstime * 1000;
+	return ts;
 }

 int px4_clock_gettime(clockid_t clk_id, struct timespec *tp)
@@ -496,7 +498,7 @@ int px4_clock_gettime(clockid_t clk_id, struct timespec *tp)
 	if (clk_id == CLOCK_MONOTONIC) {
 #if defined(ENABLE_LOCKSTEP_SCHEDULER)
 		const uint64_t abstime = lockstep_scheduler->get_absolute_time();
-		abstime_to_ts(tp, abstime - px4_timestart_monotonic);
+		*tp = abstime_to_ts(abstime - px4_timestart_monotonic);
 		return 0;
 #else // defined(ENABLE_LOCKSTEP_SCHEDULER)
 #if defined(__PX4_DARWIN)
@@ -102,7 +102,7 @@ __EXPORT extern hrt_abstime ts_to_abstime(const struct timespec *ts);
 /**
 * Convert absolute time to a timespec.
 */
-__EXPORT extern void	abstime_to_ts(struct timespec *ts, hrt_abstime abstime);
+__EXPORT extern struct timespec abstime_to_ts(hrt_abstime abstime);

 /**
 * Compute the delta between a timestamp taken in the past
@@ -38,6 +38,7 @@ px4_add_module(
 	MAIN gps
 	COMPILE_FLAGS
 		-Wno-stringop-overflow # due to https://gcc.gnu.org/bugzilla/show_bug.cgi?id=91707
+		-Wno-error;
 	SRCS
 		gps.cpp
 		devices/src/gps_helper.cpp
@@ -56,9 +56,3 @@
 */
 #define gps_absolute_time hrt_absolute_time
 typedef hrt_abstime gps_abstime;
-
-
-// TODO: this functionality is not available on the Snapdragon yet
-#ifdef __PX4_QURT
-#define NO_MKTIME
-#endif
@@ -365,8 +365,6 @@ int GPS::callback(GPSCallbackType type, void *data1, int data2, void *user)
 {
 	GPS *gps = (GPS *)user;

-	timespec rtc_system_time;
-
 	switch (type) {
 	case GPSCallbackType::readDeviceData: {
 			int timeout;
@@ -395,22 +393,6 @@ int GPS::callback(GPSCallbackType type, void *data1, int data2, void *user)

 	case GPSCallbackType::surveyInStatus:
 		/* not used */
-		break;
-
-	case GPSCallbackType::setClock:
-
-		px4_clock_gettime(CLOCK_REALTIME, &rtc_system_time);
-		timespec rtc_gps_time = *(timespec *)data1;
-		int drift_time = abs(rtc_system_time.tv_sec - rtc_gps_time.tv_sec);
-
-		if (drift_time >= SET_CLOCK_DRIFT_TIME_S) {
-			// as of 2021 setting the time on Nuttx temporarily pauses interrupts
-			// so only set the time if it is very wrong.
-			// TODO: clock slewing of the RTC for small time differences
-			px4_clock_settime(CLOCK_REALTIME, &rtc_gps_time);
-		}
-
-
 		break;
 	}

@@ -1124,7 +1106,7 @@ GPS::publish()
 {
 	if (_instance == Instance::Main || _is_gps_main_advertised.load()) {
 		_report_gps_pos.device_id = get_device_id();
-
+		_report_gps_pos.timestamp = hrt_absolute_time();
 		_report_gps_pos_pub.publish(_report_gps_pos);
 		// Heading/yaw data can be updated at a lower rate than the other navigation data.
 		// The uORB message definition requires this data to be set to a NAN if no new valid data is available.
@@ -1138,6 +1120,7 @@ GPS::publishSatelliteInfo()
 {
 	if (_instance == Instance::Main) {
 		if (_p_report_sat_info != nullptr) {
+			_p_report_sat_info->timestamp = hrt_absolute_time();
 			_report_sat_info_pub.publish(*_p_report_sat_info);
 		}

@@ -1282,7 +1265,7 @@ int GPS::task_spawn(int argc, char *argv[], Instance instance)
 	}

 	int task_id = px4_task_spawn_cmd("gps", SCHED_DEFAULT,
-				   SCHED_PRIORITY_SLOW_DRIVER, TASK_STACK_SIZE,
+				   SCHED_PRIORITY_FAST_DRIVER, TASK_STACK_SIZE,
 				   entry_point, (char *const *)argv);

 	if (task_id < 0) {
@@ -314,7 +314,7 @@ void UavcanGnssBridge::process_fixx(const uavcan::ReceivedDataStructure<FixType>
 	 * to use an independent time source (based on hardware TIM5) instead of HRT.
 	 * The proper solution is to be developed.
 	 */
-	report.timestamp = hrt_absolute_time();
+	report.timestamp_sample = hrt_absolute_time();

 	report.lat           = msg.latitude_deg_1e8 / 10;
 	report.lon           = msg.longitude_deg_1e8 / 10;
@@ -372,8 +372,6 @@ void UavcanGnssBridge::process_fixx(const uavcan::ReceivedDataStructure<FixType>
 	report.cog_rad = atan2f(report.vel_e_m_s, report.vel_n_m_s);
 	report.vel_ned_valid = true;

-	report.timestamp_time_relative = 0;
-
 	const uint64_t gnss_ts_usec = uavcan::UtcTime(msg.gnss_timestamp).toUSec();

 	switch (msg.gnss_time_standard) {
@@ -430,7 +428,7 @@ void UavcanGnssBridge::process_fixx(const uavcan::ReceivedDataStructure<FixType>
 	report.heading = heading;
 	report.heading_offset = heading_offset;
 	report.heading_accuracy = heading_accuracy;
-
+	report.timestamp = hrt_absolute_time();
 	publish(msg.getSrcNodeID().get(), &report);
 }

@@ -59,6 +59,7 @@ void FakeGps::Run()
 	}

 	sensor_gps_s sensor_gps{};
+	sensor_gps.timestamp_sample = hrt_absolute_time();
 	sensor_gps.time_utc_usec = hrt_absolute_time() + 1613692609599954;
 	sensor_gps.lat = _latitude;
 	sensor_gps.lon = _longitude;
@@ -77,7 +78,6 @@ void FakeGps::Run()
 	sensor_gps.vel_e_m_s = 0.0200f;
 	sensor_gps.vel_d_m_s = -0.0570f;
 	sensor_gps.cog_rad = 0.3988f;
-	sensor_gps.timestamp_time_relative = 0;
 	sensor_gps.heading = NAN;
 	sensor_gps.heading_offset = 0.0000;
 	sensor_gps.fix_type = 4;
@@ -66,6 +66,7 @@ add_subdirectory(systemlib)
 add_subdirectory(system_identification)
 add_subdirectory(tecs)
 add_subdirectory(terrain_estimation)
+add_subdirectory(timesync)
 add_subdirectory(tunes)
 add_subdirectory(version)
 add_subdirectory(weather_vane)
@@ -0,0 +1,37 @@
+############################################################################
+#
+#   Copyright (c) 2021 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.
+#
+############################################################################
+
+px4_add_library(timesync
+	Timesync.cpp
+	Timesync.hpp
+)
@@ -0,0 +1,128 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2018-2022 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.
+ *
+ ****************************************************************************/
+
+#include "Timesync.hpp"
+
+#include <stdlib.h>
+
+void Timesync::update(int64_t offset_us)
+{
+	const bool converged = sync_converged();
+
+	// Calculate the difference of this sample from the current estimate
+	uint64_t deviation = llabs((int64_t)_time_offset - offset_us);
+
+	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("Time jump detected. Resetting time synchroniser.");
+			// Reset the filter
+			reset_filter();
+		}
+
+	} else {
+		// Filter gain scheduling
+		if (!sync_converged()) {
+			// Interpolate with a sigmoid function
+			double progress = (double)_sequence / (double)CONVERGENCE_WINDOW;
+			double p = 1. - exp(0.5 * (1. - 1. / (1. - progress)));
+			_filter_alpha = p * ALPHA_GAIN_FINAL + (1. - p) * ALPHA_GAIN_INITIAL;
+			_filter_beta = p * BETA_GAIN_FINAL + (1. - p) * 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;
+	}
+
+	// Publish status message
+	timesync_status_s timesync_status{};
+	timesync_status.observed_offset = offset_us;
+	timesync_status.estimated_offset = (int64_t)round(_time_offset);
+	timesync_status.deviation = deviation;
+	timesync_status.sequence = _sequence;
+	timesync_status.source = _source;
+	timesync_status.converged = sync_converged();
+	timesync_status.timestamp = hrt_absolute_time();
+	_timesync_status_pub.publish(timesync_status);
+
+	if (!converged && sync_converged()) {
+		PX4_DEBUG("new time offset: %.1f", _time_offset);
+	}
+}
+
+void Timesync::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. - _filter_alpha) * (_time_offset + _time_skew);
+
+		// Update the clock skew estimate
+		_time_skew = _filter_beta * (_time_offset - time_offset_prev) + (1. - _filter_beta) * _time_skew;
+	}
+}
+
+void Timesync::reset_filter()
+{
+	PX4_DEBUG("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;
+}
@@ -40,8 +40,6 @@

 #pragma once

-#include "mavlink_bridge_header.h"
-
 #include <uORB/PublicationMulti.hpp>
 #include <uORB/topics/timesync_status.h>

@@ -75,64 +73,63 @@ static constexpr double BETA_GAIN_FINAL = 0.003;
 // 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.
-static constexpr uint32_t CONVERGENCE_WINDOW = 500;
+static constexpr uint32_t CONVERGENCE_WINDOW = 200;

 // 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?
-static constexpr uint64_t MAX_RTT_SAMPLE = 10_ms;
-static constexpr uint64_t MAX_DEVIATION_SAMPLE = 100_ms;
-static constexpr uint32_t MAX_CONSECUTIVE_HIGH_RTT = 5;
-static constexpr uint32_t MAX_CONSECUTIVE_HIGH_DEVIATION = 5;
+static constexpr uint64_t MAX_DEVIATION_SAMPLE = 50_ms;
+static constexpr uint32_t MAX_CONSECUTIVE_HIGH_DEVIATION = 10;

-class Mavlink;
-
-class MavlinkTimesync
+class Timesync
 {
 public:
-	explicit MavlinkTimesync(Mavlink *mavlink);
-	~MavlinkTimesync() = default;
+	Timesync() = default;
+	~Timesync() = default;

-	void handle_message(const mavlink_message_t *msg);
+	void update(int64_t offset_us);
+
+	int64_t time_offset() const { return (int64_t)round(_time_offset); }

 	/**
 	 * Convert remote timestamp to local hrt time (usec)
 	 * Use synchronised time if available, monotonic boot time otherwise
 	 */
-	uint64_t sync_stamp(uint64_t usec);
+	uint64_t sync_stamp(uint64_t usec) const
+	{
+		// Only return synchronised stamp if we have converged to a good value
+		if (sync_converged()) {
+			return usec + time_offset();

-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);
+		} else {
+			return hrt_absolute_time();
+		}
+	}

 	/**
 	 * Return true if the timesync algorithm converged to a good estimate,
 	 * return false otherwise
 	 */
-	bool sync_converged();
+	bool sync_converged() const { return _sequence >= CONVERGENCE_WINDOW; }
+
+	void set_source(uint8_t source) { _source = source; }

 	/**
 	 * Reset the exponential filter and its states
 	 */
 	void reset_filter();

-	uORB::PublicationMulti<timesync_status_s>  _timesync_status_pub{ORB_ID(timesync_status)};
+private:
+	/**
+	 * Online exponential filter to smooth time offset
+	 */
+	void add_sample(int64_t offset_us);
+
+	uORB::PublicationMulti<timesync_status_s> _timesync_status_pub{ORB_ID(timesync_status)};

 	uint32_t _sequence{0};

@@ -146,7 +143,6 @@ protected:

 	// Outlier rejection and filter reset
 	uint32_t _high_deviation_count{0};
-	uint32_t _high_rtt_count{0};

-	Mavlink *const _mavlink;
+	uint8_t _source{timesync_status_s::SOURCE_UNKNOWN};
 };
@@ -1682,7 +1682,7 @@ void EKF2::UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps)
 		}

 		gps_message gps_msg{
-			.time_usec = vehicle_gps_position.timestamp,
+			.time_usec = vehicle_gps_position.timestamp_sample,
 			.lat = vehicle_gps_position.lat,
 			.lon = vehicle_gps_position.lon,
 			.alt = vehicle_gps_position.alt,
@@ -1700,8 +1700,7 @@ void EKF2::UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps)
 			},
 			.vel_ned_valid = vehicle_gps_position.vel_ned_valid,
 			.nsats = vehicle_gps_position.satellites_used,
-			.pdop = sqrtf(vehicle_gps_position.hdop *vehicle_gps_position.hdop
-				      + vehicle_gps_position.vdop * vehicle_gps_position.vdop),
+			.pdop = vehicle_gps_position.pdop,
 		};
 		_ekf.setGpsData(gps_msg);

@@ -123,6 +123,7 @@ void LoggedTopics::add_default_topics()
 	add_topic_multi("control_allocator_status", 200, 2);
 	add_optional_topic_multi("rate_ctrl_status", 200, 2);
 	add_optional_topic_multi("telemetry_status", 1000, 4);
+	add_topic_multi("timesync_status", 1000, 3);

 	// EKF multi topics (currently max 9 estimators)
 #if CONSTRAINED_MEMORY
@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2015-2021 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2015-2022 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
@@ -91,7 +91,6 @@ px4_add_module(
 		mavlink_shell.cpp
 		mavlink_simple_analyzer.cpp
 		mavlink_stream.cpp
-		mavlink_timesync.cpp
 		mavlink_ulog.cpp
 		MavlinkStatustextHandler.cpp
 		tune_publisher.cpp
@@ -87,8 +87,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
 	_mavlink_ftp(parent),
 	_mavlink_log_handler(parent),
 	_mission_manager(parent),
-	_parameters_manager(parent),
-	_mavlink_timesync(parent)
+	_parameters_manager(parent)
 {
 	_handle_sens_flow_maxhgt = param_find("SENS_FLOW_MAXHGT");
 	_handle_sens_flow_maxr = param_find("SENS_FLOW_MAXR");
@@ -304,6 +303,14 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 		handle_message_request_event(msg);
 		break;

+	case MAVLINK_MSG_ID_TIMESYNC:
+		handle_message_timesync(msg);
+		break;
+
+	case MAVLINK_MSG_ID_SYSTEM_TIME:
+		handle_message_system_time(msg);
+		break;
+
 	default:
 		break;
 	}
@@ -967,7 +974,7 @@ MavlinkReceiver::handle_message_att_pos_mocap(mavlink_message_t *msg)
 	vehicle_odometry_s mocap_odom{};

 	mocap_odom.timestamp = hrt_absolute_time();
-	mocap_odom.timestamp_sample = _mavlink_timesync.sync_stamp(mocap.time_usec);
+	mocap_odom.timestamp_sample = _timesync.sync_stamp(mocap.time_usec);

 	mocap_odom.x = mocap.x;
 	mocap_odom.y = mocap.y;
@@ -1350,7 +1357,7 @@ MavlinkReceiver::handle_message_vision_position_estimate(mavlink_message_t *msg)
 	vehicle_odometry_s visual_odom{};

 	visual_odom.timestamp = hrt_absolute_time();
-	visual_odom.timestamp_sample = _mavlink_timesync.sync_stamp(ev.usec);
+	visual_odom.timestamp_sample = _timesync.sync_stamp(ev.usec);

 	visual_odom.x = ev.x;
 	visual_odom.y = ev.y;
@@ -1389,7 +1396,7 @@ MavlinkReceiver::handle_message_odometry(mavlink_message_t *msg)
 	vehicle_odometry_s odometry{};

 	odometry.timestamp = hrt_absolute_time();
-	odometry.timestamp_sample = _mavlink_timesync.sync_stamp(odom.time_usec);
+	odometry.timestamp_sample = _timesync.sync_stamp(odom.time_usec);

 	/* The position is in a local FRD frame */
 	odometry.x = odom.x;
@@ -1927,7 +1934,7 @@ MavlinkReceiver::handle_message_trajectory_representation_bezier(mavlink_message

 	vehicle_trajectory_bezier_s trajectory_bezier{};

-	trajectory_bezier.timestamp =  _mavlink_timesync.sync_stamp(trajectory.time_usec);
+	trajectory_bezier.timestamp =  _timesync.sync_stamp(trajectory.time_usec);

 	for (int i = 0; i < vehicle_trajectory_bezier_s::NUMBER_POINTS; ++i) {
 		trajectory_bezier.control_points[i].position[0] = trajectory.pos_x[i];
@@ -2383,7 +2390,6 @@ MavlinkReceiver::handle_message_hil_gps(mavlink_message_t *msg)
 				hil_gps.cog * 1e-2f))); // cdeg -> rad
 	gps.vel_ned_valid = true;

-	gps.timestamp_time_relative = 0;
 	gps.time_utc_usec = hil_gps.time_usec;

 	gps.satellites_used = hil_gps.satellites_visible;
@@ -2424,7 +2430,7 @@ 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 = _mavlink_timesync.sync_stamp(landing_target.time_usec);
+		landing_target_pose.timestamp = _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;
@@ -2914,6 +2920,82 @@ void MavlinkReceiver::handle_message_statustext(mavlink_message_t *msg)
 	}
 }

+void MavlinkReceiver::handle_message_system_time(mavlink_message_t *msg)
+{
+	mavlink_system_time_t system_time;
+	mavlink_msg_system_time_decode(msg, &system_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 = system_time.time_unix_usec > PX4_EPOCH_SECS * 1000000ULL;
+
+	if (!onb_unix_valid && ofb_unix_valid) {
+
+		const time_t tv_sec = system_time.time_unix_usec / 1000000ULL;
+
+		if (llabs(tv.tv_sec - tv_sec) > 5) {
+			tv.tv_sec = system_time.time_unix_usec / 1000000ULL;
+			tv.tv_nsec = (system_time.time_unix_usec % 1000000ULL) * 1000ULL;
+
+			if (px4_clock_settime(CLOCK_REALTIME, &tv) == OK) {
+				// convert to date time
+				char datetime_buf[40] {};
+				struct tm date_time;
+				time_t utc_time_sec_current = tv.tv_sec + (tv.tv_nsec / 1e9);
+				localtime_r(&utc_time_sec_current, &date_time);
+				strftime(datetime_buf, sizeof(datetime_buf), "%a %Y-%m-%d %H:%M:%S %Z", &date_time);
+				PX4_INFO("[SYSTEM_TIME] setting time: %s", datetime_buf);
+
+			} else {
+				PX4_ERR("[SYSTEM_TIME] Failed setting realtime clock");
+			}
+		}
+	}
+}
+
+void MavlinkReceiver::handle_message_timesync(mavlink_message_t *msg)
+{
+	const hrt_abstime now = hrt_absolute_time();
+
+	mavlink_timesync_t tsync{};
+	mavlink_msg_timesync_decode(msg, &tsync);
+
+	if (tsync.tc1 == 0) {
+		// Message originating from remote system, timestamp and return it
+		mavlink_timesync_t rsync{};
+		rsync.ts1 = tsync.ts1;
+		rsync.tc1 = now * 1000ULL;
+		mavlink_msg_timesync_send_struct(_mavlink->get_channel(), &rsync);
+
+	} else if (tsync.tc1 > 0) {
+		// Message originating from this system, compute time offset from it
+
+		// 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);
+
+		// Samples with round-trip time higher than 10 milliseconds are not used to update the filter.
+		if (rtt_us > 10_ms) {
+			// More than 5 number of such events in a row will throw a warning
+			if (++_high_rtt_count > 5) {
+				PX4_WARN("[timesync] RTT too high for timesync: %llu ms (sender: %i)", rtt_us / 1000ULL, msg->compid);
+				// Reset counter to rate-limit warnings
+				_high_rtt_count = 0;
+			}
+
+		} else {
+			_timesync.set_source(timesync_status_s::SOURCE_PROTOCOL_MAVLINK);
+			// 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;
+
+			_timesync.update(offset_us);
+		}
+	}
+}
+
 void MavlinkReceiver::CheckHeartbeats(const hrt_abstime &t, bool force)
 {
 	// check HEARTBEATs for timeout
@@ -3182,9 +3264,6 @@ MavlinkReceiver::run()
 						/* 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);

@@ -46,7 +46,6 @@
 #include "mavlink_mission.h"
 #include "mavlink_parameters.h"
 #include "MavlinkStatustextHandler.hpp"
-#include "mavlink_timesync.h"
 #include "tune_publisher.h"

 #include <geo/geo.h>
@@ -55,6 +54,7 @@
 #include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
 #include <lib/drivers/magnetometer/PX4Magnetometer.hpp>
 #include <lib/systemlib/mavlink_log.h>
+#include <lib/timesync/Timesync.hpp>
 #include <px4_platform_common/module_params.h>
 #include <uORB/Publication.hpp>
 #include <uORB/PublicationMulti.hpp>
@@ -164,7 +164,9 @@ private:
 	void handle_message_distance_sensor(mavlink_message_t *msg);
 	void handle_message_follow_target(mavlink_message_t *msg);
 	void handle_message_generator_status(mavlink_message_t *msg);
-	void handle_message_set_gps_global_origin(mavlink_message_t *msg);
+	void handle_message_gimbal_device_information(mavlink_message_t *msg);
+	void handle_message_gimbal_manager_set_attitude(mavlink_message_t *msg);
+	void handle_message_gimbal_manager_set_manual_control(mavlink_message_t *msg);
 	void handle_message_gps_rtcm_data(mavlink_message_t *msg);
 	void handle_message_heartbeat(mavlink_message_t *msg);
 	void handle_message_hil_gps(mavlink_message_t *msg);
@@ -184,21 +186,22 @@ private:
 	void handle_message_radio_status(mavlink_message_t *msg);
 	void handle_message_rc_channels(mavlink_message_t *msg);
 	void handle_message_rc_channels_override(mavlink_message_t *msg);
+	void handle_message_request_event(mavlink_message_t *msg);
 	void handle_message_serial_control(mavlink_message_t *msg);
 	void handle_message_set_actuator_control_target(mavlink_message_t *msg);
 	void handle_message_set_attitude_target(mavlink_message_t *msg);
+	void handle_message_set_gps_global_origin(mavlink_message_t *msg);
 	void handle_message_set_mode(mavlink_message_t *msg);
 	void handle_message_set_position_target_global_int(mavlink_message_t *msg);
 	void handle_message_set_position_target_local_ned(mavlink_message_t *msg);
 	void handle_message_statustext(mavlink_message_t *msg);
+	void handle_message_system_time(mavlink_message_t *msg);
+	void handle_message_timesync(mavlink_message_t *msg);
 	void handle_message_tunnel(mavlink_message_t *msg);
 	void handle_message_trajectory_representation_bezier(mavlink_message_t *msg);
 	void handle_message_trajectory_representation_waypoints(mavlink_message_t *msg);
 	void handle_message_utm_global_position(mavlink_message_t *msg);
 	void handle_message_vision_position_estimate(mavlink_message_t *msg);
-	void handle_message_gimbal_manager_set_attitude(mavlink_message_t *msg);
-	void handle_message_gimbal_manager_set_manual_control(mavlink_message_t *msg);
-	void handle_message_gimbal_device_information(mavlink_message_t *msg);

 #if !defined(CONSTRAINED_FLASH)
 	void handle_message_debug(mavlink_message_t *msg);
@@ -206,7 +209,6 @@ private:
 	void handle_message_debug_vect(mavlink_message_t *msg);
 	void handle_message_named_value_float(mavlink_message_t *msg);
 #endif // !CONSTRAINED_FLASH
-	void handle_message_request_event(mavlink_message_t *msg);

 	void CheckHeartbeats(const hrt_abstime &t, bool force = false);

@@ -245,7 +247,8 @@ private:
 	MavlinkLogHandler		_mavlink_log_handler;
 	MavlinkMissionManager		_mission_manager;
 	MavlinkParametersManager	_parameters_manager;
-	MavlinkTimesync			_mavlink_timesync;
+
+	Timesync			_timesync;
 	MavlinkStatustextHandler	_mavlink_statustext_handler;

 	mavlink_status_t		_status{}; ///< receiver status, used for mavlink_parse_char()
@@ -393,6 +396,8 @@ private:
 	hrt_abstime _heartbeat_component_udp_bridge{0};
 	hrt_abstime _heartbeat_component_uart_bridge{0};

+	uint32_t _high_rtt_count{0};
+
 	param_t _handle_sens_flow_maxhgt{PARAM_INVALID};
 	param_t _handle_sens_flow_maxr{PARAM_INVALID};
 	param_t _handle_sens_flow_minhgt{PARAM_INVALID};
@@ -407,6 +412,7 @@ private:
 	float _param_ekf2_min_rng{NAN};
 	float _param_ekf2_rng_a_hmax{NAN};

+
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::BAT_CRIT_THR>)     _param_bat_crit_thr,
 		(ParamFloat<px4::params::BAT_EMERGEN_THR>)  _param_bat_emergen_thr,
@@ -1,238 +0,0 @@
-/****************************************************************************
- *
- *   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"
-
-#include <stdlib.h>
-
-MavlinkTimesync::MavlinkTimesync(Mavlink *mavlink) :
-	_mavlink(mavlink)
-{
-}
-
-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
-							double progress = (double)_sequence / (double)CONVERGENCE_WINDOW;
-							double p = 1.0 - exp(0.5 * (1.0 - 1.0 / (1.0 - progress)));
-							_filter_alpha = p * ALPHA_GAIN_FINAL + (1.0 - p) * ALPHA_GAIN_INITIAL;
-							_filter_beta = p * BETA_GAIN_FINAL + (1.0 - p) * 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 (sender: %i)", rtt_us / 1000ULL, msg->compid);
-						// Reset counter to rate-limit warnings
-						_high_rtt_count = 0;
-					}
-
-				}
-
-				// Publish status message
-				timesync_status_s tsync_status{};
-
-				tsync_status.timestamp = hrt_absolute_time();
-				tsync_status.source_protocol = timesync_status_s::SOURCE_PROTOCOL_MAVLINK;
-				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;
-
-				_timesync_status_pub.publish(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 * 1000000ULL;
-
-			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;
-
-}
@@ -953,8 +953,7 @@ Replay::run()
 		hrt_abstime t = hrt_absolute_time();

 		for (int i = 0; i < 1000; ++i) {
-			struct timespec ts;
-			abstime_to_ts(&ts, t);
+			struct timespec ts = abstime_to_ts(t);
 			px4_clock_settime(CLOCK_MONOTONIC, &ts);
 			t += 10_ms;
 		}
@@ -998,8 +997,7 @@ Replay::handleTopicDelay(uint64_t next_file_time, uint64_t timestamp_offset)
 		}

 		// adjust the lockstep time to the publication time
-		struct timespec ts;
-		abstime_to_ts(&ts, publish_timestamp);
+		struct timespec ts = abstime_to_ts(publish_timestamp);
 		px4_clock_settime(CLOCK_MONOTONIC, &ts);
 	}

@@ -41,6 +41,7 @@ target_link_libraries(vehicle_gps_position
 	PRIVATE
 		geo
 		px4_work_queue
+		timesync
 )

 px4_add_unit_gtest(SRC gps_blending_test.cpp LINKLIBS vehicle_gps_position)
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2022 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
@@ -43,6 +43,8 @@ VehicleGPSPosition::VehicleGPSPosition() :
 	ModuleParams(nullptr),
 	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers)
 {
+	_timesync[0].set_source(timesync_status_s::SOURCE_GPS1);
+	_timesync[1].set_source(timesync_status_s::SOURCE_GPS2);
 }

 VehicleGPSPosition::~VehicleGPSPosition()
@@ -118,8 +120,98 @@ void VehicleGPSPosition::Run()
 		if (gps_updated) {
 			any_gps_updated = true;

-			_sensor_gps_sub[i].copy(&gps_data);
-			_gps_blending.setGpsData(gps_data, i);
+			if (_sensor_gps_sub[i].copy(&gps_data)) {
+
+				const bool sync_converged = _timesync[i].sync_converged();
+
+				if ((gps_data.fix_type >= 3) && (gps_data.time_utc_usec > _gps_utc_time_us[i])) {
+					// Calculate time offset between GPS time and hrt
+					int64_t offset_us = (int64_t)gps_data.time_utc_usec - (int64_t)gps_data.timestamp_sample;
+
+					_timesync[i].update(offset_us);
+
+				} else {
+					_timesync[i].reset_filter();
+				}
+
+				_gps_utc_time_us[i] = gps_data.time_utc_usec;
+
+				if (!sync_converged && _timesync[i].sync_converged()) {
+#if defined(__PX4_NUTTX)
+					auto flags = px4_enter_critical_section();
+#endif // __PX4_NUTTX
+
+					// use converged offset between HRT and GPS utc to update system CLOCK_REALTIME
+					struct timespec ts {};
+					px4_clock_gettime(CLOCK_REALTIME, &ts);
+
+					// convert to date time
+					char datetime_prev[80] {};
+					struct tm date_time;
+					time_t utc_time_sec_current = ts.tv_sec + (ts.tv_nsec / 1e9);
+					localtime_r(&utc_time_sec_current, &date_time);
+					strftime(datetime_prev, sizeof(datetime_prev), "%Y-%m-%d %H:%M:%S %Z", &date_time);
+
+					uint64_t ts_abstime_current_us = ts.tv_sec * 1e6 + ts.tv_nsec / 1000;
+
+					uint64_t ts_abstime_us = hrt_absolute_time() + _timesync[i].time_offset();
+
+					if (llabs((int64_t)ts_abstime_current_us - (int64_t)ts_abstime_us) > (int64_t)10_ms) {
+						struct timespec ts_new {};
+						ts_new.tv_sec = ts_abstime_us / 1000000;
+						ts_abstime_us -= ts_new.tv_sec * 1000000;
+						ts_new.tv_nsec = ts_abstime_us * 1000; // microseconds -> nanoseconds
+
+						// TODO: only update if the difference exceeds threshold
+						if (px4_clock_settime(CLOCK_REALTIME, &ts_new) == 0) {
+							// convert to date time
+							char datetime_new[80] {};
+							time_t utc_time_sec = ts_new.tv_sec + (ts_new.tv_nsec / 1e9);
+							localtime_r(&utc_time_sec, &date_time);
+							strftime(datetime_new, sizeof(datetime_new), "%Y-%m-%d %H:%M:%S %Z", &date_time);
+
+							int64_t old_us = ts.tv_sec * 1e6 + ts.tv_nsec / 1e3;
+							int64_t new_us = ts_new.tv_sec * 1e6 + ts_new.tv_nsec / 1e3;
+
+							PX4_INFO("sensor_gps:%d updated system time: %s-> %s (%" PRIi64 " ms)", i, datetime_prev, datetime_new,
+								 (new_us - old_us) / 1000);
+
+							_gps_time_set = true;
+
+							// reset all
+							for (auto &timesync : _timesync) {
+								timesync.reset_filter();
+							}
+						}
+					}
+
+#if defined(__PX4_NUTTX)
+					px4_leave_critical_section(flags);
+#endif // __PX4_NUTTX
+				}
+
+				// set timestamp_sample from GPS UTC time
+				if (_timesync[i].sync_converged()) {
+					const int64_t time_orig = gps_data.timestamp_sample;
+					const int64_t time_adjusted = gps_data.time_utc_usec - _timesync[i].time_offset();
+
+					// warn if there was a significant change
+					int64_t max_diff_us = (gps_data.timestamp - gps_data.timestamp_sample);
+					int64_t diff = time_adjusted - time_orig;
+
+					if (diff > max_diff_us || diff < -max_diff_us) {
+						PX4_ERR("bad timestamp sample update %" PRIi64 " -> %" PRIi64 " (diff %" PRIi64 "us), resetting",
+							time_orig, time_adjusted, diff);
+
+						_timesync[i].reset_filter();
+
+					} else {
+						gps_data.timestamp_sample = gps_data.time_utc_usec - _timesync[i].time_offset();
+					}
+				}
+
+				_gps_blending.setGpsData(gps_data, i);
+			}

 			if (!_sensor_gps_sub[i].registered()) {
 				_sensor_gps_sub[i].registerCallback();
@@ -142,7 +234,8 @@ void VehicleGPSPosition::Publish(const sensor_gps_s &gps, uint8_t selected)
 {
 	vehicle_gps_position_s gps_output{};

-	gps_output.timestamp = gps.timestamp;
+	gps_output.timestamp_sample = gps.timestamp_sample;
+
 	gps_output.time_utc_usec = gps.time_utc_usec;
 	gps_output.lat = gps.lat;
 	gps_output.lon = gps.lon;
@@ -154,6 +247,7 @@ void VehicleGPSPosition::Publish(const sensor_gps_s &gps, uint8_t selected)
 	gps_output.epv = gps.epv;
 	gps_output.hdop = gps.hdop;
 	gps_output.vdop = gps.vdop;
+	gps_output.pdop = sqrtf(gps.hdop * gps.hdop + gps.vdop * gps.vdop);
 	gps_output.noise_per_ms = gps.noise_per_ms;
 	gps_output.jamming_indicator = gps.jamming_indicator;
 	gps_output.jamming_state = gps.jamming_state;
@@ -162,7 +256,6 @@ void VehicleGPSPosition::Publish(const sensor_gps_s &gps, uint8_t selected)
 	gps_output.vel_e_m_s = gps.vel_e_m_s;
 	gps_output.vel_d_m_s = gps.vel_d_m_s;
 	gps_output.cog_rad = gps.cog_rad;
-	gps_output.timestamp_time_relative = gps.timestamp_time_relative;
 	gps_output.heading = gps.heading;
 	gps_output.heading_offset = gps.heading_offset;
 	gps_output.fix_type = gps.fix_type;
@@ -170,13 +263,20 @@ void VehicleGPSPosition::Publish(const sensor_gps_s &gps, uint8_t selected)
 	gps_output.satellites_used = gps.satellites_used;

 	gps_output.selected = selected;
-
+	gps_output.timestamp = hrt_absolute_time();
 	_vehicle_gps_position_pub.publish(gps_output);
 }

 void VehicleGPSPosition::PrintStatus()
 {
 	//PX4_INFO_RAW("[vehicle_gps_position] selected GPS: %d\n", _gps_select_index);
+
+	for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
+		if (_timesync[i].sync_converged()) {
+
+		}
+	}
+
 }

 }; // namespace sensors
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2021 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
@@ -36,6 +36,7 @@
 #include <lib/mathlib/math/Limits.hpp>
 #include <lib/matrix/matrix/math.hpp>
 #include <lib/perf/perf_counter.h>
+#include <lib/timesync/Timesync.hpp>
 #include <px4_platform_common/log.h>
 #include <px4_platform_common/module_params.h>
 #include <px4_platform_common/px4_config.h>
@@ -90,10 +91,16 @@ private:
 		{this, ORB_ID(sensor_gps), 1},
 	};

+	Timesync _timesync[GPS_MAX_RECEIVERS] {};
+
+	uint64_t _gps_utc_time_us[GPS_MAX_RECEIVERS] {};
+
 	perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};

 	GpsBlending _gps_blending;

+	bool _gps_time_set{false};
+
 	DEFINE_PARAMETERS(
 		(ParamInt<px4::params::SENS_GPS_MASK>) _param_sens_gps_mask,
 		(ParamFloat<px4::params::SENS_GPS_TAU>) _param_sens_gps_tau,
@@ -108,22 +108,22 @@ bool GpsBlending::blend_gps_data(uint64_t hrt_now_us)

 		float raw_dt = 0.f;

-		if (_gps_state[i].timestamp > _time_prev_us[i]) {
-			raw_dt = 1e-6f * (_gps_state[i].timestamp - _time_prev_us[i]);
+		if (_gps_state[i].timestamp_sample > _time_prev_us[i]) {
+			raw_dt = 1e-6f * (_gps_state[i].timestamp_sample - _time_prev_us[i]);
 		}

 		float present_dt = 0.f;

-		if (hrt_now_us > _gps_state[i].timestamp) {
-			present_dt = 1e-6f * (hrt_now_us - _gps_state[i].timestamp);
+		if (hrt_now_us > _gps_state[i].timestamp_sample) {
+			present_dt = 1e-6f * (hrt_now_us - _gps_state[i].timestamp_sample);
 		}

 		if (raw_dt > 0.0f && raw_dt < GPS_TIMEOUT_S) {
 			_gps_dt[i] = 0.1f * raw_dt + 0.9f * _gps_dt[i];

-		} else if ((present_dt >= GPS_TIMEOUT_S) && (_gps_state[i].timestamp > 0)) {
+		} else if ((present_dt >= GPS_TIMEOUT_S) && (_gps_state[i].timestamp_sample > 0)) {
 			// Timed out - kill the stored fix for this receiver and don't track its (stale) gps_dt
-			_gps_state[i].timestamp = 0;
+			_gps_state[i].timestamp_sample = 0;
 			_gps_state[i].fix_type = 0;
 			_gps_state[i].satellites_used = 0;
 			_gps_state[i].vel_ned_valid = 0;
@@ -160,13 +160,13 @@ bool GpsBlending::blend_gps_data(uint64_t hrt_now_us)

 	for (uint8_t i = 0; i < GPS_MAX_RECEIVERS_BLEND; i++) {
 		// Find largest and smallest times
-		if (_gps_state[i].timestamp > max_us) {
-			max_us = _gps_state[i].timestamp;
+		if (_gps_state[i].timestamp_sample > max_us) {
+			max_us = _gps_state[i].timestamp_sample;
 			_gps_newest_index = i;
 		}

-		if ((_gps_state[i].timestamp < min_us) && (_gps_state[i].timestamp > 0)) {
-			min_us = _gps_state[i].timestamp;
+		if ((_gps_state[i].timestamp_sample < min_us) && (_gps_state[i].timestamp_sample > 0)) {
+			min_us = _gps_state[i].timestamp_sample;
 		}
 	}

@@ -197,7 +197,7 @@ bool GpsBlending::blend_gps_data(uint64_t hrt_now_us)
 		// both receivers running at different rates
 		_gps_time_ref_index = _gps_slowest_index;

-		if (_gps_state[_gps_time_ref_index].timestamp > _time_prev_us[_gps_time_ref_index]) {
+		if (_gps_state[_gps_time_ref_index].timestamp_sample > _time_prev_us[_gps_time_ref_index]) {
 			// blend data at the rate of the slower receiver
 			gps_new_output_data = true;
 		}
@@ -360,7 +360,7 @@ sensor_gps_s GpsBlending::gps_blend_states(float blend_weights[GPS_MAX_RECEIVERS
 	// combine the the GPS states into a blended solution using the weights calculated in calc_blend_weights()
 	for (uint8_t i = 0; i < GPS_MAX_RECEIVERS_BLEND; i++) {
 		// blend the timing data
-		gps_blended_state.timestamp += (uint64_t)((double)_gps_state[i].timestamp * (double)blend_weights[i]);
+		gps_blended_state.timestamp_sample += (uint64_t)((double)_gps_state[i].timestamp_sample * (double)blend_weights[i]);

 		// use the highest status
 		if (_gps_state[i].fix_type > gps_blended_state.fix_type) {
@@ -499,12 +499,12 @@ void GpsBlending::update_gps_offsets(const sensor_gps_s &gps_blended_state)
 	float omega_lpf = 1.0f / fmaxf(_blending_time_constant, 1.0f);

 	for (uint8_t i = 0; i < GPS_MAX_RECEIVERS_BLEND; i++) {
-		if (_gps_state[i].timestamp - _time_prev_us[i] > 0) {
+		if (_gps_state[i].timestamp_sample - _time_prev_us[i] > 0) {
 			// calculate the filter coefficient that achieves the time constant specified by the user adjustable parameter
-			alpha[i] = constrain(omega_lpf * 1e-6f * (float)(_gps_state[i].timestamp - _time_prev_us[i]),
+			alpha[i] = constrain(omega_lpf * 1e-6f * (float)(_gps_state[i].timestamp_sample - _time_prev_us[i]),
 					     0.0f, 1.0f);

-			_time_prev_us[i] = _gps_state[i].timestamp;
+			_time_prev_us[i] = _gps_state[i].timestamp_sample;
 		}
 	}

@@ -58,7 +58,7 @@ public:
 sensor_gps_s GpsBlendingTest::getDefaultGpsData()
 {
 	sensor_gps_s gps_data{};
-	gps_data.timestamp = _time_now_us - 10e3;
+	gps_data.timestamp_sample = _time_now_us - 10e3;
 	gps_data.time_utc_usec = 0;
 	gps_data.lat = 47e7;
 	gps_data.lon = 9e7;
@@ -77,7 +77,6 @@ sensor_gps_s GpsBlendingTest::getDefaultGpsData()
 	gps_data.vel_e_m_s = 1.f;
 	gps_data.vel_d_m_s = 1.f;
 	gps_data.cog_rad = 0.f;
-	gps_data.timestamp_time_relative = 0;
 	gps_data.heading = NAN;
 	gps_data.heading_offset = 0.f;
 	gps_data.fix_type = 4;
@@ -97,7 +96,7 @@ void GpsBlendingTest::runSeconds(float duration_s, GpsBlending &gps_blending, se
 		gps_blending.update(_time_now_us);

 		_time_now_us += dt_us;
-		gps_data.timestamp += dt_us;
+		gps_data.timestamp_sample += dt_us;
 	}
 }

@@ -114,8 +113,8 @@ void GpsBlendingTest::runSeconds(float duration_s, GpsBlending &gps_blending, se
 		gps_blending.update(_time_now_us);

 		_time_now_us += dt_us;
-		gps_data0.timestamp += dt_us;
-		gps_data1.timestamp += dt_us;
+		gps_data0.timestamp_sample += dt_us;
+		gps_data1.timestamp_sample += dt_us;
 	}
 }

@@ -143,7 +142,7 @@ TEST_F(GpsBlendingTest, singleReceiver)
 	gps_blending.update(_time_now_us);

 	_time_now_us += 200e3;
-	gps_data.timestamp = _time_now_us - 10e3;
+	gps_data.timestamp_sample = _time_now_us - 10e3;
 	gps_blending.setGpsData(gps_data, 1);
 	gps_blending.update(_time_now_us);

@@ -261,7 +260,7 @@ TEST_F(GpsBlendingTest, dualReceiverFailover)

 	// AND IF: the primary receiver is available again and has
 	// better metrics than the secondary one
-	gps_data0.timestamp = gps_data1.timestamp;
+	gps_data0.timestamp_sample = gps_data1.timestamp_sample;
 	gps_data0.satellites_used = gps_data1.satellites_used + 2;

 	runSeconds(1.f, gps_blending, gps_data0, gps_data1);
@@ -150,7 +150,7 @@ void SensorGpsSim::Run()
 			sensor_gps.vdop = 100.f;
 		}

-		// sensor_gps.timestamp_sample = gpos.timestamp;
+		sensor_gps.timestamp_sample = gpos.timestamp;
 		sensor_gps.time_utc_usec = 0;
 		sensor_gps.device_id = device_id.devid;
 		sensor_gps.lat = roundf(latitude * 1e7); // Latitude in 1E-7 degrees
@@ -165,7 +165,6 @@ void SensorGpsSim::Run()
 		sensor_gps.vel_d_m_s = gps_vel(2);
 		sensor_gps.cog_rad = atan2(gps_vel(1),
 					   gps_vel(0)); // Course over ground (NOT heading, but direction of movement), -PI..PI, (radians)
-		sensor_gps.timestamp_time_relative = 0;
 		sensor_gps.heading = NAN;
 		sensor_gps.heading_offset = NAN;
 		sensor_gps.heading_accuracy = 0;
@@ -173,7 +172,6 @@ void SensorGpsSim::Run()
 		sensor_gps.jamming_state = 0;
 		sensor_gps.vel_ned_valid = true;
 		sensor_gps.satellites_used = _sim_gps_used.get();
-
 		sensor_gps.timestamp = hrt_absolute_time();
 		_sensor_gps_pub.publish(sensor_gps);
 	}
@@ -391,7 +391,7 @@ void Simulator::handle_message_hil_gps(const mavlink_message_t *msg)

 	if (!_gps_blocked) {
 		sensor_gps_s gps{};
-
+		gps.timestamp_sample = hrt_absolute_time();
 		gps.lat = hil_gps.lat;
 		gps.lon = hil_gps.lon;
 		gps.alt = hil_gps.alt;
@@ -419,7 +419,6 @@ void Simulator::handle_message_hil_gps(const mavlink_message_t *msg)
 		gps.cog_rad = ((hil_gps.cog == 65535) ? NAN : matrix::wrap_2pi(math::radians(hil_gps.cog * 1e-2f))); // cdeg -> rad
 		gps.vel_ned_valid = true;

-		gps.timestamp_time_relative = 0;
 		gps.time_utc_usec = hil_gps.time_usec;

 		gps.satellites_used = hil_gps.satellites_visible;
@@ -463,8 +462,7 @@ void Simulator::handle_message_hil_sensor(const mavlink_message_t *msg)
 	mavlink_hil_sensor_t imu;
 	mavlink_msg_hil_sensor_decode(msg, &imu);

-	struct timespec ts;
-	abstime_to_ts(&ts, imu.time_usec);
+	struct timespec ts = abstime_to_ts(imu.time_usec);
 	px4_clock_settime(CLOCK_MONOTONIC, &ts);

 	hrt_abstime now_us = hrt_absolute_time();
@@ -124,4 +124,4 @@ $ system_time get
 	PRINT_MODULE_USAGE_COMMAND_DESCR("set", "Set the system time, provide time in unix epoch time format");
 	PRINT_MODULE_USAGE_COMMAND_DESCR("get", "Get the system time");

-}
+}