From 81e1df7259c1e50d76940247a34adb695f5b5d32 Mon Sep 17 00:00:00 2001
From: Starrah <starrah@foxmail.com>
Date: Fri, 14 Apr 2023 18:02:48 +0800
Subject: [PATCH] fix: UTC_TIME for RS128 is in "s.ns" format.

This commit fixs the error in decoder_RS128.hpp. For RS128, the timestamp data in the MSOP header is in "s.ns" format, instead of "s.us".
However, the old code treats the timestamp as in "s.us" format, which cause significantly error when parsing the UTC timestamp from the LiDAR.
---
 src/rs_driver/driver/decoder/basic_attr.hpp   | 25 +++++++++++++++++++
 .../driver/decoder/decoder_RS128.hpp          |  3 ++-
 2 files changed, 27 insertions(+), 1 deletion(-)

diff --git a/src/rs_driver/driver/decoder/basic_attr.hpp b/src/rs_driver/driver/decoder/basic_attr.hpp
index 5bc1118..a6d824b 100644
--- a/src/rs_driver/driver/decoder/basic_attr.hpp
+++ b/src/rs_driver/driver/decoder/basic_attr.hpp
@@ -142,6 +142,31 @@ inline void createTimeUTCWithUs(uint64_t us, RSTimestampUTC* tsUtc)
   }
 }
 
+inline std::pair<uint64_t, uint32_t> parseTimeUTCWithNs(const RSTimestampUTC* tsUtc)
+{
+  // sec
+  uint64_t sec = 0;
+  for (int i = 0; i < 6; i++)
+  {
+    sec <<= 8;
+    sec += tsUtc->sec[i];
+  }
+
+  // ns
+  uint32_t ns = 0;
+  for (int i = 0; i < 4; i++)
+  {
+    ns <<= 8;
+    ns += tsUtc->ss[i];
+  }
+
+#ifdef ENABLE_STAMP_WITH_LOCAL
+  sec -= getTimezone();
+#endif
+
+  return std::make_pair(sec, ns);
+}
+
 inline uint64_t parseTimeYMD(const RSTimestampYMD* tsYmd)
 {
   std::tm stm;
diff --git a/src/rs_driver/driver/decoder/decoder_RS128.hpp b/src/rs_driver/driver/decoder/decoder_RS128.hpp
index 32569ef..90fa891 100644
--- a/src/rs_driver/driver/decoder/decoder_RS128.hpp
+++ b/src/rs_driver/driver/decoder/decoder_RS128.hpp
@@ -216,7 +216,8 @@ inline bool DecoderRS128<T_PointCloud>::internDecodeMsopPkt(const uint8_t* packe
   double pkt_ts = 0;
   if (this->param_.use_lidar_clock)
   {
-    pkt_ts = parseTimeUTCWithUs(&pkt.header.timestamp) * 1e-6;
+    std::pair<uint64_t, uint32_t> secAndNs = parseTimeUTCWithNs(&pkt.header.timestamp);
+    pkt_ts = secAndNs.first + secAndNs.second * 1e-9;
   }
   else
   {