Merge pull request #45 from RoboSense-LiDAR/dev_opt

[feat,fix]:add decoder_RSMX.fix firing_tss of Helios/Helios16P/RubyPlus

CHANGELOG.md

@@ -1,6 +1,15 @@
 # CHANGLOG
 
 ## Unreleased
+
+## v1.5.13 2024-05-10
+### Added
+- Support RSMX.
+### Fixed
+- Update timestamp parsing unit and the number of packets per frame in decoder_RSE1.
+- Update firing_tss of Helios/Helios16P/RubyPlus.
+- Fix compilation bug of unit test.
+
 ## v1.5.12 2023-12-28
 ### Fixed
 - Fix bug in getting device info and status.

CMakeLists.txt

@@ -6,7 +6,7 @@ if(WIN32)
   cmake_policy(SET CMP0074 NEW) # CMake 3.12 required
 endif(WIN32)
 
-project(rs_driver VERSION 1.5.12)
+project(rs_driver VERSION 1.5.13)
 
 #========================
 #  Project setup

demo/demo_online.cpp

@@ -129,7 +129,7 @@ int main(int argc, char* argv[])
   param.input_param.difop_port = 7788;  ///< Set the lidar difop port number, the default is 7788
   param.lidar_type = LidarType::RSM1;   ///< Set the lidar type. Make sure this type is correct
   param.print();
-
+   
   LidarDriver<PointCloudMsg> driver;               ///< Declare the driver object
   driver.regPointCloudCallback(driverGetPointCloudFromCallerCallback, driverReturnPointCloudToCallerCallback); ///< Register the point cloud callback functions
   driver.regExceptionCallback(exceptionCallback);  ///< Register the exception callback function

src/rs_driver/driver/decoder/basic_attr.hpp

@@ -119,7 +119,30 @@ inline uint64_t parseTimeUTCWithUs(const RSTimestampUTC* tsUtc)
 
   return (sec * 1000000 + us);
 }
+inline uint64_t parseTimeUTCWithNs(const RSTimestampUTC* tsUtc)
+{
+  // sec
+  uint64_t sec = 0;
+  for (int i = 0; i < 6; i++)
+  {
+    sec <<= 8;
+    sec += tsUtc->sec[i];
+  }
+
+  // ns
+  uint64_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 (sec * 1000000000 + ns);
+}
 inline void createTimeUTCWithUs(uint64_t us, RSTimestampUTC* tsUtc)
 {
   uint64_t sec  = us / 1000000;
@@ -141,6 +164,27 @@ inline void createTimeUTCWithUs(uint64_t us, RSTimestampUTC* tsUtc)
     usec >>= 8;
   }
 }
+inline void createTimeUTCWithNs(uint64_t ns, RSTimestampUTC* tsUtc)
+{
+  uint64_t sec  = ns / 1000000000;
+  uint64_t nanoSec = ns % 1000000000;
+
+#ifdef ENABLE_STAMP_WITH_LOCAL
+  sec += getTimezone();
+#endif
+
+  for (int i = 5; i >= 0; i--)
+  {
+    tsUtc->sec[i] = sec & 0xFF;
+    sec >>= 8;
+  }
+
+  for (int i = 3; i >= 0; i--)
+  {
+    tsUtc->ss[i] = nanoSec & 0xFF;
+    nanoSec >>= 8;
+  }
+}
 
 inline uint64_t parseTimeYMD(const RSTimestampYMD* tsYmd)
 {
@@ -232,6 +276,15 @@ inline uint64_t getTimeHost(void)
   return t_us.count();
 }
 
+inline uint64_t getTimeHostWithNs(void)
+{
+  std::chrono::system_clock::time_point t = std::chrono::system_clock::now();
+  std::chrono::system_clock::duration t_s = t.time_since_epoch();
+
+  std::chrono::duration<uint64_t, std::nano> t_ns = 
+    std::chrono::duration_cast<std::chrono::duration<uint64_t, std::nano>>(t_s);
+  return t_ns.count();
+}
 inline int16_t parseTempInLe(const RSTemperature* tmp) // format of little endian
 {
   // | lsb | padding | neg | msb |

src/rs_driver/driver/decoder/decoder_RSBP.hpp

@@ -240,6 +240,7 @@ inline bool DecoderRSBP<T_PointCloud>::internDecodeMsopPkt(const uint8_t* packet
     }
 
   }
+
 
   double pkt_ts = 0;
   if (this->param_.use_lidar_clock)

src/rs_driver/driver/decoder/decoder_RSE1.hpp

@@ -84,7 +84,7 @@ class DecoderRSE1 : public Decoder<T_PointCloud>
 public:
 
   constexpr static double FRAME_DURATION = 0.1;
-  constexpr static uint32_t SINGLE_PKT_NUM = 112;
+  constexpr static uint32_t SINGLE_PKT_NUM = 288;
   constexpr static int VECTOR_BASE = 32768;
 
   virtual void decodeDifopPkt(const uint8_t* pkt, size_t size);
@@ -175,7 +175,7 @@ inline bool DecoderRSE1<T_PointCloud>::decodeMsopPkt(const uint8_t* packet, size
 
     if (this->write_pkt_ts_)
     {
-      createTimeUTCWithUs (ts, (RSTimestampUTC*)&pkt.header.timestamp);
+      createTimeUTCWithUs(ts, (RSTimestampUTC*)&pkt.header.timestamp);
     }
   }
 
@@ -191,7 +191,7 @@ inline bool DecoderRSE1<T_PointCloud>::decodeMsopPkt(const uint8_t* packet, size
   {
     const RSEOSBlock& block = pkt.blocks[blk];
 
-    double point_time = pkt_ts + ntohs(block.time_offset) * 1e-6;
+    double point_time = pkt_ts + ntohs(block.time_offset) * 1e-9;
 
     for (uint16_t chan = 0; chan < this->const_param_.CHANNELS_PER_BLOCK; chan++)
     {

src/rs_driver/driver/decoder/decoder_RSHELIOS.hpp

@@ -145,10 +145,10 @@ inline RSDecoderMechConstParam& DecoderRSHELIOS<T_PointCloud>::getConstParam()
   float blk_ts = 55.56f;
   float firing_tss[] = 
   {
-    0.00f,  1.57f,  3.15f,  4.72f,  6.30f,  7.87f,  9.45f, 11.36f, 
-    13.26f, 15.17f, 17.08f, 18.99f, 20.56f, 22.14f, 23.71f, 25.29f,
-    26.53f, 29.01f, 27.77f, 30.25f, 31.49f, 33.98f, 32.73f, 35.22f, 
-    36.46f, 37.70f, 38.94f, 40.18f, 41.42f, 42.67f, 43.91f, 45.15f
+    0.00f,  1.73f,  3.46f,  5.19f,  6.92f,  8.65f,  10.38f, 12.11f, 
+    13.84f, 15.57f, 17.3f, 19.03f, 20.76f, 22.49f, 24.22f, 25.95f,
+    27.68f, 29.41f, 31.14f, 32.87f, 34.6f, 36.33f, 38.06f, 39.79f,
+    41.52f, 43.25f, 44.98f, 46.71f, 48.44f, 50.17f, 51.9f, 53.63f
   };
 
   param.BLOCK_DURATION = blk_ts / 1000000;

src/rs_driver/driver/decoder/decoder_RSHELIOS_16P.hpp

@@ -100,10 +100,10 @@ inline void DecoderRSHELIOS_16P<T_PointCloud>::calcParam()
   float blk_ts = 55.56f;
   float firing_tss[] = 
   {
-     0.00f,  3.15f,  6.30f,  9.45f, 13.26f, 17.08f, 20.56f, 23.71f,
-    26.53f, 27.77f, 31.49f, 32.73f, 36.46f, 38.94f, 41.42f, 43.91f,
-    55.56f, 58.70f, 61.85f, 65.00f, 68.82f, 72.64f, 76.12f, 79.27f, 
-    82.08f, 83.32f, 87.05f, 88.29f, 92.01f, 94.50f, 96.98f, 99.46f
+    0.00f,  1.73f,  3.46f,  5.19f,  6.92f,  8.65f,  10.38f, 12.11f, 
+    13.84f, 15.57f, 17.3f, 19.03f, 20.76f, 22.49f, 24.22f, 25.95f,
+    27.68f, 29.41f, 31.14f, 32.87f, 34.6f, 36.33f, 38.06f, 39.79f,
+    41.52f, 43.25f, 44.98f, 46.71f, 48.44f, 50.17f, 51.9f, 53.63f
   };
 
   if (this->echo_mode_ == RSEchoMode::ECHO_SINGLE)

src/rs_driver/driver/decoder/decoder_RSM1.hpp

@@ -120,15 +120,14 @@ typedef struct
   uint8_t reserved1[1];
   uint8_t frame_rate;
   RSM1DifopEther eth;
-  RSM1DifopFov fov;
   RSM1DifopVerInfo version;
   RSSN sn;
   uint8_t return_mode;
   RSTimeInfo time_info;
   RSM1DifopRunSts status;
   uint8_t reserved2[40];
   RSM1DifopCalibration cali_param[20];
-  uint8_t reserved3[71];
+  uint8_t reserved3[79];
 } RSM1DifopPkt;
 
 #pragma pack(pop)