From 04134dccab9c85d741da6940befe50d0686f8d5d Mon Sep 17 00:00:00 2001
From: Jacob Dahl <37091262+dakejahl@users.noreply.github.com>
Date: Fri, 3 Apr 2026 13:08:24 -0800
Subject: [PATCH] fix(uavcan): use node-published timestamps for CAN sensor
 bridges (#26945)

The FC-side DroneCAN sensor bridges (accel, gyro, rangefinder) used
hrt_absolute_time() in the receive callback as timestamp_sample,
adding ~3-16ms of systematic CAN transport delay.

For messages with a uavcan.Timestamp field, the cannode can publish
the actual sample time via UAVCAN GlobalTimeSync. The RawIMU publisher
already did this for IMU data; apply the same pattern to the range
sensor publisher, and update all three FC bridges to prefer the
message timestamp with a fallback to hrt_absolute_time() for nodes
that don't set it.
---
 src/drivers/uavcan/sensors/accel.cpp                         | 2 +-
 src/drivers/uavcan/sensors/gyro.cpp                          | 3 ++-
 src/drivers/uavcan/sensors/rangefinder.cpp                   | 3 ++-
 src/drivers/uavcannode/Publishers/RangeSensorMeasurement.hpp | 1 +
 4 files changed, 6 insertions(+), 3 deletions(-)

diff --git a/src/drivers/uavcan/sensors/accel.cpp b/src/drivers/uavcan/sensors/accel.cpp
index 0efa5c5e6e..9f3d6a20ca 100644
--- a/src/drivers/uavcan/sensors/accel.cpp
+++ b/src/drivers/uavcan/sensors/accel.cpp
@@ -63,7 +63,7 @@ void UavcanAccelBridge::imu_sub_cb(const uavcan::ReceivedDataStructure<uavcan::e
 {
 	uavcan_bridge::Channel *channel = get_channel_for_node(msg.getSrcNodeID().get(), msg.getIfaceIndex());
 
-	const hrt_abstime timestamp_sample = hrt_absolute_time();
+	const hrt_abstime timestamp_sample = (msg.timestamp.usec > 0) ? msg.timestamp.usec : hrt_absolute_time();
 
 	if (channel == nullptr) {
 		// Something went wrong - no channel to publish on; return
diff --git a/src/drivers/uavcan/sensors/gyro.cpp b/src/drivers/uavcan/sensors/gyro.cpp
index ab72693866..fca4ba7b4f 100644
--- a/src/drivers/uavcan/sensors/gyro.cpp
+++ b/src/drivers/uavcan/sensors/gyro.cpp
@@ -75,7 +75,8 @@ void UavcanGyroBridge::imu_sub_cb(const uavcan::ReceivedDataStructure<uavcan::eq
 		return;
 	}
 
-	gyro->update(hrt_absolute_time(),
+	const hrt_abstime timestamp_sample = (msg.timestamp.usec > 0) ? msg.timestamp.usec : hrt_absolute_time();
+	gyro->update(timestamp_sample,
 		     msg.rate_gyro_latest[0],
 		     msg.rate_gyro_latest[1],
 		     msg.rate_gyro_latest[2]);
diff --git a/src/drivers/uavcan/sensors/rangefinder.cpp b/src/drivers/uavcan/sensors/rangefinder.cpp
index b476c66947..f66fd65442 100644
--- a/src/drivers/uavcan/sensors/rangefinder.cpp
+++ b/src/drivers/uavcan/sensors/rangefinder.cpp
@@ -116,7 +116,8 @@ void UavcanRangefinderBridge::range_sub_cb(const
 		quality = 100;
 	}
 
-	rangefinder->update(hrt_absolute_time(), msg.range, quality);
+	const hrt_abstime timestamp_sample = (msg.timestamp.usec > 0) ? msg.timestamp.usec : hrt_absolute_time();
+	rangefinder->update(timestamp_sample, msg.range, quality);
 
 	// Register device capability if not already done
 	if (_node_info_publisher != nullptr) {
diff --git a/src/drivers/uavcannode/Publishers/RangeSensorMeasurement.hpp b/src/drivers/uavcannode/Publishers/RangeSensorMeasurement.hpp
index 134caea21a..ca8226b15a 100644
--- a/src/drivers/uavcannode/Publishers/RangeSensorMeasurement.hpp
+++ b/src/drivers/uavcannode/Publishers/RangeSensorMeasurement.hpp
@@ -75,6 +75,7 @@ public:
 		if (uORB::SubscriptionCallbackWorkItem::update(&dist)) {
 			uavcan::equipment::range_sensor::Measurement range_sensor{};
 
+			range_sensor.timestamp.usec = getNode().getUtcTime().toUSec() - (hrt_absolute_time() - dist.timestamp);
 			range_sensor.sensor_id = get_instance();
 			range_sensor.range = dist.current_distance;
 			range_sensor.field_of_view = dist.h_fov;