42 #define min(a, b) (((a) < (b)) ? (a) : (b)) 63 : _isConnected(false), _isScanning(false), _isSupportingMotorCtrl(false)
80 const char* port_path,
_u32 baudrate,
_u32 flag)
130 rplidar_response_device_health_t& healthinfo,
_u32 timeout)
146 rplidar_ans_header_t response_header;
160 if (header_size <
sizeof(rplidar_response_device_health_t))
171 reinterpret_cast<_u8*>(&healthinfo),
sizeof(healthinfo));
177 rplidar_response_device_info_t& info,
_u32 timeout)
193 rplidar_ans_header_t response_header;
207 if (header_size <
sizeof(rplidar_response_device_info_t))
218 _rxtx->
recvdata(reinterpret_cast<_u8*>(&info),
sizeof(info));
224 bool inExpressMode,
size_t count,
float& frequency,
bool& is4kmode)
228 frequency = 1000000.0f / (
count * sample_duration);
230 if (sample_duration <= 277)
261 rplidar_ans_header_t response_header;
275 if (header_size <
sizeof(rplidar_response_measurement_node_t))
291 bool& support,
_u32 timeout)
293 rplidar_response_device_info_t devinfo;
300 if (devinfo.firmware_version >= ((0x1 << 8) | 17))
303 rplidar_response_sample_rate_t sample_rate;
313 bool fixedAngle,
_u32 timeout)
324 rplidar_payload_express_scan_t scanReq;
325 scanReq.working_mode =
328 scanReq.reserved = 0;
338 rplidar_ans_header_t response_header;
352 if (header_size <
sizeof(rplidar_response_capsule_measurement_nodes_t))
370 bool isExpressModeSupported;
379 if (isExpressModeSupported)
407 rplidar_response_measurement_node_t local_buf[128];
410 size_t scan_count = 0;
412 memset(local_scan, 0,
sizeof(local_scan));
429 for (
size_t pos = 0; pos <
count; ++pos)
442 sizeof(rplidar_response_measurement_node_t));
449 local_scan[scan_count++] = local_buf[pos];
450 if (scan_count ==
_countof(local_scan))
459 const rplidar_response_capsule_measurement_nodes_t& capsule,
460 rplidar_response_measurement_node_t* nodebuffer,
size_t& nodeCount)
466 int currentStartAngle_q8 =
467 ((capsule.start_angle_sync_q6 & 0x7FFF) << 2);
468 int prevStartAngle_q8 =
471 diffAngle_q8 = (currentStartAngle_q8) - (prevStartAngle_q8);
472 if (prevStartAngle_q8 > currentStartAngle_q8)
474 diffAngle_q8 += (360 << 8);
477 int angleInc_q16 = (diffAngle_q8 << 3);
478 int currentAngle_raw_q16 = (prevStartAngle_q8 << 8);
493 int angle_offset1_q3 =
499 int angle_offset2_q3 =
507 ((currentAngle_raw_q16 - (angle_offset1_q3 << 13)) >> 10);
508 syncBit[0] = (((currentAngle_raw_q16 + angleInc_q16) %
509 (360 << 16)) < angleInc_q16)
512 currentAngle_raw_q16 += angleInc_q16;
515 ((currentAngle_raw_q16 - (angle_offset2_q3 << 13)) >> 10);
516 syncBit[1] = (((currentAngle_raw_q16 + angleInc_q16) %
517 (360 << 16)) < angleInc_q16)
520 currentAngle_raw_q16 += angleInc_q16;
522 for (
int cpos = 0; cpos < 2; ++cpos)
524 if (angle_q6[cpos] < 0) angle_q6[cpos] += (360 << 6);
525 if (angle_q6[cpos] >= (360 << 6)) angle_q6[cpos] -= (360 << 6);
527 rplidar_response_measurement_node_t node;
529 node.sync_quality = (syncBit[cpos] | ((!syncBit[cpos]) << 1));
534 node.angle_q6_checkbit = (1 | (angle_q6[cpos] << 1));
535 node.distance_q2 = dist_q2[cpos];
537 nodebuffer[nodeCount++] = node;
548 rplidar_response_capsule_measurement_nodes_t capsule_node;
549 rplidar_response_measurement_node_t local_buf[128];
552 size_t scan_count = 0;
554 memset(local_scan, 0,
sizeof(local_scan));
577 for (
size_t pos = 0; pos <
count; ++pos)
590 sizeof(rplidar_response_measurement_node_t));
597 local_scan[scan_count++] = local_buf[pos];
598 if (scan_count ==
_countof(local_scan))
608 rplidar_response_measurement_node_t* nodebuffer,
size_t&
count,
627 size_to_copy *
sizeof(rplidar_response_measurement_node_t));
628 count = size_to_copy;
640 rplidar_response_measurement_node_t* nodebuffer,
size_t count)
642 float inc_origin_angle = 360.0f /
count;
646 for (i = 0; i <
count && nodebuffer[i].distance_q2 == 0; i++)
655 float expect_angle = (nodebuffer[i + 1].angle_q6_checkbit >>
659 if (expect_angle < 0.0f) expect_angle = 0.0f;
662 nodebuffer[i].angle_q6_checkbit =
663 (((
_u16)(expect_angle * 64.0f))
669 for (i =
count - 1; nodebuffer[i].distance_q2 == 0; i--)
672 while (i != (
count - 1))
675 float expect_angle = (nodebuffer[i - 1].angle_q6_checkbit >>
679 if (expect_angle > 360.0f) expect_angle -= 360.0f;
682 nodebuffer[i].angle_q6_checkbit =
683 (((
_u16)(expect_angle * 64.0f))
689 float frontAngle = (nodebuffer[0].angle_q6_checkbit >>
692 for (i = 1; i <
count; i++)
696 float expect_angle = frontAngle + i * inc_origin_angle;
697 if (expect_angle > 360.0f) expect_angle -= 360.0f;
698 _u16 checkbit = nodebuffer[i].angle_q6_checkbit &
700 nodebuffer[i].angle_q6_checkbit =
701 (((
_u16)(expect_angle * 64.0f))
708 for (i = 0; i < (
count - 1); i++)
710 for (
size_t j = (i + 1); j <
count; j++)
715 rplidar_response_measurement_node_t temp = nodebuffer[i];
716 nodebuffer[i] = nodebuffer[j];
717 nodebuffer[j] = temp;
726 rplidar_response_measurement_node_t* node,
_u32 timeout)
730 _u8 recvBuffer[
sizeof(rplidar_response_measurement_node_t)];
731 _u8* nodeBuffer = (
_u8*)node;
734 while ((waitTime =
getms() - startTs) <= timeout)
737 sizeof(rplidar_response_measurement_node_t) - recvPos;
746 if (recvSize > remainSize) recvSize = remainSize;
750 for (
size_t pos = 0; pos < recvSize; ++pos)
752 _u8 currentByte = recvBuffer[pos];
757 _u8 tmp = (currentByte >> 1);
758 if ((tmp ^ currentByte) & 0x1)
782 nodeBuffer[recvPos++] = currentByte;
784 if (recvPos ==
sizeof(rplidar_response_measurement_node_t))
795 rplidar_response_measurement_node_t* nodebuffer,
size_t&
count,
804 size_t recvNodeCount = 0;
809 while ((waitTime =
getms() - startTs) <= timeout && recvNodeCount <
count)
811 rplidar_response_measurement_node_t node;
817 nodebuffer[recvNodeCount++] = node;
821 count = recvNodeCount;
826 rplidar_response_capsule_measurement_nodes_t& node,
_u32 timeout)
830 _u8 recvBuffer[
sizeof(rplidar_response_capsule_measurement_nodes_t)];
831 _u8* nodeBuffer = (
_u8*)&node;
834 while ((waitTime =
getms() - startTs) <= timeout)
837 sizeof(rplidar_response_capsule_measurement_nodes_t) - recvPos;
846 if (recvSize > remainSize) recvSize = remainSize;
850 for (
size_t pos = 0; pos < recvSize; ++pos)
852 _u8 currentByte = recvBuffer[pos];
857 _u8 tmp = (currentByte >> 4);
871 _u8 tmp = (currentByte >> 4);
885 nodeBuffer[recvPos++] = currentByte;
887 if (recvPos ==
sizeof(rplidar_response_capsule_measurement_nodes_t))
892 ((node.s_checksum_1 & 0xF) | (node.s_checksum_2 << 4));
894 rplidar_response_capsule_measurement_nodes_t,
897 sizeof(rplidar_response_capsule_measurement_nodes_t);
900 checksum ^= nodeBuffer[cpos];
902 if (recvChecksum == checksum)
905 if (node.start_angle_sync_q6 &
925 _u8 cmd,
const void* payload,
size_t payloadsize)
928 rplidar_cmd_packet_t*
header =
929 reinterpret_cast<rplidar_cmd_packet_t*
>(pkt_header);
934 if (payloadsize && payload)
949 checksum ^= (payloadsize & 0xFF);
952 for (
size_t pos = 0; pos < payloadsize; ++pos)
954 checksum ^= ((
_u8*)payload)[pos];
958 _u8 sizebyte = payloadsize;
976 _u8 recvBuffer[
sizeof(rplidar_ans_header_t)];
980 while ((waitTime =
getms() - startTs) <= timeout)
982 size_t remainSize =
sizeof(rplidar_ans_header_t) - recvPos;
991 if (recvSize > remainSize) recvSize = remainSize;
995 for (
size_t pos = 0; pos < recvSize; ++pos)
997 _u8 currentByte = recvBuffer[pos];
1015 headerBuffer[recvPos++] = currentByte;
1017 if (recvPos ==
sizeof(rplidar_ans_header_t))
1034 rplidar_response_sample_rate_t& rateInfo,
_u32 timeout)
1040 rplidar_response_device_info_t devinfo;
1047 if (devinfo.firmware_version < ((0x1 << 8) | 17))
1062 rplidar_ans_header_t response_header;
1076 if (header_size <
sizeof(rplidar_response_sample_rate_t))
1086 _rxtx->
recvdata(reinterpret_cast<_u8*>(&rateInfo),
sizeof(rateInfo));
1092 bool& support,
_u32 timeout)
1104 rplidar_payload_acc_board_flag_t flag;
1114 rplidar_ans_header_t response_header;
1128 if (header_size <
sizeof(rplidar_response_acc_board_flag_t))
1138 rplidar_response_acc_board_flag_t acc_board_flag;
1140 reinterpret_cast<_u8*>(&acc_board_flag),
sizeof(acc_board_flag));
1142 if (acc_board_flag.support_flag &
1154 rplidar_payload_motor_pwm_t motor_pwm;
1155 motor_pwm.pwm_value = pwm;
1163 sizeof(motor_pwm))))
virtual int recvdata(unsigned char *data, size_t size)=0
#define offsetof(_structure, _field)
#define RPLIDAR_RESP_MEASUREMENT_CHECKBIT
#define CLASS_THREAD(c, x)
#define RPLIDAR_RESP_MEASUREMENT_EXP_SYNCBIT
virtual bool bind(const char *portname, _u32 baudrate, _u32 flags=0)=0
RPlidarDriverSerialImpl()
bool _isSupportingMotorCtrl
GLuint GLuint GLsizei count
#define RPLIDAR_CMD_FORCE_SCAN
#define RPLIDAR_EXPRESS_SCAN_MODE_FIXANGLE
_u16 _cached_sampleduration_express
virtual u_result startScanNormal(bool force, _u32 timeout=DEFAULT_TIMEOUT)
virtual bool isConnected()
Returns TRUE when the connection has been established.
#define RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT
#define RPLIDAR_ANS_TYPE_MEASUREMENT_CAPSULED
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 _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's current scanning frequency from the given scan data Please refer to the applicatio...
#define RPLIDAR_ANS_TYPE_SAMPLE_RATE
size_t _cached_scan_node_count
void set(bool isSignal=true)
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
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
#define RPLIDAR_ANS_TYPE_DEVHEALTH
#define RPLIDAR_CMD_SYNC_BYTE
#define RPLIDAR_RESP_MEASUREMENT_SYNCBIT
virtual int senddata(const unsigned char *data, size_t size)=0
static serial_rxtx * CreateRxTx()
#define RESULT_INSUFFICIENT_MEMORY
#define RPLIDAR_ANS_SYNC_BYTE1
virtual void disconnect()
Disconnect with the RPLIDAR and close the serial port.
#define RPLIDAR_CMD_GET_SAMPLERATE
static void ReleaseRxTx(serial_rxtx *)
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.
u_result _cacheCapsuledScanData()
virtual u_result stopMotor()
Stop RPLIDAR's motor when using accessory board.
u_result join(unsigned long timeout=-1)
#define RPLIDAR_CMD_RESET
nv_oem6_header_t header
Novatel frame: NV_OEM6_BESTPOS.
#define RPLIDAR_RESP_ACC_BOARD_FLAG_MOTOR_CTRL_SUPPORT_MASK
#define RPLIDAR_CMDFLAG_HAS_PAYLOAD
#define RPLIDAR_ANS_TYPE_ACC_BOARD_FLAG
bool _is_previous_capsuledataRdy
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
#define RPLIDAR_CMD_EXPRESS_SCAN
rp::hal::serial_rxtx * _rxtx
virtual u_result startMotor()
Start RPLIDAR's motor when using accessory board.
virtual u_result setMotorPWM(_u16 pwm)
Set the RPLIDAR's motor pwm when using accessory board, currently valid for A2 only.
Locker::LOCK_STATUS lock(unsigned long timeout=0xFFFFFFFF)
u_result _waitResponseHeader(rplidar_ans_header_t *header, _u32 timeout=DEFAULT_TIMEOUT)
#define RESULT_OPERATION_TIMEOUT
u_result _waitNode(rplidar_response_measurement_node_t *node, _u32 timeout=DEFAULT_TIMEOUT)
#define RPLIDAR_ANS_TYPE_MEASUREMENT
#define RPLIDAR_CMD_GET_ACC_BOARD_FLAG
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
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 ~RPlidarDriverSerialImpl()
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]
u_result _cacheScanData()
unsigned long wait(unsigned long timeout=0xFFFFFFFF)
u_result _waitCapsuledNode(rplidar_response_capsule_measurement_nodes_t &node, _u32 timeout=DEFAULT_TIMEOUT)
#define RPLIDAR_RESP_MEASUREMENT_EXP_SYNC_1
#define DEFAULT_MOTOR_PWM
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...
_u16 _cached_sampleduration_std
#define RESULT_OPERATION_FAIL
#define RESULT_ALREADY_DONE
virtual void flush(_u32 flags)=0
#define RPLIDAR_RESP_MEASUREMENT_EXP_SYNC_2
#define RPLIDAR_CMD_GET_DEVICE_HEALTH
void _disableDataGrabbing()
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
#define RPLIDAR_ANS_SYNC_BYTE2
rp::hal::Thread _cachethread
virtual int waitfordata(size_t data_count, _u32 timeout=-1, size_t *returned_size=nullptr)=0
#define RESULT_INVALID_DATA
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".
#define RPLIDAR_ANS_TYPE_DEVINFO
u_result _waitScanData(rplidar_response_measurement_node_t *nodebuffer, size_t &count, _u32 timeout=DEFAULT_TIMEOUT)