From 087fa7ce45bad18145ea91850a1343ed6baab14c Mon Sep 17 00:00:00 2001 From: Pride Leong Date: Fri, 10 Apr 2026 20:02:24 +0800 Subject: [PATCH] chore(lidar/livox): using sys time if time not synchronized or diff is exceeded Signed-off-by: Pride Leong --- .../lidar/livox/component/livox_component.cpp | 69 +++++++++++++++---- .../lidar/livox/component/livox_component.h | 9 ++- modules/drivers/lidar/livox/proto/livox.proto | 21 ++++++ 3 files changed, 84 insertions(+), 15 deletions(-) diff --git a/modules/drivers/lidar/livox/component/livox_component.cpp b/modules/drivers/lidar/livox/component/livox_component.cpp index 5d2d1ce3..f1bdee2b 100644 --- a/modules/drivers/lidar/livox/component/livox_component.cpp +++ b/modules/drivers/lidar/livox/component/livox_component.cpp @@ -16,6 +16,7 @@ #include "modules/drivers/lidar/livox/component/livox_component.h" +#include #include #include @@ -25,16 +26,60 @@ namespace lidar { static LivoxLidarComponent* g_livox_component = nullptr; -uint64_t GetEthPacketTimestamp(uint8_t timestamp_type, uint8_t* time_stamp, - uint8_t size) { +uint64_t LivoxLidarComponent::GetEthPacketTimestamp(uint8_t handle, + uint8_t timestamp_type, + uint8_t* time_stamp, + uint8_t size) { LdsStamp time; memcpy(time.stamp_bytes, time_stamp, size); + uint64_t packet_timestamp = time.stamp; + uint64_t system_time_ns = cyber::Time::Now().ToNanosecond(); + + // Default is use_lidar_clock=true, check if explicitly disabled + bool use_lidar_clock = config_.has_use_lidar_clock() ? + config_.use_lidar_clock() : true; + + // If use_lidar_clock is false, always use system time + // Only warn once since this is the user's explicit configuration + if (!use_lidar_clock) { + if (!warned_not_use_lidar_clock_) { + AWARN << "[Livox Timestamp] use_lidar_clock is DISABLED. " + << "Using system time (data arrival time) instead of lidar clock. " + << "This means measurement_time will NOT reflect laser scan time!"; + warned_not_use_lidar_clock_ = true; + } + return system_time_ns; + } - if (timestamp_type == kTimestampTypeGptpOrPtp || - timestamp_type == kTimestampTypeGps) { - return time.stamp; + // use_lidar_clock=true: Try to use packet timestamp (laser scan time) + double max_diff_s = config_.has_max_timestamp_diff_s() ? + config_.max_timestamp_diff_s() : 10.0; + uint64_t max_diff_ns = static_cast(max_diff_s * 1e9); + + // Calculate absolute difference + uint64_t diff_ns; + if (packet_timestamp > system_time_ns) { + diff_ns = packet_timestamp - system_time_ns; + } else { + diff_ns = system_time_ns - packet_timestamp; } - return cyber::Time::Now().ToNanosecond(); + + // If diff is too large, packet time is likely not synchronized + if (diff_ns > max_diff_ns) { + double packet_time_s = packet_timestamp / 1e9; + double system_time_s = system_time_ns / 1e9; + double diff_s = static_cast(diff_ns) / 1e9; + AWARN_EVERY(10000) << std::fixed << std::setprecision(3) + << "[Livox Ts] Handle=" << static_cast(handle) + << " type=" << static_cast(timestamp_type) + << " pkt_ts=" << packet_time_s + << " sys_ts=" << system_time_s + << " diff=" << diff_s << "s > " << max_diff_s << "s, using sys time"; + return system_time_ns; + } + + // Use packet timestamp (lidar clock / laser scan time) + return packet_timestamp; } void LivoxLidarComponent::BinaryDataProcess(const unsigned char* data, @@ -98,14 +143,14 @@ void LivoxLidarComponent::PointCloudCallback(uint8_t handle, return; } - AINFO << boost::format( - "point cloud handle: %d, data_num: %d, data_type: %d\n") % - static_cast(handle) % data_num % - static_cast(data->data_type); + ADEBUG << boost::format( + "point cloud handle: %d, data_num: %d, data_type: %d") % + static_cast(handle) % data_num % + static_cast(data->data_type); size_t byte_size = g_livox_component->GetEthPacketByteSize(data, data_num); - uint64_t pkt_timestamp = GetEthPacketTimestamp( - data->timestamp_type, data->timestamp, sizeof(data->timestamp)); + uint64_t pkt_timestamp = g_livox_component->GetEthPacketTimestamp( + handle, data->timestamp_type, data->timestamp, sizeof(data->timestamp)); g_livox_component->BinaryDataProcess(data->data, data->data_type, data_num, pkt_timestamp); diff --git a/modules/drivers/lidar/livox/component/livox_component.h b/modules/drivers/lidar/livox/component/livox_component.h index 8bf793cd..1c10ed6f 100644 --- a/modules/drivers/lidar/livox/component/livox_component.h +++ b/modules/drivers/lidar/livox/component/livox_component.h @@ -49,9 +49,6 @@ typedef union { int64_t stamp; } LdsStamp; -uint64_t GetEthPacketTimestamp(uint8_t timestamp_type, uint8_t* time_stamp, - uint8_t size); - class LivoxLidarComponent final : public LidarComponentBase { public: void BinaryDataProcess(const unsigned char* data, int data_type, @@ -68,6 +65,11 @@ class LivoxLidarComponent final : public LidarComponentBase { static void OnDeviceStateUpdate(const DeviceInfo* device, DeviceEvent event); static void OnLidarStartSamplingCb(livox_status status, uint8_t handle, uint8_t response, void* client_data); + + private: + uint64_t GetEthPacketTimestamp(uint8_t handle, uint8_t timestamp_type, + uint8_t* time_stamp, uint8_t size); + livox::Config config_; std::deque integral_queue_; uint64_t last_pointcloud_pub_timestamp_{0}; @@ -75,6 +77,7 @@ class LivoxLidarComponent final : public LidarComponentBase { double integral_time_ = {0.4}; uint8_t lidar_handle_ = {0}; bool sdk_initialized_ = {false}; + bool warned_not_use_lidar_clock_ = {false}; }; CYBER_REGISTER_COMPONENT(LivoxLidarComponent) } // namespace lidar diff --git a/modules/drivers/lidar/livox/proto/livox.proto b/modules/drivers/lidar/livox/proto/livox.proto index ff290a8a..1e494ea2 100644 --- a/modules/drivers/lidar/livox/proto/livox.proto +++ b/modules/drivers/lidar/livox/proto/livox.proto @@ -9,6 +9,27 @@ message Config { optional string broadcast_code = 2; optional double integral_time = 3 [default = 0.1]; optional bool enable_sdk_console_log = 4 [default = false]; + // Use lidar clock (packet timestamp) instead of system time + // When true, use lidar hardware timestamp (laser scan time) + // When false, use system time (data arrival time) + // Default: true (use lidar clock for accurate laser scan time) + optional bool use_lidar_clock = 5 [default = true]; + + // Max allowed diff between packet timestamp and system time (seconds) + // If the diff exceeds this value and use_lidar_clock is true, + // fall back to system time and warn about time sync issue + // This helps detect when lidar hardware time is not synchronized + // + // Livox timestamp sync modes (timestamp_type in packet): + // 0 = NoSync - No sync signal mode (may have time drift) + // 1 = PTP - 1588v2.0 PTP sync mode + // 2 = Reserved - Reserved for future use + // 3 = PPS+GPS - PPS (Pulse Per Second) + GPS sync mode + // 4 = PPS - PPS only sync mode + // 5 = Unknown - Unknown sync mode + // + // Default: 10.0 seconds + optional double max_timestamp_diff_s = 6 [default = 10.0]; } message LivoxScan {