From b73fe49eafde0b5e8e09c2c747db2d9e5f79c4f8 Mon Sep 17 00:00:00 2001 From: zhanyiaini Date: Tue, 27 Feb 2024 10:44:07 +0800 Subject: [PATCH] =?UTF-8?q?=E9=92=88=E5=AF=B9=E6=97=B6=E9=97=B4=E6=88=B3?= =?UTF-8?q?=E5=8D=8F=E8=AE=AE=E8=BF=9B=E8=A1=8C=E4=BF=AE=E6=94=B9?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- CMakeLists.txt | 4 +- core/common/ydlidar_help.h | 72 ++++++++++++- core/common/ydlidar_protocol.h | 39 +++---- examples/tea_test.cpp | 121 ++++++++++++++++------ src/CYdLidar.cpp | 21 ++-- src/CYdLidar.h | 1 + src/GSLidarDriver.cpp | 35 +++---- src/GSLidarDriver.h | 10 +- src/YDlidarDriver.cpp | 183 +++++++++++++++++++++------------ 9 files changed, 329 insertions(+), 157 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 2a6a852..24c49fc 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -4,8 +4,8 @@ project(ydlidar_sdk C CXX) ######################################################### # version set(YDLIDAR_SDK_VERSION_MAJOR 1) -set(YDLIDAR_SDK_VERSION_MINOR 1) -set(YDLIDAR_SDK_VERSION_PATCH 23) +set(YDLIDAR_SDK_VERSION_MINOR 2) +set(YDLIDAR_SDK_VERSION_PATCH 0) set(YDLIDAR_SDK_VERSION ${YDLIDAR_SDK_VERSION_MAJOR}.${YDLIDAR_SDK_VERSION_MINOR}.${YDLIDAR_SDK_VERSION_PATCH}) ########################################################## diff --git a/core/common/ydlidar_help.h b/core/common/ydlidar_help.h index 27994ba..9db3dbf 100644 --- a/core/common/ydlidar_help.h +++ b/core/common/ydlidar_help.h @@ -43,6 +43,7 @@ #include "DriverInterface.h" #include #include +#include /** * @brief ydlidar @@ -1024,13 +1025,82 @@ inline bool isV1Protocol(uint8_t protocol) { //以16进制打印数据 inline void printHex(const uint8_t *data, int size) { - if (!data) + if (!data || !size) return; for (int i=0; itm_year), \ + (1 + localTime->tm_mon), \ + localTime->tm_mday, \ + localTime->tm_hour, \ + localTime->tm_min, \ + localTime->tm_sec); +//格式化字符串 +#define FORMAT_STDOUT \ + char buff[1024] = {0}; \ + va_list ap; \ + va_start(ap, fmt); \ + vsprintf(buff, fmt, ap); \ + va_end(ap); \ + printf(buff); \ + printf("\n"); + +//调试 +inline void debug(char* fmt, ...) +{ +#ifdef _WIN32 +#else + UNIX_PRINT_TIME +#endif + printf("[debug] "); + FORMAT_STDOUT + fflush(stdout); +} + +//常规 +inline void info(char* fmt, ...) +{ +#ifdef _WIN32 +#else + UNIX_PRINT_TIME +#endif + printf("[info] "); + FORMAT_STDOUT + fflush(stdout); +} + +//警告 +inline void warn(char* fmt, ...) +{ +#ifdef _WIN32 +#else + UNIX_PRINT_TIME +#endif + printf("[warn] "); + FORMAT_STDOUT + fflush(stdout); +} + +//错误 +inline void error(char* fmt, ...) +{ +#ifdef _WIN32 +#else + UNIX_PRINT_TIME +#endif + printf("[error] "); + FORMAT_STDOUT + fflush(stdout); +} + }//common }//core }//ydlidar diff --git a/core/common/ydlidar_protocol.h b/core/common/ydlidar_protocol.h index ee75297..39bef40 100644 --- a/core/common/ydlidar_protocol.h +++ b/core/common/ydlidar_protocol.h @@ -137,21 +137,14 @@ #define LIDAR_MODULE_3 0x04 #define LIDAR_MODULE_ALL 0x00 #define LIDAR_MAXCOUNT 3 //最大模组数 -#define GSMAXPOINTCOUNT 160 //GS数据包中最大点云数 +#define LIDAR_PACKMAXPOINTSIZE 160 //单包最大点数 //GS -#define Angle_Px 1.22 -#define Angle_Py 5.315 -#define Angle_PAngle 22.5 -#define PackageMaxModuleNums 0x03 -#define GS_MAXPOINTS 160 //GS2固定160个点 -#define MaxPointsPerPackge_GS1 216 //GS1固定216个点 -#define PackagePaidBytes_GS 8 -#define NORMAL_PACKAGE_SIZE 331 - -/// Maximuum number of samples in a packet -#define PackageSampleMaxLngth 0x100 -#define MaximumNumberOfPackages 765 //= 255 * 3 +#define Angle_Px 1.22 +#define Angle_Py 5.315 +#define Angle_PAngle 22.5 +#define GS_PACKHEADSIZE 8 +#define GS_MAXPOINTSIZE 160 //GS数据包中最大点云数 #define SDK_SNLEN 16 //序列号长度 @@ -161,18 +154,16 @@ #define Node_Sync 1 /// Normal Node #define Node_NotSync 2 -/// Package Header Size -#define PackagePaidBytes 10 /// Package Header #define PH 0x55AA #define PH1 0xAA #define PH2 0x55 //AA55是点云数据 #define PH3 0x66 //AA66是时间戳数据 +/// Package Header Size +#define TRI_PACKHEADSIZE 10 /// Normal Package size -#define TrianglePackageDataSize 40 -/// TOF Normal package size -#define TOFPackageDataSize 80 +#define TRI_PACKDATASIZE 40 #define FREINDEX 0 #define USERVERSIONNDEX 1 @@ -252,7 +243,7 @@ struct tri_node_package { uint16_t firstAngle;///< first sample angle uint16_t lastAngle;///< last sample angle uint16_t cs;///< checksum - uint16_t nodes[PackageSampleMaxLngth]; + uint16_t nodes[LIDAR_PACKMAXPOINTSIZE]; } __attribute__((packed)); //LiDAR Intensity Nodes Package @@ -263,7 +254,7 @@ struct tri_node_package2 { uint16_t firstAngle;///< first sample angle uint16_t lastAngle;///< last sample angle uint16_t cs;///< checksum - tri_node2 nodes[PackageSampleMaxLngth]; + tri_node2 nodes[LIDAR_PACKMAXPOINTSIZE]; } __attribute__((packed)); // TOF LiDAR Intensity Nodes Package @@ -274,7 +265,7 @@ struct tof_node_package { uint16_t firstAngle; uint16_t lastAngle; uint16_t cs; - tof_node nodes[PackageSampleMaxLngth]; + tof_node nodes[LIDAR_PACKMAXPOINTSIZE]; } __attribute__((packed)); //时间戳结构体 @@ -369,12 +360,12 @@ struct gs_packages { int moduleNum; bool left = false; bool right = false; - node_info points[GS_MAXPOINTS]; + node_info points[GS_MAXPOINTSIZE]; } __attribute__((packed)); struct gs_module_nodes { int moduleNum = 0; int pointCount = 0; - node_info points[GS_MAXPOINTS]; + node_info points[GS_MAXPOINTSIZE]; } __attribute__((packed)); //GS点数据结构 @@ -392,7 +383,7 @@ struct gs_node_package { uint8_t ct; uint16_t size; uint16_t env; - gs_node nodes[GS_MAXPOINTS]; + gs_node nodes[GS_MAXPOINTSIZE]; uint8_t cs; } __attribute__((packed)); #define GSPACKSIZE sizeof(gs_node_package) //定义GS点大小 diff --git a/examples/tea_test.cpp b/examples/tea_test.cpp index ef16f2f..fe8eb98 100644 --- a/examples/tea_test.cpp +++ b/examples/tea_test.cpp @@ -36,6 +36,7 @@ #include #include #include "CYdLidar.h" +#include "core/common/ydlidar_help.h" #include "filters/NoiseFilter.h" #include "filters/StrongLightFilter.h" @@ -46,6 +47,7 @@ using namespace ydlidar; #pragma comment(lib, "ydlidar_sdk.lib") #endif + int main(int argc, char *argv[]) { printf("__ ______ _ ___ ____ _ ____ \n"); @@ -99,34 +101,78 @@ int main(int argc, char *argv[]) printf("Please enter the lidar IP: "); std::cin >> port; } - } - int baudrate = 8090; + //串口 + // ports = ydlidar::lidarPortList(); + // if (ports.size() == 1) + // { + // port = ports.begin()->second; + // } + // else + // { + // int id = 0; - if (!ydlidar::os_isOk()) - { - return 0; - } + // for (it = ports.begin(); it != ports.end(); it++) + // { + // printf("[%d] %s %s\n", id, it->first.c_str(), it->second.c_str()); + // id++; + // } - bool isSingleChannel = false; + // if (ports.empty()) + // { + // printf("Not Lidar was detected. Please enter the lidar serial port:"); + // std::cin >> port; + // } + // else + // { + // while (ydlidar::os_isOk()) + // { + // printf("Please select the lidar port:"); + // std::string number; + // std::cin >> number; - std::string input_frequency; + // if ((size_t)atoi(number.c_str()) >= ports.size()) + // { + // continue; + // } - float frequency = 20.0f; + // it = ports.begin(); + // id = atoi(number.c_str()); - while (ydlidar::os_isOk() && !isSingleChannel) - { - printf("Please input the lidar scan frequency[10-30]: "); - std::cin >> input_frequency; - frequency = atof(input_frequency.c_str()); - if (frequency <= 30.0 && frequency >= 10.0) - { - break; - } + // while (id) + // { + // id--; + // it++; + // } + + // port = it->second; + // break; + // } + // } + // } - fprintf(stderr, "Invalid scan frequency Please re-input.\n"); } + // int baudrate = 512000; //8090 + int baudrate = 8090; + + bool isSingleChannel = false; + + float frequency = 20.0f; + + // std::string input_frequency; + // while (ydlidar::os_isOk() && !isSingleChannel) + // { + // printf("Please input the lidar scan frequency[10-30]: "); + // std::cin >> input_frequency; + // frequency = atof(input_frequency.c_str()); + // if (frequency <= 30.0 && frequency >= 10.0) + // { + // break; + // } + // fprintf(stderr, "Invalid scan frequency Please re-input.\n"); + // } + /// instance CYdLidar laser; //////////////////////string property///////////////// @@ -145,6 +191,7 @@ int main(int argc, char *argv[]) int optval = TYPE_TOF; laser.setlidaropt(LidarPropLidarType, &optval, sizeof(int)); /// device type + // optval = YDLIDAR_TYPE_SERIAL; optval = YDLIDAR_TYPE_TCP; laser.setlidaropt(LidarPropDeviceType, &optval, sizeof(int)); /// sample rate @@ -158,7 +205,7 @@ int main(int argc, char *argv[]) //////////////////////bool property///////////////// /// fixed angle resolution - bool b_optvalue = true; + bool b_optvalue = false; laser.setlidaropt(LidarPropFixedResolution, &b_optvalue, sizeof(bool)); /// rotate 180 b_optvalue = false; @@ -191,6 +238,8 @@ int main(int argc, char *argv[]) /// unit: Hz laser.setlidaropt(LidarPropScanFrequency, &frequency, sizeof(float)); + // laser.setEnableDebug(true); //启用调试 + /// initialize SDK and LiDAR. bool ret = laser.initialize(); @@ -209,23 +258,35 @@ int main(int argc, char *argv[]) LaserScan outScan; StrongLightFilter filter; //拖尾滤波器 filter.setMaxDist(0.1); //最大距离阈值 + filter.setMaxAngle(12.0); //最大角度阈值 filter.setMinNoise(2); //最小连续噪点数 + float minScanTime = 1.0f / (frequency + 2.5); + float maxScanTime = 1.0f / (frequency - 2.5); while (ret && ydlidar::os_isOk()) { //循环获取点云数据 if (laser.doProcessSimple(scan)) { - fprintf(stdout, "Scan received [%llu] points is [%f]s [%f]\n", - scan.points.size(), - scan.config.scan_time, - scan.config.time_increment); - fflush(stdout); - for (size_t i = 0; i < scan.points.size(); ++i) - { - const LaserPoint &p = scan.points.at(i); - printf("%d d %.0f a %.02f i %.0f\n", i, p.range, p.angle * 180.0 / M_PI, p.intensity); - } + // if (scan.config.scan_time < minScanTime || + // scan.config.scan_time > maxScanTime) + // { + // core::common::error("[%u] points ScanTime [%f]s IncrTime [%f]s", + // uint32_t(scan.points.size()), + // scan.config.scan_time, + // scan.config.time_increment); + // } + + // core::common::info("[%u] points Stamp [%u]ms", + // uint32_t(scan.points.size()), + // uint32_t(scan.stamp / 1000000)); + + // for (size_t i = 0; i < scan.points.size(); ++i) + // { + // const LaserPoint &p = scan.points.at(i); + // printf("%d d %.0f a %.02f i %.0f\n", i, p.range, p.angle * 180.0 / M_PI, p.intensity); + // } + // fflush(stdout); // 使用强光滤波器 // filter.filter(scan, 0, 0, outScan); diff --git a/src/CYdLidar.cpp b/src/CYdLidar.cpp index eb3e92f..15b70b6 100644 --- a/src/CYdLidar.cpp +++ b/src/CYdLidar.cpp @@ -40,6 +40,7 @@ using namespace std; using namespace impl; using namespace ydlidar::core; +using namespace ydlidar::core::common; using namespace ydlidar::core::math; /*------------------------------------------------------------- @@ -524,9 +525,9 @@ bool CYdLidar::turnOn() m_AllNode = 0; m_PointTime = lidarPtr->getPointTime(); lidarPtr->setAutoReconnect(m_AutoReconnect); - printf("[YDLIDAR] Now lidar is scanning...\n"); - fflush(stdout); + info("[YDLIDAR] Now lidar is scanning..."); + lastStamp = 0; scanning = true; return true; } @@ -553,7 +554,7 @@ bool CYdLidar::doProcessSimple(LaserScan &outscan) uint64_t tim_scan_start = getTime(); uint64_t startTs = tim_scan_start; //从缓存中获取已采集的一圈扫描数据 - result_t op_result = lidarPtr->grabScanData(global_nodes, count, 500); + result_t op_result = lidarPtr->grabScanData(global_nodes, count, 1000); uint64_t tim_scan_end = getTime(); uint64_t endTs = tim_scan_end; uint64_t sys_scan_time = tim_scan_end - tim_scan_start; //获取一圈数据所花费的时间 @@ -630,11 +631,15 @@ bool CYdLidar::doProcessSimple(LaserScan &outscan) outscan.config.min_angle = math::from_degrees(m_MinAngle); outscan.config.max_angle = math::from_degrees(m_MaxAngle); //将首末点采集时间差作为采集时长 - outscan.config.scan_time = static_cast((global_nodes[count - 1].stamp - - global_nodes[0].stamp)) / 1e9; - //outscan.config.scan_time = static_cast(scan_time * 1.0 / 1e9); - // if (outscan.config.scan_time > 1) - // printf("stamp [%llu]-[%llu]\n", global_nodes[0].stamp, global_nodes[count - 1].stamp); + // outscan.config.scan_time = static_cast((global_nodes[count - 1].stamp - + // global_nodes[0].stamp)) / 1e9; + // outscan.config.scan_time = sys_scan_time / 1e9; + if (lastStamp > 0 && global_nodes[0].stamp > 0) + outscan.config.scan_time = double(global_nodes[0].stamp - lastStamp) / 1e9; + else + outscan.config.scan_time = 0; + lastStamp = global_nodes[0].stamp; + //计算时间增量 if (!ISZERO(outscan.config.scan_time)) outscan.config.time_increment = outscan.config.scan_time / (count - 1); else diff --git a/src/CYdLidar.h b/src/CYdLidar.h index 02ae758..84a258e 100644 --- a/src/CYdLidar.h +++ b/src/CYdLidar.h @@ -370,6 +370,7 @@ class YDLIDAR_API CYdLidar { bool m_GlassNoise = false; //玻璃噪点过滤标识 std::string otaName; //OTA文件路径 bool otaEncode = true; //OTA是否加密 + uint64_t lastStamp = 0; //时间戳 }; // End of class #endif // CYDLIDAR_H diff --git a/src/GSLidarDriver.cpp b/src/GSLidarDriver.cpp index 7bec995..3fef469 100644 --- a/src/GSLidarDriver.cpp +++ b/src/GSLidarDriver.cpp @@ -589,8 +589,8 @@ result_t GSLidarDriver::checkAutoConnecting() int GSLidarDriver::cacheScanData() { - node_info local_buf[GSMAXPOINTCOUNT]; - size_t count = GSMAXPOINTCOUNT; + node_info local_buf[GS_MAXPOINTSIZE]; + size_t count = GS_MAXPOINTSIZE; size_t scan_count = 0; result_t ans = RESULT_FAIL; @@ -601,7 +601,7 @@ int GSLidarDriver::cacheScanData() while (m_isScanning) { - count = GSMAXPOINTCOUNT; + count = GS_MAXPOINTSIZE; ans = waitScanData(local_buf, count); // Thread::needExit(); if (!IS_OK(ans)) @@ -621,11 +621,9 @@ int GSLidarDriver::cacheScanData() // 重连雷达 if (!isAutoReconnect) { - fprintf(stderr, "exit scanning thread!!\n"); + fprintf(stderr, "[YDLIDAR] Exit scanning thread!!\n"); fflush(stderr); - { - m_isScanning = false; - } + m_isScanning = false; return RESULT_FAIL; } else if (timeout_count > DEFAULT_TIMEOUT_COUNT) @@ -641,12 +639,13 @@ int GSLidarDriver::cacheScanData() return RESULT_FAIL; } } - } else { + } + else + { timeout_count = 0; retryCount = 0; - } - { + { //数据存入数组 ScopedLocker l(_lock); gs_module_nodes nodes; @@ -655,9 +654,7 @@ int GSLidarDriver::cacheScanData() memcpy(nodes.points, local_buf, count * SDKNODESIZE); datas.push_back(nodes); scan_count = 0; - // if (datas.size() >= 2) - // printf("[YDLIDAR] GS[%d] store array %d\n", - // moduleNum, int(datas.size())); + } } } @@ -686,7 +683,7 @@ result_t GSLidarDriver::waitPackage(node_info *node, uint32_t timeout) while ((waitTime = getms() - startTs) < timeout) { //解析协议头部分 - remainSize = PackagePaidBytes_GS - pos; + remainSize = GS_PACKHEADSIZE - pos; recvSize = 0; ret = waitForData(remainSize, timeout - waitTime, &recvSize); if (!IS_OK(ret)) @@ -760,7 +757,7 @@ result_t GSLidarDriver::waitPackage(node_info *node, uint32_t timeout) packageBuffer[pos++] = c; // 如果解析到协议头 - if (pos == PackagePaidBytes_GS) + if (pos == GS_PACKHEADSIZE) { // 如果协议数据长度不对则跳过，继续解析协议头 if (!sample_lens || sample_lens >= GSPACKSIZE) @@ -779,7 +776,7 @@ result_t GSLidarDriver::waitPackage(node_info *node, uint32_t timeout) { int offset = 0; // 缓存偏移量 // 如果解析协议头时接收数据长度超过定义的长度则认为是从校验和错误处跳转过来的 - if (recvSize > PackagePaidBytes_GS) + if (recvSize > GS_PACKHEADSIZE) { offset = i + 1; } @@ -817,7 +814,7 @@ result_t GSLidarDriver::waitPackage(node_info *node, uint32_t timeout) if (CheckSumCal != CheckSum) { CheckSumResult = false; - printf("[YDLIDAR] GS2 cs 0x%02X != 0x%02X\n", CheckSumCal, CheckSum); + error("GS cs 0x%02X != 0x%02X", CheckSumCal, CheckSum); // 如果校验和不一致，则需要跳转去当前缓存中查找协议头， // 以免因当前数据包有缺失导致下一包数据解析失败 goto PARSEHEAD; @@ -832,7 +829,7 @@ result_t GSLidarDriver::waitPackage(node_info *node, uint32_t timeout) } break; - } // end if (pos == PackagePaidBytes_GS) + } // end if (pos == GS_PACKHEADSIZE) } //end for (size_t i = 0; i < recvSize; ++i) if (CheckSumResult) break; @@ -1242,7 +1239,7 @@ result_t GSLidarDriver::getDevicePara(gs_device_para &info, uint32_t timeout) { return ans; } gs_package_head h; - for (int i = 0; i < PackageMaxModuleNums && i < moduleCount; i++) + for (int i = 0; i < LIDAR_MAXCOUNT && i < moduleCount; i++) { if ((ans = waitResponseHeaderEx(&h, GS_LIDAR_CMD_GET_PARAMETER, timeout)) != RESULT_OK) { return ans; diff --git a/src/GSLidarDriver.h b/src/GSLidarDriver.h index 741e8d6..3515280 100644 --- a/src/GSLidarDriver.h +++ b/src/GSLidarDriver.h @@ -497,11 +497,11 @@ namespace ydlidar uint8_t *globalRecvBuffer = nullptr; - double k0[PackageMaxModuleNums]; - double k1[PackageMaxModuleNums]; - double b0[PackageMaxModuleNums]; - double b1[PackageMaxModuleNums]; - double bias[PackageMaxModuleNums]; + double k0[LIDAR_MAXCOUNT]; + double k1[LIDAR_MAXCOUNT]; + double b0[LIDAR_MAXCOUNT]; + double b1[LIDAR_MAXCOUNT]; + double bias[LIDAR_MAXCOUNT]; uint8_t moduleNum = 0; // 模块编号 uint8_t moduleCount = 1; // 当前模组数量 diff --git a/src/YDlidarDriver.cpp b/src/YDlidarDriver.cpp index 089e804..23e68e1 100644 --- a/src/YDlidarDriver.cpp +++ b/src/YDlidarDriver.cpp @@ -308,8 +308,11 @@ result_t YDlidarDriver::sendData(const uint8_t *data, size_t size) { return RESULT_FAIL; } - // printf("send: "); - // printHex(data, r); + if (m_Debug) + { + printf("send: "); + printHex(data, r); + } size -= r; data += r; @@ -331,8 +334,11 @@ result_t YDlidarDriver::getData(uint8_t *data, size_t size) return RESULT_FAIL; } - // printf("recv: "); - // printHex(data, r); + if (m_Debug) + { + printf("recv: "); + printHex(data, r); + } size -= r; data += r; @@ -790,7 +796,7 @@ result_t YDlidarDriver::parseResponseHeader( while ((waitTime = getms() - startTs) <= timeout) { - size_t remainSize = PackagePaidBytes - recvPos; + size_t remainSize = TRI_PACKHEADSIZE - recvPos; size_t recvSize = 0; ans = waitForData(remainSize, timeout - waitTime, &recvSize); if (!IS_OK(ans)) @@ -850,14 +856,13 @@ result_t YDlidarDriver::parseResponseHeader( recvSize = remainSize; getData(&globalRecvBuffer[lastSize], recvSize); recvSize += lastSize; - pos = PackagePaidBytes; + pos = TRI_PACKHEADSIZE; } else { pos += 6; } - - //校验和检测 + //时间戳校验和检测 uint8_t csc = 0; //计算校验和 uint8_t csr = 0; //实际校验和 for (int i=0; i 0x%"PRIx64"\n", sp.stamp, stamp); - fflush(stdout); + // fflush(stdout); + //测试扫描时长 + // static uint32_t s_scanTime = 0; + // if (s_scanTime > 0) + // { + // uint32_t dt = sp.stamp - s_scanTime; + // if (dt < 44 || dt > 57) + // { + // error("单帧时长[%u]ms超出标准[%u~%u]", + // dt, 44, 57); + // } + // } + // s_scanTime = sp.stamp; } - continue; } else @@ -893,20 +910,9 @@ result_t YDlidarDriver::parseResponseHeader( case 2: SampleNumlAndCTCal = currentByte; package_type = currentByte & 0x01; //是否是零位包标识 - - if ((package_type == CT_Normal) || - (package_type == CT_RingStart)) + if (package_type == CT_RingStart) { - if (package_type == CT_RingStart) - { - scan_frequence = (currentByte & 0xFE) >> 1; - } - } - else - { - has_package_error = true; - recvPos = 0; - continue; + scan_frequence = (currentByte & 0xFE) >> 1; } break; @@ -996,7 +1002,7 @@ result_t YDlidarDriver::parseResponseHeader( packageBuffer[recvPos++] = currentByte; } - if (recvPos == PackagePaidBytes) + if (recvPos == TRI_PACKHEADSIZE) { ans = RESULT_OK; break; @@ -1062,7 +1068,7 @@ result_t YDlidarDriver::parseResponseScanData( } } - packageBuffer[PackagePaidBytes + recvPos] = globalRecvBuffer[pos]; + packageBuffer[TRI_PACKHEADSIZE + recvPos] = globalRecvBuffer[pos]; recvPos++; } @@ -1140,7 +1146,7 @@ result_t YDlidarDriver::waitPackage(node_info *node, uint32_t timeout) (*node).error = 0; (*node).debugInfo = 0xff; - if (nodeIndex == 0) + if (nodeIndex == 0) { uint8_t *packageBuffer = (m_intensities) ? (isTOFLidar(m_LidarType) ? (uint8_t *)&tof_package.head : @@ -1150,13 +1156,10 @@ result_t YDlidarDriver::waitPackage(node_info *node, uint32_t timeout) if (!IS_OK(ans)) { return ans; } - // printf("index %u\n", nodeIndex); - ans = parseResponseScanData(packageBuffer, timeout); if (!IS_OK(ans)) { return ans; } - calcuteCheckSum(node); calcutePackageCT(); @@ -1164,20 +1167,25 @@ result_t YDlidarDriver::waitPackage(node_info *node, uint32_t timeout) // parseStampData(); //解析时间戳 } - parseNodeDebugFromBuffer(node); - parseNodeFromeBuffer(node); + parseNodeDebugFromBuffer(node); //解析调试信息 + parseNodeFromeBuffer(node); //解析点数据 return RESULT_OK; } -void YDlidarDriver::calcuteCheckSum(node_info *node) { +void YDlidarDriver::calcuteCheckSum(node_info *node) +{ CheckSumCal ^= SampleNumlAndCTCal; CheckSumCal ^= LastSampleAngleCal; - if (CheckSumCal != CheckSum) { + if (CheckSumCal != CheckSum) + { CheckSumResult = false; has_package_error = true; (*node).error = 1; - } else { + error("Check Sum 0x%04X != 0x%04X", CheckSumCal, CheckSum); + } + else + { CheckSumResult = true; } } @@ -1385,16 +1393,16 @@ result_t YDlidarDriver::waitScanData( //计算延时时间 size_t size = _serial->available(); uint64_t delayTime = 0; - if (size > PackagePaidBytes) { + if (size > TRI_PACKHEADSIZE) { size_t packageNum = 0; size_t Number = 0; - size_t PackageSize = TrianglePackageDataSize; + size_t PackageSize = TRI_PACKDATASIZE; packageNum = size / PackageSize; Number = size % PackageSize; - delayTime = packageNum * (PackageSize - PackagePaidBytes) * m_PointTime / 2; + delayTime = packageNum * (PackageSize - TRI_PACKHEADSIZE) * m_PointTime / 2; - if (Number > PackagePaidBytes) { - delayTime += m_PointTime * ((Number - PackagePaidBytes) / 2); + if (Number > TRI_PACKHEADSIZE) { + delayTime += m_PointTime * ((Number - TRI_PACKHEADSIZE) / 2); } } nodebuffer[recvNodeCount - 1].delayTime = size * trans_delay + delayTime; @@ -1413,35 +1421,73 @@ result_t YDlidarDriver::waitScanData( return RESULT_FAIL; } -result_t YDlidarDriver::grabScanData(node_info *nodebuffer, - size_t &count, - uint32_t timeout) +result_t YDlidarDriver::grabScanData( + node_info *nodes, + size_t &count, + uint32_t timeout) { - switch (_dataEvent.wait(timeout)) - { - case Event::EVENT_TIMEOUT: - count = 0; - return RESULT_TIMEOUT; - - case Event::EVENT_OK: + // switch (_dataEvent.wait(timeout)) + // { + // case Event::EVENT_TIMEOUT: + // count = 0; + // return RESULT_TIMEOUT; + // case Event::EVENT_OK: + // { + // ScopedLocker l(_lock); + // size_t size_to_copy = min(count, scan_node_count); + // memcpy(nodes, scan_node_buf, size_to_copy * sizeof(node_info)); + // count = size_to_copy; + // scan_node_count = 0; + // return RESULT_OK; + // } + // default: + // count = 0; + // return RESULT_FAIL; + // } + + node_info packNodes[LIDAR_PACKMAXPOINTSIZE]; + size_t packCount = 0; //单包点数 + size_t currCount = 0; //当前点数 + result_t ans = RESULT_FAIL; + uint32_t st = getms(); + uint32_t wt = 0; + while ((wt = getms() - st) < timeout) { - ScopedLocker l(_lock); - // if (scan_node_count == 0) - // { - // return RESULT_FAIL; - // } + packCount = LIDAR_PACKMAXPOINTSIZE; + ans = waitScanData(packNodes, packCount, timeout - wt); + if (!IS_OK(ans)) + { + return ans; //失败时直接返回 + } + else + { + bool hasZero = false; //当前包中是否有零位标记 + for (size_t i = 0; i < packCount; ++i) + { + if (packNodes[i].sync & LIDAR_RESP_SYNCBIT) + { + hasZero = true; + } - size_t size_to_copy = min(count, scan_node_count); - memcpy(nodebuffer, scan_node_buf, size_to_copy * sizeof(node_info)); - count = size_to_copy; - scan_node_count = 0; + nodes[currCount ++] = packNodes[i]; + if (currCount >= count) + { + hasZero = true; + printf("[YDLIDAR] Current points count %d > buffer size %d\n", + currCount, count); + fflush(stdout); + break; + } + } + if (hasZero) + { + count = currCount; + return RESULT_OK; + } + } } - return RESULT_OK; - default: - count = 0; - return RESULT_FAIL; - } + return RESULT_TIMEOUT; } result_t YDlidarDriver::ascendScanData(node_info *nodebuffer, size_t count) { @@ -1815,7 +1861,6 @@ result_t YDlidarDriver::startScan(bool force, uint32_t timeout) //此处仅获取模组设备信息 { - // printf("waitDevicePackage\n"); waitDevicePackage(1000); } //非Tmini系列雷达才自动获取强度标识 @@ -1826,12 +1871,14 @@ result_t YDlidarDriver::startScan(bool force, uint32_t timeout) } //创建数据解析线程 - ret = createThread(); + // ret = createThread(); } if (isSupportMotorCtrl(model)) startMotor(); + m_isScanning = true; + return ret; } @@ -2441,11 +2488,11 @@ result_t YDlidarDriver::parseHeader( uint32_t waitTime = 0; uint8_t package_type = 0; result_t ans = RESULT_TIMEOUT; - static uint8_t s_buff[PackagePaidBytes * 2] = {0}; + static uint8_t s_buff[TRI_PACKHEADSIZE * 2] = {0}; while ((waitTime = getms() - startTime) <= timeout) { - size_t remainSize = PackagePaidBytes - recvPos; + size_t remainSize = TRI_PACKHEADSIZE - recvPos; size_t recvSize = 0; ans = waitForData(remainSize, timeout - waitTime, &recvSize); if (!IS_OK(ans)) @@ -2533,7 +2580,7 @@ result_t YDlidarDriver::parseHeader( recvPos ++; } - if (recvPos == PackagePaidBytes) + if (recvPos == TRI_PACKHEADSIZE) { ans = RESULT_OK; break;