42 #define min(a, b) (((a) < (b)) ? (a) : (b))
67 : _isConnected(false), _isScanning(false), _isSupportingMotorCtrl(false)
84 const char* port_path,
_u32 baudrate,
_u32 flag)
134 rplidar_response_device_health_t& healthinfo,
_u32 timeout)
150 rplidar_ans_header_t response_header;
164 if (header_size <
sizeof(rplidar_response_device_health_t))
175 reinterpret_cast<_u8*
>(&healthinfo),
sizeof(healthinfo));
181 rplidar_response_device_info_t& info,
_u32 timeout)
197 rplidar_ans_header_t response_header;
211 if (header_size <
sizeof(rplidar_response_device_info_t))
228 bool inExpressMode,
size_t count,
float& frequency,
bool& is4kmode)
232 frequency = 1000000.0f / (
count * sample_duration);
234 if (sample_duration <= 277)
265 rplidar_ans_header_t response_header;
279 if (header_size <
sizeof(rplidar_response_measurement_node_t))
295 bool& support,
_u32 timeout)
297 rplidar_response_device_info_t devinfo;
304 if (devinfo.firmware_version >= ((0x1 << 8) | 17))
307 rplidar_response_sample_rate_t sample_rate;
317 bool fixedAngle,
_u32 timeout)
328 rplidar_payload_express_scan_t scanReq;
329 scanReq.working_mode =
332 scanReq.reserved = 0;
342 rplidar_ans_header_t response_header;
356 if (header_size <
sizeof(rplidar_response_capsule_measurement_nodes_t))
374 bool isExpressModeSupported;
383 if (isExpressModeSupported)
411 rplidar_response_measurement_node_t local_buf[128];
414 size_t scan_count = 0;
416 memset(local_scan, 0,
sizeof(local_scan));
433 for (
size_t pos = 0; pos <
count; ++pos)
446 sizeof(rplidar_response_measurement_node_t));
453 local_scan[scan_count++] = local_buf[pos];
454 if (scan_count ==
_countof(local_scan))
463 const rplidar_response_capsule_measurement_nodes_t& capsule,
464 rplidar_response_measurement_node_t* nodebuffer,
size_t& nodeCount)
470 int currentStartAngle_q8 =
471 ((capsule.start_angle_sync_q6 & 0x7FFF) << 2);
472 int prevStartAngle_q8 =
475 diffAngle_q8 = (currentStartAngle_q8) - (prevStartAngle_q8);
476 if (prevStartAngle_q8 > currentStartAngle_q8)
478 diffAngle_q8 += (360 << 8);
481 int angleInc_q16 = (diffAngle_q8 << 3);
482 int currentAngle_raw_q16 = (prevStartAngle_q8 << 8);
497 int angle_offset1_q3 =
503 int angle_offset2_q3 =
511 ((currentAngle_raw_q16 - (angle_offset1_q3 << 13)) >> 10);
512 syncBit[0] = (((currentAngle_raw_q16 + angleInc_q16) %
513 (360 << 16)) < angleInc_q16)
516 currentAngle_raw_q16 += angleInc_q16;
519 ((currentAngle_raw_q16 - (angle_offset2_q3 << 13)) >> 10);
520 syncBit[1] = (((currentAngle_raw_q16 + angleInc_q16) %
521 (360 << 16)) < angleInc_q16)
524 currentAngle_raw_q16 += angleInc_q16;
526 for (
int cpos = 0; cpos < 2; ++cpos)
528 if (angle_q6[cpos] < 0) angle_q6[cpos] += (360 << 6);
529 if (angle_q6[cpos] >= (360 << 6)) angle_q6[cpos] -= (360 << 6);
531 rplidar_response_measurement_node_t node;
533 node.sync_quality = (syncBit[cpos] | ((!syncBit[cpos]) << 1));
538 node.angle_q6_checkbit = (1 | (angle_q6[cpos] << 1));
539 node.distance_q2 = dist_q2[cpos];
541 nodebuffer[nodeCount++] = node;
552 rplidar_response_capsule_measurement_nodes_t capsule_node;
553 rplidar_response_measurement_node_t local_buf[128];
556 size_t scan_count = 0;
558 memset(local_scan, 0,
sizeof(local_scan));
581 for (
size_t pos = 0; pos <
count; ++pos)
594 sizeof(rplidar_response_measurement_node_t));
601 local_scan[scan_count++] = local_buf[pos];
602 if (scan_count ==
_countof(local_scan))
612 rplidar_response_measurement_node_t* nodebuffer,
size_t&
count,
631 size_to_copy *
sizeof(rplidar_response_measurement_node_t));
632 count = size_to_copy;
644 rplidar_response_measurement_node_t* nodebuffer,
size_t count)
646 float inc_origin_angle = 360.0f /
count;
650 for (i = 0; i <
count && nodebuffer[i].distance_q2 == 0; i++)
659 float expect_angle = (nodebuffer[i + 1].angle_q6_checkbit >>
663 if (expect_angle < 0.0f) expect_angle = 0.0f;
666 nodebuffer[i].angle_q6_checkbit =
667 (((
_u16)(expect_angle * 64.0f))
673 for (i =
count - 1; nodebuffer[i].distance_q2 == 0; i--)
676 while (i != (
count - 1))
679 float expect_angle = (nodebuffer[i - 1].angle_q6_checkbit >>
683 if (expect_angle > 360.0f) expect_angle -= 360.0f;
686 nodebuffer[i].angle_q6_checkbit =
687 (((
_u16)(expect_angle * 64.0f))
693 float frontAngle = (nodebuffer[0].angle_q6_checkbit >>
696 for (i = 1; i <
count; i++)
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))
712 for (i = 0; i < (
count - 1); i++)
714 for (
size_t j = (i + 1); j <
count; j++)
719 rplidar_response_measurement_node_t temp = nodebuffer[i];
720 nodebuffer[i] = nodebuffer[j];
721 nodebuffer[j] = temp;
730 rplidar_response_measurement_node_t* node,
_u32 timeout)
734 _u8 recvBuffer[
sizeof(rplidar_response_measurement_node_t)];
735 _u8* nodeBuffer = (
_u8*)node;
738 while ((waitTime =
getms() - startTs) <= timeout)
741 sizeof(rplidar_response_measurement_node_t) - recvPos;
750 if (recvSize > remainSize) recvSize = remainSize;
754 for (
size_t pos = 0; pos < recvSize; ++pos)
756 _u8 currentByte = recvBuffer[pos];
761 _u8 tmp = (currentByte >> 1);
762 if ((tmp ^ currentByte) & 0x1)
786 nodeBuffer[recvPos++] = currentByte;
788 if (recvPos ==
sizeof(rplidar_response_measurement_node_t))
799 rplidar_response_measurement_node_t* nodebuffer,
size_t&
count,
808 size_t recvNodeCount = 0;
813 while ((waitTime =
getms() - startTs) <= timeout && recvNodeCount <
count)
815 rplidar_response_measurement_node_t node;
821 nodebuffer[recvNodeCount++] = node;
825 count = recvNodeCount;
830 rplidar_response_capsule_measurement_nodes_t& node,
_u32 timeout)
834 _u8 recvBuffer[
sizeof(rplidar_response_capsule_measurement_nodes_t)];
835 _u8* nodeBuffer = (
_u8*)&node;
838 while ((waitTime =
getms() - startTs) <= timeout)
841 sizeof(rplidar_response_capsule_measurement_nodes_t) - recvPos;
850 if (recvSize > remainSize) recvSize = remainSize;
854 for (
size_t pos = 0; pos < recvSize; ++pos)
856 _u8 currentByte = recvBuffer[pos];
861 _u8 tmp = (currentByte >> 4);
875 _u8 tmp = (currentByte >> 4);
889 nodeBuffer[recvPos++] = currentByte;
891 if (recvPos ==
sizeof(rplidar_response_capsule_measurement_nodes_t))
896 ((node.s_checksum_1 & 0xF) | (node.s_checksum_2 << 4));
898 rplidar_response_capsule_measurement_nodes_t,
901 sizeof(rplidar_response_capsule_measurement_nodes_t);
904 checksum ^= nodeBuffer[cpos];
906 if (recvChecksum == checksum)
909 if (node.start_angle_sync_q6 &
929 _u8 cmd,
const void* payload,
size_t payloadsize)
932 rplidar_cmd_packet_t*
header =
933 reinterpret_cast<rplidar_cmd_packet_t*
>(pkt_header);
938 if (payloadsize && payload)
953 checksum ^= (payloadsize & 0xFF);
956 for (
size_t pos = 0; pos < payloadsize; ++pos)
958 checksum ^= ((
_u8*)payload)[pos];
962 _u8 sizebyte = payloadsize;
980 _u8 recvBuffer[
sizeof(rplidar_ans_header_t)];
984 while ((waitTime =
getms() - startTs) <= timeout)
986 size_t remainSize =
sizeof(rplidar_ans_header_t) - recvPos;
995 if (recvSize > remainSize) recvSize = remainSize;
999 for (
size_t pos = 0; pos < recvSize; ++pos)
1001 _u8 currentByte = recvBuffer[pos];
1019 headerBuffer[recvPos++] = currentByte;
1021 if (recvPos ==
sizeof(rplidar_ans_header_t))
1038 rplidar_response_sample_rate_t& rateInfo,
_u32 timeout)
1044 rplidar_response_device_info_t devinfo;
1051 if (devinfo.firmware_version < ((0x1 << 8) | 17))
1066 rplidar_ans_header_t response_header;
1080 if (header_size <
sizeof(rplidar_response_sample_rate_t))
1096 bool& support,
_u32 timeout)
1108 rplidar_payload_acc_board_flag_t flag;
1118 rplidar_ans_header_t response_header;
1132 if (header_size <
sizeof(rplidar_response_acc_board_flag_t))
1142 rplidar_response_acc_board_flag_t acc_board_flag;
1144 reinterpret_cast<_u8*
>(&acc_board_flag),
sizeof(acc_board_flag));
1146 if (acc_board_flag.support_flag &
1158 rplidar_payload_motor_pwm_t motor_pwm;
1159 motor_pwm.pwm_value = pwm;
1167 sizeof(motor_pwm))))