YDLIDAR SDK  V1.3.2
ydlidar_driver.h
1 #ifndef YDLIDAR_DRIVER_H
2 #define YDLIDAR_DRIVER_H
3 #include <stdlib.h>
4 #include <atomic>
5 #include "locker.h"
6 #include "serial.h"
7 #include "thread.h"
8 
9 #if !defined(__cplusplus)
10 #ifndef __cplusplus
11 #error "The YDLIDAR SDK requires a C++ compiler to be built"
12 #endif
13 #endif
14 #if !defined(_countof)
15 #define _countof(_Array) (int)(sizeof(_Array) / sizeof(_Array[0]))
16 #endif
17 
18 #define LIDAR_CMD_STOP 0x65
19 #define LIDAR_CMD_SCAN 0x60
20 #define LIDAR_CMD_FORCE_SCAN 0x61
21 #define LIDAR_CMD_RESET 0x80
22 #define LIDAR_CMD_FORCE_STOP 0x00
23 #define LIDAR_CMD_GET_EAI 0x55
24 #define LIDAR_CMD_GET_DEVICE_INFO 0x90
25 #define LIDAR_CMD_GET_DEVICE_HEALTH 0x92
26 #define LIDAR_ANS_TYPE_DEVINFO 0x4
27 #define LIDAR_ANS_TYPE_DEVHEALTH 0x6
28 #define LIDAR_CMD_SYNC_BYTE 0xA5
29 #define LIDAR_CMDFLAG_HAS_PAYLOAD 0x80
30 #define LIDAR_ANS_SYNC_BYTE1 0xA5
31 #define LIDAR_ANS_SYNC_BYTE2 0x5A
32 #define LIDAR_ANS_TYPE_MEASUREMENT 0x81
33 #define LIDAR_RESP_MEASUREMENT_SYNCBIT (0x1<<0)
34 #define LIDAR_RESP_MEASUREMENT_QUALITY_SHIFT 2
35 #define LIDAR_RESP_MEASUREMENT_CHECKBIT (0x1<<0)
36 #define LIDAR_RESP_MEASUREMENT_ANGLE_SHIFT 1
37 
38 #define LIDAR_CMD_RUN_POSITIVE 0x06
39 #define LIDAR_CMD_RUN_INVERSION 0x07
40 #define LIDAR_CMD_SET_AIMSPEED_ADDMIC 0x09
41 #define LIDAR_CMD_SET_AIMSPEED_DISMIC 0x0A
42 #define LIDAR_CMD_SET_AIMSPEED_ADD 0x0B
43 #define LIDAR_CMD_SET_AIMSPEED_DIS 0x0C
44 #define LIDAR_CMD_GET_AIMSPEED 0x0D
45 
46 #define LIDAR_CMD_SET_SAMPLING_RATE 0xD0
47 #define LIDAR_CMD_GET_SAMPLING_RATE 0xD1
48 #define LIDAR_STATUS_OK 0x0
49 #define LIDAR_STATUS_WARNING 0x1
50 #define LIDAR_STATUS_ERROR 0x2
51 
52 #define LIDAR_CMD_ENABLE_LOW_POWER 0x01
53 #define LIDAR_CMD_DISABLE_LOW_POWER 0x02
54 #define LIDAR_CMD_STATE_MODEL_MOTOR 0x05
55 #define LIDAR_CMD_ENABLE_CONST_FREQ 0x0E
56 #define LIDAR_CMD_DISABLE_CONST_FREQ 0x0F
57 
58 #define LIDAR_CMD_SAVE_SET_EXPOSURE 0x94
59 #define LIDAR_CMD_SET_LOW_EXPOSURE 0x95
60 #define LIDAR_CMD_ADD_EXPOSURE 0x96
61 #define LIDAR_CMD_DIS_EXPOSURE 0x97
62 
63 #define LIDAR_CMD_SET_HEART_BEAT 0xD9
64 #define LIDAR_CMD_SET_SETPOINTSFORONERINGFLAG 0xae
65 
66 #define PackageSampleMaxLngth 0x100
67 typedef enum {
68  CT_Normal = 0,
69  CT_RingStart = 1,
70  CT_Tail,
71 }CT;
72 #define Node_Default_Quality (10<<2)
73 #define Node_Sync 1
74 #define Node_NotSync 2
75 #define PackagePaidBytes 10
76 #define PH 0x55AA
77 
78 #if defined(_WIN32)
79 #pragma pack(1)
80 #endif
81 
82 struct node_info {
83  uint8_t sync_quality;
84  uint16_t angle_q6_checkbit;
85  uint16_t distance_q2;
86  uint64_t stamp;
87 } __attribute__((packed)) ;
88 
89 struct PackageNode {
90  uint8_t PakageSampleQuality;
91  uint16_t PakageSampleDistance;
92 }__attribute__((packed));
93 
94 struct node_package {
95  uint16_t package_Head;
96  uint8_t package_CT;
97  uint8_t nowPackageNum;
98  uint16_t packageFirstSampleAngle;
99  uint16_t packageLastSampleAngle;
100  uint16_t checkSum;
101  PackageNode packageSample[PackageSampleMaxLngth];
102 } __attribute__((packed)) ;
103 
105  uint16_t package_Head;
106  uint8_t package_CT;
107  uint8_t nowPackageNum;
108  uint16_t packageFirstSampleAngle;
109  uint16_t packageLastSampleAngle;
110  uint16_t checkSum;
111  uint16_t packageSampleDistance[PackageSampleMaxLngth];
112 } __attribute__((packed)) ;
113 
114 
115 struct device_info{
116  uint8_t model;
117  uint16_t firmware_version;
119  uint8_t serialnum[16];
120 } __attribute__((packed)) ;
121 
123  uint8_t status;
124  uint16_t error_code;
125 } __attribute__((packed)) ;
126 
128  uint8_t rate;
129 } __attribute__((packed)) ;
130 
132  uint32_t frequency;
133 } __attribute__((packed)) ;
134 
136  uint8_t rotation;
137 } __attribute__((packed)) ;
138 
140  uint8_t exposure;
141 } __attribute__((packed)) ;
142 
144  uint8_t enable;
145 } __attribute__((packed));
146 
147 struct scan_points {
148  uint8_t flag;
149 } __attribute__((packed)) ;
150 
152  uint8_t state;
153 } __attribute__((packed)) ;
154 
155 struct cmd_packet {
156  uint8_t syncByte;
157  uint8_t cmd_flag;
158  uint8_t size;
159  uint8_t data;
160 } __attribute__((packed)) ;
161 
163  uint8_t syncByte1;
164  uint8_t syncByte2;
165  uint32_t size:30;
166  uint32_t subType:2;
167  uint8_t type;
168 } __attribute__((packed));
169 
170 struct scanDot {
171  uint8_t quality;
172  float angle;
173  float dist;
174 };
175 
176 
178 struct LaserConfig {
180  float min_angle;
182  float max_angle;
188  float scan_time;
190  float min_range;
192  float max_range;
194  float range_res;
195 };
196 
197 
199 struct LaserScan {
201  std::vector<float> ranges;
203  std::vector<float> intensities;
205  uint64_t self_time_stamp;
210 };
211 
212 using namespace std;
213 using namespace serial;
214 
215 namespace ydlidar{
216 
218  {
219  public:
220  static YDlidarDriver* singleton(){
221  return _impl;
222  }
223  static void initDriver(){
224  _impl = new YDlidarDriver;
225  }
226  static void done(){
227  if(_impl){
228  delete _impl;
229  _impl = NULL;
230  }
231  }
232 
248  result_t connect(const char * port_path, uint32_t baudrate);
249 
253  void disconnect();
254 
260  static std::string getSDKVersion();
261 
268  const bool isscanning() const;
269 
276  const bool isconnected() const;
277 
286  void setIntensities(const bool isintensities);
287 
294  const bool getHeartBeat() const;
295 
303  void setHeartBeat(const bool enable);
304 
312  result_t sendHeartBeat();
313 
320  result_t getHealth(device_health & health, uint32_t timeout = DEFAULT_TIMEOUT);
321 
330  result_t getDeviceInfo(device_info & info, uint32_t timeout = DEFAULT_TIMEOUT);
331 
341  result_t startScan(bool force = false, uint32_t timeout = DEFAULT_TIMEOUT) ;
342 
349  result_t stop();
350 
351 
362  result_t grabScanData(node_info * nodebuffer, size_t & count, uint32_t timeout = DEFAULT_TIMEOUT) ;
363 
364 
375  result_t ascendScanData(node_info * nodebuffer, size_t count);
376 
385  result_t reset(uint32_t timeout = DEFAULT_TIMEOUT);
386 
393  result_t startMotor();
394 
401  result_t stopMotor();
402 
403 
413  result_t getScanFrequency(scan_frequency & frequency, uint32_t timeout = DEFAULT_TIMEOUT);
414 
424  result_t setScanFrequencyAdd(scan_frequency & frequency, uint32_t timeout = DEFAULT_TIMEOUT);
425 
435  result_t setScanFrequencyDis(scan_frequency & frequency, uint32_t timeout = DEFAULT_TIMEOUT);
436 
446  result_t setScanFrequencyAddMic(scan_frequency & frequency, uint32_t timeout = DEFAULT_TIMEOUT);
447 
457  result_t setScanFrequencyDisMic(scan_frequency & frequency, uint32_t timeout = DEFAULT_TIMEOUT);
458 
468  result_t getSamplingRate(sampling_rate & rate, uint32_t timeout = DEFAULT_TIMEOUT);
469 
479  result_t setSamplingRate(sampling_rate & rate, uint32_t timeout = DEFAULT_TIMEOUT);
480 
490  result_t setRotationPositive(scan_rotation & rotation, uint32_t timeout = DEFAULT_TIMEOUT);
491 
501  result_t setRotationInversion(scan_rotation & rotation, uint32_t timeout = DEFAULT_TIMEOUT);
502 
513  result_t enableLowerPower(function_state & state, uint32_t timeout = DEFAULT_TIMEOUT);
514 
525  result_t disableLowerPower(function_state & state, uint32_t timeout = DEFAULT_TIMEOUT);
526 
536  result_t getMotorState(function_state & state, uint32_t timeout = DEFAULT_TIMEOUT);
537 
547  result_t enableConstFreq(function_state & state, uint32_t timeout = DEFAULT_TIMEOUT);
548 
558  result_t disableConstFreq(function_state & state, uint32_t timeout = DEFAULT_TIMEOUT);
559 
570  result_t setSaveLowExposure(scan_exposure& low_exposure, uint32_t timeout = DEFAULT_TIMEOUT);
571 
581  result_t setLowExposure(scan_exposure& low_exposure, uint32_t timeout = DEFAULT_TIMEOUT);
582 
592  result_t setLowExposureAdd(scan_exposure & exposure, uint32_t timeout = DEFAULT_TIMEOUT);
593 
603  result_t setLowExposurerDis(scan_exposure & exposure, uint32_t timeout = DEFAULT_TIMEOUT);
604 
614  result_t setScanHeartbeat(scan_heart_beat& beat,uint32_t timeout = DEFAULT_TIMEOUT);
615 
625  result_t setPointsForOneRingFlag(scan_points& points,uint32_t timeout = DEFAULT_TIMEOUT);
626 
634  void simpleScanData(std::vector<scanDot> * scan_data , node_info *buffer, size_t count);
635 
636  protected:
641  YDlidarDriver();
642 
647  virtual ~YDlidarDriver();
648 
653  result_t createThread();
654 
660  result_t waitPackage(node_info * node, uint32_t timeout = DEFAULT_TIMEOUT);
661 
672  result_t waitScanData(node_info * nodebuffer, size_t & count, uint32_t timeout = DEFAULT_TIMEOUT);
673 
677  int cacheScanData();
678 
688  result_t sendCommand(uint8_t cmd, const void * payload = NULL, size_t payloadsize = 0);
689 
700  result_t waitResponseHeader(lidar_ans_header * header, uint32_t timeout = DEFAULT_TIMEOUT);
701 
713  result_t waitForData(size_t data_count,uint32_t timeout = DEFAULT_TIMEOUT, size_t * returned_size = NULL);
714 
723  result_t getData(uint8_t * data, size_t size);
724 
733  result_t sendData(const uint8_t * data, size_t size);
734 
735 
739  void disableDataGrabbing();
740 
744  void setDTR();
745 
749  void clearDTR();
750 
751 
752  public:
753  std::atomic<bool> isConnected;
754  std::atomic<bool> isScanning;
755  std::atomic<bool> isHeartbeat;
756 
757  enum {
758  DEFAULT_TIMEOUT = 2000,
759  DEFAULT_HEART_BEAT = 1000,
760  MAX_SCAN_NODES = 2048,
761  };
762  enum {
763  YDLIDAR_F4=1,
764  YDLIDAR_T1=2,
765  YDLIDAR_F2=3,
766  YDLIDAR_S4=4,
767  YDLIDAR_G4=5,
768  YDLIDAR_X4=6,
769  YDLIDAR_F4PRO=6,
770  YDLIDAR_G4C=9,
772  };
773  node_info scan_node_buf[2048];
778 
779  private:
780  int PackageSampleBytes;
781  static YDlidarDriver* _impl;
782  serial::Serial *_serial;
783  bool m_intensities;
784  int _sampling_rate;
785  int model;
786  uint32_t _baudrate;
787  bool isSupportMotorCtrl;
788  uint64_t m_ns;
789  uint64_t m_calc_ns;
790  uint32_t m_pointTime;
791  uint32_t trans_delay;
792 
793  node_package package;
794  node_packages packages;
795 
796  uint16_t package_Sample_Index;
797  float IntervalSampleAngle;
798  float IntervalSampleAngle_LastPackage;
799  uint16_t FirstSampleAngle;
800  uint16_t LastSampleAngle;
801  uint16_t CheckSun;
802 
803  uint16_t CheckSunCal;
804  uint16_t SampleNumlAndCTCal;
805  uint16_t LastSampleAngleCal;
806  bool CheckSunResult;
807  uint16_t Valu8Tou16;
808 
809  };
810 }
811 
812 #endif // YDLIDAR_DRIVER_H
float ang_increment
Scan resolution [rad].
Definition: ydlidar_driver.h:184
uint8_t rate
采样频率
Definition: ydlidar_driver.h:128
std::atomic< bool > isScanning
扫图状态
Definition: ydlidar_driver.h:754
size_t scan_node_count
激光点数
Definition: ydlidar_driver.h:774
Definition: ydlidar_driver.h:215
float min_angle
Start angle for the laser scan [rad]. 0 is forward and angles are measured clockwise when viewing YDL...
Definition: ydlidar_driver.h:180
std::atomic< bool > isHeartbeat
掉电保护状态
Definition: ydlidar_driver.h:755
uint8_t status
健康状体
Definition: ydlidar_driver.h:123
Event _dataEvent
数据同步事件
Definition: ydlidar_driver.h:775
Definition: ydlidar_driver.h:155
Definition: ydlidar_driver.h:122
Definition: locker.h:167
Thread _thread
线程id
Definition: ydlidar_driver.h:777
uint16_t error_code
错误代码
Definition: ydlidar_driver.h:124
float range_res
Range Resolution [m].
Definition: ydlidar_driver.h:194
Definition: ydlidar_driver.h:170
Definition: ydlidar_driver.h:162
A struct for returning configuration from the YDLIDAR.
Definition: ydlidar_driver.h:178
Locker _lock
线程锁
Definition: ydlidar_driver.h:776
A struct for returning laser readings from the YDLIDAR.
Definition: ydlidar_driver.h:199
uint64_t self_time_stamp
Self reported time stamp in nanoseconds.
Definition: ydlidar_driver.h:205
std::atomic< bool > isConnected
串口连接状体
Definition: ydlidar_driver.h:753
uint64_t system_time_stamp
System time when first range was measured in nanoseconds.
Definition: ydlidar_driver.h:207
Definition: thread.h:22
Definition: ydlidar_driver.h:94
Definition: ydlidar_driver.h:143
std::vector< float > ranges
Array of ranges.
Definition: ydlidar_driver.h:201
Definition: serial.h:11
uint8_t enable
掉电保护状态
Definition: ydlidar_driver.h:144
uint8_t exposure
低光功率模式
Definition: ydlidar_driver.h:140
Definition: ydlidar_driver.h:115
Definition: ydlidar_driver.h:127
std::vector< float > intensities
Array of intensities.
Definition: ydlidar_driver.h:203
float time_increment
Scan resoltuion [s].
Definition: ydlidar_driver.h:186
float scan_time
Time between scans.
Definition: ydlidar_driver.h:188
Definition: ydlidar_driver.h:217
float max_angle
Stop angle for the laser scan [rad]. 0 is forward and angles are measured clockwise when viewing YDLI...
Definition: ydlidar_driver.h:182
float max_range
Maximum range [m].
Definition: ydlidar_driver.h:192
uint16_t firmware_version
固件版本号
Definition: ydlidar_driver.h:117
Definition: ydlidar_driver.h:139
uint8_t model
雷达型号
Definition: ydlidar_driver.h:116
uint8_t hardware_version
硬件版本号
Definition: ydlidar_driver.h:118
Definition: ydlidar_driver.h:147
float min_range
Minimum range [m].
Definition: ydlidar_driver.h:190
Definition: ydlidar_driver.h:135
uint32_t frequency
扫描频率
Definition: ydlidar_driver.h:132
LaserConfig config
Configuration of scan.
Definition: ydlidar_driver.h:209
Definition: ydlidar_driver.h:82
Definition: ydlidar_driver.h:131
Definition: serial.h:107
Definition: ydlidar_driver.h:89
Definition: ydlidar_driver.h:151
Definition: locker.h:20
Definition: ydlidar_driver.h:104