mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-03 12:48:53 +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:
@@ -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());
|
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) {
|
if (channel == nullptr) {
|
||||||
// Something went wrong - no channel to publish on; return
|
// Something went wrong - no channel to publish on; return
|
||||||
|
|||||||
@@ -75,7 +75,8 @@ void UavcanGyroBridge::imu_sub_cb(const uavcan::ReceivedDataStructure<uavcan::eq
|
|||||||
return;
|
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[0],
|
||||||
msg.rate_gyro_latest[1],
|
msg.rate_gyro_latest[1],
|
||||||
msg.rate_gyro_latest[2]);
|
msg.rate_gyro_latest[2]);
|
||||||
|
|||||||
@@ -116,7 +116,8 @@ void UavcanRangefinderBridge::range_sub_cb(const
|
|||||||
quality = 100;
|
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
|
// Register device capability if not already done
|
||||||
if (_node_info_publisher != nullptr) {
|
if (_node_info_publisher != nullptr) {
|
||||||
|
|||||||
@@ -75,6 +75,7 @@ public:
|
|||||||
if (uORB::SubscriptionCallbackWorkItem::update(&dist)) {
|
if (uORB::SubscriptionCallbackWorkItem::update(&dist)) {
|
||||||
uavcan::equipment::range_sensor::Measurement range_sensor{};
|
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.sensor_id = get_instance();
|
||||||
range_sensor.range = dist.current_distance;
|
range_sensor.range = dist.current_distance;
|
||||||
range_sensor.field_of_view = dist.h_fov;
|
range_sensor.field_of_view = dist.h_fov;
|
||||||
|
|||||||
Reference in New Issue
Block a user