fix: fix bug in getting device info and status

RoboSense-LiDAR · Apr 29, 2024 · 800d3a3 · 800d3a3
1 parent 06cc242
commit 800d3a3
Show file tree

Hide file tree

Showing 24 changed files with 203 additions and 29 deletions.
diff --git a/CHANGELOG.md b/CHANGELOG.md
@@ -1,6 +1,10 @@
 # CHANGLOG
 
 ## Unreleased
+## v1.5.12 2023-12-28
+### Fixed
+- Fix bug in getting device info and status.
+- Fix bug in getting device temperature.
 
 ## v1.5.11 2023-12-18
 

diff --git a/CMakeLists.txt b/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.11)
+project(rs_driver VERSION 1.5.12)
 
 #========================
 #  Project setup

diff --git a/doc/howto/21_how_to_parse_difop.md b/doc/howto/21_how_to_parse_difop.md
@@ -77,15 +77,63 @@ typedef struct
 
 struct DeviceInfo
 {
+  DeviceInfo()
+  {
+      init();
+  }
   uint8_t sn[6];
   uint8_t mac[6];
   uint8_t top_ver[5];
   uint8_t bottom_ver[5];
+  bool state;
+
+  void init()
+  {
+    memset(sn, 0, sizeof(sn));
+    memset(mac, 0, sizeof(mac));
+    memset(top_ver, 0, sizeof(top_ver));
+    memset(bottom_ver, 0, sizeof(bottom_ver));
+    state = false;
+  }
+
+  DeviceInfo& operator=(const DeviceInfo& other)
+  {
+    if (this != &other) 
+    {
+      memcpy(sn, other.sn, sizeof(sn));
+      memcpy(mac, other.mac, sizeof(mac));
+      memcpy(top_ver, other.top_ver, sizeof(top_ver));
+      memcpy(bottom_ver, other.bottom_ver, sizeof(bottom_ver));
+      state = other.state;
+    }
+    return *this;
+  }
 };
 
 struct DeviceStatus
 {
+  DeviceStatus()
+  {
+      init();
+  }
   float voltage = 0.0f;
+    bool state;
+
+  void init()
+  {
+    voltage = 0.0f;
+    state = false;
+  }
+
+  DeviceStatus& operator=(const DeviceStatus& other)
+  {
+    if (this != &other) 
+    {
+      voltage = other.voltage;
+      state = other.state;
+    }
+    return *this;
+  }
 };
 ```
 
@@ -132,8 +180,10 @@ inline void DecoderRSM1<T_PointCloud>::decodeDifopPkt(const uint8_t* packet, siz
   memcpy (this->device_info_.top_ver, pkt.version.pl_ver, 5),
   memcpy (this->device_info_.bottom_ver, pkt.version.ps_ver, 5),
 
+  this->device_info_.state = true;
   // device status
   this->device_status_.voltage = ntohs(pkt.status.voltage_1);
+  this->device_status_.state = true;
 #endif
 } 
 ```
@@ -187,8 +237,10 @@ inline void DecoderMech<T_PointCloud>::decodeDifopCommon(const T_Difop& pkt)
   memcpy (this->device_info_.top_ver, pkt.version.top_ver, 5), 
   memcpy (this->device_info_.bottom_ver, pkt.version.bottom_ver, 5),
 
+  this->device_info_.state = true;
   // device status
   this->device_status_.voltage = ntohs(pkt.status.vol_12v);
+  this->device_status_.state = true;
 #endif
 }
 ```

diff --git a/doc/howto/21_how_to_parse_difop_CN.md b/doc/howto/21_how_to_parse_difop_CN.md
@@ -79,15 +79,63 @@ DIFOP包中包含配置和状态数据。
 
 struct DeviceInfo
 {
+  DeviceInfo()
+  {
+      init();
+  }
   uint8_t sn[6];
   uint8_t mac[6];
   uint8_t top_ver[5];
   uint8_t bottom_ver[5];
+  bool state;
+
+  void init()
+  {
+    memset(sn, 0, sizeof(sn));
+    memset(mac, 0, sizeof(mac));
+    memset(top_ver, 0, sizeof(top_ver));
+    memset(bottom_ver, 0, sizeof(bottom_ver));
+    state = false;
+  }
+
+  DeviceInfo& operator=(const DeviceInfo& other)
+  {
+    if (this != &other) 
+    {
+      memcpy(sn, other.sn, sizeof(sn));
+      memcpy(mac, other.mac, sizeof(mac));
+      memcpy(top_ver, other.top_ver, sizeof(top_ver));
+      memcpy(bottom_ver, other.bottom_ver, sizeof(bottom_ver));
+      state = other.state;
+    }
+    return *this;
+  }
 };
 
 struct DeviceStatus
 {
+  DeviceStatus()
+  {
+      init();
+  }
   float voltage = 0.0f;
+    bool state;
+
+  void init()
+  {
+    voltage = 0.0f;
+    state = false;
+  }
+
+  DeviceStatus& operator=(const DeviceStatus& other)
+  {
+    if (this != &other) 
+    {
+      voltage = other.voltage;
+      state = other.state;
+    }
+    return *this;
+  }
 };
 ```
 
@@ -134,8 +182,10 @@ inline void DecoderRSM1<T_PointCloud>::decodeDifopPkt(const uint8_t* packet, siz
   memcpy (this->device_info_.top_ver, pkt.version.pl_ver, 5),
   memcpy (this->device_info_.bottom_ver, pkt.version.ps_ver, 5),
 
+  this->device_info_.state = true;
   // device status
   this->device_status_.voltage = ntohs(pkt.status.voltage_1);
+  this->device_status_.state = true;
 #endif
 } 
 ```
@@ -187,8 +237,10 @@ inline void DecoderMech<T_PointCloud>::decodeDifopCommon(const T_Difop& pkt)
   memcpy (this->device_info_.top_ver, pkt.version.top_ver, 5), 
   memcpy (this->device_info_.bottom_ver, pkt.version.bottom_ver, 5),
 
+  this->device_info_.state = true;
   // device status
   this->device_status_.voltage = ntohs(pkt.status.vol_12v);
+  this->device_status_.state = true;
 #endif
 }
 ```

diff --git a/src/rs_driver/driver/decoder/decoder.hpp b/src/rs_driver/driver/decoder/decoder.hpp
@@ -263,7 +263,7 @@ class Decoder
 
   explicit Decoder(const RSDecoderConstParam& const_param, const RSDecoderParam& param);
 
-  float getTemperature();
+  bool getTemperature(float& temp);
   bool getDeviceInfo(DeviceInfo& info);
   bool getDeviceStatus(DeviceStatus& status);
   double getPacketDuration();
@@ -289,6 +289,7 @@ class Decoder
   std::function<void(const Error&)> cb_excep_;
   bool write_pkt_ts_;
 
+
 #ifdef ENABLE_TRANSFORM
   Eigen::Matrix4d trans_;
 #endif
@@ -309,6 +310,7 @@ class Decoder
   double prev_pkt_ts_; // timestamp of prevous packet
   double prev_point_ts_; // timestamp of previous point
   double first_point_ts_; // timestamp of first point
+  bool is_get_temperature_{false};
 };
 
 template <typename T_PointCloud>
@@ -351,22 +353,36 @@ inline void Decoder<T_PointCloud>::enableWritePktTs(bool value)
 }
 
 template <typename T_PointCloud>
-inline float Decoder<T_PointCloud>::getTemperature()
+inline bool Decoder<T_PointCloud>::getTemperature(float& temp)
 {
-  return temperature_;
+  if(!is_get_temperature_)
+  {
+    return false;
+  }
+
+  temp = temperature_;
+  return true;
 }
 
 template <typename T_PointCloud>
 inline bool Decoder<T_PointCloud>::getDeviceInfo(DeviceInfo& info)
 {
-  memcpy (&info, &device_info_, sizeof(DeviceInfo));
+  if(!device_info_.state)
+  {
+    return false;
+  }
+  info = device_info_;
   return true;
 }
 
 template <typename T_PointCloud>
 inline bool Decoder<T_PointCloud>::getDeviceStatus(DeviceStatus& status)
 {
-  memcpy (&status, &device_status_, sizeof(DeviceStatus));
+  if(!device_status_.state)
+  {
+    return false;
+  }
+  status = device_status_;
   return true;
 }
 

diff --git a/src/rs_driver/driver/decoder/decoder_RS128.hpp b/src/rs_driver/driver/decoder/decoder_RS128.hpp
@@ -212,7 +212,7 @@ inline bool DecoderRS128<T_PointCloud>::internDecodeMsopPkt(const uint8_t* packe
   bool ret = false;
 
   this->temperature_ = parseTempInBe(&(pkt.header.temp)) * this->const_param_.TEMPERATURE_RES;
-
+  this->is_get_temperature_ = true;
   double pkt_ts = 0;
   if (this->param_.use_lidar_clock)
   {

diff --git a/src/rs_driver/driver/decoder/decoder_RS16.hpp b/src/rs_driver/driver/decoder/decoder_RS16.hpp
@@ -252,7 +252,7 @@ inline bool DecoderRS16<T_PointCloud>::internDecodeMsopPkt(const uint8_t* packet
   bool ret = false;
 
   this->temperature_ = parseTempInLe(&(pkt.header.temp)) * this->const_param_.TEMPERATURE_RES;
-
+  this->is_get_temperature_ = true;
   double pkt_ts = 0;
   if (this->param_.use_lidar_clock)
   {

diff --git a/src/rs_driver/driver/decoder/decoder_RS32.hpp b/src/rs_driver/driver/decoder/decoder_RS32.hpp
@@ -235,7 +235,7 @@ inline bool DecoderRS32<T_PointCloud>::internDecodeMsopPkt(const uint8_t* packet
   bool ret = false;
 
   this->temperature_ = parseTempInLe(&(pkt.header.temp)) * this->const_param_.TEMPERATURE_RES;
-
+  this->is_get_temperature_ = true;
   double pkt_ts = 0;
   if (this->param_.use_lidar_clock)
   {

diff --git a/src/rs_driver/driver/decoder/decoder_RS48.hpp b/src/rs_driver/driver/decoder/decoder_RS48.hpp
@@ -161,7 +161,7 @@ inline bool DecoderRS48<T_PointCloud>::internDecodeMsopPkt(const uint8_t* packet
   bool ret = false;
 
   this->temperature_ = parseTempInBe(&(pkt.header.temp)) * this->const_param_.TEMPERATURE_RES;
-
+  this->is_get_temperature_ = true;
   double pkt_ts = 0;
   if (this->param_.use_lidar_clock)
   {

diff --git a/src/rs_driver/driver/decoder/decoder_RS80.hpp b/src/rs_driver/driver/decoder/decoder_RS80.hpp
@@ -207,7 +207,7 @@ inline bool DecoderRS80<T_PointCloud>::internDecodeMsopPkt(const uint8_t* packet
   bool ret = false;
 
   this->temperature_ = parseTempInBe(&(pkt.header.temp)) * this->const_param_.TEMPERATURE_RES;
-
+  this->is_get_temperature_ = true;
   double pkt_ts = 0;
   if (this->param_.use_lidar_clock)
   {

diff --git a/src/rs_driver/driver/decoder/decoder_RSBP.hpp b/src/rs_driver/driver/decoder/decoder_RSBP.hpp
@@ -211,7 +211,7 @@ inline bool DecoderRSBP<T_PointCloud>::internDecodeMsopPkt(const uint8_t* packet
   static bool isBpV4 = false;
   static bool isFirstPkt = true;
   this->temperature_ = parseTempInLe(&(pkt.header.temp)) * this->const_param_.TEMPERATURE_RES;
-
+  this->is_get_temperature_ = true;
   if(isFirstPkt)
   {
     isFirstPkt = false;

diff --git a/src/rs_driver/driver/decoder/decoder_RSE1.hpp b/src/rs_driver/driver/decoder/decoder_RSE1.hpp
@@ -160,7 +160,7 @@ inline bool DecoderRSE1<T_PointCloud>::decodeMsopPkt(const uint8_t* packet, size
   bool ret = false;
 
   this->temperature_ = static_cast<float>((int)pkt.header.temperature - this->const_param_.TEMPERATURE_RES);
-
+  this->is_get_temperature_ = true;
   double pkt_ts = 0;
   if (this->param_.use_lidar_clock)
   {

diff --git a/src/rs_driver/driver/decoder/decoder_RSHELIOS.hpp b/src/rs_driver/driver/decoder/decoder_RSHELIOS.hpp
@@ -214,7 +214,7 @@ inline bool DecoderRSHELIOS<T_PointCloud>::internDecodeMsopPkt(const uint8_t* pa
   bool ret = false;
 
   this->temperature_ = parseTempInLe(&(pkt.header.temp)) * this->const_param_.TEMPERATURE_RES;
-
+  this->is_get_temperature_ = true;
   double pkt_ts = 0;
   if (this->param_.use_lidar_clock)
   {

diff --git a/src/rs_driver/driver/decoder/decoder_RSHELIOS_16P.hpp b/src/rs_driver/driver/decoder/decoder_RSHELIOS_16P.hpp
@@ -181,7 +181,7 @@ inline bool DecoderRSHELIOS_16P<T_PointCloud>::internDecodeMsopPkt(const uint8_t
   bool ret = false;
 
   this->temperature_ = parseTempInLe(&(pkt.header.temp)) * this->const_param_.TEMPERATURE_RES;
-
+  this->is_get_temperature_ = true;
   double pkt_ts = 0;
   if (this->param_.use_lidar_clock)
   {

diff --git a/src/rs_driver/driver/decoder/decoder_RSM1.hpp b/src/rs_driver/driver/decoder/decoder_RSM1.hpp
@@ -215,9 +215,10 @@ inline void DecoderRSM1<T_PointCloud>::decodeDifopPkt(const uint8_t* packet, siz
   memcpy (this->device_info_.mac, pkt.eth.mac_addr, 6);
   memcpy (this->device_info_.top_ver, pkt.version.pl_ver, 5);
   memcpy (this->device_info_.bottom_ver, pkt.version.ps_ver, 5);
-
+  this->device_info_.state = true;
   // device status
   this->device_status_.voltage = ntohs(pkt.status.voltage_1);
+  this->device_status_.state = true;
 #endif
 }
 
@@ -228,7 +229,7 @@ inline bool DecoderRSM1<T_PointCloud>::decodeMsopPkt(const uint8_t* packet, size
   bool ret = false;
 
   this->temperature_ = static_cast<float>((int)pkt.header.temperature - this->const_param_.TEMPERATURE_RES);
-
+  this->is_get_temperature_ = true;
   double pkt_ts = 0;
   if (this->param_.use_lidar_clock)
   {

diff --git a/src/rs_driver/driver/decoder/decoder_RSM1_Jumbo.hpp b/src/rs_driver/driver/decoder/decoder_RSM1_Jumbo.hpp
@@ -178,7 +178,7 @@ inline bool DecoderRSM1_Jumbo<T_PointCloud>::internDecodeMsopPkt(const uint8_t*
     return false;
 
   this->temperature_ = static_cast<float>((int)pkt.header.temperature - this->const_param_.TEMPERATURE_RES);
-
+  this->is_get_temperature_ = true;
   double pkt_ts = 0;
   if (this->param_.use_lidar_clock)
   {

diff --git a/src/rs_driver/driver/decoder/decoder_RSM2.hpp b/src/rs_driver/driver/decoder/decoder_RSM2.hpp
@@ -163,9 +163,11 @@ inline void DecoderRSM2<T_PointCloud>::decodeDifopPkt(const uint8_t* packet, siz
   memcpy (this->device_info_.mac, pkt.eth.mac_addr, 6);
   memcpy (this->device_info_.top_ver, pkt.version.pl_ver, 5);
   memcpy (this->device_info_.bottom_ver, pkt.version.ps_ver, 5);
-
+  this->device_info_.state = true;
   // device status
   this->device_status_.voltage = ntohs(pkt.status.voltage_1);
+  this->device_status_.state = true;
+
 #endif
 }
 
@@ -176,7 +178,7 @@ inline bool DecoderRSM2<T_PointCloud>::decodeMsopPkt(const uint8_t* packet, size
   bool ret = false;
 
   this->temperature_ = static_cast<float>((int)pkt.header.temperature - this->const_param_.TEMPERATURE_RES);
-
+  this->is_get_temperature_ = true;
   double pkt_ts = 0;
   if (this->param_.use_lidar_clock)
   {

diff --git a/src/rs_driver/driver/decoder/decoder_RSP128.hpp b/src/rs_driver/driver/decoder/decoder_RSP128.hpp
@@ -217,7 +217,7 @@ inline bool DecoderRSP128<T_PointCloud>::internDecodeMsopPkt(const uint8_t* pack
   bool ret = false;
 
   this->temperature_ = parseTempInBe(&(pkt.header.temp)) * this->const_param_.TEMPERATURE_RES;
-
+  this->is_get_temperature_ = true;
   double pkt_ts = 0;
   if (this->param_.use_lidar_clock)
   {