MRPT  1.9.9
rplidar_driver.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2019, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://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 "hal/abs_rxtx.h"
35 #include "hal/event.h"
36 #include "hal/locker.h"
37 #include "hal/thread.h"
38 #include "rplidar_driver_serial.h"
39 #include "rptypes.h"
40 #include "sdkcommon.h"
41 
42 #ifndef min
43 #define min(a, b) (((a) < (b)) ? (a) : (b))
44 #endif
45 
47 {
48 // Factory Impl
50 {
51  switch (drivertype)
52  {
54  return new RPlidarDriverSerialImpl();
55  default:
56  return nullptr;
57  }
58 }
59 
60 void RPlidarDriver::DisposeDriver(RPlidarDriver* drv) { delete drv; }
61 // Serial Driver Impl
62 
64 
65 {
70 }
71 
73 {
74  // force disconnection
75  disconnect();
76 
78 }
79 
81  const char* port_path, _u32 baudrate, _u32 flag)
82 {
83  if (isConnected()) return RESULT_ALREADY_DONE;
84 
85  if (!_rxtx) return RESULT_INSUFFICIENT_MEMORY;
86 
87  {
89 
90  // establish the serial connection...
91  if (!_rxtx->bind(port_path, baudrate) || !_rxtx->open())
92  {
93  return RESULT_INVALID_DATA;
94  }
95 
96  _rxtx->flush(0);
97  }
98 
99  _isConnected = true;
100 
102  stopMotor();
103 
104  return RESULT_OK;
105 }
106 
108 {
109  if (!_isConnected) return;
110  stop();
111  _rxtx->close();
112 }
113 
116 {
117  u_result ans;
118 
119  {
121 
123  {
124  return ans;
125  }
126  }
127  return RESULT_OK;
128 }
129 
131  rplidar_response_device_health_t& healthinfo, _u32 timeout)
132 {
133  u_result ans;
134 
135  if (!isConnected()) return RESULT_OPERATION_FAIL;
136 
138 
139  {
141 
143  {
144  return ans;
145  }
146 
147  rplidar_ans_header_t response_header;
148  if (IS_FAIL(ans = _waitResponseHeader(&response_header, timeout)))
149  {
150  return ans;
151  }
152 
153  // verify whether we got a correct header
154  if (response_header.type != RPLIDAR_ANS_TYPE_DEVHEALTH)
155  {
156  return RESULT_INVALID_DATA;
157  }
158 
159  _u32 header_size =
160  (response_header.size_q30_subtype & RPLIDAR_ANS_HEADER_SIZE_MASK);
161  if (header_size < sizeof(rplidar_response_device_health_t))
162  {
163  return RESULT_INVALID_DATA;
164  }
165 
166  if (_rxtx->waitfordata(header_size, timeout) !=
168  {
170  }
171  _rxtx->recvdata(
172  reinterpret_cast<_u8*>(&healthinfo), sizeof(healthinfo));
173  }
174  return RESULT_OK;
175 }
176 
178  rplidar_response_device_info_t& info, _u32 timeout)
179 {
180  u_result ans;
181 
182  if (!isConnected()) return RESULT_OPERATION_FAIL;
183 
185 
186  {
188 
190  {
191  return ans;
192  }
193 
194  rplidar_ans_header_t response_header;
195  if (IS_FAIL(ans = _waitResponseHeader(&response_header, timeout)))
196  {
197  return ans;
198  }
199 
200  // verify whether we got a correct header
201  if (response_header.type != RPLIDAR_ANS_TYPE_DEVINFO)
202  {
203  return RESULT_INVALID_DATA;
204  }
205 
206  _u32 header_size =
207  (response_header.size_q30_subtype & RPLIDAR_ANS_HEADER_SIZE_MASK);
208  if (header_size < sizeof(rplidar_response_device_info_t))
209  {
210  return RESULT_INVALID_DATA;
211  }
212 
213  if (_rxtx->waitfordata(header_size, timeout) !=
215  {
217  }
218 
219  _rxtx->recvdata(reinterpret_cast<_u8*>(&info), sizeof(info));
220  }
221  return RESULT_OK;
222 }
223 
225  bool inExpressMode, size_t count, float& frequency, bool& is4kmode)
226 {
227  _u16 sample_duration = inExpressMode ? _cached_sampleduration_express
229  frequency = 1000000.0f / (count * sample_duration);
230 
231  if (sample_duration <= 277)
232  {
233  is4kmode = true;
234  }
235  else
236  {
237  is4kmode = false;
238  }
239 
240  return RESULT_OK;
241 }
242 
244 {
245  u_result ans;
246  if (!isConnected()) return RESULT_OPERATION_FAIL;
247  if (_isScanning) return RESULT_ALREADY_DONE;
248 
249  stop(); // force the previous operation to stop
250 
251  {
253 
254  if (IS_FAIL(
255  ans = _sendCommand(
257  {
258  return ans;
259  }
260 
261  // waiting for confirmation
262  rplidar_ans_header_t response_header;
263  if (IS_FAIL(ans = _waitResponseHeader(&response_header, timeout)))
264  {
265  return ans;
266  }
267 
268  // verify whether we got a correct header
269  if (response_header.type != RPLIDAR_ANS_TYPE_MEASUREMENT)
270  {
271  return RESULT_INVALID_DATA;
272  }
273 
274  _u32 header_size =
275  (response_header.size_q30_subtype & RPLIDAR_ANS_HEADER_SIZE_MASK);
276  if (header_size < sizeof(rplidar_response_measurement_node_t))
277  {
278  return RESULT_INVALID_DATA;
279  }
280 
281  _isScanning = true;
283  if (_cachethread.getHandle() == 0)
284  {
285  return RESULT_OPERATION_FAIL;
286  }
287  }
288  return RESULT_OK;
289 }
290 
292  bool& support, _u32 timeout)
293 {
294  rplidar_response_device_info_t devinfo;
295 
296  support = false;
297  u_result ans = getDeviceInfo(devinfo, timeout);
298 
299  if (IS_FAIL(ans)) return ans;
300 
301  if (devinfo.firmware_version >= ((0x1 << 8) | 17))
302  {
303  support = true;
304  rplidar_response_sample_rate_t sample_rate;
305  getSampleDuration_uS(sample_rate);
306  _cached_sampleduration_express = sample_rate.express_sample_duration_us;
307  _cached_sampleduration_std = sample_rate.std_sample_duration_us;
308  }
309 
310  return RESULT_OK;
311 }
312 
314  bool fixedAngle, _u32 timeout)
315 {
316  u_result ans;
317  if (!isConnected()) return RESULT_OPERATION_FAIL;
318  if (_isScanning) return RESULT_ALREADY_DONE;
319 
320  stop(); // force the previous operation to stop
321 
322  {
324 
325  rplidar_payload_express_scan_t scanReq;
326  scanReq.working_mode =
329  scanReq.reserved = 0;
330 
331  if (IS_FAIL(
332  ans = _sendCommand(
333  RPLIDAR_CMD_EXPRESS_SCAN, &scanReq, sizeof(scanReq))))
334  {
335  return ans;
336  }
337 
338  // waiting for confirmation
339  rplidar_ans_header_t response_header;
340  if (IS_FAIL(ans = _waitResponseHeader(&response_header, timeout)))
341  {
342  return ans;
343  }
344 
345  // verify whether we got a correct header
346  if (response_header.type != RPLIDAR_ANS_TYPE_MEASUREMENT_CAPSULED)
347  {
348  return RESULT_INVALID_DATA;
349  }
350 
351  _u32 header_size =
352  (response_header.size_q30_subtype & RPLIDAR_ANS_HEADER_SIZE_MASK);
353  if (header_size < sizeof(rplidar_response_capsule_measurement_nodes_t))
354  {
355  return RESULT_INVALID_DATA;
356  }
357 
358  _isScanning = true;
359  _cachethread =
361  if (_cachethread.getHandle() == 0)
362  {
363  return RESULT_OPERATION_FAIL;
364  }
365  }
366  return RESULT_OK;
367 }
368 
369 u_result RPlidarDriverSerialImpl::startScan(bool force, bool autoExpressMode)
370 {
371  bool isExpressModeSupported;
372  u_result ans;
373 
374  if (autoExpressMode)
375  {
376  ans = checkExpressScanSupported(isExpressModeSupported);
377 
378  if (IS_FAIL(ans)) return ans;
379 
380  if (isExpressModeSupported)
381  {
382  return startScanExpress(false);
383  }
384  }
385 
386  return startScanNormal(force);
387 }
388 
390 {
391  u_result ans;
393 
394  {
396 
398  {
399  return ans;
400  }
401  }
402 
403  return RESULT_OK;
404 }
405 
407 {
408  rplidar_response_measurement_node_t local_buf[128];
409  size_t count = 128;
410  rplidar_response_measurement_node_t local_scan[MAX_SCAN_NODES];
411  size_t scan_count = 0;
412  u_result ans;
413  memset(local_scan, 0, sizeof(local_scan));
414 
416  local_buf,
417  count); // // always discard the first data since it may be incomplete
418 
419  while (_isScanning)
420  {
421  if (IS_FAIL(ans = _waitScanData(local_buf, count)))
422  {
423  if (ans != RESULT_OPERATION_TIMEOUT)
424  {
425  _isScanning = false;
426  return RESULT_OPERATION_FAIL;
427  }
428  }
429 
430  for (size_t pos = 0; pos < count; ++pos)
431  {
432  if (local_buf[pos].sync_quality & RPLIDAR_RESP_MEASUREMENT_SYNCBIT)
433  {
434  // only publish the data when it contains a full 360 degree scan
435 
436  if ((local_scan[0].sync_quality &
438  {
439  _lock.lock();
440  memcpy(
441  _cached_scan_node_buf, local_scan,
442  scan_count *
443  sizeof(rplidar_response_measurement_node_t));
444  _cached_scan_node_count = scan_count;
445  _dataEvt.set();
446  _lock.unlock();
447  }
448  scan_count = 0;
449  }
450  local_scan[scan_count++] = local_buf[pos];
451  if (scan_count == _countof(local_scan))
452  scan_count -= 1; // prevent overflow
453  }
454  }
455  _isScanning = false;
456  return RESULT_OK;
457 }
458 
460  const rplidar_response_capsule_measurement_nodes_t& capsule,
461  rplidar_response_measurement_node_t* nodebuffer, size_t& nodeCount)
462 {
463  nodeCount = 0;
465  {
466  int diffAngle_q8;
467  int currentStartAngle_q8 =
468  ((capsule.start_angle_sync_q6 & 0x7FFF) << 2);
469  int prevStartAngle_q8 =
470  ((_cached_previous_capsuledata.start_angle_sync_q6 & 0x7FFF) << 2);
471 
472  diffAngle_q8 = (currentStartAngle_q8) - (prevStartAngle_q8);
473  if (prevStartAngle_q8 > currentStartAngle_q8)
474  {
475  diffAngle_q8 += (360 << 8);
476  }
477 
478  int angleInc_q16 = (diffAngle_q8 << 3);
479  int currentAngle_raw_q16 = (prevStartAngle_q8 << 8);
480  for (auto& cabin : _cached_previous_capsuledata.cabins)
481  {
482  int dist_q2[2];
483  int angle_q6[2];
484  int syncBit[2];
485 
486  dist_q2[0] = (cabin.distance_angle_1 & 0xFFFC);
487  dist_q2[1] = (cabin.distance_angle_2 & 0xFFFC);
488 
489  int angle_offset1_q3 =
490  ((cabin.offset_angles_q3 & 0xF) |
491  ((cabin.distance_angle_1 & 0x3) << 4));
492  int angle_offset2_q3 =
493  ((cabin.offset_angles_q3 >> 4) |
494  ((cabin.distance_angle_2 & 0x3) << 4));
495 
496  angle_q6[0] =
497  ((currentAngle_raw_q16 - (angle_offset1_q3 << 13)) >> 10);
498  syncBit[0] = (((currentAngle_raw_q16 + angleInc_q16) %
499  (360 << 16)) < angleInc_q16)
500  ? 1
501  : 0;
502  currentAngle_raw_q16 += angleInc_q16;
503 
504  angle_q6[1] =
505  ((currentAngle_raw_q16 - (angle_offset2_q3 << 13)) >> 10);
506  syncBit[1] = (((currentAngle_raw_q16 + angleInc_q16) %
507  (360 << 16)) < angleInc_q16)
508  ? 1
509  : 0;
510  currentAngle_raw_q16 += angleInc_q16;
511 
512  for (int cpos = 0; cpos < 2; ++cpos)
513  {
514  if (angle_q6[cpos] < 0) angle_q6[cpos] += (360 << 6);
515  if (angle_q6[cpos] >= (360 << 6)) angle_q6[cpos] -= (360 << 6);
516 
517  rplidar_response_measurement_node_t node;
518 
519  node.sync_quality = (syncBit[cpos] | ((!syncBit[cpos]) << 1));
520  if (dist_q2[cpos])
521  node.sync_quality |=
523 
524  node.angle_q6_checkbit = (1 | (angle_q6[cpos] << 1));
525  node.distance_q2 = dist_q2[cpos];
526 
527  nodebuffer[nodeCount++] = node;
528  }
529  }
530  }
531 
534 }
535 
537 {
538  rplidar_response_capsule_measurement_nodes_t capsule_node;
539  rplidar_response_measurement_node_t local_buf[128];
540  size_t count = 128;
541  rplidar_response_measurement_node_t local_scan[MAX_SCAN_NODES];
542  size_t scan_count = 0;
543  u_result ans;
544  memset(local_scan, 0, sizeof(local_scan));
545 
546  _waitCapsuledNode(capsule_node); // // always discard the first data since
547  // it may be incomplete
548 
549  while (_isScanning)
550  {
551  if (IS_FAIL(ans = _waitCapsuledNode(capsule_node)))
552  {
553  if (ans != RESULT_OPERATION_TIMEOUT && ans != RESULT_INVALID_DATA)
554  {
555  _isScanning = false;
556  return RESULT_OPERATION_FAIL;
557  }
558  else
559  {
560  // current data is invalid, do not use it.
561  continue;
562  }
563  }
564 
565  _capsuleToNormal(capsule_node, local_buf, count);
566 
567  for (size_t pos = 0; pos < count; ++pos)
568  {
569  if (local_buf[pos].sync_quality & RPLIDAR_RESP_MEASUREMENT_SYNCBIT)
570  {
571  // only publish the data when it contains a full 360 degree scan
572 
573  if ((local_scan[0].sync_quality &
575  {
576  _lock.lock();
577  memcpy(
578  _cached_scan_node_buf, local_scan,
579  scan_count *
580  sizeof(rplidar_response_measurement_node_t));
581  _cached_scan_node_count = scan_count;
582  _dataEvt.set();
583  _lock.unlock();
584  }
585  scan_count = 0;
586  }
587  local_scan[scan_count++] = local_buf[pos];
588  if (scan_count == _countof(local_scan))
589  scan_count -= 1; // prevent overflow
590  }
591  }
592  _isScanning = false;
593 
594  return RESULT_OK;
595 }
596 
598  rplidar_response_measurement_node_t* nodebuffer, size_t& count,
599  _u32 timeout)
600 {
601  switch (_dataEvt.wait(timeout))
602  {
604  count = 0;
607  {
608  if (_cached_scan_node_count == 0)
609  return RESULT_OPERATION_TIMEOUT; // consider as timeout
610 
612 
613  size_t size_to_copy = min(count, _cached_scan_node_count);
614 
615  memcpy(
616  nodebuffer, _cached_scan_node_buf,
617  size_to_copy * sizeof(rplidar_response_measurement_node_t));
618  count = size_to_copy;
620  }
621  return RESULT_OK;
622 
623  default:
624  count = 0;
625  return RESULT_OPERATION_FAIL;
626  }
627 }
628 
630  rplidar_response_measurement_node_t* nodebuffer, size_t count)
631 {
632  float inc_origin_angle = 360.0f / count;
633  size_t i = 0;
634 
635  // Tune head
636  for (i = 0; i < count && nodebuffer[i].distance_q2 == 0; i++)
637  ;
638 
639  // all the data is invalid
640  if (i == count) return RESULT_OPERATION_FAIL;
641 
642  while (i != 0)
643  {
644  i--;
645  float expect_angle = (nodebuffer[i + 1].angle_q6_checkbit >>
647  64.0f -
648  inc_origin_angle;
649  if (expect_angle < 0.0f) expect_angle = 0.0f;
650  _u16 checkbit =
651  nodebuffer[i].angle_q6_checkbit & RPLIDAR_RESP_MEASUREMENT_CHECKBIT;
652  nodebuffer[i].angle_q6_checkbit =
653  (((_u16)(expect_angle * 64.0f))
655  checkbit;
656  }
657 
658  // Tune tail
659  for (i = count - 1; nodebuffer[i].distance_q2 == 0; i--)
660  ;
661 
662  while (i != (count - 1))
663  {
664  i++;
665  float expect_angle = (nodebuffer[i - 1].angle_q6_checkbit >>
667  64.0f +
668  inc_origin_angle;
669  if (expect_angle > 360.0f) expect_angle -= 360.0f;
670  _u16 checkbit =
671  nodebuffer[i].angle_q6_checkbit & RPLIDAR_RESP_MEASUREMENT_CHECKBIT;
672  nodebuffer[i].angle_q6_checkbit =
673  (((_u16)(expect_angle * 64.0f))
675  checkbit;
676  }
677 
678  // Fill invalid angle in the scan
679  float frontAngle = (nodebuffer[0].angle_q6_checkbit >>
681  64.0f;
682  for (i = 1; i < count; i++)
683  {
684  if (nodebuffer[i].distance_q2 == 0)
685  {
686  float expect_angle = frontAngle + i * inc_origin_angle;
687  if (expect_angle > 360.0f) expect_angle -= 360.0f;
688  _u16 checkbit = nodebuffer[i].angle_q6_checkbit &
690  nodebuffer[i].angle_q6_checkbit =
691  (((_u16)(expect_angle * 64.0f))
693  checkbit;
694  }
695  }
696 
697  // Reorder the scan according to the angle value
698  for (i = 0; i < (count - 1); i++)
699  {
700  for (size_t j = (i + 1); j < count; j++)
701  {
702  if (nodebuffer[i].angle_q6_checkbit >
703  nodebuffer[j].angle_q6_checkbit)
704  {
705  rplidar_response_measurement_node_t temp = nodebuffer[i];
706  nodebuffer[i] = nodebuffer[j];
707  nodebuffer[j] = temp;
708  }
709  }
710  }
711 
712  return RESULT_OK;
713 }
714 
716  rplidar_response_measurement_node_t* node, _u32 timeout)
717 {
718  int recvPos = 0;
719  _u32 startTs = getms();
720  _u8 recvBuffer[sizeof(rplidar_response_measurement_node_t)];
721  _u8* nodeBuffer = (_u8*)node;
722  _u32 waitTime;
723 
724  while ((waitTime = getms() - startTs) <= timeout)
725  {
726  size_t remainSize =
727  sizeof(rplidar_response_measurement_node_t) - recvPos;
728  size_t recvSize;
729 
730  int ans = _rxtx->waitfordata(remainSize, timeout - waitTime, &recvSize);
732  return RESULT_OPERATION_FAIL;
733  else if (ans == rp::hal::serial_rxtx::ANS_TIMEOUT)
735 
736  if (recvSize > remainSize) recvSize = remainSize;
737 
738  _rxtx->recvdata(recvBuffer, recvSize);
739 
740  for (size_t pos = 0; pos < recvSize; ++pos)
741  {
742  _u8 currentByte = recvBuffer[pos];
743  switch (recvPos)
744  {
745  case 0: // expect the sync bit and its reverse in this byte
746  {
747  _u8 tmp = (currentByte >> 1);
748  if ((tmp ^ currentByte) & 0x1)
749  {
750  // pass
751  }
752  else
753  {
754  continue;
755  }
756  }
757  break;
758  case 1: // expect the highest bit to be 1
759  {
760  if (currentByte & RPLIDAR_RESP_MEASUREMENT_CHECKBIT)
761  {
762  // pass
763  }
764  else
765  {
766  recvPos = 0;
767  continue;
768  }
769  }
770  break;
771  }
772  nodeBuffer[recvPos++] = currentByte;
773 
774  if (recvPos == sizeof(rplidar_response_measurement_node_t))
775  {
776  return RESULT_OK;
777  }
778  }
779  }
780 
782 }
783 
785  rplidar_response_measurement_node_t* nodebuffer, size_t& count,
786  _u32 timeout)
787 {
788  if (!_isConnected)
789  {
790  count = 0;
791  return RESULT_OPERATION_FAIL;
792  }
793 
794  size_t recvNodeCount = 0;
795  _u32 startTs = getms();
796  _u32 waitTime;
797  u_result ans;
798 
799  while ((waitTime = getms() - startTs) <= timeout && recvNodeCount < count)
800  {
801  rplidar_response_measurement_node_t node;
802  if (IS_FAIL(ans = _waitNode(&node, timeout - waitTime)))
803  {
804  return ans;
805  }
806 
807  nodebuffer[recvNodeCount++] = node;
808 
809  if (recvNodeCount == count) return RESULT_OK;
810  }
811  count = recvNodeCount;
813 }
814 
816  rplidar_response_capsule_measurement_nodes_t& node, _u32 timeout)
817 {
818  int recvPos = 0;
819  _u32 startTs = getms();
820  _u8 recvBuffer[sizeof(rplidar_response_capsule_measurement_nodes_t)];
821  _u8* nodeBuffer = (_u8*)&node;
822  _u32 waitTime;
823 
824  while ((waitTime = getms() - startTs) <= timeout)
825  {
826  size_t remainSize =
827  sizeof(rplidar_response_capsule_measurement_nodes_t) - recvPos;
828  size_t recvSize;
829 
830  int ans = _rxtx->waitfordata(remainSize, timeout - waitTime, &recvSize);
832  return RESULT_OPERATION_FAIL;
833  else if (ans == rp::hal::serial_rxtx::ANS_TIMEOUT)
835 
836  if (recvSize > remainSize) recvSize = remainSize;
837 
838  _rxtx->recvdata(recvBuffer, recvSize);
839 
840  for (size_t pos = 0; pos < recvSize; ++pos)
841  {
842  _u8 currentByte = recvBuffer[pos];
843  switch (recvPos)
844  {
845  case 0: // expect the sync bit 1
846  {
847  _u8 tmp = (currentByte >> 4);
849  {
850  // pass
851  }
852  else
853  {
855  continue;
856  }
857  }
858  break;
859  case 1: // expect the sync bit 2
860  {
861  _u8 tmp = (currentByte >> 4);
863  {
864  // pass
865  }
866  else
867  {
868  recvPos = 0;
870  continue;
871  }
872  }
873  break;
874  }
875  nodeBuffer[recvPos++] = currentByte;
876 
877  if (recvPos == sizeof(rplidar_response_capsule_measurement_nodes_t))
878  {
879  // calc the checksum ...
880  _u8 checksum = 0;
881  _u8 recvChecksum =
882  ((node.s_checksum_1 & 0xF) | (node.s_checksum_2 << 4));
883  for (size_t cpos = offsetof(
884  rplidar_response_capsule_measurement_nodes_t,
886  cpos <
887  sizeof(rplidar_response_capsule_measurement_nodes_t);
888  ++cpos)
889  {
890  checksum ^= nodeBuffer[cpos];
891  }
892  if (recvChecksum == checksum)
893  {
894  // only consider vaild if the checksum matches...
895  if (node.start_angle_sync_q6 &
897  {
898  // this is the first capsule frame in logic, discard the
899  // previous cached data...
901  return RESULT_OK;
902  }
903  return RESULT_OK;
904  }
906  return RESULT_INVALID_DATA;
907  }
908  }
909  }
912 }
913 
915  _u8 cmd, const void* payload, size_t payloadsize)
916 {
917  _u8 pkt_header[10];
918  auto* header = reinterpret_cast<rplidar_cmd_packet_t*>(pkt_header);
919  _u8 checksum = 0;
920 
921  if (!_isConnected) return RESULT_OPERATION_FAIL;
922 
923  if (payloadsize && payload)
924  {
926  }
927 
928  header->syncByte = RPLIDAR_CMD_SYNC_BYTE;
929  header->cmd_flag = cmd;
930 
931  // send header first
932  _rxtx->senddata(pkt_header, 2);
933 
934  if (cmd & RPLIDAR_CMDFLAG_HAS_PAYLOAD)
935  {
936  checksum ^= RPLIDAR_CMD_SYNC_BYTE;
937  checksum ^= cmd;
938  checksum ^= (payloadsize & 0xFF);
939 
940  // calc checksum
941  for (size_t pos = 0; pos < payloadsize; ++pos)
942  {
943  checksum ^= ((_u8*)payload)[pos];
944  }
945 
946  // send size
947  _u8 sizebyte = payloadsize;
948  _rxtx->senddata(&sizebyte, 1);
949 
950  // send payload
951  _rxtx->senddata((const _u8*)payload, sizebyte);
952 
953  // send checksum
954  _rxtx->senddata(&checksum, 1);
955  }
956 
957  return RESULT_OK;
958 }
959 
961  rplidar_ans_header_t* header, _u32 timeout)
962 {
963  int recvPos = 0;
964  _u32 startTs = getms();
965  _u8 recvBuffer[sizeof(rplidar_ans_header_t)];
966  _u8* headerBuffer = reinterpret_cast<_u8*>(header);
967  _u32 waitTime;
968 
969  while ((waitTime = getms() - startTs) <= timeout)
970  {
971  size_t remainSize = sizeof(rplidar_ans_header_t) - recvPos;
972  size_t recvSize;
973 
974  int ans = _rxtx->waitfordata(remainSize, timeout - waitTime, &recvSize);
976  return RESULT_OPERATION_FAIL;
977  else if (ans == rp::hal::serial_rxtx::ANS_TIMEOUT)
979 
980  if (recvSize > remainSize) recvSize = remainSize;
981 
982  _rxtx->recvdata(recvBuffer, recvSize);
983 
984  for (size_t pos = 0; pos < recvSize; ++pos)
985  {
986  _u8 currentByte = recvBuffer[pos];
987  switch (recvPos)
988  {
989  case 0:
990  if (currentByte != RPLIDAR_ANS_SYNC_BYTE1)
991  {
992  continue;
993  }
994 
995  break;
996  case 1:
997  if (currentByte != RPLIDAR_ANS_SYNC_BYTE2)
998  {
999  recvPos = 0;
1000  continue;
1001  }
1002  break;
1003  }
1004  headerBuffer[recvPos++] = currentByte;
1005 
1006  if (recvPos == sizeof(rplidar_ans_header_t))
1007  {
1008  return RESULT_OK;
1009  }
1010  }
1011  }
1012 
1013  return RESULT_OPERATION_TIMEOUT;
1014 }
1015 
1017 {
1018  _isScanning = false;
1019  _cachethread.join();
1020 }
1021 
1023  rplidar_response_sample_rate_t& rateInfo, _u32 timeout)
1024 {
1025  if (!isConnected()) return RESULT_OPERATION_FAIL;
1026 
1028 
1029  rplidar_response_device_info_t devinfo;
1030  // 1. fetch the device version first...
1031  u_result ans = getDeviceInfo(devinfo, timeout);
1032 
1033  rateInfo.express_sample_duration_us = _cached_sampleduration_express;
1034  rateInfo.std_sample_duration_us = _cached_sampleduration_std;
1035 
1036  if (devinfo.firmware_version < ((0x1 << 8) | 17))
1037  {
1038  // provide fake data...
1039 
1040  return RESULT_OK;
1041  }
1042 
1043  {
1045 
1047  {
1048  return ans;
1049  }
1050 
1051  rplidar_ans_header_t response_header;
1052  if (IS_FAIL(ans = _waitResponseHeader(&response_header, timeout)))
1053  {
1054  return ans;
1055  }
1056 
1057  // verify whether we got a correct header
1058  if (response_header.type != RPLIDAR_ANS_TYPE_SAMPLE_RATE)
1059  {
1060  return RESULT_INVALID_DATA;
1061  }
1062 
1063  _u32 header_size =
1064  (response_header.size_q30_subtype & RPLIDAR_ANS_HEADER_SIZE_MASK);
1065  if (header_size < sizeof(rplidar_response_sample_rate_t))
1066  {
1067  return RESULT_INVALID_DATA;
1068  }
1069 
1070  if (_rxtx->waitfordata(header_size, timeout) !=
1072  {
1073  return RESULT_OPERATION_TIMEOUT;
1074  }
1075  _rxtx->recvdata(reinterpret_cast<_u8*>(&rateInfo), sizeof(rateInfo));
1076  }
1077  return RESULT_OK;
1078 }
1079 
1081  bool& support, _u32 timeout)
1082 {
1083  u_result ans;
1084  support = false;
1085 
1086  if (!isConnected()) return RESULT_OPERATION_FAIL;
1087 
1089 
1090  {
1092 
1093  rplidar_payload_acc_board_flag_t flag;
1094  flag.reserved = 0;
1095 
1096  if (IS_FAIL(
1097  ans = _sendCommand(
1098  RPLIDAR_CMD_GET_ACC_BOARD_FLAG, &flag, sizeof(flag))))
1099  {
1100  return ans;
1101  }
1102 
1103  rplidar_ans_header_t response_header;
1104  if (IS_FAIL(ans = _waitResponseHeader(&response_header, timeout)))
1105  {
1106  return ans;
1107  }
1108 
1109  // verify whether we got a correct header
1110  if (response_header.type != RPLIDAR_ANS_TYPE_ACC_BOARD_FLAG)
1111  {
1112  return RESULT_INVALID_DATA;
1113  }
1114 
1115  _u32 header_size =
1116  (response_header.size_q30_subtype & RPLIDAR_ANS_HEADER_SIZE_MASK);
1117  if (header_size < sizeof(rplidar_response_acc_board_flag_t))
1118  {
1119  return RESULT_INVALID_DATA;
1120  }
1121 
1122  if (_rxtx->waitfordata(header_size, timeout) !=
1124  {
1125  return RESULT_OPERATION_TIMEOUT;
1126  }
1127  rplidar_response_acc_board_flag_t acc_board_flag;
1128  _rxtx->recvdata(
1129  reinterpret_cast<_u8*>(&acc_board_flag), sizeof(acc_board_flag));
1130 
1131  if (acc_board_flag.support_flag &
1133  {
1134  support = true;
1135  }
1136  }
1137  return RESULT_OK;
1138 }
1139 
1141 {
1142  u_result ans;
1143  rplidar_payload_motor_pwm_t motor_pwm;
1144  motor_pwm.pwm_value = pwm;
1145 
1146  {
1148 
1149  if (IS_FAIL(
1150  ans = _sendCommand(
1151  RPLIDAR_CMD_SET_MOTOR_PWM, (const _u8*)&motor_pwm,
1152  sizeof(motor_pwm))))
1153  {
1154  return ans;
1155  }
1156  }
1157 
1158  return RESULT_OK;
1159 }
1160 
1162 {
1164  { // RPLIDAR A2
1166  delay(500);
1167  return RESULT_OK;
1168  }
1169  else
1170  { // RPLIDAR A1
1172  _rxtx->clearDTR();
1173  delay(500);
1174  return RESULT_OK;
1175  }
1176 }
1177 
1179 {
1181  { // RPLIDAR A2
1182  setMotorPWM(0);
1183  delay(500);
1184  return RESULT_OK;
1185  }
1186  else
1187  { // RPLIDAR A1
1189  _rxtx->setDTR();
1190  delay(500);
1191  return RESULT_OK;
1192  }
1193 }
1194 } // namespace rp::standalone::rplidar
virtual int recvdata(unsigned char *data, size_t size)=0
#define offsetof(_structure, _field)
Definition: util.h:54
#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:3532
#define RPLIDAR_CMD_FORCE_SCAN
Definition: rplidar_cmd.h:44
#define RPLIDAR_EXPRESS_SCAN_MODE_FIXANGLE
Definition: rplidar_cmd.h:67
u_result stopMotor() override
Stop RPLIDAR&#39;s motor when using accessory board.
u_result checkMotorCtrlSupport(bool &support, _u32 timeout=DEFAULT_TIMEOUT) override
Check whether the device support motor control.
#define min(a, b)
u_result startScanNormal(bool force, _u32 timeout=DEFAULT_TIMEOUT) override
u_result stop(_u32 timeout=DEFAULT_TIMEOUT) override
Ask the RPLIDAR core system to stop the current scan operation and enter idle state.
#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
u_result setMotorPWM(_u16 pwm) override
Set the RPLIDAR&#39;s motor pwm when using accessory board, currently valid for A2 only.
void unlock()
Definition: locker.h:115
void _capsuleToNormal(const rplidar_response_capsule_measurement_nodes_t &capsule, rplidar_response_measurement_node_t *nodebuffer, size_t &nodeCount)
u_result startMotor() override
Start RPLIDAR&#39;s motor when using accessory board.
_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:75
#define RPLIDAR_ANS_HEADER_SIZE_MASK
_word_size_t getHandle()
Definition: thread.h:69
#define RESULT_OK
Definition: rptypes.h:100
#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
u_result ascendScanData(rplidar_response_measurement_node_t *nodebuffer, size_t count) override
Ascending the scan data according to the angle value in the scan.
virtual int senddata(const unsigned char *data, size_t size)=0
static serial_rxtx * CreateRxTx()
#define RESULT_INSUFFICIENT_MEMORY
Definition: rptypes.h:109
#define RPLIDAR_ANS_SYNC_BYTE1
u_result getHealth(rplidar_response_device_health_t &, _u32 timeout=DEFAULT_TIMEOUT) override
Retrieve the health status of the RPLIDAR The host system can use this operation to check whether RPL...
#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:112
rplidar_response_capsule_measurement_nodes_t _cached_previous_capsuledata
uint8_t _u8
Definition: rptypes.h:63
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
#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:55
#define RPLIDAR_CMD_EXPRESS_SCAN
Definition: rplidar_cmd.h:54
u_result getFrequency(bool inExpressMode, size_t count, float &frequency, bool &is4kmode) override
Calcuate RPLIDAR&#39;s current scanning frequency from the given scan data Please refer to the applicatio...
Locker::LOCK_STATUS lock(unsigned long timeout=0xFFFFFFFF)
Definition: locker.h:56
u_result _waitResponseHeader(rplidar_ans_header_t *header, _u32 timeout=DEFAULT_TIMEOUT)
#define RESULT_OPERATION_TIMEOUT
Definition: rptypes.h:105
u_result _waitNode(rplidar_response_measurement_node_t *node, _u32 timeout=DEFAULT_TIMEOUT)
u_result grabScanData(rplidar_response_measurement_node_t *nodebuffer, size_t &count, _u32 timeout=DEFAULT_TIMEOUT) override
Wait and grab a complete 0-360 degree scan data previously received.
#define RPLIDAR_ANS_TYPE_MEASUREMENT
Definition: rplidar_cmd.h:91
#define RPLIDAR_CMD_GET_ACC_BOARD_FLAG
Definition: rplidar_cmd.h:58
u_result checkExpressScanSupported(bool &support, _u32 timeout=DEFAULT_TIMEOUT) override
Check whether the device support express mode.
u_result startScan(bool force=false, bool autoExpressMode=true) override
Ask the RPLIDAR core system to enter the scan mode(Normal/Express, Express mode is 4k mode) A backgro...
u_result reset(_u32 timeout=DEFAULT_TIMEOUT) override
Ask the RPLIDAR core system to reset it self The host system can use the Reset operation to help RPLI...
_u16 distance_q2
Definition: rplidar_cmd.h:21
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
void disconnect() override
Disconnect with the RPLIDAR and close the serial port.
bool isConnected() override
Returns TRUE when the connection has been established.
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:104
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
#define RESULT_OPERATION_FAIL
Definition: rptypes.h:104
#define RESULT_ALREADY_DONE
Definition: rptypes.h:102
#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:66
static RPlidarDriver * CreateDriver(_u32 drivertype=DRIVER_TYPE_SERIALPORT)
Create an RPLIDAR Driver Instance This interface should be invoked first before any other operations...
u_result getDeviceInfo(rplidar_response_device_info_t &, _u32 timeout=DEFAULT_TIMEOUT) override
Get the device information of the RPLIDAR include the serial number, firmware version, device model etc.
u_result getSampleDuration_uS(rplidar_response_sample_rate_t &rateInfo, _u32 timeout=DEFAULT_TIMEOUT) override
Get the sample duration information of the RPLIDAR.
#define RPLIDAR_EXPRESS_SCAN_MODE_NORMAL
Definition: rplidar_cmd.h:66
u_result startScanExpress(bool fixedAngle, _u32 timeout=DEFAULT_TIMEOUT) override
#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
u_result connect(const char *port_path, _u32 baudrate, _u32 flag) override
Open the specified serial port and connect to a target RPLIDAR device.
#define RESULT_INVALID_DATA
Definition: rptypes.h:103
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:358
uint32_t u_result
Definition: rptypes.h:98
#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: 765b969e7 Sun Sep 22 19:55:28 2019 +0200 at dom sep 22 20:00:14 CEST 2019