Main MRPT website > C++ reference for MRPT 1.9.9
rplidar_driver.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 /*
10  * Redistribution and use in source and binary forms, with or without
11  * modification, are permitted provided that the following conditions are met:
12  *
13  * 1. Redistributions of source code must retain the above copyright notice,
14  * this list of conditions and the following disclaimer.
15  *
16  * 2. Redistributions in binary form must reproduce the above copyright notice,
17  * this list of conditions and the following disclaimer in the documentation
18  * and/or other materials provided with the distribution.
19  *
20  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
21  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
22  * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
23  * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
24  * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
25  * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
26  * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
27  * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
28  * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
29  * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
30  * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
31  *
32  */
33 
34 #include "sdkcommon.h"
35 #include "hal/abs_rxtx.h"
36 #include "hal/thread.h"
37 #include "hal/locker.h"
38 #include "hal/event.h"
39 #include "rplidar_driver_serial.h"
40 
41 #ifndef min
42 #define min(a, b) (((a) < (b)) ? (a) : (b))
43 #endif
44 
45 namespace rp
46 {
47 namespace standalone
48 {
49 namespace rplidar
50 {
51 // Factory Impl
53 {
54  switch (drivertype)
55  {
57  return new RPlidarDriverSerialImpl();
58  default:
59  return nullptr;
60  }
61 }
62 
63 void RPlidarDriver::DisposeDriver(RPlidarDriver* drv) { delete drv; }
64 // Serial Driver Impl
65 
67  : _isConnected(false), _isScanning(false), _isSupportingMotorCtrl(false)
68 {
73 }
74 
76 {
77  // force disconnection
78  disconnect();
79 
81 }
82 
84  const char* port_path, _u32 baudrate, _u32 flag)
85 {
86  if (isConnected()) return RESULT_ALREADY_DONE;
87 
88  if (!_rxtx) return RESULT_INSUFFICIENT_MEMORY;
89 
90  {
92 
93  // establish the serial connection...
94  if (!_rxtx->bind(port_path, baudrate) || !_rxtx->open())
95  {
96  return RESULT_INVALID_DATA;
97  }
98 
99  _rxtx->flush(0);
100  }
101 
102  _isConnected = true;
103 
105  stopMotor();
106 
107  return RESULT_OK;
108 }
109 
111 {
112  if (!_isConnected) return;
113  stop();
114  _rxtx->close();
115 }
116 
119 {
120  u_result ans;
121 
122  {
124 
126  {
127  return ans;
128  }
129  }
130  return RESULT_OK;
131 }
132 
134  rplidar_response_device_health_t& healthinfo, _u32 timeout)
135 {
136  u_result ans;
137 
138  if (!isConnected()) return RESULT_OPERATION_FAIL;
139 
141 
142  {
144 
146  {
147  return ans;
148  }
149 
150  rplidar_ans_header_t response_header;
151  if (IS_FAIL(ans = _waitResponseHeader(&response_header, timeout)))
152  {
153  return ans;
154  }
155 
156  // verify whether we got a correct header
157  if (response_header.type != RPLIDAR_ANS_TYPE_DEVHEALTH)
158  {
159  return RESULT_INVALID_DATA;
160  }
161 
162  _u32 header_size =
163  (response_header.size_q30_subtype & RPLIDAR_ANS_HEADER_SIZE_MASK);
164  if (header_size < sizeof(rplidar_response_device_health_t))
165  {
166  return RESULT_INVALID_DATA;
167  }
168 
169  if (_rxtx->waitfordata(header_size, timeout) !=
171  {
173  }
174  _rxtx->recvdata(
175  reinterpret_cast<_u8*>(&healthinfo), sizeof(healthinfo));
176  }
177  return RESULT_OK;
178 }
179 
181  rplidar_response_device_info_t& info, _u32 timeout)
182 {
183  u_result ans;
184 
185  if (!isConnected()) return RESULT_OPERATION_FAIL;
186 
188 
189  {
191 
193  {
194  return ans;
195  }
196 
197  rplidar_ans_header_t response_header;
198  if (IS_FAIL(ans = _waitResponseHeader(&response_header, timeout)))
199  {
200  return ans;
201  }
202 
203  // verify whether we got a correct header
204  if (response_header.type != RPLIDAR_ANS_TYPE_DEVINFO)
205  {
206  return RESULT_INVALID_DATA;
207  }
208 
209  _u32 header_size =
210  (response_header.size_q30_subtype & RPLIDAR_ANS_HEADER_SIZE_MASK);
211  if (header_size < sizeof(rplidar_response_device_info_t))
212  {
213  return RESULT_INVALID_DATA;
214  }
215 
216  if (_rxtx->waitfordata(header_size, timeout) !=
218  {
220  }
221 
222  _rxtx->recvdata(reinterpret_cast<_u8*>(&info), sizeof(info));
223  }
224  return RESULT_OK;
225 }
226 
228  bool inExpressMode, size_t count, float& frequency, bool& is4kmode)
229 {
230  _u16 sample_duration = inExpressMode ? _cached_sampleduration_express
232  frequency = 1000000.0f / (count * sample_duration);
233 
234  if (sample_duration <= 277)
235  {
236  is4kmode = true;
237  }
238  else
239  {
240  is4kmode = false;
241  }
242 
243  return RESULT_OK;
244 }
245 
247 {
248  u_result ans;
249  if (!isConnected()) return RESULT_OPERATION_FAIL;
250  if (_isScanning) return RESULT_ALREADY_DONE;
251 
252  stop(); // force the previous operation to stop
253 
254  {
256 
257  if (IS_FAIL(
258  ans = _sendCommand(
260  {
261  return ans;
262  }
263 
264  // waiting for confirmation
265  rplidar_ans_header_t response_header;
266  if (IS_FAIL(ans = _waitResponseHeader(&response_header, timeout)))
267  {
268  return ans;
269  }
270 
271  // verify whether we got a correct header
272  if (response_header.type != RPLIDAR_ANS_TYPE_MEASUREMENT)
273  {
274  return RESULT_INVALID_DATA;
275  }
276 
277  _u32 header_size =
278  (response_header.size_q30_subtype & RPLIDAR_ANS_HEADER_SIZE_MASK);
279  if (header_size < sizeof(rplidar_response_measurement_node_t))
280  {
281  return RESULT_INVALID_DATA;
282  }
283 
284  _isScanning = true;
286  if (_cachethread.getHandle() == 0)
287  {
288  return RESULT_OPERATION_FAIL;
289  }
290  }
291  return RESULT_OK;
292 }
293 
295  bool& support, _u32 timeout)
296 {
297  rplidar_response_device_info_t devinfo;
298 
299  support = false;
300  u_result ans = getDeviceInfo(devinfo, timeout);
301 
302  if (IS_FAIL(ans)) return ans;
303 
304  if (devinfo.firmware_version >= ((0x1 << 8) | 17))
305  {
306  support = true;
307  rplidar_response_sample_rate_t sample_rate;
308  getSampleDuration_uS(sample_rate);
309  _cached_sampleduration_express = sample_rate.express_sample_duration_us;
310  _cached_sampleduration_std = sample_rate.std_sample_duration_us;
311  }
312 
313  return RESULT_OK;
314 }
315 
317  bool fixedAngle, _u32 timeout)
318 {
319  u_result ans;
320  if (!isConnected()) return RESULT_OPERATION_FAIL;
321  if (_isScanning) return RESULT_ALREADY_DONE;
322 
323  stop(); // force the previous operation to stop
324 
325  {
327 
328  rplidar_payload_express_scan_t scanReq;
329  scanReq.working_mode =
332  scanReq.reserved = 0;
333 
334  if (IS_FAIL(
335  ans = _sendCommand(
336  RPLIDAR_CMD_EXPRESS_SCAN, &scanReq, sizeof(scanReq))))
337  {
338  return ans;
339  }
340 
341  // waiting for confirmation
342  rplidar_ans_header_t response_header;
343  if (IS_FAIL(ans = _waitResponseHeader(&response_header, timeout)))
344  {
345  return ans;
346  }
347 
348  // verify whether we got a correct header
349  if (response_header.type != RPLIDAR_ANS_TYPE_MEASUREMENT_CAPSULED)
350  {
351  return RESULT_INVALID_DATA;
352  }
353 
354  _u32 header_size =
355  (response_header.size_q30_subtype & RPLIDAR_ANS_HEADER_SIZE_MASK);
356  if (header_size < sizeof(rplidar_response_capsule_measurement_nodes_t))
357  {
358  return RESULT_INVALID_DATA;
359  }
360 
361  _isScanning = true;
362  _cachethread =
364  if (_cachethread.getHandle() == 0)
365  {
366  return RESULT_OPERATION_FAIL;
367  }
368  }
369  return RESULT_OK;
370 }
371 
372 u_result RPlidarDriverSerialImpl::startScan(bool force, bool autoExpressMode)
373 {
374  bool isExpressModeSupported;
375  u_result ans;
376 
377  if (autoExpressMode)
378  {
379  ans = checkExpressScanSupported(isExpressModeSupported);
380 
381  if (IS_FAIL(ans)) return ans;
382 
383  if (isExpressModeSupported)
384  {
385  return startScanExpress(false);
386  }
387  }
388 
389  return startScanNormal(force);
390 }
391 
393 {
394  u_result ans;
396 
397  {
399 
401  {
402  return ans;
403  }
404  }
405 
406  return RESULT_OK;
407 }
408 
410 {
411  rplidar_response_measurement_node_t local_buf[128];
412  size_t count = 128;
413  rplidar_response_measurement_node_t local_scan[MAX_SCAN_NODES];
414  size_t scan_count = 0;
415  u_result ans;
416  memset(local_scan, 0, sizeof(local_scan));
417 
419  local_buf,
420  count); // // always discard the first data since it may be incomplete
421 
422  while (_isScanning)
423  {
424  if (IS_FAIL(ans = _waitScanData(local_buf, count)))
425  {
426  if (ans != RESULT_OPERATION_TIMEOUT)
427  {
428  _isScanning = false;
429  return RESULT_OPERATION_FAIL;
430  }
431  }
432 
433  for (size_t pos = 0; pos < count; ++pos)
434  {
435  if (local_buf[pos].sync_quality & RPLIDAR_RESP_MEASUREMENT_SYNCBIT)
436  {
437  // only publish the data when it contains a full 360 degree scan
438 
439  if ((local_scan[0].sync_quality &
441  {
442  _lock.lock();
443  memcpy(
444  _cached_scan_node_buf, local_scan,
445  scan_count *
446  sizeof(rplidar_response_measurement_node_t));
447  _cached_scan_node_count = scan_count;
448  _dataEvt.set();
449  _lock.unlock();
450  }
451  scan_count = 0;
452  }
453  local_scan[scan_count++] = local_buf[pos];
454  if (scan_count == _countof(local_scan))
455  scan_count -= 1; // prevent overflow
456  }
457  }
458  _isScanning = false;
459  return RESULT_OK;
460 }
461 
463  const rplidar_response_capsule_measurement_nodes_t& capsule,
464  rplidar_response_measurement_node_t* nodebuffer, size_t& nodeCount)
465 {
466  nodeCount = 0;
468  {
469  int diffAngle_q8;
470  int currentStartAngle_q8 =
471  ((capsule.start_angle_sync_q6 & 0x7FFF) << 2);
472  int prevStartAngle_q8 =
473  ((_cached_previous_capsuledata.start_angle_sync_q6 & 0x7FFF) << 2);
474 
475  diffAngle_q8 = (currentStartAngle_q8) - (prevStartAngle_q8);
476  if (prevStartAngle_q8 > currentStartAngle_q8)
477  {
478  diffAngle_q8 += (360 << 8);
479  }
480 
481  int angleInc_q16 = (diffAngle_q8 << 3);
482  int currentAngle_raw_q16 = (prevStartAngle_q8 << 8);
483  for (size_t pos = 0;
484  pos < _countof(_cached_previous_capsuledata.cabins); ++pos)
485  {
486  int dist_q2[2];
487  int angle_q6[2];
488  int syncBit[2];
489 
490  dist_q2[0] =
491  (_cached_previous_capsuledata.cabins[pos].distance_angle_1 &
492  0xFFFC);
493  dist_q2[1] =
494  (_cached_previous_capsuledata.cabins[pos].distance_angle_2 &
495  0xFFFC);
496 
497  int angle_offset1_q3 =
498  ((_cached_previous_capsuledata.cabins[pos].offset_angles_q3 &
499  0xF) |
500  ((_cached_previous_capsuledata.cabins[pos].distance_angle_1 &
501  0x3)
502  << 4));
503  int angle_offset2_q3 =
504  ((_cached_previous_capsuledata.cabins[pos].offset_angles_q3 >>
505  4) |
506  ((_cached_previous_capsuledata.cabins[pos].distance_angle_2 &
507  0x3)
508  << 4));
509 
510  angle_q6[0] =
511  ((currentAngle_raw_q16 - (angle_offset1_q3 << 13)) >> 10);
512  syncBit[0] = (((currentAngle_raw_q16 + angleInc_q16) %
513  (360 << 16)) < angleInc_q16)
514  ? 1
515  : 0;
516  currentAngle_raw_q16 += angleInc_q16;
517 
518  angle_q6[1] =
519  ((currentAngle_raw_q16 - (angle_offset2_q3 << 13)) >> 10);
520  syncBit[1] = (((currentAngle_raw_q16 + angleInc_q16) %
521  (360 << 16)) < angleInc_q16)
522  ? 1
523  : 0;
524  currentAngle_raw_q16 += angleInc_q16;
525 
526  for (int cpos = 0; cpos < 2; ++cpos)
527  {
528  if (angle_q6[cpos] < 0) angle_q6[cpos] += (360 << 6);
529  if (angle_q6[cpos] >= (360 << 6)) angle_q6[cpos] -= (360 << 6);
530 
531  rplidar_response_measurement_node_t node;
532 
533  node.sync_quality = (syncBit[cpos] | ((!syncBit[cpos]) << 1));
534  if (dist_q2[cpos])
535  node.sync_quality |=
537 
538  node.angle_q6_checkbit = (1 | (angle_q6[cpos] << 1));
539  node.distance_q2 = dist_q2[cpos];
540 
541  nodebuffer[nodeCount++] = node;
542  }
543  }
544  }
545 
548 }
549 
551 {
552  rplidar_response_capsule_measurement_nodes_t capsule_node;
553  rplidar_response_measurement_node_t local_buf[128];
554  size_t count = 128;
555  rplidar_response_measurement_node_t local_scan[MAX_SCAN_NODES];
556  size_t scan_count = 0;
557  u_result ans;
558  memset(local_scan, 0, sizeof(local_scan));
559 
560  _waitCapsuledNode(capsule_node); // // always discard the first data since
561  // it may be incomplete
562 
563  while (_isScanning)
564  {
565  if (IS_FAIL(ans = _waitCapsuledNode(capsule_node)))
566  {
567  if (ans != RESULT_OPERATION_TIMEOUT && ans != RESULT_INVALID_DATA)
568  {
569  _isScanning = false;
570  return RESULT_OPERATION_FAIL;
571  }
572  else
573  {
574  // current data is invalid, do not use it.
575  continue;
576  }
577  }
578 
579  _capsuleToNormal(capsule_node, local_buf, count);
580 
581  for (size_t pos = 0; pos < count; ++pos)
582  {
583  if (local_buf[pos].sync_quality & RPLIDAR_RESP_MEASUREMENT_SYNCBIT)
584  {
585  // only publish the data when it contains a full 360 degree scan
586 
587  if ((local_scan[0].sync_quality &
589  {
590  _lock.lock();
591  memcpy(
592  _cached_scan_node_buf, local_scan,
593  scan_count *
594  sizeof(rplidar_response_measurement_node_t));
595  _cached_scan_node_count = scan_count;
596  _dataEvt.set();
597  _lock.unlock();
598  }
599  scan_count = 0;
600  }
601  local_scan[scan_count++] = local_buf[pos];
602  if (scan_count == _countof(local_scan))
603  scan_count -= 1; // prevent overflow
604  }
605  }
606  _isScanning = false;
607 
608  return RESULT_OK;
609 }
610 
612  rplidar_response_measurement_node_t* nodebuffer, size_t& count,
613  _u32 timeout)
614 {
615  switch (_dataEvt.wait(timeout))
616  {
618  count = 0;
621  {
622  if (_cached_scan_node_count == 0)
623  return RESULT_OPERATION_TIMEOUT; // consider as timeout
624 
626 
627  size_t size_to_copy = min(count, _cached_scan_node_count);
628 
629  memcpy(
630  nodebuffer, _cached_scan_node_buf,
631  size_to_copy * sizeof(rplidar_response_measurement_node_t));
632  count = size_to_copy;
634  }
635  return RESULT_OK;
636 
637  default:
638  count = 0;
639  return RESULT_OPERATION_FAIL;
640  }
641 }
642 
644  rplidar_response_measurement_node_t* nodebuffer, size_t count)
645 {
646  float inc_origin_angle = 360.0f / count;
647  size_t i = 0;
648 
649  // Tune head
650  for (i = 0; i < count && nodebuffer[i].distance_q2 == 0; i++)
651  ;
652 
653  // all the data is invalid
654  if (i == count) return RESULT_OPERATION_FAIL;
655 
656  while (i != 0)
657  {
658  i--;
659  float expect_angle = (nodebuffer[i + 1].angle_q6_checkbit >>
661  64.0f -
662  inc_origin_angle;
663  if (expect_angle < 0.0f) expect_angle = 0.0f;
664  _u16 checkbit =
665  nodebuffer[i].angle_q6_checkbit & RPLIDAR_RESP_MEASUREMENT_CHECKBIT;
666  nodebuffer[i].angle_q6_checkbit =
667  (((_u16)(expect_angle * 64.0f))
669  checkbit;
670  }
671 
672  // Tune tail
673  for (i = count - 1; nodebuffer[i].distance_q2 == 0; i--)
674  ;
675 
676  while (i != (count - 1))
677  {
678  i++;
679  float expect_angle = (nodebuffer[i - 1].angle_q6_checkbit >>
681  64.0f +
682  inc_origin_angle;
683  if (expect_angle > 360.0f) expect_angle -= 360.0f;
684  _u16 checkbit =
685  nodebuffer[i].angle_q6_checkbit & RPLIDAR_RESP_MEASUREMENT_CHECKBIT;
686  nodebuffer[i].angle_q6_checkbit =
687  (((_u16)(expect_angle * 64.0f))
689  checkbit;
690  }
691 
692  // Fill invalid angle in the scan
693  float frontAngle = (nodebuffer[0].angle_q6_checkbit >>
695  64.0f;
696  for (i = 1; i < count; i++)
697  {
698  if (nodebuffer[i].distance_q2 == 0)
699  {
700  float expect_angle = frontAngle + i * inc_origin_angle;
701  if (expect_angle > 360.0f) expect_angle -= 360.0f;
702  _u16 checkbit = nodebuffer[i].angle_q6_checkbit &
704  nodebuffer[i].angle_q6_checkbit =
705  (((_u16)(expect_angle * 64.0f))
707  checkbit;
708  }
709  }
710 
711  // Reorder the scan according to the angle value
712  for (i = 0; i < (count - 1); i++)
713  {
714  for (size_t j = (i + 1); j < count; j++)
715  {
716  if (nodebuffer[i].angle_q6_checkbit >
717  nodebuffer[j].angle_q6_checkbit)
718  {
719  rplidar_response_measurement_node_t temp = nodebuffer[i];
720  nodebuffer[i] = nodebuffer[j];
721  nodebuffer[j] = temp;
722  }
723  }
724  }
725 
726  return RESULT_OK;
727 }
728 
730  rplidar_response_measurement_node_t* node, _u32 timeout)
731 {
732  int recvPos = 0;
733  _u32 startTs = getms();
734  _u8 recvBuffer[sizeof(rplidar_response_measurement_node_t)];
735  _u8* nodeBuffer = (_u8*)node;
736  _u32 waitTime;
737 
738  while ((waitTime = getms() - startTs) <= timeout)
739  {
740  size_t remainSize =
741  sizeof(rplidar_response_measurement_node_t) - recvPos;
742  size_t recvSize;
743 
744  int ans = _rxtx->waitfordata(remainSize, timeout - waitTime, &recvSize);
746  return RESULT_OPERATION_FAIL;
747  else if (ans == rp::hal::serial_rxtx::ANS_TIMEOUT)
749 
750  if (recvSize > remainSize) recvSize = remainSize;
751 
752  _rxtx->recvdata(recvBuffer, recvSize);
753 
754  for (size_t pos = 0; pos < recvSize; ++pos)
755  {
756  _u8 currentByte = recvBuffer[pos];
757  switch (recvPos)
758  {
759  case 0: // expect the sync bit and its reverse in this byte
760  {
761  _u8 tmp = (currentByte >> 1);
762  if ((tmp ^ currentByte) & 0x1)
763  {
764  // pass
765  }
766  else
767  {
768  continue;
769  }
770  }
771  break;
772  case 1: // expect the highest bit to be 1
773  {
774  if (currentByte & RPLIDAR_RESP_MEASUREMENT_CHECKBIT)
775  {
776  // pass
777  }
778  else
779  {
780  recvPos = 0;
781  continue;
782  }
783  }
784  break;
785  }
786  nodeBuffer[recvPos++] = currentByte;
787 
788  if (recvPos == sizeof(rplidar_response_measurement_node_t))
789  {
790  return RESULT_OK;
791  }
792  }
793  }
794 
796 }
797 
799  rplidar_response_measurement_node_t* nodebuffer, size_t& count,
800  _u32 timeout)
801 {
802  if (!_isConnected)
803  {
804  count = 0;
805  return RESULT_OPERATION_FAIL;
806  }
807 
808  size_t recvNodeCount = 0;
809  _u32 startTs = getms();
810  _u32 waitTime;
811  u_result ans;
812 
813  while ((waitTime = getms() - startTs) <= timeout && recvNodeCount < count)
814  {
815  rplidar_response_measurement_node_t node;
816  if (IS_FAIL(ans = _waitNode(&node, timeout - waitTime)))
817  {
818  return ans;
819  }
820 
821  nodebuffer[recvNodeCount++] = node;
822 
823  if (recvNodeCount == count) return RESULT_OK;
824  }
825  count = recvNodeCount;
827 }
828 
830  rplidar_response_capsule_measurement_nodes_t& node, _u32 timeout)
831 {
832  int recvPos = 0;
833  _u32 startTs = getms();
834  _u8 recvBuffer[sizeof(rplidar_response_capsule_measurement_nodes_t)];
835  _u8* nodeBuffer = (_u8*)&node;
836  _u32 waitTime;
837 
838  while ((waitTime = getms() - startTs) <= timeout)
839  {
840  size_t remainSize =
841  sizeof(rplidar_response_capsule_measurement_nodes_t) - recvPos;
842  size_t recvSize;
843 
844  int ans = _rxtx->waitfordata(remainSize, timeout - waitTime, &recvSize);
846  return RESULT_OPERATION_FAIL;
847  else if (ans == rp::hal::serial_rxtx::ANS_TIMEOUT)
849 
850  if (recvSize > remainSize) recvSize = remainSize;
851 
852  _rxtx->recvdata(recvBuffer, recvSize);
853 
854  for (size_t pos = 0; pos < recvSize; ++pos)
855  {
856  _u8 currentByte = recvBuffer[pos];
857  switch (recvPos)
858  {
859  case 0: // expect the sync bit 1
860  {
861  _u8 tmp = (currentByte >> 4);
863  {
864  // pass
865  }
866  else
867  {
869  continue;
870  }
871  }
872  break;
873  case 1: // expect the sync bit 2
874  {
875  _u8 tmp = (currentByte >> 4);
877  {
878  // pass
879  }
880  else
881  {
882  recvPos = 0;
884  continue;
885  }
886  }
887  break;
888  }
889  nodeBuffer[recvPos++] = currentByte;
890 
891  if (recvPos == sizeof(rplidar_response_capsule_measurement_nodes_t))
892  {
893  // calc the checksum ...
894  _u8 checksum = 0;
895  _u8 recvChecksum =
896  ((node.s_checksum_1 & 0xF) | (node.s_checksum_2 << 4));
897  for (size_t cpos = offsetof(
898  rplidar_response_capsule_measurement_nodes_t,
900  cpos <
901  sizeof(rplidar_response_capsule_measurement_nodes_t);
902  ++cpos)
903  {
904  checksum ^= nodeBuffer[cpos];
905  }
906  if (recvChecksum == checksum)
907  {
908  // only consider vaild if the checksum matches...
909  if (node.start_angle_sync_q6 &
911  {
912  // this is the first capsule frame in logic, discard the
913  // previous cached data...
915  return RESULT_OK;
916  }
917  return RESULT_OK;
918  }
920  return RESULT_INVALID_DATA;
921  }
922  }
923  }
926 }
927 
929  _u8 cmd, const void* payload, size_t payloadsize)
930 {
931  _u8 pkt_header[10];
932  rplidar_cmd_packet_t* header =
933  reinterpret_cast<rplidar_cmd_packet_t*>(pkt_header);
934  _u8 checksum = 0;
935 
936  if (!_isConnected) return RESULT_OPERATION_FAIL;
937 
938  if (payloadsize && payload)
939  {
941  }
942 
943  header->syncByte = RPLIDAR_CMD_SYNC_BYTE;
944  header->cmd_flag = cmd;
945 
946  // send header first
947  _rxtx->senddata(pkt_header, 2);
948 
949  if (cmd & RPLIDAR_CMDFLAG_HAS_PAYLOAD)
950  {
951  checksum ^= RPLIDAR_CMD_SYNC_BYTE;
952  checksum ^= cmd;
953  checksum ^= (payloadsize & 0xFF);
954 
955  // calc checksum
956  for (size_t pos = 0; pos < payloadsize; ++pos)
957  {
958  checksum ^= ((_u8*)payload)[pos];
959  }
960 
961  // send size
962  _u8 sizebyte = payloadsize;
963  _rxtx->senddata(&sizebyte, 1);
964 
965  // send payload
966  _rxtx->senddata((const _u8*)payload, sizebyte);
967 
968  // send checksum
969  _rxtx->senddata(&checksum, 1);
970  }
971 
972  return RESULT_OK;
973 }
974 
976  rplidar_ans_header_t* header, _u32 timeout)
977 {
978  int recvPos = 0;
979  _u32 startTs = getms();
980  _u8 recvBuffer[sizeof(rplidar_ans_header_t)];
981  _u8* headerBuffer = reinterpret_cast<_u8*>(header);
982  _u32 waitTime;
983 
984  while ((waitTime = getms() - startTs) <= timeout)
985  {
986  size_t remainSize = sizeof(rplidar_ans_header_t) - recvPos;
987  size_t recvSize;
988 
989  int ans = _rxtx->waitfordata(remainSize, timeout - waitTime, &recvSize);
991  return RESULT_OPERATION_FAIL;
992  else if (ans == rp::hal::serial_rxtx::ANS_TIMEOUT)
994 
995  if (recvSize > remainSize) recvSize = remainSize;
996 
997  _rxtx->recvdata(recvBuffer, recvSize);
998 
999  for (size_t pos = 0; pos < recvSize; ++pos)
1000  {
1001  _u8 currentByte = recvBuffer[pos];
1002  switch (recvPos)
1003  {
1004  case 0:
1005  if (currentByte != RPLIDAR_ANS_SYNC_BYTE1)
1006  {
1007  continue;
1008  }
1009 
1010  break;
1011  case 1:
1012  if (currentByte != RPLIDAR_ANS_SYNC_BYTE2)
1013  {
1014  recvPos = 0;
1015  continue;
1016  }
1017  break;
1018  }
1019  headerBuffer[recvPos++] = currentByte;
1020 
1021  if (recvPos == sizeof(rplidar_ans_header_t))
1022  {
1023  return RESULT_OK;
1024  }
1025  }
1026  }
1027 
1028  return RESULT_OPERATION_TIMEOUT;
1029 }
1030 
1032 {
1033  _isScanning = false;
1034  _cachethread.join();
1035 }
1036 
1038  rplidar_response_sample_rate_t& rateInfo, _u32 timeout)
1039 {
1040  if (!isConnected()) return RESULT_OPERATION_FAIL;
1041 
1043 
1044  rplidar_response_device_info_t devinfo;
1045  // 1. fetch the device version first...
1046  u_result ans = getDeviceInfo(devinfo, timeout);
1047 
1048  rateInfo.express_sample_duration_us = _cached_sampleduration_express;
1049  rateInfo.std_sample_duration_us = _cached_sampleduration_std;
1050 
1051  if (devinfo.firmware_version < ((0x1 << 8) | 17))
1052  {
1053  // provide fake data...
1054 
1055  return RESULT_OK;
1056  }
1057 
1058  {
1060 
1062  {
1063  return ans;
1064  }
1065 
1066  rplidar_ans_header_t response_header;
1067  if (IS_FAIL(ans = _waitResponseHeader(&response_header, timeout)))
1068  {
1069  return ans;
1070  }
1071 
1072  // verify whether we got a correct header
1073  if (response_header.type != RPLIDAR_ANS_TYPE_SAMPLE_RATE)
1074  {
1075  return RESULT_INVALID_DATA;
1076  }
1077 
1078  _u32 header_size =
1079  (response_header.size_q30_subtype & RPLIDAR_ANS_HEADER_SIZE_MASK);
1080  if (header_size < sizeof(rplidar_response_sample_rate_t))
1081  {
1082  return RESULT_INVALID_DATA;
1083  }
1084 
1085  if (_rxtx->waitfordata(header_size, timeout) !=
1087  {
1088  return RESULT_OPERATION_TIMEOUT;
1089  }
1090  _rxtx->recvdata(reinterpret_cast<_u8*>(&rateInfo), sizeof(rateInfo));
1091  }
1092  return RESULT_OK;
1093 }
1094 
1096  bool& support, _u32 timeout)
1097 {
1098  u_result ans;
1099  support = false;
1100 
1101  if (!isConnected()) return RESULT_OPERATION_FAIL;
1102 
1104 
1105  {
1107 
1108  rplidar_payload_acc_board_flag_t flag;
1109  flag.reserved = 0;
1110 
1111  if (IS_FAIL(
1112  ans = _sendCommand(
1113  RPLIDAR_CMD_GET_ACC_BOARD_FLAG, &flag, sizeof(flag))))
1114  {
1115  return ans;
1116  }
1117 
1118  rplidar_ans_header_t response_header;
1119  if (IS_FAIL(ans = _waitResponseHeader(&response_header, timeout)))
1120  {
1121  return ans;
1122  }
1123 
1124  // verify whether we got a correct header
1125  if (response_header.type != RPLIDAR_ANS_TYPE_ACC_BOARD_FLAG)
1126  {
1127  return RESULT_INVALID_DATA;
1128  }
1129 
1130  _u32 header_size =
1131  (response_header.size_q30_subtype & RPLIDAR_ANS_HEADER_SIZE_MASK);
1132  if (header_size < sizeof(rplidar_response_acc_board_flag_t))
1133  {
1134  return RESULT_INVALID_DATA;
1135  }
1136 
1137  if (_rxtx->waitfordata(header_size, timeout) !=
1139  {
1140  return RESULT_OPERATION_TIMEOUT;
1141  }
1142  rplidar_response_acc_board_flag_t acc_board_flag;
1143  _rxtx->recvdata(
1144  reinterpret_cast<_u8*>(&acc_board_flag), sizeof(acc_board_flag));
1145 
1146  if (acc_board_flag.support_flag &
1148  {
1149  support = true;
1150  }
1151  }
1152  return RESULT_OK;
1153 }
1154 
1156 {
1157  u_result ans;
1158  rplidar_payload_motor_pwm_t motor_pwm;
1159  motor_pwm.pwm_value = pwm;
1160 
1161  {
1163 
1164  if (IS_FAIL(
1165  ans = _sendCommand(
1166  RPLIDAR_CMD_SET_MOTOR_PWM, (const _u8*)&motor_pwm,
1167  sizeof(motor_pwm))))
1168  {
1169  return ans;
1170  }
1171  }
1172 
1173  return RESULT_OK;
1174 }
1175 
1177 {
1179  { // RPLIDAR A2
1181  delay(500);
1182  return RESULT_OK;
1183  }
1184  else
1185  { // RPLIDAR A1
1187  _rxtx->clearDTR();
1188  delay(500);
1189  return RESULT_OK;
1190  }
1191 }
1192 
1194 {
1196  { // RPLIDAR A2
1197  setMotorPWM(0);
1198  delay(500);
1199  return RESULT_OK;
1200  }
1201  else
1202  { // RPLIDAR A1
1204  _rxtx->setDTR();
1205  delay(500);
1206  return RESULT_OK;
1207  }
1208 }
1209 }
1210 }
1211 }
virtual int recvdata(unsigned char *data, size_t size)=0
#define offsetof(_structure, _field)
Definition: util.h:52
#define RPLIDAR_RESP_MEASUREMENT_CHECKBIT
Definition: rplidar_cmd.h:112
#define CLASS_THREAD(c, x)
Definition: thread.h:37
#define RPLIDAR_RESP_MEASUREMENT_EXP_SYNCBIT
Definition: rplidar_cmd.h:142
virtual bool bind(const char *portname, _u32 baudrate, _u32 flags=0)=0
GLuint GLuint GLsizei count
Definition: glext.h:3528
#define RPLIDAR_CMD_FORCE_SCAN
Definition: rplidar_cmd.h:44
#define RPLIDAR_EXPRESS_SCAN_MODE_FIXANGLE
Definition: rplidar_cmd.h:67
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:20
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:21
#define RPLIDAR_ANS_TYPE_SAMPLE_RATE
Definition: rplidar_cmd.h:96
void set(bool isSignal=true)
Definition: event.h:67
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:97
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:48
#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:106
#define RPLIDAR_ANS_SYNC_BYTE1
virtual void disconnect()
Disconnect with the RPLIDAR and close the serial port.
#define delay(x)
Definition: win32/timer.h:38
#define RPLIDAR_CMD_GET_SAMPLERATE
Definition: rplidar_cmd.h:51
static void ReleaseRxTx(serial_rxtx *)
#define IS_FAIL(x)
Definition: rptypes.h:109
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:60
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:45
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:57
_u8 sync_quality
Definition: rplidar_cmd.h:19
#define getms()
Definition: linux/timer.h:59
#define RPLIDAR_CMD_EXPRESS_SCAN
Definition: rplidar_cmd.h:54
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:58
u_result _waitResponseHeader(rplidar_ans_header_t *header, _u32 timeout=DEFAULT_TIMEOUT)
#define RESULT_OPERATION_TIMEOUT
Definition: rptypes.h:102
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:58
_u16 distance_q2
Definition: rplidar_cmd.h:21
virtual u_result checkExpressScanSupported(bool &support, _u32 timeout=DEFAULT_TIMEOUT)
Check whether the device support express mode.
u_result _sendCommand(_u8 cmd, const void *payload=nullptr, size_t payloadsize=0)
#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 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:66
unsigned long wait(unsigned long timeout=0xFFFFFFFF)
Definition: event.h:96
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:139
#define DEFAULT_MOTOR_PWM
Definition: rplidar_cmd.h:75
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:101
#define RESULT_ALREADY_DONE
Definition: rptypes.h:99
#define RPLIDAR_CMD_STOP
Definition: rplidar_cmd.h:42
virtual void flush(_u32 flags)=0
#define RPLIDAR_RESP_MEASUREMENT_EXP_SYNC_2
Definition: rplidar_cmd.h:140
#define RPLIDAR_CMD_GET_DEVICE_HEALTH
Definition: rplidar_cmd.h:49
uint16_t _u16
Definition: rptypes.h:63
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:66
#define RPLIDAR_ANS_SYNC_BYTE2
#define _countof(_Array)
Definition: util.h:40
virtual int waitfordata(size_t data_count, _u32 timeout=-1, size_t *returned_size=nullptr)=0
#define RESULT_INVALID_DATA
Definition: rptypes.h:100
static void DisposeDriver(RPlidarDriver *drv)
Dispose the RPLIDAR Driver Instance specified by the drv parameter Applications should invoke this in...
void memcpy(void *dest, size_t destSize, const void *src, size_t copyCount) noexcept
An OS and compiler independent version of "memcpy".
Definition: os.cpp:356
uint32_t u_result
Definition: rptypes.h:95
#define RPLIDAR_ANS_TYPE_DEVINFO
Definition: rplidar_cmd.h:88
#define RPLIDAR_CMD_SCAN
Definition: rplidar_cmd.h:43
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.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019