feat: update helios resolution

mjseppa · Nov 13, 2020 · 3f8d8f9 · 3f8d8f9
1 parent 0ec42d4
commit 3f8d8f9
Show file tree

Hide file tree

Showing 3 changed files with 5 additions and 3 deletions.
diff --git a/src/rs_driver/driver/decoder/decoder_RSHELIOS.hpp b/src/rs_driver/driver/decoder/decoder_RSHELIOS.hpp
@@ -128,7 +128,7 @@ inline RSDecoderResult DecoderRSHELIOS<T_Point>::decodeMsopPkt(const uint8_t* pk
                                                                int& height, int& azimuth)
 {
   height = this->lidar_const_param_.LASER_NUM;
-  RSHELIOSMsopPkt* mpkt_ptr = (RSHELIOSMsopPkt*)pkt;
+  const RSHELIOSMsopPkt* mpkt_ptr = reinterpret_cast<const RSHELIOSMsopPkt*>(pkt);
   if (mpkt_ptr->header.id != this->lidar_const_param_.MSOP_ID)
   {
     return RSDecoderResult::WRONG_PKT_HEADER;
@@ -187,7 +187,8 @@ inline RSDecoderResult DecoderRSHELIOS<T_Point>::decodeMsopPkt(const uint8_t* pk
           azi_diff * this->lidar_const_param_.DSR_TOFFSET * this->lidar_const_param_.FIRING_FREQUENCY *
               ((-0.014f * static_cast<float>(channel_idx) + 1.8965f) * static_cast<float>(channel_idx) - 0.6543f);
       int azi_channel_final = this->azimuthCalibration(azi_channel_ori, channel_idx);
-      float distance = RS_SWAP_SHORT(mpkt_ptr->blocks[blk_idx].channels[channel_idx].distance) * RS_DIS_RESOLUTION;
+      float distance =
+          RS_SWAP_SHORT(mpkt_ptr->blocks[blk_idx].channels[channel_idx].distance) * RS_HELIOS_DIS_RESOLUTION;
       int angle_horiz = (int)(azi_channel_ori + RS_ONE_ROUND) % RS_ONE_ROUND;
       int angle_vert = ((this->vert_angle_list_[channel_idx]) + RS_ONE_ROUND) % RS_ONE_ROUND;
 

diff --git a/src/rs_driver/driver/decoder/decoder_base.hpp b/src/rs_driver/driver/decoder/decoder_base.hpp
@@ -60,6 +60,7 @@ DEFINE_MEMBER_CHECKER(timestamp)
 #define RS_SWAP_LONG(x) ((((x)&0xFF) << 24) | (((x)&0xFF00) << 8) | (((x)&0xFF0000) >> 8) | (((x)&0xFF000000) >> 24))
 #define RS_TO_RADS(x) ((x) * (M_PI) / 180)
 constexpr float RS_DIS_RESOLUTION = 0.005;
+constexpr float RS_HELIOS_DIS_RESOLUTION = 0.0025;
 constexpr float RS_ANGLE_RESOLUTION = 0.01;
 constexpr float MICRO = 1000000.0;
 constexpr float NANO = 1000000000.0;

diff --git a/tool/rs_driver_viewer.cpp b/tool/rs_driver_viewer.cpp
@@ -161,7 +161,7 @@ void printParam(const RSDriverParam& param)
   RS_INFOL << "z: ";
   RS_INFO << std::fixed << param.decoder_param.transform_param.z << RS_REND;
   RS_INFOL << "roll: ";
-  RS_INFO << std::fixed << param.decoder_param.transform_param.yaw << RS_REND;
+  RS_INFO << std::fixed << param.decoder_param.transform_param.roll << RS_REND;
   RS_INFOL << "pitch: ";
   RS_INFO << std::fixed << param.decoder_param.transform_param.pitch << RS_REND;
   RS_INFOL << "yaw: ";