mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-04-14 10:07:39 +08:00
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:
parent
4d0efccb55
commit
04134dccab
@ -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
|
||||
|
||||
@ -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]);
|
||||
|
||||
@ -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) {
|
||||
|
||||
@ -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;
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user