diff --git a/CHANGELOG.md b/CHANGELOG.md index d445786..f47c27e 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -1,5 +1,13 @@ # CHANGLOG +## v1.5.16 2024-08-27 +### Added +- Support sn parsing of RSMX. +### Fixed +- Update msop protocol of RSMX. +- Fix compilation bug in epoll socket. +- Fix compilation warning for unit tests. + ## v1.5.15 2024-08-07 ### Added - Support RSM3. @@ -8,7 +16,7 @@ ### Added - Support multiple lidars with different multicast addresses and the same port. ### Fixed -- Fixed the bug that only one lidar was parsed correctly when multiple bp4.0 were used. +- Fix the bug that only one lidar was parsed correctly when multiple bp4.0 were used. ## v1.5.13 2024-05-10 ### Added diff --git a/CMakeLists.txt b/CMakeLists.txt index c595696..a7f9318 100644 --- 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.15) +project(rs_driver VERSION 1.5.16) #======================== # Project setup diff --git a/doc/src_intro/rs_driver_intro_CN.md b/doc/src_intro/rs_driver_intro_CN.md index 5d5907e..a1652a1 100644 --- a/doc/src_intro/rs_driver_intro_CN.md +++ b/doc/src_intro/rs_driver_intro_CN.md @@ -1192,7 +1192,7 @@ Block间的角度差 = 360 / 每帧Block数 | RX | 0.01473 | 光心相对于物理中心的X坐标 | | RY | 0.0085 | 光心相对于物理中心的Y坐标 | | RZ | 0.09427 | 光心相对于物理中心的Z坐标 | -| BLOCK_DURATION | 55.52 | Block的持续时间，单位纳秒 | +| BLOCK_DURATION | 55.52 | Block的持续时间，单位微秒 | | CHAN_TSS[] | - | 从发射时间列表得到 | | CHAN_AZIS[] | - | 从发射时间列表得到 | diff --git a/src/rs_driver/driver/decoder/decoder_RSMX.hpp b/src/rs_driver/driver/decoder/decoder_RSMX.hpp index 8fc1566..25a9d84 100644 --- a/src/rs_driver/driver/decoder/decoder_RSMX.hpp +++ b/src/rs_driver/driver/decoder/decoder_RSMX.hpp @@ -65,9 +65,11 @@ typedef struct { uint8_t id[4]; uint16_t pkt_seq; - uint8_t protocol_version; - uint8_t device_mode; + uint16_t protocol_version; + uint8_t return_mode; + uint8_t time_mode; RSTimestampUTC timestamp; + uint8_t reserved[10]; uint8_t lidar_type; uint8_t temperature; } RSMXMsopHeader; @@ -96,10 +98,19 @@ typedef struct { RSMXMsopHeader header; RSMXBlock blocks[50]; + uint8_t reserved[16]; uint8_t crc32[4]; uint8_t rolling_counter[2]; } RSMXMsopPkt; +typedef struct +{ + uint8_t id[8]; + uint8_t reserved1[30]; + RSSN sn; + uint8_t reserved2[212]; +} RSMXDifopPkt; + #pragma pack(pop) template @@ -129,7 +140,7 @@ inline RSDecoderConstParam& DecoderRSMX::getConstParam() { static RSDecoderConstParam param = { - 1376// msop len + 1404// msop len , 256 // difop len , 4 // msop id len , 8 // difop id len @@ -175,6 +186,14 @@ template inline void DecoderRSMX::decodeDifopPkt(const uint8_t* packet, size_t size) { + #ifdef ENABLE_DIFOP_PARSE + const RSMXDifopPkt& pkt = *(RSMXDifopPkt*)packet; + // device info + memcpy (this->device_info_.sn, pkt.sn.num, 4); + this->device_info_.state = true; + // device status + this->device_status_.state = false; + #endif } template @@ -203,7 +222,7 @@ inline bool DecoderRSMX::decodeMsopPkt(const uint8_t* packet, size } } - + uint16_t pkt_seq = ntohs(pkt.header.pkt_seq); if (split_strategy_.newPacket(pkt_seq)) { @@ -211,8 +230,9 @@ inline bool DecoderRSMX::decodeMsopPkt(const uint8_t* packet, size this->first_point_ts_ = pkt_ts; ret = true; } + uint8_t loop_time; - if((pkt.header.device_mode & 0xF) == 0) + if(pkt.header.return_mode == 0x0) { loop_time = 2; // dual return }else @@ -243,6 +263,7 @@ inline bool DecoderRSMX::decodeMsopPkt(const uint8_t* packet, size distance = ntohs(channel.radius_sd) * this->const_param_.DISTANCE_RES; intensity = channel.intensity_sd; } + if (this->distance_section_.in(distance)) { uint16_t vector_x = RS_SWAP_INT16(channel.x); diff --git a/src/rs_driver/driver/input/unix/input_sock_epoll.hpp b/src/rs_driver/driver/input/unix/input_sock_epoll.hpp index 09c5f02..9ef8793 100644 --- a/src/rs_driver/driver/input/unix/input_sock_epoll.hpp +++ b/src/rs_driver/driver/input/unix/input_sock_epoll.hpp @@ -179,11 +179,7 @@ inline int InputSock::createSocket(uint16_t port, const std::string& hostIp, con goto failOption; } - if (hostIp != "0.0.0.0" && grpIp != "0.0.0.0") - { - inet_pton(AF_INET, grpIp.c_str(), &(host_addr.sin_addr)); - } - + struct sockaddr_in host_addr; memset(&host_addr, 0, sizeof(host_addr)); host_addr.sin_family = AF_INET; @@ -194,6 +190,12 @@ inline int InputSock::createSocket(uint16_t port, const std::string& hostIp, con inet_pton(AF_INET, hostIp.c_str(), &(host_addr.sin_addr)); } + if (hostIp != "0.0.0.0" && grpIp != "0.0.0.0") + { + inet_pton(AF_INET, grpIp.c_str(), &(host_addr.sin_addr)); + } + + ret = bind(fd, (struct sockaddr*)&host_addr, sizeof(host_addr)); if (ret < 0) { diff --git a/src/rs_driver/macro/version.hpp b/src/rs_driver/macro/version.hpp index 1d4a1c6..5d33e21 100644 --- a/src/rs_driver/macro/version.hpp +++ b/src/rs_driver/macro/version.hpp @@ -1,3 +1,3 @@ #define RSLIDAR_VERSION_MAJOR 1 #define RSLIDAR_VERSION_MINOR 5 -#define RSLIDAR_VERSION_PATCH 15 +#define RSLIDAR_VERSION_PATCH 16 diff --git a/test/decoder_test.cpp b/test/decoder_test.cpp index d66163d..e2ef49d 100644 --- a/test/decoder_test.cpp +++ b/test/decoder_test.cpp @@ -13,16 +13,21 @@ typedef PointCloudT PointCloud; #pragma pack(push, 1) struct MyMsopPkt { - uint8_t id[8]; + uint8_t id[8]{0}; }; struct MyDifopPkt { uint8_t id[8]; uint16_t rpm; + RSFOV fov; RSCalibrationAngle vert_angle_cali[2]; RSCalibrationAngle horiz_angle_cali[2]; + RSSN sn; + RSEthNetV2 eth; + RSVersionV2 version; + RSStatusV1 status; }; #pragma pack(pop) @@ -282,6 +287,10 @@ TEST(TestDecoder, processMsopPkt) memcpy (pkt.id, id, 8); errCode = ERRCODE_SUCCESS; decoder.processMsopPkt((const uint8_t*)&pkt, sizeof(pkt)); +#ifdef ENABLE_CRC32_CHECK + ASSERT_EQ(errCode, ERRCODE_WRONGCRC32); +#else ASSERT_EQ(errCode, ERRCODE_SUCCESS); +#endif }