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.
This commit is contained in:
Jacob Dahl 2026-04-03 13:08:24 -08:00 committed by GitHub
parent 4d0efccb55
commit 04134dccab
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
4 changed files with 6 additions and 3 deletions

View File

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

View File

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

View File

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

View File

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