diff --git a/CMakeLists.txt b/CMakeLists.txt index 0935f72..6401e40 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -5,7 +5,7 @@ project(ydlidar_sdk C CXX) # version set(YDLIDAR_SDK_VERSION_MAJOR 1) set(YDLIDAR_SDK_VERSION_MINOR 0) -set(YDLIDAR_SDK_VERSION_PATCH 1) +set(YDLIDAR_SDK_VERSION_PATCH 2) set(YDLIDAR_SDK_VERSION ${YDLIDAR_SDK_VERSION_MAJOR}.${YDLIDAR_SDK_VERSION_MINOR}.${YDLIDAR_SDK_VERSION_PATCH}) ########################################################## diff --git a/doc/howto/how_to_build_and_install.md b/doc/howto/how_to_build_and_install.md index f421e37..4888d69 100755 --- a/doc/howto/how_to_build_and_install.md +++ b/doc/howto/how_to_build_and_install.md @@ -13,7 +13,7 @@ You can install these packages using apt: ```shell sudo apt install cmake pkg-config ``` -if you want to use python API, you need to install pyhton and swig: +if you want to use python API, you need to install pyhton and swig(3.0 or higher): ```shell sudo apt-get install python swig sudo apt-get install python-pip diff --git a/python/examples/plot_tof_test.py b/python/examples/plot_tof_test.py index f949ea8..69e7137 100644 --- a/python/examples/plot_tof_test.py +++ b/python/examples/plot_tof_test.py @@ -29,6 +29,10 @@ laser.setlidaropt(ydlidar.LidarPropScanFrequency, 10.0); laser.setlidaropt(ydlidar.LidarPropSampleRate, 20); laser.setlidaropt(ydlidar.LidarPropSingleChannel, False); +laser.setlidaropt(ydlidar.LidarPropMaxAngle, 180.0); +laser.setlidaropt(ydlidar.LidarPropMinAngle, -180.0); +laser.setlidaropt(ydlidar.LidarPropMaxRange, 32.0); +laser.setlidaropt(ydlidar.LidarPropMinRange, 0.01); scan = ydlidar.LaserScan() def animate(num): diff --git a/python/examples/ydlidar_test.py b/python/examples/ydlidar_test.py index 3297c5a..9943b63 100644 --- a/python/examples/ydlidar_test.py +++ b/python/examples/ydlidar_test.py @@ -16,6 +16,10 @@ laser.setlidaropt(ydlidar.LidarPropScanFrequency, 10.0); laser.setlidaropt(ydlidar.LidarPropSampleRate, 9); laser.setlidaropt(ydlidar.LidarPropSingleChannel, False); + laser.setlidaropt(ydlidar.LidarPropMaxAngle, 180.0); + laser.setlidaropt(ydlidar.LidarPropMinAngle, -180.0); + laser.setlidaropt(ydlidar.LidarPropMaxRange, 16.0); + laser.setlidaropt(ydlidar.LidarPropMinRange, 0.08); ret = laser.initialize(); if ret: diff --git a/setup.py b/setup.py index e9b3305..77dc06e 100644 --- a/setup.py +++ b/setup.py @@ -70,7 +70,7 @@ def build_extension(self, ext): setup( name='ydlidar', - version='1.0.0', + version='1.0.2', author='Tony', author_email='chushuifurong618@eaibot.com', url='https://github.com/YDLIDAR/YDLidar-SDK', diff --git a/src/CYdLidar.cpp b/src/CYdLidar.cpp index 284475a..01a541d 100644 --- a/src/CYdLidar.cpp +++ b/src/CYdLidar.cpp @@ -151,7 +151,7 @@ bool CYdLidar::setlidaropt(int optname, const void *optval, int optlen) { m_IgnoreString = (const char *)optval; m_IgnoreArray = ydlidar::split(m_IgnoreString, ','); - if (m_IgnoreArray.size() / 2 != 0) { + if (m_IgnoreArray.size() % 2 != 0) { m_IgnoreArray.clear(); ret = false; } @@ -430,7 +430,8 @@ bool CYdLidar::turnOn() { if (checkLidarAbnormal()) { lidarPtr->stop(); fprintf(stderr, - "[CYdLidar] Failed to turn on the Lidar, because the lidar is blocked or the lidar hardware is faulty.\n"); + "[CYdLidar] Failed to turn on the Lidar, because the lidar is [%s].\n", + DriverInterface::DescribeDriverError(lidarPtr->getDriverError())); isScanning = false; return false; } @@ -565,7 +566,6 @@ bool CYdLidar::doProcessSimple(LaserScan &outscan) { float intensity = 0.0; float angle = 0.0; debug.MaxDebugIndex = 0; - std::vector points; for (int i = 0; i < count; i++) { if (isNetTOFLidar(m_LidarType)) { @@ -658,6 +658,12 @@ bool CYdLidar::doProcessSimple(LaserScan &outscan) { // Error? Retry connection } + if (lidarPtr->getDriverError() != NoError) { + fprintf(stderr, "[YDLIDAR ERROR]: %s\n", + DriverInterface::DescribeDriverError(lidarPtr->getDriverError())); + fflush(stderr); + } + m_AllNode = 0; m_FristNodeTime = tim_scan_start; } @@ -723,6 +729,18 @@ const char *CYdLidar::DescribeError() const { return value; } +/*------------------------------------------------------------- + getDriverError +-------------------------------------------------------------*/ +DriverError CYdLidar::getDriverError() const { + DriverError er = UnknownError; + + if (lidarPtr) { + return lidarPtr->getDriverError(); + } + + return er; +} /*------------------------------------------------------------- isRangeValid @@ -865,6 +883,8 @@ bool CYdLidar::checkLidarAbnormal() { return !IS_OK(op_result); } } + } else { + check_abnormal_count++; } } diff --git a/src/CYdLidar.h b/src/CYdLidar.h index 711a270..3f8ac28 100644 --- a/src/CYdLidar.h +++ b/src/CYdLidar.h @@ -911,6 +911,12 @@ class YDLIDAR_API CYdLidar { */ const char *DescribeError() const; + /** + * @brief getDriverError + * @return + */ + DriverError getDriverError() const; + private: /** * @brief check LiDAR instance and connect to LiDAR, diff --git a/src/ETLidarDriver.cpp b/src/ETLidarDriver.cpp index 8d96adb..b67756d 100644 --- a/src/ETLidarDriver.cpp +++ b/src/ETLidarDriver.cpp @@ -125,6 +125,7 @@ result_t ETLidarDriver::connect(const char *port_path, uint32_t baudrate) { m_isConnected = false; if (!configPortConnect(serial_port.c_str(), m_port)) { + setDriverError(NotOpenError); m_isValidDevice = false; return RESULT_FAIL; } @@ -155,6 +156,7 @@ result_t ETLidarDriver::connect(const char *port_path, uint32_t baudrate) { if (!dataPortConnect(m_config.deviceIp, m_config.dataRecvPort)) { stopMeasure(); + setDriverError(NotOpenError); return RESULT_FAIL; } @@ -896,6 +898,8 @@ result_t ETLidarDriver::startAutoScan(bool force, uint32_t timeout) { result_t ETLidarDriver::checkAutoConnecting() { result_t ans = RESULT_FAIL; isAutoconnting = true; + m_InvalidNodeCount = 0; + setDriverError(TimeoutError); while (isAutoReconnect && isAutoconnting) { { @@ -906,6 +910,21 @@ result_t ETLidarDriver::checkAutoConnecting() { return RESULT_FAIL; } + if (socket_data->isOpen()) { + size_t buffer_size = socket_data->available(); + + if (m_BufferSize && m_BufferSize % 90 == 0) { + setDriverError(BlockError); + } else { + if (buffer_size > 0 || m_BufferSize > 0) { + setDriverError(TrembleError); + m_BufferSize += buffer_size; + } else { + setDriverError(NotBufferError); + } + } + } + socket_data->Close(); } retryCount++; @@ -914,7 +933,14 @@ result_t ETLidarDriver::checkAutoConnecting() { retryCount = 100; } - delay(100 * retryCount); + int tempCount = 0; + + while (isAutoReconnect && isscanning() && tempCount < retryCount) { + delay(100); + tempCount++; + } + + tempCount = 0; int retryConnect = 0; while (isAutoReconnect && @@ -925,7 +951,12 @@ result_t ETLidarDriver::checkAutoConnecting() { retryConnect = 25; } - delay(200 * retryConnect); + setDriverError(NotOpenError); + + while (isAutoReconnect && isscanning() && tempCount < retryConnect) { + delay(200); + tempCount++; + } } if (!isAutoReconnect) { @@ -944,8 +975,14 @@ result_t ETLidarDriver::checkAutoConnecting() { } if (IS_OK(ans)) { + if (getDriverError() == DeviceNotFoundError) { + setDriverError(NoError); + } + isAutoconnting = false; return ans; + } else { + setDriverError(DeviceNotFoundError); } } } @@ -954,6 +991,20 @@ result_t ETLidarDriver::checkAutoConnecting() { } +void ETLidarDriver::CheckLaserStatus() { + if (m_InvalidNodeCount < 2) { + if (m_driverErrno == NoError) { + setDriverError(LaserFailureError); + } + } else { + if (m_driverErrno == LaserFailureError) { + setDriverError(NoError); + } + } + + m_InvalidNodeCount = 0; +} + int ETLidarDriver::cacheScanData() { node_info local_buf[100]; size_t count = 100; @@ -965,6 +1016,9 @@ int ETLidarDriver::cacheScanData() { int timeout_count = 0; retryCount = 0; + m_BufferSize = 0; + m_InvalidNodeCount = 0; + bool m_last_frame_valid = false; while (m_isScanning) { count = 100; @@ -980,6 +1034,11 @@ int ETLidarDriver::cacheScanData() { } return RESULT_FAIL; } else { + if (m_last_frame_valid) { + m_BufferSize = 0; + m_last_frame_valid = false; + } + ans = checkAutoConnecting(); if (IS_OK(ans)) { @@ -993,12 +1052,23 @@ int ETLidarDriver::cacheScanData() { } else { timeout_count++; local_scan[0].sync_flag = Node_NotSync; + + if (m_driverErrno == NoError) { + setDriverError(TimeoutError); + } + fprintf(stderr, "timout count: %d\n", timeout_count); fflush(stderr); } } else { timeout_count = 0; retryCount = 0; + m_BufferSize = 0; + m_last_frame_valid = true; + + if (retryCount != 0) { + setDriverError(NoError); + } } @@ -1056,6 +1126,7 @@ result_t ETLidarDriver::waitScanData(node_info *nodebuffer, size_t &count, if (node.sync_flag & LIDAR_RESP_MEASUREMENT_SYNCBIT) { count = recvNodeCount; + CheckLaserStatus(); return RESULT_OK; } @@ -1090,6 +1161,10 @@ result_t ETLidarDriver::waitPackage(node_info *node, uint32_t timeout) { (*node).distance_q2 = static_cast(DSL(frame.frameBuf[offset + 2], 8) | DSL(frame.frameBuf[offset + 3], 0)); + if ((*node).distance_q2 > 0) { + m_InvalidNodeCount++; + } + if (isV1Protocol(frame.dataFormat)) { (*node).sync_quality = (uint16_t)(DSL(frame.frameBuf[offset], 8) | DSL(frame.frameBuf[offset + 1], 0)); diff --git a/src/ETLidarDriver.h b/src/ETLidarDriver.h index e845e9a..4727a86 100644 --- a/src/ETLidarDriver.h +++ b/src/ETLidarDriver.h @@ -398,6 +398,11 @@ class ETLidarDriver : public DriverInterface { */ result_t checkAutoConnecting(); + /** + * @brief CheckLaserStatus + */ + void CheckLaserStatus(); + /** * @brief parsing scan \n */ diff --git a/src/ydlidar_driver.cpp b/src/ydlidar_driver.cpp index a383b1c..59d8b45 100644 --- a/src/ydlidar_driver.cpp +++ b/src/ydlidar_driver.cpp @@ -69,6 +69,9 @@ YDlidarDriver::YDlidarDriver(uint8_t type): LastSampleAngleCal = 0; CheckSumResult = true; Valu8Tou16 = 0; + package_CT = CT_Normal; + nowPackageNum = 0; + package_Sample_Num = 0; last_device_byte = 0x00; asyncRecvPos = 0; @@ -86,6 +89,7 @@ YDlidarDriver::YDlidarDriver(uint8_t type): get_device_info_success = false; IntervalSampleAngle_LastPackage = 0.0; m_heartbeat_ts = getms(); + m_BlockRevSize = 0; } YDlidarDriver::~YDlidarDriver() { @@ -143,6 +147,7 @@ result_t YDlidarDriver::connect(const char *port_path, uint32_t baudrate) { ScopedLocker l(_lock); if (!_serial->open()) { + setDriverError(NotOpenError); return RESULT_FAIL; } @@ -405,10 +410,14 @@ result_t YDlidarDriver::waitForData(size_t data_count, uint32_t timeout, return (result_t)_serial->waitfordata(data_count, timeout, returned_size); } -result_t YDlidarDriver::checkAutoConnecting() { +result_t YDlidarDriver::checkAutoConnecting(bool serialError) { result_t ans = RESULT_FAIL; isAutoconnting = true; + m_InvalidNodeCount = 0; + if (m_driverErrno != BlockError) { + setDriverError(TimeoutError); + } while (isAutoReconnect && isAutoconnting) { { @@ -416,10 +425,29 @@ result_t YDlidarDriver::checkAutoConnecting() { if (_serial) { if (_serial->isOpen() || m_isConnected) { - m_isConnected = false; - _serial->closePort(); - delete _serial; - _serial = NULL; + size_t buffer_size = _serial->available(); + m_BufferSize += buffer_size; + + if (m_BufferSize && m_BufferSize % 7 == 0) { + setDriverError(BlockError); + } else { + if (buffer_size > 0 || m_BufferSize > 0) { + if (m_driverErrno != BlockError) { + setDriverError(TrembleError); + } + } else { + setDriverError(NotBufferError); + } + } + + if ((retryCount % 2 == 1) || serialError) { + m_isConnected = false; + _serial->closePort(); + delete _serial; + _serial = NULL; + } else { + m_BufferSize -= buffer_size; + } } } } @@ -448,6 +476,7 @@ result_t YDlidarDriver::checkAutoConnecting() { } tempCount = 0; + setDriverError(NotOpenError); while (isAutoReconnect && isscanning() && tempCount < retryConnect) { delay(200); @@ -461,7 +490,22 @@ result_t YDlidarDriver::checkAutoConnecting() { } if (isconnected()) { - delay(100); + delay(50); + + if (!m_SingleChannel && m_driverErrno != BlockError) { + device_info devinfo; + ans = getDeviceInfo(devinfo); + + if (!IS_OK(ans)) { + stopScan(); + ans = getDeviceInfo(devinfo); + } + + if (!IS_OK(ans)) { + setDriverError(DeviceNotFoundError); + continue; + } + } { ScopedLocker l(_cmd_lock); @@ -474,7 +518,14 @@ result_t YDlidarDriver::checkAutoConnecting() { if (IS_OK(ans)) { isAutoconnting = false; + + if (getDriverError() == DeviceNotFoundError) { + setDriverError(NoError); + } + return ans; + } else { + setDriverError(DeviceNotFoundError); } } } @@ -504,6 +555,20 @@ void YDlidarDriver::KeepLiveHeartBeat() { } } +void YDlidarDriver::CheckLaserStatus() { + if (m_InvalidNodeCount < 2) { + if (m_driverErrno == NoError) { + setDriverError(LaserFailureError); + } + } else { + if (m_driverErrno == LaserFailureError) { + setDriverError(NoError); + } + } + + m_InvalidNodeCount = 0; +} + int YDlidarDriver::cacheScanData() { node_info local_buf[128]; size_t count = 128; @@ -521,11 +586,13 @@ int YDlidarDriver::cacheScanData() { int timeout_count = 0; retryCount = 0; + m_BufferSize = 0; m_heartbeat_ts = getms(); + bool m_last_frame_valid = false; while (m_isScanning) { count = 128; - ans = waitScanData(local_buf, count); + ans = waitScanData(local_buf, count, DEFAULT_TIMEOUT / 2); if (!IS_OK(ans)) { if (IS_FAIL(ans) || timeout_count > DEFAULT_TIMEOUT_COUNT) { @@ -537,7 +604,12 @@ int YDlidarDriver::cacheScanData() { } return RESULT_FAIL; } else { - ans = checkAutoConnecting(); + if (m_last_frame_valid) { + m_BufferSize = 0; + m_last_frame_valid = false; + } + + ans = checkAutoConnecting(IS_FAIL(ans)); if (IS_OK(ans)) { timeout_count = 0; @@ -550,12 +622,24 @@ int YDlidarDriver::cacheScanData() { } else { timeout_count++; local_scan[0].sync_flag = Node_NotSync; + + if (m_driverErrno == NoError) { + setDriverError(TimeoutError); + } + fprintf(stderr, "timout count: %d\n", timeout_count); fflush(stderr); } } else { timeout_count = 0; + m_last_frame_valid = true; + + if (retryCount != 0) { + setDriverError(NoError); + } + retryCount = 0; + m_BufferSize = 0; } @@ -783,238 +867,297 @@ result_t YDlidarDriver::waitDevicePackage(uint32_t timeout) { } -result_t YDlidarDriver::waitPackage(node_info *node, uint32_t timeout) { +void YDlidarDriver::checkBlockStatus(uint8_t currentByte) { + switch (m_BlockRevSize) { + case 0: + if (currentByte == LIDAR_ANS_SYNC_BYTE1) { + m_BlockRevSize++; + } + + break; + + case 1: + if (currentByte == LIDAR_ANS_SYNC_BYTE2) { + setDriverError(BlockError); + m_BlockRevSize = 0; + } + + break; + + default: + break; + } +} + +result_t YDlidarDriver::parseResponseHeader(uint8_t *packageBuffer, + uint32_t timeout) { int recvPos = 0; uint32_t startTs = getms(); uint32_t waitTime = 0; - uint8_t *packageBuffer = (m_intensities) ? (isTOFLidar(m_LidarType) ? - (uint8_t *)&tof_package.package_Head : (uint8_t *)&package.package_Head) : - (uint8_t *)&packages.package_Head; - uint8_t package_Sample_Num = 0; - int32_t AngleCorrectForDistance = 0; - int package_recvPos = 0; + m_BlockRevSize = 0; + package_Sample_Num = 0; uint8_t package_type = 0; - (*node).index = 255; - (*node).scan_frequence = 0; - (*node).error_package = 0; - (*node).debugInfo = 0xff; - - if (package_Sample_Index == 0) { - recvPos = 0; - - while ((waitTime = getms() - startTs) <= timeout) { - size_t remainSize = PackagePaidBytes - recvPos; - size_t recvSize = 0; - result_t ans = waitForData(remainSize, timeout - waitTime, &recvSize); + result_t ans = RESULT_TIMEOUT; - if (!IS_OK(ans)) { - return ans; - } + while ((waitTime = getms() - startTs) <= timeout) { + size_t remainSize = PackagePaidBytes - recvPos; + size_t recvSize = 0; + ans = waitForData(remainSize, timeout - waitTime, &recvSize); - if (recvSize > remainSize) { - recvSize = remainSize; - } + if (!IS_OK(ans)) { + return ans; + } - getData(globalRecvBuffer, recvSize); + if (recvSize > remainSize) { + recvSize = remainSize; + } - for (size_t pos = 0; pos < recvSize; ++pos) { - uint8_t currentByte = globalRecvBuffer[pos]; + getData(globalRecvBuffer, recvSize); - switch (recvPos) { - case 0: - if (currentByte == (PH & 0xFF)) { - } else { - continue; - } + for (size_t pos = 0; pos < recvSize; ++pos) { + uint8_t currentByte = globalRecvBuffer[pos]; - break; + switch (recvPos) { + case 0: + if (currentByte == (PH & 0xFF)) { + } else { + checkBlockStatus(currentByte); + continue; + } - case 1: - CheckSumCal = PH; + break; - if (currentByte == (PH >> 8)) { + case 1: + CheckSumCal = PH; - } else { - has_package_error = true; - recvPos = 0; - continue; + if (currentByte == (PH >> 8)) { + if (m_driverErrno == BlockError) { + setDriverError(NoError); } + } else { + has_package_error = true; + recvPos = 0; + continue; + } - break; + break; - case 2: - SampleNumlAndCTCal = currentByte; - package_type = currentByte & 0x01; + case 2: + SampleNumlAndCTCal = currentByte; + package_type = currentByte & 0x01; - if ((package_type == CT_Normal) || (package_type == CT_RingStart)) { - if (package_type == CT_RingStart) { - scan_frequence = (currentByte & 0xFE) >> 1; - } - } else { - has_package_error = true; - recvPos = 0; - continue; + if ((package_type == CT_Normal) || (package_type == CT_RingStart)) { + if (package_type == CT_RingStart) { + scan_frequence = (currentByte & 0xFE) >> 1; } + } else { + has_package_error = true; + recvPos = 0; + continue; + } - break; + break; - case 3: - SampleNumlAndCTCal += (currentByte * 0x100); - package_Sample_Num = currentByte; - break; + case 3: + SampleNumlAndCTCal += (currentByte * 0x100); + package_Sample_Num = currentByte; + break; - case 4: - if (currentByte & LIDAR_RESP_MEASUREMENT_CHECKBIT) { - FirstSampleAngle = currentByte; - } else { - has_package_error = true; - recvPos = 0; - continue; - } + case 4: + if (currentByte & LIDAR_RESP_MEASUREMENT_CHECKBIT) { + FirstSampleAngle = currentByte; + } else { + has_package_error = true; + recvPos = 0; + continue; + } - break; + break; - case 5: - FirstSampleAngle += currentByte * 0x100; - CheckSumCal ^= FirstSampleAngle; - FirstSampleAngle = FirstSampleAngle >> 1; - break; + case 5: + FirstSampleAngle += currentByte * 0x100; + CheckSumCal ^= FirstSampleAngle; + FirstSampleAngle = FirstSampleAngle >> 1; + break; - case 6: - if (currentByte & LIDAR_RESP_MEASUREMENT_CHECKBIT) { - LastSampleAngle = currentByte; - } else { - has_package_error = true; - recvPos = 0; - continue; - } + case 6: + if (currentByte & LIDAR_RESP_MEASUREMENT_CHECKBIT) { + LastSampleAngle = currentByte; + } else { + has_package_error = true; + recvPos = 0; + continue; + } - break; + break; - case 7: - LastSampleAngle = currentByte * 0x100 + LastSampleAngle; - LastSampleAngleCal = LastSampleAngle; - LastSampleAngle = LastSampleAngle >> 1; + case 7: + LastSampleAngle = currentByte * 0x100 + LastSampleAngle; + LastSampleAngleCal = LastSampleAngle; + LastSampleAngle = LastSampleAngle >> 1; - if (package_Sample_Num == 1) { - IntervalSampleAngle = 0; - } else { - if (LastSampleAngle < FirstSampleAngle) { - if ((FirstSampleAngle > 270 * 64) && (LastSampleAngle < 90 * 64)) { - IntervalSampleAngle = (float)((360 * 64 + LastSampleAngle - - FirstSampleAngle) / (( - package_Sample_Num - 1) * 1.0)); - IntervalSampleAngle_LastPackage = IntervalSampleAngle; - } else { - IntervalSampleAngle = IntervalSampleAngle_LastPackage; - } - } else { - IntervalSampleAngle = (float)((LastSampleAngle - FirstSampleAngle) / (( - package_Sample_Num - 1) * 1.0)); + if (package_Sample_Num == 1) { + IntervalSampleAngle = 0; + } else { + if (LastSampleAngle < FirstSampleAngle) { + if ((FirstSampleAngle > 270 * 64) && (LastSampleAngle < 90 * 64)) { + IntervalSampleAngle = (float)((360 * 64 + LastSampleAngle - + FirstSampleAngle) / (( + package_Sample_Num - 1) * 1.0)); IntervalSampleAngle_LastPackage = IntervalSampleAngle; + } else { + IntervalSampleAngle = IntervalSampleAngle_LastPackage; } + } else { + IntervalSampleAngle = (float)((LastSampleAngle - FirstSampleAngle) / (( + package_Sample_Num - 1) * 1.0)); + IntervalSampleAngle_LastPackage = IntervalSampleAngle; } + } - break; - - case 8: - CheckSum = currentByte; - break; + break; - case 9: - CheckSum += (currentByte * 0x100); - break; - } + case 8: + CheckSum = currentByte; + break; - packageBuffer[recvPos++] = currentByte; + case 9: + CheckSum += (currentByte * 0x100); + break; } - if (recvPos == PackagePaidBytes) { - package_recvPos = recvPos; - break; - } + packageBuffer[recvPos++] = currentByte; } - if (PackagePaidBytes == recvPos) { - startTs = getms(); - recvPos = 0; + if (recvPos == PackagePaidBytes) { + ans = RESULT_OK; + break; + } - while ((waitTime = getms() - startTs) <= timeout) { - size_t remainSize = package_Sample_Num * PackageSampleBytes - recvPos; - size_t recvSize = 0; - result_t ans = waitForData(remainSize, timeout - waitTime, &recvSize); + ans = RESULT_TIMEOUT; + } - if (!IS_OK(ans)) { - return ans; - } + return ans; +} - if (recvSize > remainSize) { - recvSize = remainSize; - } +result_t YDlidarDriver::parseResponseScanData(uint8_t *packageBuffer, + uint32_t timeout) { + int recvPos = 0; + uint32_t startTs = getms(); + uint32_t waitTime = 0; + result_t ans = RESULT_TIMEOUT; - getData(globalRecvBuffer, recvSize); + while ((waitTime = getms() - startTs) <= timeout) { + size_t remainSize = package_Sample_Num * PackageSampleBytes - recvPos; + size_t recvSize = 0; + ans = waitForData(remainSize, timeout - waitTime, &recvSize); - for (size_t pos = 0; pos < recvSize; ++pos) { - if (m_intensities && !isTOFLidar(m_LidarType)) { - if (recvPos % 3 == 2) { - Valu8Tou16 += globalRecvBuffer[pos] * 0x100; - CheckSumCal ^= Valu8Tou16; - } else if (recvPos % 3 == 1) { - Valu8Tou16 = globalRecvBuffer[pos]; - } else { - CheckSumCal ^= globalRecvBuffer[pos]; - } - } else { - if (recvPos % 2 == 1) { - Valu8Tou16 += globalRecvBuffer[pos] * 0x100; - CheckSumCal ^= Valu8Tou16; - } else { - Valu8Tou16 = globalRecvBuffer[pos]; - } - } + if (!IS_OK(ans)) { + return ans; + } - packageBuffer[package_recvPos + recvPos] = globalRecvBuffer[pos]; - recvPos++; - } + if (recvSize > remainSize) { + recvSize = remainSize; + } - if (package_Sample_Num * PackageSampleBytes == recvPos) { - package_recvPos += recvPos; - break; + getData(globalRecvBuffer, recvSize); + + for (size_t pos = 0; pos < recvSize; ++pos) { + if (m_intensities && !isTOFLidar(m_LidarType)) { + if (recvPos % 3 == 2) { + Valu8Tou16 += globalRecvBuffer[pos] * 0x100; + CheckSumCal ^= Valu8Tou16; + } else if (recvPos % 3 == 1) { + Valu8Tou16 = globalRecvBuffer[pos]; + } else { + CheckSumCal ^= globalRecvBuffer[pos]; + } + } else { + if (recvPos % 2 == 1) { + Valu8Tou16 += globalRecvBuffer[pos] * 0x100; + CheckSumCal ^= Valu8Tou16; + } else { + Valu8Tou16 = globalRecvBuffer[pos]; } } - if (package_Sample_Num * PackageSampleBytes != recvPos) { - return RESULT_FAIL; - } - } else { - return RESULT_FAIL; + packageBuffer[PackagePaidBytes + recvPos] = globalRecvBuffer[pos]; + recvPos++; } - CheckSumCal ^= SampleNumlAndCTCal; - CheckSumCal ^= LastSampleAngleCal; + if (package_Sample_Num * PackageSampleBytes == recvPos) { + ans = RESULT_OK; + break; + } + } - if (CheckSumCal != CheckSum) { - CheckSumResult = false; - has_package_error = true; - (*node).error_package = 1; - } else { - CheckSumResult = true; + if (package_Sample_Num * PackageSampleBytes != recvPos) { + return RESULT_FAIL; + } + + return ans; +} + +result_t YDlidarDriver::waitPackage(node_info *node, uint32_t timeout) { + (*node).index = 255; + (*node).scan_frequence = 0; + (*node).error_package = 0; + (*node).debugInfo = 0xff; + + if (package_Sample_Index == 0) { + uint8_t *packageBuffer = (m_intensities) ? (isTOFLidar(m_LidarType) ? + (uint8_t *)&tof_package.package_Head : (uint8_t *)&package.package_Head) : + (uint8_t *)&packages.package_Head; + result_t ans = parseResponseHeader(packageBuffer, timeout); + + if (!IS_OK(ans)) { + return ans; + } + + ans = parseResponseScanData(packageBuffer, timeout); + + if (!IS_OK(ans)) { + return ans; } + calcuteCheckSum(node); + calcutePackageCT(); } - uint8_t package_CT; + parseNodeDebugFromBuffer(node); + parseNodeFromeBuffer(node); + return RESULT_OK; +} +void YDlidarDriver::calcuteCheckSum(node_info *node) { + CheckSumCal ^= SampleNumlAndCTCal; + CheckSumCal ^= LastSampleAngleCal; + + if (CheckSumCal != CheckSum) { + CheckSumResult = false; + has_package_error = true; + (*node).error_package = 1; + } else { + CheckSumResult = true; + } +} + +void YDlidarDriver::calcutePackageCT() { if (m_intensities) { if (isTOFLidar(m_LidarType)) { package_CT = tof_package.package_CT; + nowPackageNum = tof_package.nowPackageNum; } else { package_CT = package.package_CT; + nowPackageNum = package.nowPackageNum; } } else { package_CT = packages.package_CT; + nowPackageNum = packages.nowPackageNum; } +} +void YDlidarDriver::parseNodeDebugFromBuffer(node_info *node) { if ((package_CT & 0x01) == CT_Normal) { (*node).sync_flag = Node_NotSync; (*node).debugInfo = 0xff; @@ -1041,7 +1184,10 @@ result_t YDlidarDriver::waitPackage(node_info *node, uint32_t timeout) { (*node).scan_frequence = scan_frequence; } } +} +void YDlidarDriver::parseNodeFromeBuffer(node_info *node) { + int32_t AngleCorrectForDistance = 0; (*node).sync_quality = Node_Default_Quality; (*node).delay_time = 0; (*node).stamp = 0; @@ -1084,6 +1230,8 @@ result_t YDlidarDriver::waitPackage(node_info *node, uint32_t timeout) { *node).distance_q2 / 4.0))) * 180.0 / 3.1415) * 64.0); } } + + m_InvalidNodeCount++; } else { AngleCorrectForDistance = 0; } @@ -1114,27 +1262,12 @@ result_t YDlidarDriver::waitPackage(node_info *node, uint32_t timeout) { (*node).scan_frequence = 0; } - - uint8_t nowPackageNum; - - if (m_intensities) { - if (isTOFLidar(m_LidarType)) { - nowPackageNum = tof_package.nowPackageNum; - } else { - nowPackageNum = package.nowPackageNum; - } - } else { - nowPackageNum = packages.nowPackageNum; - } - package_Sample_Index++; if (package_Sample_Index >= nowPackageNum) { package_Sample_Index = 0; CheckSumResult = false; } - - return RESULT_OK; } result_t YDlidarDriver::waitScanData(node_info *nodebuffer, size_t &count, @@ -1168,11 +1301,6 @@ result_t YDlidarDriver::waitScanData(node_info *nodebuffer, size_t &count, size_t packageNum = 0; size_t Number = 0; size_t PackageSize = TrianglePackageDataSize; - -// if (isTOFLidar(m_LidarType)) { -// PackageSize = TOFPackageDataSize; -// } - packageNum = size / PackageSize; Number = size % PackageSize; delayTime = packageNum * (PackageSize - PackagePaidBytes) * m_PointTime / 2; @@ -1186,6 +1314,7 @@ result_t YDlidarDriver::waitScanData(node_info *nodebuffer, size_t &count, nodebuffer[recvNodeCount - 1].scan_frequence = node.scan_frequence; nodebuffer[recvNodeCount - 1].stamp = getTime(); count = recvNodeCount; + CheckLaserStatus(); return RESULT_OK; } @@ -1409,7 +1538,7 @@ result_t YDlidarDriver::getDeviceInfo(device_info &info, uint32_t timeout) { return RESULT_OK; } - disableDataGrabbing(); +// disableDataGrabbing(); flushSerial(); { ScopedLocker l(_lock); diff --git a/src/ydlidar_driver.h b/src/ydlidar_driver.h index 0f66d92..78f2f5c 100644 --- a/src/ydlidar_driver.h +++ b/src/ydlidar_driver.h @@ -412,6 +412,24 @@ class YDlidarDriver : public DriverInterface { * @retval RESULT_FAILE failed */ result_t waitDevicePackage(uint32_t timeout = DEFAULT_TIMEOUT); + + /** + * @brief parseResponseHeader + * @param packageBuffer + * @param timeout + * @return + */ + result_t parseResponseHeader(uint8_t *packageBuffer, + uint32_t timeout = DEFAULT_TIMEOUT); + + /** + * @brief parseResponseScanData + * @param packageBuffer + * @param timeout + * @return + */ + result_t parseResponseScanData(uint8_t *packageBuffer, + uint32_t timeout = DEFAULT_TIMEOUT); /** * @brief Unpacking \n * @param[in] node lidar point information @@ -524,7 +542,7 @@ class YDlidarDriver : public DriverInterface { /*! * @brief checkAutoConnecting */ - result_t checkAutoConnecting(); + result_t checkAutoConnecting(bool serialError = true); /** * @brief autoHeartBeat @@ -537,6 +555,35 @@ class YDlidarDriver : public DriverInterface { */ void KeepLiveHeartBeat(); + /** + * @brief CheckLaserStatus + */ + void CheckLaserStatus(); + + /** + * @brief checkBlockStatus + */ + void checkBlockStatus(uint8_t currentByte); + + /** + * @brief calcuteCheckSum + * @param node + */ + void calcuteCheckSum(node_info *node); + /** + * @brief calcutePackageCT + */ + void calcutePackageCT(); + /** + * @brief parseNodeDebugFromBuffer + */ + void parseNodeDebugFromBuffer(node_info *node); + + /** + * @brief parseNodeFromeBuffer + */ + void parseNodeFromeBuffer(node_info *node); + private: /// package sample bytes int PackageSampleBytes; @@ -576,6 +623,9 @@ class YDlidarDriver : public DriverInterface { uint16_t LastSampleAngleCal; bool CheckSumResult; uint16_t Valu8Tou16; + uint8_t package_CT; + uint8_t nowPackageNum; + uint8_t package_Sample_Num; uint8_t *globalRecvBuffer; bool has_device_header; @@ -596,6 +646,7 @@ class YDlidarDriver : public DriverInterface { int package_index; bool has_package_error; uint32_t m_heartbeat_ts; + uint8_t m_BlockRevSize; };