36 #include <ydlidar_protocol.h> 80 inline std::string lidarModelToString(
int model) {
81 std::string name =
"unkown";
204 inline int lidarModelDefaultSampleRate(
int model) {
302 inline bool isOctaveLidar(
int model) {
305 if (model == YDLIDAR_G6 ||
306 model == YDLIDAR_TG15 ||
307 model == YDLIDAR_TG30 ||
308 model == YDLIDAR_TG50) {
320 inline bool hasSampleRate(
int model) {
323 if (model == YDLIDAR_G4 ||
324 model == YDLIDAR_G4PRO ||
325 model == YDLIDAR_F4PRO ||
326 model == YDLIDAR_G6 ||
327 model == YDLIDAR_TG15 ||
328 model == YDLIDAR_TG50 ||
329 model == YDLIDAR_TG30) {
341 inline bool hasZeroAngle(
int model) {
344 if (model == YDLIDAR_R2 ||
345 model == YDLIDAR_G2A ||
346 model == YDLIDAR_G2B ||
347 model == YDLIDAR_G2C ||
348 model == YDLIDAR_G1) {
360 inline bool hasScanFrequencyCtrl(
int model) {
363 if (model == YDLIDAR_S4 ||
364 model == YDLIDAR_S4B ||
365 model == YDLIDAR_S2 ||
366 model == YDLIDAR_X4) {
378 inline bool isSupportLidar(
int model) {
381 if (model < YDLIDAR_F4 || (model > YDLIDAR_G1 &&
382 model < YDLIDAR_TG15) ||
383 model > YDLIDAR_TG50) {
396 inline bool hasIntensity(
int model) {
399 if (model == YDLIDAR_G2B ||
400 model == YDLIDAR_G4B ||
401 model == YDLIDAR_S4B) {
413 inline bool isSupportMotorCtrl(
int model) {
416 if (model == YDLIDAR_X4 ||
417 model == YDLIDAR_S2 ||
418 model == YDLIDAR_S4 ||
419 model == YDLIDAR_S4B) {
433 inline bool isSupportScanFrequency(
int model,
double frequency) {
436 if (model >= YDLIDAR_TG15) {
437 if (3 <= frequency && frequency <= 15.7) {
441 if (5 <= frequency && frequency <= 15.7) {
449 inline bool isTOFLidar(
int type) {
452 if (type == TYPE_TOF) {
459 inline bool isOldVersionTOFLidar(
int model,
int Major,
int Minor) {
462 if (model == YDLIDAR_TG15 ||
463 model == YDLIDAR_TG50 ||
464 model == YDLIDAR_TG30) {
465 if (Major <= 1 && Minor <= 2) {
474 inline bool isValidSampleRate(std::map<int, int> smap) {
475 if (smap.size() < 1) {
479 if (smap.size() == 1) {
480 if (smap.begin()->second > 1) {
490 inline int ConvertUserToLidarSmaple(
int model,
int m_SampleRate,
494 switch (m_SampleRate) {
496 _samp_rate = YDLIDAR_RATE_4K;
500 _samp_rate = YDLIDAR_RATE_8K;
504 _samp_rate = YDLIDAR_RATE_9K;
508 _samp_rate = YDLIDAR_RATE_10K;
512 _samp_rate = defaultRate;
516 if (!isOctaveLidar(model)) {
519 switch (m_SampleRate) {
521 _samp_rate = YDLIDAR_RATE_4K;
525 _samp_rate = YDLIDAR_RATE_8K;
529 _samp_rate = YDLIDAR_RATE_9K;
536 if (model == YDLIDAR_F4PRO) {
539 switch (m_SampleRate) {
541 _samp_rate = YDLIDAR_RATE_4K;
545 _samp_rate = YDLIDAR_RATE_8K;
559 inline int ConvertLidarToUserSmaple(
int model,
int rate) {
563 case YDLIDAR_RATE_4K:
566 if (!isOctaveLidar(model)) {
572 case YDLIDAR_RATE_8K:
575 if (!isOctaveLidar(model)) {
578 if (model == YDLIDAR_F4PRO) {
585 case YDLIDAR_RATE_9K:
588 if (!isOctaveLidar(model)) {
594 case YDLIDAR_RATE_10K:
597 if (!isOctaveLidar(model)) {
606 if (!isOctaveLidar(model)) {
617 inline bool isValidValue(uint8_t value) {
625 inline bool isVersionValid(
const LaserDebug &info) {
628 if (isValidValue(info.W3F4CusMajor_W4F0CusMinor) &&
629 isValidValue(info.W4F3Model_W3F0DebugInfTranVer) &&
630 isValidValue(info.W3F4HardwareVer_W4F0FirewareMajor) &&
631 isValidValue(info.W3F4BoradHardVer_W4F0Moth)) {
638 inline bool isSerialNumbValid(
const LaserDebug &info) {
641 if (isValidValue(info.W2F5Output2K4K5K_W5F0Date) &&
642 isValidValue(info.W1F6GNoise_W1F5SNoise_W1F4MotorCtl_W4F0SnYear) &&
643 isValidValue(info.W7F0SnNumH) &&
644 isValidValue(info.W7F0SnNumH)) {
653 uint8_t CustomVerMajor = (
static_cast<uint8_t
> 654 (info.W3F4CusMajor_W4F0CusMinor) >> 4);
655 uint8_t CustomVerMinor =
static_cast<uint8_t
> 656 (info.W3F4CusMajor_W4F0CusMinor) & 0x0F;
657 uint8_t lidarmodel = (
static_cast<uint8_t
>(info.W4F3Model_W3F0DebugInfTranVer)
659 uint8_t hardwareVer =
static_cast<uint8_t
> 660 (info.W3F4HardwareVer_W4F0FirewareMajor) >> 4;
661 uint8_t Moth =
static_cast<uint8_t
>(info.W3F4BoradHardVer_W4F0Moth) & 0x0F;
663 uint8_t Date =
static_cast<uint8_t
>(info.W2F5Output2K4K5K_W5F0Date) & 0x1F;
664 uint8_t Year =
static_cast<uint8_t
> 665 (info.W1F6GNoise_W1F5SNoise_W1F4MotorCtl_W4F0SnYear) & 0x0F;
666 uint16_t Number = ((
static_cast<uint8_t
>(info.W7F0SnNumH) << 7) |
667 static_cast<uint8_t
>(info.W7F0SnNumL));
669 if (isVersionValid(info) && info.MaxDebugIndex > 0 && Year) {
671 if (isSerialNumbValid(info) && info.MaxDebugIndex > 8) {
674 value.
model = lidarmodel;
675 uint32_t year = Year + 2015;
676 sprintf(reinterpret_cast<char *>(value.
serialnum),
"%04d", year);
677 sprintf(reinterpret_cast<char *>(value.
serialnum + 4),
"%02d", Moth);
678 sprintf(reinterpret_cast<char *>(value.
serialnum + 6),
"%02d", Date);
679 sprintf(reinterpret_cast<char *>(value.
serialnum + 8),
"%08d", Number);
681 for (
int i = 0; i < 16; i++) {
Definition: help_info.h:39
Definition: ydlidar_protocol.h:250
Definition: ydlidar_protocol.h:172
uint8_t serialnum[16]
系列号
Definition: ydlidar_protocol.h:176
uint16_t firmware_version
固件版本号
Definition: ydlidar_protocol.h:174
uint8_t model
雷达型号
Definition: ydlidar_protocol.h:173
uint8_t hardware_version
硬件版本号
Definition: ydlidar_protocol.h:175