Skip to content

Commit

Permalink
修改单通V2版本协议无法读取固件版本问题
Browse files Browse the repository at this point in the history
  • Loading branch information
@zhanyiaini
zhanyiaini committed Jun 24, 2022
1 parent 04f737f commit 1dde80a
Show file tree
Hide file tree
Showing 5 changed files with 47 additions and 51 deletions.
56 changes: 26 additions & 30 deletions core/common/ydlidar_help.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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:
Expand All @@ -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<uint8_t>
(info.W3F4CusMajor_W4F0CusMinor) >> 4);
(info.W3F4HardwareVer_W4F0FirewareMajor & 0x0F));
uint8_t CustomVerMinor = static_cast<uint8_t>
(info.W3F4CusMajor_W4F0CusMinor) & 0x0F;
(info.W7F0FirewareMinor);
uint8_t lidarmodel = (static_cast<uint8_t>(info.W4F3Model_W3F0DebugInfTranVer)
>> 3);
uint8_t hardwareVer = static_cast<uint8_t>
(info.W3F4HardwareVer_W4F0FirewareMajor) >> 4;
uint8_t Moth = static_cast<uint8_t>(info.W3F4BoradHardVer_W4F0Moth) & 0x0F;

uint8_t Date = static_cast<uint8_t>(info.W2F5Output2K4K5K_W5F0Date) & 0x1F;
uint8_t Year = static_cast<uint8_t>
(info.W1F6GNoise_W1F5SNoise_W1F4MotorCtl_W4F0SnYear) & 0x0F;
(info.W1F6GNoise_W1F5SNoise_W1F4MotorCtl_W4F0SnYear >> 2);
uint8_t Moth = static_cast<uint8_t>(info.W3F4BoradHardVer_W4F0Moth >> 3);
uint8_t Date = static_cast<uint8_t>(info.W2F5Output2K4K5K_W5F0Date >> 2);

uint16_t Number = ((static_cast<uint8_t>(info.W7F0SnNumH) << 7) |
static_cast<uint8_t>(info.W7F0SnNumL));

Expand All @@ -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<char *>(value.serialnum), "%04d", year);
sprintf(reinterpret_cast<char *>(value.serialnum + 4), "%02d", Moth);
sprintf(reinterpret_cast<char *>(value.serialnum + 6), "%02d", Date);
Expand All @@ -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;
Expand Down
0 samples/etlidar_test.cpp → samples/et_test.cpp
View file
File renamed without changes.
5 changes: 1 addition & 4 deletions samples/ydlidar_test.cpp → samples/tri_test.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -182,9 +182,6 @@ int main(int argc, char *argv[]) {
return 0;
}




CYdLidar laser;
//////////////////////string property/////////////////
/// lidar port
Expand Down Expand Up @@ -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;
Expand Down
20 changes: 10 additions & 10 deletions src/CYdLidar.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down Expand Up @@ -925,6 +925,7 @@ void CYdLidar::handleVersionInfoByPackage(const LaserDebug &debug)
}

device_info info;
memset(&info, 0, sizeof(device_info));

if (ParseLaserDebugInfo(debug, info))
{
Expand Down Expand Up @@ -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;
}

Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -1852,10 +1855,7 @@ bool CYdLidar::checkStatus()
{
getDeviceHealth();

if (!getDeviceInfo())
{
return false;
}
getDeviceInfo();

return true;
}
Expand Down
17 changes: 10 additions & 7 deletions src/YDlidarDriver.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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();
Expand Down

0 comments on commit 1dde80a

Please sign in to comment.