diff --git a/core/common/ydlidar_help.h b/core/common/ydlidar_help.h index 0b5b09a..c1f8e29 100644 --- a/core/common/ydlidar_help.h +++ b/core/common/ydlidar_help.h @@ -813,7 +813,6 @@ inline bool isSerialNumbValid(const LaserDebug &info) { inline void parsePackageNode(const node_info &node, LaserDebug &info) { switch (node.index) { case 0://scan frequency - break; case 1://W3F3CusHardVer_W4F0CusSoftVer; @@ -825,48 +824,42 @@ inline void parsePackageNode(const node_info &node, LaserDebug &info) { break; case 3://W3F4HardwareVer_W4F0FirewareMajor - info.W3F4HardwareVer_W4F0FirewareMajor = node.debugInfo; - + //健康信息 + info.W7F0Health = node.debugInfo; break; case 4://W7F0FirewareMinor - info.W7F0FirewareMinor = node.debugInfo; - + info.W3F4HardwareVer_W4F0FirewareMajor = node.debugInfo; break; - case 5://W3F4BoradHardVer_W4F0Moth - info.W3F4BoradHardVer_W4F0Moth = node.debugInfo; - + info.W7F0FirewareMinor = node.debugInfo; break; case 6://W2F5Output2K4K5K_W5F0Date - info.W2F5Output2K4K5K_W5F0Date = node.debugInfo; break; - case 7://W1F6GNoise_W1F5SNoise_W1F4MotorCtl_W4F0SnYear - info.W1F6GNoise_W1F5SNoise_W1F4MotorCtl_W4F0SnYear = - node.debugInfo; break; - case 8://W7F0SnNumH - info.W7F0SnNumH = node.debugInfo; break; case 9://W7F0SnNumL - info.W7F0SnNumL = node.debugInfo; - + info.W1F6GNoise_W1F5SNoise_W1F4MotorCtl_W4F0SnYear = + node.debugInfo; break; - case 10://W7F0Health - info.W7F0Health = node.debugInfo; - + info.W3F4BoradHardVer_W4F0Moth = node.debugInfo; break; - case 11://W3F4CusHardVer_W4F0CusSoftVer - info.W3F4CusHardVer_W4F0CusSoftVer = node.debugInfo; + info.W2F5Output2K4K5K_W5F0Date = node.debugInfo; + // info.W3F4CusHardVer_W4F0CusSoftVer = node.debugInfo; + break; case 12://W7F0LaserCurrent - info.W7F0LaserCurrent = node.debugInfo; + // info.W7F0LaserCurrent = node.debugInfo; + info.W7F0SnNumH = node.debugInfo; + break; + case 13: + info.W7F0SnNumL = node.debugInfo; break; default: @@ -891,18 +884,19 @@ inline void parsePackageNode(const node_info &node, LaserDebug &info) { inline bool ParseLaserDebugInfo(const LaserDebug &info, device_info &value) { bool ret = false; uint8_t CustomVerMajor = (static_cast - (info.W3F4CusMajor_W4F0CusMinor) >> 4); + (info.W3F4HardwareVer_W4F0FirewareMajor & 0x0F)); uint8_t CustomVerMinor = static_cast - (info.W3F4CusMajor_W4F0CusMinor) & 0x0F; + (info.W7F0FirewareMinor); uint8_t lidarmodel = (static_cast(info.W4F3Model_W3F0DebugInfTranVer) >> 3); uint8_t hardwareVer = static_cast (info.W3F4HardwareVer_W4F0FirewareMajor) >> 4; - uint8_t Moth = static_cast(info.W3F4BoradHardVer_W4F0Moth) & 0x0F; - uint8_t Date = static_cast(info.W2F5Output2K4K5K_W5F0Date) & 0x1F; uint8_t Year = static_cast - (info.W1F6GNoise_W1F5SNoise_W1F4MotorCtl_W4F0SnYear) & 0x0F; + (info.W1F6GNoise_W1F5SNoise_W1F4MotorCtl_W4F0SnYear >> 2); + uint8_t Moth = static_cast(info.W3F4BoradHardVer_W4F0Moth >> 3); + uint8_t Date = static_cast(info.W2F5Output2K4K5K_W5F0Date >> 2); + uint16_t Number = ((static_cast(info.W7F0SnNumH) << 7) | static_cast(info.W7F0SnNumL)); @@ -912,7 +906,7 @@ inline bool ParseLaserDebugInfo(const LaserDebug &info, device_info &value) { value.firmware_version = (CustomVerMajor << 8 | CustomVerMinor); value.hardware_version = hardwareVer; value.model = lidarmodel; - uint32_t year = Year + 2015; + uint32_t year = Year + 2020; sprintf(reinterpret_cast(value.serialnum), "%04d", year); sprintf(reinterpret_cast(value.serialnum + 4), "%02d", Moth); sprintf(reinterpret_cast(value.serialnum + 6), "%02d", Date); @@ -936,8 +930,10 @@ inline bool ParseLaserDebugInfo(const LaserDebug &info, device_info &value) { * @param baudrate LiDAR serial baudrate or network port * @return true if Device information is valid, otherwise false */ -inline bool printfVersionInfo(const device_info &info, const std::string &port, - int baudrate) { +inline bool printfVersionInfo(const device_info &info, + const std::string &port, + int baudrate) +{ if (info.firmware_version == 0 && info.hardware_version == 0) { return false; diff --git a/samples/etlidar_test.cpp b/samples/et_test.cpp similarity index 100% rename from samples/etlidar_test.cpp rename to samples/et_test.cpp diff --git a/samples/ydlidar_test.cpp b/samples/tri_test.cpp similarity index 96% rename from samples/ydlidar_test.cpp rename to samples/tri_test.cpp index 8e94871..87f9274 100644 --- a/samples/ydlidar_test.cpp +++ b/samples/tri_test.cpp @@ -182,9 +182,6 @@ int main(int argc, char *argv[]) { return 0; } - - - CYdLidar laser; //////////////////////string property///////////////// /// lidar port @@ -227,7 +224,7 @@ int main(int argc, char *argv[]) { /// one-way communication laser.setlidaropt(LidarPropSingleChannel, &isSingleChannel, sizeof(bool)); /// intensity - b_optvalue = false; + b_optvalue = true; laser.setlidaropt(LidarPropIntenstiy, &b_optvalue, sizeof(bool)); /// Motor DTR b_optvalue = true; diff --git a/src/CYdLidar.cpp b/src/CYdLidar.cpp index c797635..df9c2fb 100644 --- a/src/CYdLidar.cpp +++ b/src/CYdLidar.cpp @@ -755,7 +755,7 @@ bool CYdLidar::doProcessSimple(LaserScan &outscan) outscan.points.resize(all_node_count); } - // parsing version + //解析V2协议雷达扫描数据中ct信息中的设备信息 handleVersionInfoByPackage(debug); // resample sample rate resample(scanfrequency, count, tim_scan_end, tim_scan_start); @@ -925,6 +925,7 @@ void CYdLidar::handleVersionInfoByPackage(const LaserDebug &debug) } device_info info; + memset(&info, 0, sizeof(device_info)); if (ParseLaserDebugInfo(debug, info)) { @@ -1410,19 +1411,20 @@ bool CYdLidar::getDeviceInfo() bool ret = false; device_info devinfo; - result_t op_result = lidarPtr->getDeviceInfo(devinfo, - DriverInterface::DEFAULT_TIMEOUT / 2); + memset(&devinfo, 0, sizeof(device_info)); + result_t op_result = lidarPtr->getDeviceInfo(devinfo, + DriverInterface::DEFAULT_TIMEOUT / 2); if (!IS_OK(op_result)) { - fprintf(stderr, "get Device Information Error\n"); + fprintf(stderr, "[YDLIDAR INFO] Fail to get device information\n"); return false; } if (!isSupportLidar(devinfo.model)) { printf("[YDLIDAR INFO] Current SDK does not support current lidar models[%s]\n", - lidarModelToString(devinfo.model).c_str()); + lidarModelToString(devinfo.model).c_str()); return false; } @@ -1533,8 +1535,9 @@ void CYdLidar::handleSingleChannelDevice() } device_info devinfo; - result_t op_result = lidarPtr->getDeviceInfo(devinfo); + memset(&devinfo, 0, sizeof(device_info)); + result_t op_result = lidarPtr->getDeviceInfo(devinfo); if (!IS_OK(op_result)) { return; @@ -1852,10 +1855,7 @@ bool CYdLidar::checkStatus() { getDeviceHealth(); - if (!getDeviceInfo()) - { - return false; - } + getDeviceInfo(); return true; } diff --git a/src/YDlidarDriver.cpp b/src/YDlidarDriver.cpp index b1ad5d5..759bceb 100644 --- a/src/YDlidarDriver.cpp +++ b/src/YDlidarDriver.cpp @@ -1571,16 +1571,19 @@ result_t YDlidarDriver::getDeviceInfo(device_info &info, uint32_t timeout) { return RESULT_FAIL; } - if (m_SingleChannel) { - if (get_device_info_success) { + if (m_SingleChannel) + { + if (get_device_info_success) + { info = this->info_; return RESULT_OK; } - //未获取到设备信息时,返回一个无效的设备信息 - info.model = YDLIDAR_S2; - info.firmware_version = 0; - info.hardware_version = 0; - return RESULT_OK; + else + { + //未获取到设备信息时,返回一个无效的设备信息 + info.model = YDLIDAR_S2; + return RESULT_OK; + } } // disableDataGrabbing();