YDLIDAR SDK  V1.4.5
help_info.h
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2020, EAIBOT, Inc.
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the Willow Garage nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34 #pragma once
35 
36 #include <ydlidar_protocol.h>
37 #include <map>
38 
39 namespace ydlidar {
40 
41 enum YDLIDAR_MODLES {
42  YDLIDAR_F4 = 1,
43  YDLIDAR_T1 = 2,
44  YDLIDAR_F2 = 3,
45  YDLIDAR_S4 = 4,
46  YDLIDAR_G4 = 5,
47  YDLIDAR_X4 = 6,
48  YDLIDAR_G4PRO = 7,
49  YDLIDAR_F4PRO = 8,
50  YDLIDAR_R2 = 9,
51  YDLIDAR_G10 = 10,
52  YDLIDAR_S4B = 11,
53  YDLIDAR_S2 = 12,
54  YDLIDAR_G6 = 13,
55  YDLIDAR_G2A = 14,
56  YDLIDAR_G2B = 15,
57  YDLIDAR_G2C = 16,
58  YDLIDAR_G4B = 17,
59  YDLIDAR_G4C = 18,
60  YDLIDAR_G1 = 19,
62  YDLIDAR_TG15 = 100,
63  YDLIDAR_TG30 = 101,
64  YDLIDAR_TG50 = 102,
65  YDLIDAR_Tail,
66 };
67 
68 enum YDLIDAR_RATE {
69  YDLIDAR_RATE_4K = 0,
70  YDLIDAR_RATE_8K = 1,
71  YDLIDAR_RATE_9K = 2,
72  YDLIDAR_RATE_10K = 3,
73 };
74 
80 inline std::string lidarModelToString(int model) {
81  std::string name = "unkown";
82 
83  switch (model) {
84  case YDLIDAR_F4:
85  name = "F4";
86  break;
87 
88  case YDLIDAR_T1:
89  name = "T1";
90 
91  break;
92 
93  case YDLIDAR_F2:
94  name = "F2";
95 
96  break;
97 
98  case YDLIDAR_S4:
99  name = "S4";
100 
101  break;
102 
103  case YDLIDAR_G4:
104  name = "G4";
105 
106  break;
107 
108  case YDLIDAR_X4:
109  name = "X4";
110 
111  break;
112 
113  case YDLIDAR_G4PRO:
114  name = "G4PRO";
115 
116  break;
117 
118  case YDLIDAR_F4PRO:
119  name = "F4PRO";
120 
121  break;
122 
123  case YDLIDAR_R2:
124  name = "R2";
125 
126  break;
127 
128  case YDLIDAR_G10:
129  name = "G10";
130 
131  break;
132 
133  case YDLIDAR_S4B:
134  name = "S4B";
135 
136  break;
137 
138  case YDLIDAR_S2:
139  name = "S2";
140 
141  break;
142 
143  case YDLIDAR_G6:
144  name = "G6";
145 
146  break;
147 
148  case YDLIDAR_G2A:
149  name = "G2A";
150 
151  break;
152 
153  case YDLIDAR_G2B:
154  name = "G2B";
155 
156  break;
157 
158  case YDLIDAR_G2C:
159  name = "G2C";
160 
161  break;
162 
163  case YDLIDAR_G4B:
164  name = "G4B";
165 
166  break;
167 
168  case YDLIDAR_G4C:
169  name = "G4C";
170  break;
171 
172  case YDLIDAR_G1:
173  name = "G1";
174 
175  break;
176 
177  case YDLIDAR_TG15:
178  name = "TG15";
179 
180  break;
181 
182  case YDLIDAR_TG30:
183  name = "TG30";
184 
185  break;
186 
187  case YDLIDAR_TG50:
188  name = "TG50";
189  break;
190 
191  default:
192  name = "unkown";
193  break;
194  }
195 
196  return name;
197 }
198 
204 inline int lidarModelDefaultSampleRate(int model) {
205  int sample_rate = 4;
206 
207  switch (model) {
208  case YDLIDAR_F4:
209  break;
210 
211  case YDLIDAR_T1:
212  break;
213 
214  case YDLIDAR_F2:
215  break;
216 
217  case YDLIDAR_S4:
218  break;
219 
220  case YDLIDAR_G4:
221  sample_rate = 9;
222  break;
223 
224  case YDLIDAR_X4:
225  sample_rate = 5;
226  break;
227 
228  case YDLIDAR_G4PRO:
229  sample_rate = 9;
230  break;
231 
232  case YDLIDAR_F4PRO:
233  sample_rate = 4;
234  break;
235 
236  case YDLIDAR_R2:
237  sample_rate = 5;
238  break;
239 
240  case YDLIDAR_G10:
241  sample_rate = 10;
242  break;
243 
244  case YDLIDAR_S4B:
245  sample_rate = 4;
246  break;
247 
248  case YDLIDAR_S2:
249  sample_rate = 3;
250  break;
251 
252  case YDLIDAR_G6:
253  sample_rate = 18;
254  break;
255 
256  case YDLIDAR_G2A:
257  sample_rate = 5;
258  break;
259 
260  case YDLIDAR_G2B:
261  sample_rate = 5;
262  break;
263 
264  case YDLIDAR_G2C:
265  sample_rate = 4;
266  break;
267 
268  case YDLIDAR_G4B:
269  break;
270 
271  case YDLIDAR_G4C:
272  break;
273 
274  case YDLIDAR_G1:
275  sample_rate = 9;
276  break;
277 
278  case YDLIDAR_TG15:
279  sample_rate = 20;
280  break;
281 
282  case YDLIDAR_TG30:
283  sample_rate = 20;
284  break;
285 
286  case YDLIDAR_TG50:
287  sample_rate = 20;
288  break;
289 
290  default:
291  break;
292  }
293 
294  return sample_rate ;
295 }
296 
302 inline bool isOctaveLidar(int model) {
303  bool ret = false;
304 
305  if (model == YDLIDAR_G6 ||
306  model == YDLIDAR_TG15 ||
307  model == YDLIDAR_TG30 ||
308  model == YDLIDAR_TG50) {
309  ret = true;
310  }
311 
312  return ret;
313 }
314 
320 inline bool hasSampleRate(int model) {
321  bool ret = false;
322 
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) {
330  ret = true;
331  }
332 
333  return ret;
334 }
341 inline bool hasZeroAngle(int model) {
342  bool ret = false;
343 
344  if (model == YDLIDAR_R2 ||
345  model == YDLIDAR_G2A ||
346  model == YDLIDAR_G2B ||
347  model == YDLIDAR_G2C ||
348  model == YDLIDAR_G1) {
349  ret = true;
350  }
351 
352  return ret;
353 }
354 
360 inline bool hasScanFrequencyCtrl(int model) {
361  bool ret = true;
362 
363  if (model == YDLIDAR_S4 ||
364  model == YDLIDAR_S4B ||
365  model == YDLIDAR_S2 ||
366  model == YDLIDAR_X4) {
367  ret = false;
368  }
369 
370  return ret;
371 }
372 
378 inline bool isSupportLidar(int model) {
379  bool ret = true;
380 
381  if (model < YDLIDAR_F4 || (model > YDLIDAR_G1 &&
382  model < YDLIDAR_TG15) ||
383  model > YDLIDAR_TG50) {
384  ret = false;
385 
386  }
387 
388  return ret;
389 }
390 
396 inline bool hasIntensity(int model) {
397  bool ret = false;
398 
399  if (model == YDLIDAR_G2B ||
400  model == YDLIDAR_G4B ||
401  model == YDLIDAR_S4B) {
402  ret = true;
403  }
404 
405  return ret;
406 }
407 
413 inline bool isSupportMotorCtrl(int model) {
414  bool ret = false;
415 
416  if (model == YDLIDAR_X4 ||
417  model == YDLIDAR_S2 ||
418  model == YDLIDAR_S4 ||
419  model == YDLIDAR_S4B) {
420  ret = true;
421 
422  }
423 
424  return true;
425 }
426 
433 inline bool isSupportScanFrequency(int model, double frequency) {
434  bool ret = false;
435 
436  if (model >= YDLIDAR_TG15) {
437  if (3 <= frequency && frequency <= 15.7) {
438  ret = true;
439  }
440  } else {
441  if (5 <= frequency && frequency <= 15.7) {
442  ret = true;
443  }
444  }
445 
446  return ret;
447 }
448 
449 inline bool isTOFLidar(int type) {
450  bool ret = false;
451 
452  if (type == TYPE_TOF) {
453  ret = true;
454  }
455 
456  return ret;
457 }
458 
459 inline bool isOldVersionTOFLidar(int model, int Major, int Minor) {
460  bool ret = false;
461 
462  if (model == YDLIDAR_TG15 ||
463  model == YDLIDAR_TG50 ||
464  model == YDLIDAR_TG30) {
465  if (Major <= 1 && Minor <= 2) {
466  ret = true;
467  }
468 
469  }
470 
471  return ret;
472 }
473 
474 inline bool isValidSampleRate(std::map<int, int> smap) {
475  if (smap.size() < 1) {
476  return false;
477  }
478 
479  if (smap.size() == 1) {
480  if (smap.begin()->second > 1) {
481  return true;
482  }
483 
484  return false;
485  }
486 
487  return false;
488 }
489 
490 inline int ConvertUserToLidarSmaple(int model, int m_SampleRate,
491  int defaultRate) {
492  int _samp_rate = 9;
493 
494  switch (m_SampleRate) {
495  case 10:
496  _samp_rate = YDLIDAR_RATE_4K;
497  break;
498 
499  case 16:
500  _samp_rate = YDLIDAR_RATE_8K;
501  break;
502 
503  case 18:
504  _samp_rate = YDLIDAR_RATE_9K;
505  break;
506 
507  case 20:
508  _samp_rate = YDLIDAR_RATE_10K;
509  break;
510 
511  default:
512  _samp_rate = defaultRate;
513  break;
514  }
515 
516  if (!isOctaveLidar(model)) {
517  _samp_rate = 2;
518 
519  switch (m_SampleRate) {
520  case 4:
521  _samp_rate = YDLIDAR_RATE_4K;
522  break;
523 
524  case 8:
525  _samp_rate = YDLIDAR_RATE_8K;
526  break;
527 
528  case 9:
529  _samp_rate = YDLIDAR_RATE_9K;
530  break;
531 
532  default:
533  break;
534  }
535 
536  if (model == YDLIDAR_F4PRO) {
537  _samp_rate = 0;
538 
539  switch (m_SampleRate) {
540  case 4:
541  _samp_rate = YDLIDAR_RATE_4K;
542  break;
543 
544  case 6:
545  _samp_rate = YDLIDAR_RATE_8K;
546  break;
547 
548  default:
549  break;
550  }
551 
552  }
553  }
554 
555  return _samp_rate;
556 }
557 
558 
559 inline int ConvertLidarToUserSmaple(int model, int rate) {
560  int _samp_rate = 9;
561 
562  switch (rate) {
563  case YDLIDAR_RATE_4K:
564  _samp_rate = 10;
565 
566  if (!isOctaveLidar(model)) {
567  _samp_rate = 4;
568  }
569 
570  break;
571 
572  case YDLIDAR_RATE_8K:
573  _samp_rate = 16;
574 
575  if (!isOctaveLidar(model)) {
576  _samp_rate = 8;
577 
578  if (model == YDLIDAR_F4PRO) {
579  _samp_rate = 6;
580  }
581  }
582 
583  break;
584 
585  case YDLIDAR_RATE_9K:
586  _samp_rate = 18;
587 
588  if (!isOctaveLidar(model)) {
589  _samp_rate = 9;
590  }
591 
592  break;
593 
594  case YDLIDAR_RATE_10K:
595  _samp_rate = 20;
596 
597  if (!isOctaveLidar(model)) {
598  _samp_rate = 10;
599  }
600 
601  break;
602 
603  default:
604  _samp_rate = 9;
605 
606  if (!isOctaveLidar(model)) {
607  _samp_rate = 18;
608  }
609 
610  break;
611  }
612 
613  return _samp_rate;
614 }
615 
616 
617 inline bool isValidValue(uint8_t value) {
618  if (value & 0x80) {
619  return false;
620  }
621 
622  return true;
623 }
624 
625 inline bool isVersionValid(const LaserDebug &info) {
626  bool ret = false;
627 
628  if (isValidValue(info.W3F4CusMajor_W4F0CusMinor) &&
629  isValidValue(info.W4F3Model_W3F0DebugInfTranVer) &&
630  isValidValue(info.W3F4HardwareVer_W4F0FirewareMajor) &&
631  isValidValue(info.W3F4BoradHardVer_W4F0Moth)) {
632  ret = true;
633  }
634 
635  return ret;
636 }
637 
638 inline bool isSerialNumbValid(const LaserDebug &info) {
639  bool ret = false;
640 
641  if (isValidValue(info.W2F5Output2K4K5K_W5F0Date) &&
642  isValidValue(info.W1F6GNoise_W1F5SNoise_W1F4MotorCtl_W4F0SnYear) &&
643  isValidValue(info.W7F0SnNumH) &&
644  isValidValue(info.W7F0SnNumH)) {
645  ret = true;
646  }
647 
648  return ret;
649 }
650 
651 inline bool ParseLaserDebugInfo(const LaserDebug &info, device_info &value) {
652  bool ret = false;
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)
658  >> 3);
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;
662 
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));
668 
669  if (isVersionValid(info) && info.MaxDebugIndex > 0 && Year) {
670 
671  if (isSerialNumbValid(info) && info.MaxDebugIndex > 8) {
672  value.firmware_version = (CustomVerMajor << 8 | CustomVerMinor);
673  value.hardware_version = hardwareVer;
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);
680 
681  for (int i = 0; i < 16; i++) {
682  value.serialnum[i] -= 48;
683  }
684 
685  ret = true;
686  }
687  }
688 
689  return ret;
690 }
691 
692 }
693 
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