[WIP]: GPS clock set time updates

boards/cubepilot/io-v2/nuttx-config/nsh/defconfig

@@ -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

boards/px4/io-v2/nuttx-config/nsh/defconfig

@@ -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

msg/sensor_gps.msg

@@ -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

msg/templates/urtps/microRTPS_timesync.h.em

@@ -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]@
 };

msg/timesync_status.msg

@@ -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

msg/vehicle_gps_position.msg

@@ -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

platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp

@@ -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};

platforms/common/include/px4_platform_common/tasks.h

@@ -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)

platforms/nuttx/src/px4/nxp/imxrt/hrt/hrt.c

@@ -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;
 }
 
 /**

platforms/nuttx/src/px4/nxp/kinetis/hrt/hrt.c

@@ -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;
 }
 
 /**

platforms/nuttx/src/px4/nxp/s32k1xx/hrt/hrt.c

@@ -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;
 }
 
 /**

platforms/nuttx/src/px4/stm/stm32_common/hrt/hrt.c

@@ -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 @@ hrt_ppm_decode(uint32_t status)
 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;
+
+	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;
+	int64_t system_time_us = (system_time.tv_sec * 1000000) + (system_time.tv_nsec / 1000);
+	int64_t offset_us = system_time_us - now_us;
 
-	return result;
+	// 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) {