Main MRPT website > C++ reference for MRPT 1.5.9
rplidar_driver.cpp
Go to the documentation of this file.
1 /*
2  * RPLIDAR SDK
3  *
4  * Copyright (c) 2009 - 2014 RoboPeak Team
5  * http://www.robopeak.com
6  * Copyright (c) 2014 - 2016 Shanghai Slamtec Co., Ltd.
7  * http://www.slamtec.com
8  *
9  */
10 /*
11  * Redistribution and use in source and binary forms, with or without
12  * modification, are permitted provided that the following conditions are met:
13  *
14  * 1. Redistributions of source code must retain the above copyright notice,
15  * this list of conditions and the following disclaimer.
16  *
17  * 2. Redistributions in binary form must reproduce the above copyright notice,
18  * this list of conditions and the following disclaimer in the documentation
19  * and/or other materials provided with the distribution.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
22  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
23  * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
24  * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
25  * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
26  * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
27  * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
28  * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
29  * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
30  * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
31  * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
32  *
33  */
34 
35 #include "sdkcommon.h"
36 #include "hal/abs_rxtx.h"
37 #include "hal/thread.h"
38 #include "hal/locker.h"
39 #include "hal/event.h"
40 #include "rplidar_driver_serial.h"
41 
42 #ifndef min
43 #define min(a,b) (((a) < (b)) ? (a) : (b))
44 #endif
45 
46 namespace rp { namespace standalone{ namespace rplidar {
47 
48 
49 // Factory Impl
51 {
52  switch (drivertype) {
54  return new RPlidarDriverSerialImpl();
55  default:
56  return NULL;
57  }
58 }
59 
60 
62 {
63  delete drv;
64 }
65 
66 
67 
68 // Serial Driver Impl
69 
71  : _isConnected(false)
72  , _isScanning(false)
73  , _isSupportingMotorCtrl(false)
74 {
79 }
80 
82 {
83  // force disconnection
84  disconnect();
85 
87 }
88 
89 u_result RPlidarDriverSerialImpl::connect(const char * port_path, _u32 baudrate, _u32 flag)
90 {
91  if (isConnected()) return RESULT_ALREADY_DONE;
92 
93  if (!_rxtx) return RESULT_INSUFFICIENT_MEMORY;
94 
95  {
97 
98  // establish the serial connection...
99  if (!_rxtx->bind(port_path, baudrate) || !_rxtx->open()) {
100  return RESULT_INVALID_DATA;
101  }
102 
103  _rxtx->flush(0);
104  }
105 
106  _isConnected = true;
107 
109  stopMotor();
110 
111  return RESULT_OK;
112 }
113 
115 {
116  if (!_isConnected) return ;
117  stop();
118  _rxtx->close();
119 }
120 
122 {
123  return _isConnected;
124 }
125 
126 
128 {
129  u_result ans;
130 
131  {
133 
134  if (IS_FAIL(ans = _sendCommand(RPLIDAR_CMD_RESET))) {
135  return ans;
136  }
137  }
138  return RESULT_OK;
139 }
140 
141 u_result RPlidarDriverSerialImpl::getHealth(rplidar_response_device_health_t & healthinfo, _u32 timeout)
142 {
143  u_result ans;
144 
145  if (!isConnected()) return RESULT_OPERATION_FAIL;
146 
148 
149  {
151 
153  return ans;
154  }
155 
156  rplidar_ans_header_t response_header;
157  if (IS_FAIL(ans = _waitResponseHeader(&response_header, timeout))) {
158  return ans;
159  }
160 
161  // verify whether we got a correct header
162  if (response_header.type != RPLIDAR_ANS_TYPE_DEVHEALTH) {
163  return RESULT_INVALID_DATA;
164  }
165 
166  _u32 header_size = (response_header.size_q30_subtype & RPLIDAR_ANS_HEADER_SIZE_MASK);
167  if ( header_size < sizeof(rplidar_response_device_health_t)) {
168  return RESULT_INVALID_DATA;
169  }
170 
171  if (_rxtx->waitfordata(header_size, timeout) != rp::hal::serial_rxtx::ANS_OK) {
173  }
174  _rxtx->recvdata(reinterpret_cast<_u8 *>(&healthinfo), sizeof(healthinfo));
175  }
176  return RESULT_OK;
177 }
178 
179 u_result RPlidarDriverSerialImpl::getDeviceInfo(rplidar_response_device_info_t & info, _u32 timeout)
180 {
181  u_result ans;
182 
183  if (!isConnected()) return RESULT_OPERATION_FAIL;
184 
186 
187  {
189 
191  return ans;
192  }
193 
194  rplidar_ans_header_t response_header;
195  if (IS_FAIL(ans = _waitResponseHeader(&response_header, timeout))) {
196  return ans;
197  }
198 
199  // verify whether we got a correct header
200  if (response_header.type != RPLIDAR_ANS_TYPE_DEVINFO) {
201  return RESULT_INVALID_DATA;
202  }
203 
204  _u32 header_size = (response_header.size_q30_subtype & RPLIDAR_ANS_HEADER_SIZE_MASK);
205  if (header_size < sizeof(rplidar_response_device_info_t)) {
206  return RESULT_INVALID_DATA;
207  }
208 
209  if (_rxtx->waitfordata(header_size, timeout) != rp::hal::serial_rxtx::ANS_OK) {
211  }
212 
213  _rxtx->recvdata(reinterpret_cast<_u8 *>(&info), sizeof(info));
214  }
215  return RESULT_OK;
216 }
217 
218 u_result RPlidarDriverSerialImpl::getFrequency(bool inExpressMode, size_t count, float & frequency, bool & is4kmode)
219 {
220  _u16 sample_duration = inExpressMode?_cached_sampleduration_express:_cached_sampleduration_std;
221  frequency = 1000000.0f/(count * sample_duration);
222 
223  if (sample_duration <= 277) {
224  is4kmode = true;
225  } else {
226  is4kmode = false;
227  }
228 
229  return RESULT_OK;
230 }
231 
233 {
234  u_result ans;
235  if (!isConnected()) return RESULT_OPERATION_FAIL;
236  if (_isScanning) return RESULT_ALREADY_DONE;
237 
238  stop(); //force the previous operation to stop
239 
240  {
242 
244  return ans;
245  }
246 
247  // waiting for confirmation
248  rplidar_ans_header_t response_header;
249  if (IS_FAIL(ans = _waitResponseHeader(&response_header, timeout))) {
250  return ans;
251  }
252 
253  // verify whether we got a correct header
254  if (response_header.type != RPLIDAR_ANS_TYPE_MEASUREMENT) {
255  return RESULT_INVALID_DATA;
256  }
257 
258  _u32 header_size = (response_header.size_q30_subtype & RPLIDAR_ANS_HEADER_SIZE_MASK);
259  if (header_size < sizeof(rplidar_response_measurement_node_t)) {
260  return RESULT_INVALID_DATA;
261  }
262 
263  _isScanning = true;
265  if (_cachethread.getHandle() == 0) {
266  return RESULT_OPERATION_FAIL;
267  }
268  }
269  return RESULT_OK;
270 }
271 
273 {
274  rplidar_response_device_info_t devinfo;
275 
276  support = false;
277  u_result ans = getDeviceInfo(devinfo, timeout);
278 
279  if (IS_FAIL(ans)) return ans;
280 
281  if (devinfo.firmware_version >= ((0x1<<8) | 17)) {
282  support = true;
283  rplidar_response_sample_rate_t sample_rate;
284  getSampleDuration_uS(sample_rate);
285  _cached_sampleduration_express = sample_rate.express_sample_duration_us;
286  _cached_sampleduration_std = sample_rate.std_sample_duration_us;
287  }
288 
289  return RESULT_OK;
290 }
291 
293 {
294  u_result ans;
295  if (!isConnected()) return RESULT_OPERATION_FAIL;
296  if (_isScanning) return RESULT_ALREADY_DONE;
297 
298  stop(); //force the previous operation to stop
299 
300  {
302 
303  rplidar_payload_express_scan_t scanReq;
304  scanReq.working_mode = (fixedAngle?RPLIDAR_EXPRESS_SCAN_MODE_FIXANGLE:RPLIDAR_EXPRESS_SCAN_MODE_NORMAL);
305  scanReq.reserved = 0;
306 
307  if (IS_FAIL(ans = _sendCommand(RPLIDAR_CMD_EXPRESS_SCAN,&scanReq, sizeof(scanReq)))) {
308  return ans;
309  }
310 
311  // waiting for confirmation
312  rplidar_ans_header_t response_header;
313  if (IS_FAIL(ans = _waitResponseHeader(&response_header, timeout))) {
314  return ans;
315  }
316 
317  // verify whether we got a correct header
318  if (response_header.type != RPLIDAR_ANS_TYPE_MEASUREMENT_CAPSULED) {
319  return RESULT_INVALID_DATA;
320  }
321 
322  _u32 header_size = (response_header.size_q30_subtype & RPLIDAR_ANS_HEADER_SIZE_MASK);
323  if (header_size < sizeof(rplidar_response_capsule_measurement_nodes_t)) {
324  return RESULT_INVALID_DATA;
325  }
326 
327  _isScanning = true;
329  if (_cachethread.getHandle() == 0) {
330  return RESULT_OPERATION_FAIL;
331  }
332  }
333  return RESULT_OK;
334 }
335 
336 
337 u_result RPlidarDriverSerialImpl::startScan(bool force, bool autoExpressMode)
338 {
339  bool isExpressModeSupported;
340  u_result ans;
341 
342  if (autoExpressMode) {
343  ans = checkExpressScanSupported(isExpressModeSupported);
344 
345  if (IS_FAIL(ans)) return ans;
346 
347  if (isExpressModeSupported) {
348  return startScanExpress(false);
349  }
350  }
351 
352  return startScanNormal(force);
353 }
354 
356 {
357  u_result ans;
359 
360  {
362 
363  if (IS_FAIL(ans = _sendCommand(RPLIDAR_CMD_STOP))) {
364  return ans;
365  }
366  }
367 
368  return RESULT_OK;
369 }
370 
372 {
373  rplidar_response_measurement_node_t local_buf[128];
374  size_t count = 128;
375  rplidar_response_measurement_node_t local_scan[MAX_SCAN_NODES];
376  size_t scan_count = 0;
377  u_result ans;
378  memset(local_scan, 0, sizeof(local_scan));
379 
380  _waitScanData(local_buf, count); // // always discard the first data since it may be incomplete
381 
382  while(_isScanning)
383  {
384  if (IS_FAIL(ans=_waitScanData(local_buf, count))) {
385  if (ans != RESULT_OPERATION_TIMEOUT) {
386  _isScanning = false;
387  return RESULT_OPERATION_FAIL;
388  }
389  }
390 
391  for (size_t pos = 0; pos < count; ++pos)
392  {
393  if (local_buf[pos].sync_quality & RPLIDAR_RESP_MEASUREMENT_SYNCBIT)
394  {
395  // only publish the data when it contains a full 360 degree scan
396 
397  if ((local_scan[0].sync_quality & RPLIDAR_RESP_MEASUREMENT_SYNCBIT)) {
398  _lock.lock();
399  memcpy(_cached_scan_node_buf, local_scan, scan_count*sizeof(rplidar_response_measurement_node_t));
400  _cached_scan_node_count = scan_count;
401  _dataEvt.set();
402  _lock.unlock();
403  }
404  scan_count = 0;
405  }
406  local_scan[scan_count++] = local_buf[pos];
407  if (scan_count == _countof(local_scan)) scan_count-=1; // prevent overflow
408  }
409  }
410  _isScanning = false;
411  return RESULT_OK;
412 }
413 
414 void RPlidarDriverSerialImpl::_capsuleToNormal(const rplidar_response_capsule_measurement_nodes_t & capsule, rplidar_response_measurement_node_t *nodebuffer, size_t &nodeCount)
415 {
416  nodeCount = 0;
418  int diffAngle_q8;
419  int currentStartAngle_q8 = ((capsule.start_angle_sync_q6 & 0x7FFF)<< 2);
420  int prevStartAngle_q8 = ((_cached_previous_capsuledata.start_angle_sync_q6 & 0x7FFF) << 2);
421 
422  diffAngle_q8 = (currentStartAngle_q8) - (prevStartAngle_q8);
423  if (prevStartAngle_q8 > currentStartAngle_q8) {
424  diffAngle_q8 += (360<<8);
425  }
426 
427  int angleInc_q16 = (diffAngle_q8 << 3);
428  int currentAngle_raw_q16 = (prevStartAngle_q8 << 8);
429  for (size_t pos = 0; pos < _countof(_cached_previous_capsuledata.cabins); ++pos)
430  {
431  int dist_q2[2];
432  int angle_q6[2];
433  int syncBit[2];
434 
435  dist_q2[0] = (_cached_previous_capsuledata.cabins[pos].distance_angle_1 & 0xFFFC);
436  dist_q2[1] = (_cached_previous_capsuledata.cabins[pos].distance_angle_2 & 0xFFFC);
437 
438  int angle_offset1_q3 = ( (_cached_previous_capsuledata.cabins[pos].offset_angles_q3 & 0xF) | ((_cached_previous_capsuledata.cabins[pos].distance_angle_1 & 0x3)<<4));
439  int angle_offset2_q3 = ( (_cached_previous_capsuledata.cabins[pos].offset_angles_q3 >> 4) | ((_cached_previous_capsuledata.cabins[pos].distance_angle_2 & 0x3)<<4));
440 
441  angle_q6[0] = ((currentAngle_raw_q16 - (angle_offset1_q3<<13))>>10);
442  syncBit[0] = (( (currentAngle_raw_q16 + angleInc_q16) % (360<<16)) < angleInc_q16 )?1:0;
443  currentAngle_raw_q16 += angleInc_q16;
444 
445 
446  angle_q6[1] = ((currentAngle_raw_q16 - (angle_offset2_q3<<13))>>10);
447  syncBit[1] = (( (currentAngle_raw_q16 + angleInc_q16) % (360<<16)) < angleInc_q16 )?1:0;
448  currentAngle_raw_q16 += angleInc_q16;
449 
450  for (int cpos = 0; cpos < 2; ++cpos) {
451 
452  if (angle_q6[cpos] < 0) angle_q6[cpos] += (360<<6);
453  if (angle_q6[cpos] >= (360<<6)) angle_q6[cpos] -= (360<<6);
454 
455  rplidar_response_measurement_node_t node;
456 
457  node.sync_quality = (syncBit[cpos] | ((!syncBit[cpos]) << 1));
458  if (dist_q2[cpos]) node.sync_quality |= (0x2F << RPLIDAR_RESP_MEASUREMENT_QUALITY_SHIFT);
459 
460  node.angle_q6_checkbit = (1 | (angle_q6[cpos]<<1));
461  node.distance_q2 = dist_q2[cpos];
462 
463  nodebuffer[nodeCount++] = node;
464  }
465 
466  }
467  }
468 
471 }
472 
473 
475 {
476  rplidar_response_capsule_measurement_nodes_t capsule_node;
477  rplidar_response_measurement_node_t local_buf[128];
478  size_t count = 128;
479  rplidar_response_measurement_node_t local_scan[MAX_SCAN_NODES];
480  size_t scan_count = 0;
481  u_result ans;
482  memset(local_scan, 0, sizeof(local_scan));
483 
484  _waitCapsuledNode(capsule_node); // // always discard the first data since it may be incomplete
485 
486  while(_isScanning)
487  {
488  if (IS_FAIL(ans=_waitCapsuledNode(capsule_node))) {
489  if (ans != RESULT_OPERATION_TIMEOUT && ans != RESULT_INVALID_DATA) {
490  _isScanning = false;
491  return RESULT_OPERATION_FAIL;
492  } else {
493  // current data is invalid, do not use it.
494  continue;
495  }
496  }
497 
498  _capsuleToNormal(capsule_node, local_buf, count);
499 
500  for (size_t pos = 0; pos < count; ++pos)
501  {
502  if (local_buf[pos].sync_quality & RPLIDAR_RESP_MEASUREMENT_SYNCBIT)
503  {
504  // only publish the data when it contains a full 360 degree scan
505 
506  if ((local_scan[0].sync_quality & RPLIDAR_RESP_MEASUREMENT_SYNCBIT)) {
507  _lock.lock();
508  memcpy(_cached_scan_node_buf, local_scan, scan_count*sizeof(rplidar_response_measurement_node_t));
509  _cached_scan_node_count = scan_count;
510  _dataEvt.set();
511  _lock.unlock();
512  }
513  scan_count = 0;
514  }
515  local_scan[scan_count++] = local_buf[pos];
516  if (scan_count == _countof(local_scan)) scan_count-=1; // prevent overflow
517  }
518  }
519  _isScanning = false;
520 
521  return RESULT_OK;
522 }
523 
524 u_result RPlidarDriverSerialImpl::grabScanData(rplidar_response_measurement_node_t * nodebuffer, size_t & count, _u32 timeout)
525 {
526  switch (_dataEvt.wait(timeout))
527  {
529  count = 0;
532  {
533  if(_cached_scan_node_count == 0) return RESULT_OPERATION_TIMEOUT; //consider as timeout
534 
536 
537  size_t size_to_copy = min(count, _cached_scan_node_count);
538 
539  memcpy(nodebuffer, _cached_scan_node_buf, size_to_copy*sizeof(rplidar_response_measurement_node_t));
540  count = size_to_copy;
542  }
543  return RESULT_OK;
544 
545  default:
546  count = 0;
547  return RESULT_OPERATION_FAIL;
548  }
549 }
550 
551 u_result RPlidarDriverSerialImpl::ascendScanData(rplidar_response_measurement_node_t * nodebuffer, size_t count)
552 {
553  float inc_origin_angle = 360.0f/count;
554  size_t i = 0;
555 
556  //Tune head
557  for (i = 0; i < count && nodebuffer[i].distance_q2 == 0; i++);
558 
559  // all the data is invalid
560  if (i == count) return RESULT_OPERATION_FAIL;
561 
562  while(i != 0) {
563  i--;
564  float expect_angle = (nodebuffer[i+1].angle_q6_checkbit >> RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT)/64.0f - inc_origin_angle;
565  if (expect_angle < 0.0f) expect_angle = 0.0f;
566  _u16 checkbit = nodebuffer[i].angle_q6_checkbit & RPLIDAR_RESP_MEASUREMENT_CHECKBIT;
567  nodebuffer[i].angle_q6_checkbit = (((_u16)(expect_angle * 64.0f)) << RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT) + checkbit;
568  }
569 
570  //Tune tail
571  for (i = count - 1; nodebuffer[i].distance_q2 == 0; i--);
572 
573  while(i != (count - 1)) {
574  i++;
575  float expect_angle = (nodebuffer[i-1].angle_q6_checkbit >> RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT)/64.0f + inc_origin_angle;
576  if (expect_angle > 360.0f) expect_angle -= 360.0f;
577  _u16 checkbit = nodebuffer[i].angle_q6_checkbit & RPLIDAR_RESP_MEASUREMENT_CHECKBIT;
578  nodebuffer[i].angle_q6_checkbit = (((_u16)(expect_angle * 64.0f)) << RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT) + checkbit;
579  }
580 
581  //Fill invalid angle in the scan
582  float frontAngle = (nodebuffer[0].angle_q6_checkbit >> RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT)/64.0f;
583  for (i = 1; i < count; i++) {
584  if(nodebuffer[i].distance_q2 == 0) {
585  float expect_angle = frontAngle + i * inc_origin_angle;
586  if (expect_angle > 360.0f) expect_angle -= 360.0f;
587  _u16 checkbit = nodebuffer[i].angle_q6_checkbit & RPLIDAR_RESP_MEASUREMENT_CHECKBIT;
588  nodebuffer[i].angle_q6_checkbit = (((_u16)(expect_angle * 64.0f)) << RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT) + checkbit;
589  }
590  }
591 
592  // Reorder the scan according to the angle value
593  for (i = 0; i < (count-1); i++){
594  for (size_t j = (i+1); j < count; j++){
595  if(nodebuffer[i].angle_q6_checkbit > nodebuffer[j].angle_q6_checkbit){
596  rplidar_response_measurement_node_t temp = nodebuffer[i];
597  nodebuffer[i] = nodebuffer[j];
598  nodebuffer[j] = temp;
599  }
600  }
601  }
602 
603  return RESULT_OK;
604 }
605 
606 u_result RPlidarDriverSerialImpl::_waitNode(rplidar_response_measurement_node_t * node, _u32 timeout)
607 {
608  int recvPos = 0;
609  _u32 startTs = getms();
610  _u8 recvBuffer[sizeof(rplidar_response_measurement_node_t)];
611  _u8 *nodeBuffer = (_u8*)node;
612  _u32 waitTime;
613 
614  while ((waitTime=getms() - startTs) <= timeout) {
615  size_t remainSize = sizeof(rplidar_response_measurement_node_t) - recvPos;
616  size_t recvSize;
617 
618  int ans = _rxtx->waitfordata(remainSize, timeout-waitTime, &recvSize);
620  return RESULT_OPERATION_FAIL;
621  else if (ans == rp::hal::serial_rxtx::ANS_TIMEOUT)
623 
624  if (recvSize > remainSize) recvSize = remainSize;
625 
626  _rxtx->recvdata(recvBuffer, recvSize);
627 
628  for (size_t pos = 0; pos < recvSize; ++pos) {
629  _u8 currentByte = recvBuffer[pos];
630  switch (recvPos) {
631  case 0: // expect the sync bit and its reverse in this byte
632  {
633  _u8 tmp = (currentByte>>1);
634  if ( (tmp ^ currentByte) & 0x1 ) {
635  // pass
636  } else {
637  continue;
638  }
639 
640  }
641  break;
642  case 1: // expect the highest bit to be 1
643  {
644  if (currentByte & RPLIDAR_RESP_MEASUREMENT_CHECKBIT) {
645  // pass
646  } else {
647  recvPos = 0;
648  continue;
649  }
650  }
651  break;
652  }
653  nodeBuffer[recvPos++] = currentByte;
654 
655  if (recvPos == sizeof(rplidar_response_measurement_node_t)) {
656  return RESULT_OK;
657  }
658  }
659  }
660 
662 }
663 
664 
665 u_result RPlidarDriverSerialImpl::_waitScanData(rplidar_response_measurement_node_t * nodebuffer, size_t & count, _u32 timeout)
666 {
667  if (!_isConnected) {
668  count = 0;
669  return RESULT_OPERATION_FAIL;
670  }
671 
672  size_t recvNodeCount = 0;
673  _u32 startTs = getms();
674  _u32 waitTime;
675  u_result ans;
676 
677  while ((waitTime = getms() - startTs) <= timeout && recvNodeCount < count) {
678  rplidar_response_measurement_node_t node;
679  if (IS_FAIL(ans = _waitNode(&node, timeout - waitTime))) {
680  return ans;
681  }
682 
683  nodebuffer[recvNodeCount++] = node;
684 
685  if (recvNodeCount == count) return RESULT_OK;
686  }
687  count = recvNodeCount;
689 }
690 
691 
692 u_result RPlidarDriverSerialImpl::_waitCapsuledNode(rplidar_response_capsule_measurement_nodes_t & node, _u32 timeout)
693 {
694  int recvPos = 0;
695  _u32 startTs = getms();
696  _u8 recvBuffer[sizeof(rplidar_response_capsule_measurement_nodes_t)];
697  _u8 *nodeBuffer = (_u8*)&node;
698  _u32 waitTime;
699 
700  while ((waitTime=getms() - startTs) <= timeout) {
701  size_t remainSize = sizeof(rplidar_response_capsule_measurement_nodes_t) - recvPos;
702  size_t recvSize;
703 
704  int ans = _rxtx->waitfordata(remainSize, timeout-waitTime, &recvSize);
706  return RESULT_OPERATION_FAIL;
707  else if (ans == rp::hal::serial_rxtx::ANS_TIMEOUT)
709 
710  if (recvSize > remainSize) recvSize = remainSize;
711 
712  _rxtx->recvdata(recvBuffer, recvSize);
713 
714  for (size_t pos = 0; pos < recvSize; ++pos) {
715  _u8 currentByte = recvBuffer[pos];
716  switch (recvPos) {
717  case 0: // expect the sync bit 1
718  {
719  _u8 tmp = (currentByte>>4);
721  // pass
722  } else {
724  continue;
725  }
726 
727  }
728  break;
729  case 1: // expect the sync bit 2
730  {
731  _u8 tmp = (currentByte>>4);
733  // pass
734  } else {
735  recvPos = 0;
737  continue;
738  }
739  }
740  break;
741  }
742  nodeBuffer[recvPos++] = currentByte;
743 
744  if (recvPos == sizeof(rplidar_response_capsule_measurement_nodes_t)) {
745  // calc the checksum ...
746  _u8 checksum = 0;
747  _u8 recvChecksum = ((node.s_checksum_1 & 0xF) | (node.s_checksum_2<<4));
748  for (size_t cpos = offsetof(rplidar_response_capsule_measurement_nodes_t, start_angle_sync_q6);
749  cpos < sizeof(rplidar_response_capsule_measurement_nodes_t); ++cpos)
750  {
751  checksum ^= nodeBuffer[cpos];
752  }
753  if (recvChecksum == checksum)
754  {
755  // only consider vaild if the checksum matches...
756  if (node.start_angle_sync_q6 & RPLIDAR_RESP_MEASUREMENT_EXP_SYNCBIT)
757  {
758  // this is the first capsule frame in logic, discard the previous cached data...
760  return RESULT_OK;
761  }
762  return RESULT_OK;
763  }
765  return RESULT_INVALID_DATA;
766  }
767  }
768  }
771 }
772 
773 
774 u_result RPlidarDriverSerialImpl::_sendCommand(_u8 cmd, const void * payload, size_t payloadsize)
775 {
776  _u8 pkt_header[10];
777  rplidar_cmd_packet_t * header = reinterpret_cast<rplidar_cmd_packet_t * >(pkt_header);
778  _u8 checksum = 0;
779 
780  if (!_isConnected) return RESULT_OPERATION_FAIL;
781 
782  if (payloadsize && payload) {
784  }
785 
786  header->syncByte = RPLIDAR_CMD_SYNC_BYTE;
787  header->cmd_flag = cmd;
788 
789  // send header first
790  _rxtx->senddata(pkt_header, 2) ;
791 
792  if (cmd & RPLIDAR_CMDFLAG_HAS_PAYLOAD) {
793  checksum ^= RPLIDAR_CMD_SYNC_BYTE;
794  checksum ^= cmd;
795  checksum ^= (payloadsize & 0xFF);
796 
797  // calc checksum
798  for (size_t pos = 0; pos < payloadsize; ++pos) {
799  checksum ^= ((_u8 *)payload)[pos];
800  }
801 
802  // send size
803  _u8 sizebyte = payloadsize;
804  _rxtx->senddata(&sizebyte, 1);
805 
806  // send payload
807  _rxtx->senddata((const _u8 *)payload, sizebyte);
808 
809  // send checksum
810  _rxtx->senddata(&checksum, 1);
811  }
812 
813  return RESULT_OK;
814 }
815 
816 
818 {
819  int recvPos = 0;
820  _u32 startTs = getms();
821  _u8 recvBuffer[sizeof(rplidar_ans_header_t)];
822  _u8 *headerBuffer = reinterpret_cast<_u8 *>(header);
823  _u32 waitTime;
824 
825  while ((waitTime=getms() - startTs) <= timeout) {
826  size_t remainSize = sizeof(rplidar_ans_header_t) - recvPos;
827  size_t recvSize;
828 
829  int ans = _rxtx->waitfordata(remainSize, timeout - waitTime, &recvSize);
831  return RESULT_OPERATION_FAIL;
832  else if (ans == rp::hal::serial_rxtx::ANS_TIMEOUT)
834 
835  if(recvSize > remainSize) recvSize = remainSize;
836 
837  _rxtx->recvdata(recvBuffer, recvSize);
838 
839  for (size_t pos = 0; pos < recvSize; ++pos) {
840  _u8 currentByte = recvBuffer[pos];
841  switch (recvPos) {
842  case 0:
843  if (currentByte != RPLIDAR_ANS_SYNC_BYTE1) {
844  continue;
845  }
846 
847  break;
848  case 1:
849  if (currentByte != RPLIDAR_ANS_SYNC_BYTE2) {
850  recvPos = 0;
851  continue;
852  }
853  break;
854  }
855  headerBuffer[recvPos++] = currentByte;
856 
857  if (recvPos == sizeof(rplidar_ans_header_t)) {
858  return RESULT_OK;
859  }
860  }
861  }
862 
864 }
865 
866 
867 
869 {
870  _isScanning = false;
871  _cachethread.join();
872 }
873 
874 u_result RPlidarDriverSerialImpl::getSampleDuration_uS(rplidar_response_sample_rate_t & rateInfo, _u32 timeout)
875 {
876  if (!isConnected()) return RESULT_OPERATION_FAIL;
877 
879 
880  rplidar_response_device_info_t devinfo;
881  // 1. fetch the device version first...
882  u_result ans = getDeviceInfo(devinfo, timeout);
883 
884  rateInfo.express_sample_duration_us = _cached_sampleduration_express;
885  rateInfo.std_sample_duration_us = _cached_sampleduration_std;
886 
887  if (devinfo.firmware_version < ((0x1<<8) | 17)) {
888  // provide fake data...
889 
890  return RESULT_OK;
891  }
892 
893 
894  {
896 
898  return ans;
899  }
900 
901  rplidar_ans_header_t response_header;
902  if (IS_FAIL(ans = _waitResponseHeader(&response_header, timeout))) {
903  return ans;
904  }
905 
906  // verify whether we got a correct header
907  if (response_header.type != RPLIDAR_ANS_TYPE_SAMPLE_RATE) {
908  return RESULT_INVALID_DATA;
909  }
910 
911  _u32 header_size = (response_header.size_q30_subtype & RPLIDAR_ANS_HEADER_SIZE_MASK);
912  if ( header_size < sizeof(rplidar_response_sample_rate_t)) {
913  return RESULT_INVALID_DATA;
914  }
915 
916  if (_rxtx->waitfordata(header_size, timeout) != rp::hal::serial_rxtx::ANS_OK) {
918  }
919  _rxtx->recvdata(reinterpret_cast<_u8 *>(&rateInfo), sizeof(rateInfo));
920  }
921  return RESULT_OK;
922 }
923 
925 {
926  u_result ans;
927  support = false;
928 
929  if (!isConnected()) return RESULT_OPERATION_FAIL;
930 
932 
933  {
935 
936  rplidar_payload_acc_board_flag_t flag;
937  flag.reserved = 0;
938 
939  if (IS_FAIL(ans = _sendCommand(RPLIDAR_CMD_GET_ACC_BOARD_FLAG, &flag, sizeof(flag)))) {
940  return ans;
941  }
942 
943  rplidar_ans_header_t response_header;
944  if (IS_FAIL(ans = _waitResponseHeader(&response_header, timeout))) {
945  return ans;
946  }
947 
948  // verify whether we got a correct header
949  if (response_header.type != RPLIDAR_ANS_TYPE_ACC_BOARD_FLAG) {
950  return RESULT_INVALID_DATA;
951  }
952 
953  _u32 header_size = (response_header.size_q30_subtype & RPLIDAR_ANS_HEADER_SIZE_MASK);
954  if ( header_size < sizeof(rplidar_response_acc_board_flag_t)) {
955  return RESULT_INVALID_DATA;
956  }
957 
958  if (_rxtx->waitfordata(header_size, timeout) != rp::hal::serial_rxtx::ANS_OK) {
960  }
961  rplidar_response_acc_board_flag_t acc_board_flag;
962  _rxtx->recvdata(reinterpret_cast<_u8 *>(&acc_board_flag), sizeof(acc_board_flag));
963 
964  if (acc_board_flag.support_flag & RPLIDAR_RESP_ACC_BOARD_FLAG_MOTOR_CTRL_SUPPORT_MASK) {
965  support = true;
966  }
967  }
968  return RESULT_OK;
969 }
970 
972 {
973  u_result ans;
974  rplidar_payload_motor_pwm_t motor_pwm;
975  motor_pwm.pwm_value = pwm;
976 
977  {
979 
980  if (IS_FAIL(ans = _sendCommand(RPLIDAR_CMD_SET_MOTOR_PWM,(const _u8 *)&motor_pwm, sizeof(motor_pwm)))) {
981  return ans;
982  }
983  }
984 
985  return RESULT_OK;
986 }
987 
989 {
990  if (_isSupportingMotorCtrl) { // RPLIDAR A2
992  delay(500);
993  return RESULT_OK;
994  } else { // RPLIDAR A1
996  _rxtx->clearDTR();
997  delay(500);
998  return RESULT_OK;
999  }
1000 }
1001 
1003 {
1004  if (_isSupportingMotorCtrl) { // RPLIDAR A2
1005  setMotorPWM(0);
1006  delay(500);
1007  return RESULT_OK;
1008  } else { // RPLIDAR A1
1010  _rxtx->setDTR();
1011  delay(500);
1012  return RESULT_OK;
1013  }
1014 }
1015 
1016 }}}
virtual int recvdata(unsigned char *data, size_t size)=0
#define offsetof(_structure, _field)
Definition: util.h:55
void BASE_IMPEXP memcpy(void *dest, size_t destSize, const void *src, size_t copyCount) MRPT_NO_THROWS
An OS and compiler independent version of "memcpy".
Definition: os.cpp:358
#define RPLIDAR_RESP_MEASUREMENT_CHECKBIT
Definition: rplidar_cmd.h:112
#define CLASS_THREAD(c, x)
Definition: thread.h:38
#define RPLIDAR_RESP_MEASUREMENT_EXP_SYNCBIT
Definition: rplidar_cmd.h:140
virtual bool bind(const char *portname, _u32 baudrate, _u32 flags=0)=0
GLuint GLuint GLsizei count
Definition: glext.h:3512
#define RPLIDAR_CMD_FORCE_SCAN
Definition: rplidar_cmd.h:45
#define RPLIDAR_EXPRESS_SCAN_MODE_FIXANGLE
Definition: rplidar_cmd.h:70
virtual u_result startScanNormal(bool force, _u32 timeout=DEFAULT_TIMEOUT)
virtual bool isConnected()
Returns TRUE when the connection has been established.
#define min(a, b)
#define RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT
Definition: rplidar_cmd.h:113
#define RPLIDAR_ANS_TYPE_MEASUREMENT_CAPSULED
Definition: rplidar_cmd.h:93
_u16 angle_q6_checkbit
Definition: rplidar_cmd.h:22
virtual u_result ascendScanData(rplidar_response_measurement_node_t *nodebuffer, size_t count)
Ascending the scan data according to the angle value in the scan.
void unlock()
Definition: locker.h:117
void _capsuleToNormal(const rplidar_response_capsule_measurement_nodes_t &capsule, rplidar_response_measurement_node_t *nodebuffer, size_t &nodeCount)
virtual u_result getFrequency(bool inExpressMode, size_t count, float &frequency, bool &is4kmode)
Calcuate RPLIDAR&#39;s current scanning frequency from the given scan data Please refer to the applicatio...
_u16 start_angle_sync_q6
Definition: rplidar_cmd.h:23
#define RPLIDAR_ANS_TYPE_SAMPLE_RATE
Definition: rplidar_cmd.h:96
void set(bool isSignal=true)
Definition: event.h:70
virtual u_result getSampleDuration_uS(rplidar_response_sample_rate_t &rateInfo, _u32 timeout=DEFAULT_TIMEOUT)
Get the sample duration information of the RPLIDAR.
#define RPLIDAR_ANS_HEADER_SIZE_MASK
_word_size_t getHandle()
Definition: thread.h:71
#define RESULT_OK
Definition: rptypes.h:102
virtual u_result connect(const char *port_path, _u32 baudrate, _u32 flag)
Open the specified serial port and connect to a target RPLIDAR device.
virtual u_result startScanExpress(bool fixedAngle, _u32 timeout=DEFAULT_TIMEOUT)
#define RPLIDAR_CMD_GET_DEVICE_INFO
Definition: rplidar_cmd.h:50
#define RPLIDAR_ANS_TYPE_DEVHEALTH
Definition: rplidar_cmd.h:89
#define RPLIDAR_CMD_SYNC_BYTE
#define RPLIDAR_RESP_MEASUREMENT_SYNCBIT
Definition: rplidar_cmd.h:110
virtual void setDTR()=0
virtual int senddata(const unsigned char *data, size_t size)=0
static serial_rxtx * CreateRxTx()
#define RESULT_INSUFFICIENT_MEMORY
Definition: rptypes.h:111
#define RPLIDAR_ANS_SYNC_BYTE1
virtual void disconnect()
Disconnect with the RPLIDAR and close the serial port.
#define delay(x)
Definition: win32/timer.h:39
#define RPLIDAR_CMD_GET_SAMPLERATE
Definition: rplidar_cmd.h:53
static void ReleaseRxTx(serial_rxtx *)
#define IS_FAIL(x)
Definition: rptypes.h:114
rplidar_response_capsule_measurement_nodes_t _cached_previous_capsuledata
virtual u_result grabScanData(rplidar_response_measurement_node_t *nodebuffer, size_t &count, _u32 timeout=DEFAULT_TIMEOUT)
Wait and grab a complete 0-360 degree scan data previously received.
uint8_t _u8
Definition: rptypes.h:63
virtual u_result stopMotor()
Stop RPLIDAR&#39;s motor when using accessory board.
u_result join(unsigned long timeout=-1)
#define RPLIDAR_CMD_RESET
Definition: rplidar_cmd.h:46
nv_oem6_header_t header
Novatel frame: NV_OEM6_BESTPOS.
#define RPLIDAR_RESP_ACC_BOARD_FLAG_MOTOR_CTRL_SUPPORT_MASK
Definition: rplidar_cmd.h:100
virtual bool open()=0
#define RPLIDAR_CMDFLAG_HAS_PAYLOAD
#define RPLIDAR_ANS_TYPE_ACC_BOARD_FLAG
Definition: rplidar_cmd.h:98
virtual void close()=0
virtual u_result checkMotorCtrlSupport(bool &support, _u32 timeout=DEFAULT_TIMEOUT)
Check whether the device support motor control.
virtual u_result startScan(bool force=false, bool autoExpressMode=true)
Ask the RPLIDAR core system to enter the scan mode(Normal/Express, Express mode is 4k mode) A backgro...
#define RPLIDAR_CMD_SET_MOTOR_PWM
Definition: rplidar_cmd.h:59
_u8 sync_quality
Definition: rplidar_cmd.h:21
#define getms()
Definition: linux/timer.h:57
#define RPLIDAR_CMD_EXPRESS_SCAN
Definition: rplidar_cmd.h:56
virtual u_result startMotor()
Start RPLIDAR&#39;s motor when using accessory board.
virtual u_result setMotorPWM(_u16 pwm)
Set the RPLIDAR&#39;s motor pwm when using accessory board, currently valid for A2 only.
Locker::LOCK_STATUS lock(unsigned long timeout=0xFFFFFFFF)
Definition: locker.h:60
u_result _sendCommand(_u8 cmd, const void *payload=NULL, size_t payloadsize=0)
u_result _waitResponseHeader(rplidar_ans_header_t *header, _u32 timeout=DEFAULT_TIMEOUT)
#define RESULT_OPERATION_TIMEOUT
Definition: rptypes.h:107
u_result _waitNode(rplidar_response_measurement_node_t *node, _u32 timeout=DEFAULT_TIMEOUT)
#define RPLIDAR_ANS_TYPE_MEASUREMENT
Definition: rplidar_cmd.h:91
#define RPLIDAR_CMD_GET_ACC_BOARD_FLAG
Definition: rplidar_cmd.h:60
_u16 distance_q2
Definition: rplidar_cmd.h:23
virtual u_result checkExpressScanSupported(bool &support, _u32 timeout=DEFAULT_TIMEOUT)
Check whether the device support express mode.
#define RPLIDAR_RESP_MEASUREMENT_QUALITY_SHIFT
Definition: rplidar_cmd.h:111
virtual void clearDTR()=0
virtual u_result getDeviceInfo(rplidar_response_device_info_t &, _u32 timeout=DEFAULT_TIMEOUT)
Get the device information of the RPLIDAR include the serial number, firmware version, device model etc.
virtual int waitfordata(size_t data_count, _u32 timeout=-1, size_t *returned_size=NULL)=0
backing_store_ptr info
Definition: jmemsys.h:170
virtual u_result reset(_u32 timeout=DEFAULT_TIMEOUT)
Ask the RPLIDAR core system to reset it self The host system can use the Reset operation to help RPLI...
rplidar_response_measurement_node_t _cached_scan_node_buf[2048]
uint32_t _u32
Definition: rptypes.h:69
unsigned long wait(unsigned long timeout=0xFFFFFFFF)
Definition: event.h:98
u_result _waitCapsuledNode(rplidar_response_capsule_measurement_nodes_t &node, _u32 timeout=DEFAULT_TIMEOUT)
#define RPLIDAR_RESP_MEASUREMENT_EXP_SYNC_1
Definition: rplidar_cmd.h:137
#define DEFAULT_MOTOR_PWM
Definition: rplidar_cmd.h:77
virtual u_result stop(_u32 timeout=DEFAULT_TIMEOUT)
Ask the RPLIDAR core system to stop the current scan operation and enter idle state.
virtual u_result getHealth(rplidar_response_device_health_t &, _u32 timeout=DEFAULT_TIMEOUT)
Retrieve the health status of the RPLIDAR The host system can use this operation to check whether RPL...
#define RESULT_OPERATION_FAIL
Definition: rptypes.h:106
#define RESULT_ALREADY_DONE
Definition: rptypes.h:104
#define RPLIDAR_CMD_STOP
Definition: rplidar_cmd.h:43
virtual void flush(_u32 flags)=0
#define RPLIDAR_RESP_MEASUREMENT_EXP_SYNC_2
Definition: rplidar_cmd.h:138
#define RPLIDAR_CMD_GET_DEVICE_HEALTH
Definition: rplidar_cmd.h:51
uint16_t _u16
Definition: rptypes.h:66
static RPlidarDriver * CreateDriver(_u32 drivertype=DRIVER_TYPE_SERIALPORT)
Create an RPLIDAR Driver Instance This interface should be invoked first before any other operations...
#define RPLIDAR_EXPRESS_SCAN_MODE_NORMAL
Definition: rplidar_cmd.h:69
#define RPLIDAR_ANS_SYNC_BYTE2
#define _countof(_Array)
Definition: util.h:42
#define RESULT_INVALID_DATA
Definition: rptypes.h:105
static void DisposeDriver(RPlidarDriver *drv)
Dispose the RPLIDAR Driver Instance specified by the drv parameter Applications should invoke this in...
uint32_t u_result
Definition: rptypes.h:100
#define RPLIDAR_ANS_TYPE_DEVINFO
Definition: rplidar_cmd.h:88
#define RPLIDAR_CMD_SCAN
Definition: rplidar_cmd.h:44
u_result _waitScanData(rplidar_response_measurement_node_t *nodebuffer, size_t &count, _u32 timeout=DEFAULT_TIMEOUT)



Page generated by Doxygen 1.8.14 for MRPT 1.5.9 Git: 690a4699f Wed Apr 15 19:29:53 2020 +0200 at miƩ abr 15 19:30:12 CEST 2020