Skip to content

Commit e13f3b8

Browse files
WubinXiaWubinXia
committed
1.new feature:support vs2019; 1.bugfix: grabscandata may miss the syncbit
1 parent a9ad2ce commit e13f3b8

22 files changed

+841
-28
lines changed

‎sdk/app/frame_grabber/stdafx.h‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -75,4 +75,5 @@ using namespace WTL;
7575

7676
//STL
7777
#include <vector>
78-
#include <map>
78+
#include <map>
79+
#include <string>

‎sdk/app/simple_grabber/main.cpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -54,7 +54,7 @@ using namespace rp::standalone::rplidar;
5454
void print_usage(int argc, const char * argv[])
5555
{
5656
printf("Simple LIDAR data grabber for RPLIDAR.\n"
57-
"Version: "RPLIDAR_SDK_VERSION"\n"
57+
"Version: " RPLIDAR_SDK_VERSION "\n"
5858
"Usage:\n"
5959
"%s <com port> [baudrate]\n"
6060
"The default baudrate is 115200(for A2) or 256000(for A3). Please refer to the datasheet for details.\n"

‎sdk/app/ultra_simple/main.cpp‎

Lines changed: 2 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -90,7 +90,7 @@ int main(int argc, const char * argv[]) {
9090
bool useArgcBaudrate = false;
9191

9292
printf("Ultra simple LIDAR data grabber for RPLIDAR.\n"
93-
"Version: "RPLIDAR_SDK_VERSION"\n");
93+
"Version: " RPLIDAR_SDK_VERSION "\n");
9494

9595
// read serial port from the command line...
9696
if (argc>1) opt_com_path = argv[1]; // or set to a fixed value: e.g. "com3"
@@ -105,7 +105,7 @@ int main(int argc, const char * argv[]) {
105105
if (!opt_com_path) {
106106
#ifdef _WIN32
107107
// use default com port
108-
opt_com_path = "\\\\.\\com3";
108+
opt_com_path = "\\\\.\\com57";
109109
#elif __APPLE__
110110
opt_com_path = "/dev/tty.SLAB_USBtoUART";
111111
#else
@@ -205,7 +205,6 @@ int main(int argc, const char * argv[]) {
205205
size_t count = _countof(nodes);
206206

207207
op_result = drv->grabScanDataHq(nodes, count);
208-
209208
if (IS_OK(op_result)) {
210209
drv->ascendScanData(nodes, count);
211210
for (int pos = 0; pos < (int)count ; ++pos) {

‎sdk/sdk/src/rplidar_driver.cpp‎

Lines changed: 55 additions & 23 deletions
Original file line numberDiff line numberDiff line change
@@ -665,21 +665,49 @@ u_result RPlidarDriverImplCommon::checkExpressScanSupported(bool & support, _u32
665665
return RESULT_OK;
666666
}
667667

668+
669+
int RPlidarDriverImplCommon::_getSyncBitByAngle(const int current_angle_q16, const int angleInc_q16)
670+
{
671+
static int last_angleInc_q16 = 0;
672+
int current_angleInc_q16 = angleInc_q16;
673+
int syncBit_check_threshold = (int)((5 << 16) / angleInc_q16) + 1;//find syncBit in 0~3 degree
674+
int syncBit = 0;
675+
int predict_angle_q16 = (current_angle_q16 + angleInc_q16) % (360 << 16);
676+
677+
if (predict_angle_q16 < 0) {
678+
predict_angle_q16 += (360 << 16);
679+
}
680+
if (!_syncBit_is_finded)
681+
{
682+
if (0 < predict_angle_q16 && predict_angle_q16 < (90 << 16))
683+
syncBit = 1;
684+
if (syncBit)
685+
_syncBit_is_finded = true;
686+
}
687+
else
688+
{
689+
if(predict_angle_q16 > (270<<16))
690+
_syncBit_is_finded = false;
691+
//if (predict_angle_q16 > (syncBit_check_threshold * angleInc_q16)) {
692+
// _is_previous_syncBit = false;
693+
//}
694+
}
695+
last_angleInc_q16 = current_angleInc_q16;
696+
return syncBit;
697+
}
698+
668699
u_result RPlidarDriverImplCommon::_cacheCapsuledScanData()
669700
{
670701
rplidar_response_capsule_measurement_nodes_t capsule_node;
671-
rplidar_response_measurement_node_hq_t local_buf[128];
672-
size_t count = 128;
702+
rplidar_response_measurement_node_hq_t local_buf[512];
703+
size_t count = 512;
673704
rplidar_response_measurement_node_hq_t local_scan[MAX_SCAN_NODES];
674705
size_t scan_count = 0;
675706
u_result ans;
676707
memset(local_scan, 0, sizeof(local_scan));
677708

678709
_waitCapsuledNode(capsule_node); // // always discard the first data since it may be incomplete
679-
680-
681710

682-
683711
while(_isScanning)
684712
{
685713
if (IS_FAIL(ans=_waitCapsuledNode(capsule_node))) {
@@ -736,10 +764,11 @@ u_result RPlidarDriverImplCommon::_cacheCapsuledScanData()
736764
u_result RPlidarDriverImplCommon::_cacheUltraCapsuledScanData()
737765
{
738766
rplidar_response_ultra_capsule_measurement_nodes_t ultra_capsule_node;
739-
rplidar_response_measurement_node_hq_t local_buf[128];
740-
size_t count = 128;
767+
rplidar_response_measurement_node_hq_t local_buf[512];
768+
size_t count = 512;
741769
rplidar_response_measurement_node_hq_t local_scan[MAX_SCAN_NODES];
742770
size_t scan_count = 0;
771+
size_t last_scan_count = 0;
743772
u_result ans;
744773
memset(local_scan, 0, sizeof(local_scan));
745774

@@ -809,32 +838,33 @@ void RPlidarDriverImplCommon::_capsuleToNormal(const rplidar_response_capsul
809838
for (size_t pos = 0; pos < _countof(_cached_previous_capsuledata.cabins); ++pos)
810839
{
811840
int dist_q2[2];
812-
int angle_q6[2];
813-
int syncBit[2];
841+
int angle_q16[2];
842+
int syncBit[2] = { 0,0 };
814843

815844
dist_q2[0] = (_cached_previous_capsuledata.cabins[pos].distance_angle_1 & 0xFFFC);
816845
dist_q2[1] = (_cached_previous_capsuledata.cabins[pos].distance_angle_2 & 0xFFFC);
817846

818847
int angle_offset1_q3 = ( (_cached_previous_capsuledata.cabins[pos].offset_angles_q3 & 0xF) | ((_cached_previous_capsuledata.cabins[pos].distance_angle_1 & 0x3)<<4));
819848
int angle_offset2_q3 = ( (_cached_previous_capsuledata.cabins[pos].offset_angles_q3 >> 4) | ((_cached_previous_capsuledata.cabins[pos].distance_angle_2 & 0x3)<<4));
820849

821-
angle_q6[0] = ((currentAngle_raw_q16 - (angle_offset1_q3<<13))>>10);
822-
syncBit[0] = (( (currentAngle_raw_q16 + angleInc_q16) % (360<<16)) < angleInc_q16 )?1:0;
823-
currentAngle_raw_q16 += angleInc_q16;
850+
int syncBit_check_threshold = (int)((2 << 16) / angleInc_q16) + 1;//find syncBit in 0~1 degree
824851

852+
angle_q16[0] = (currentAngle_raw_q16 - (angle_offset1_q3<<13));
853+
syncBit[0] = _getSyncBitByAngle(currentAngle_raw_q16, angleInc_q16);
854+
currentAngle_raw_q16 += angleInc_q16;
825855

826-
angle_q6[1] = ((currentAngle_raw_q16 - (angle_offset2_q3<<13))>>10);
827-
syncBit[1] = (( (currentAngle_raw_q16 + angleInc_q16) % (360<<16)) < angleInc_q16 )?1:0;
856+
angle_q16[1] = (currentAngle_raw_q16 - (angle_offset2_q3<<13));
857+
syncBit[1] = _getSyncBitByAngle(currentAngle_raw_q16, angleInc_q16);
828858
currentAngle_raw_q16 += angleInc_q16;
829859

830860
for (int cpos = 0; cpos < 2; ++cpos) {
831861

832-
if (angle_q6[cpos] < 0) angle_q6[cpos] += (360<<6);
833-
if (angle_q6[cpos] >= (360<<6)) angle_q6[cpos] -= (360<<6);
862+
if (angle_q16[cpos] < 0) angle_q16[cpos] += (360<<16);
863+
if (angle_q16[cpos] >= (360<<16)) angle_q16[cpos] -= (360<<16);
834864

835865
rplidar_response_measurement_node_hq_t node;
836866

837-
node.angle_z_q14 = _u16((angle_q6[cpos] << 8) / 90);
867+
node.angle_z_q14 = _u16((angle_q16[cpos] >> 2) / 90);
838868
node.flag = (syncBit[cpos] | ((!syncBit[cpos]) << 1));
839869
node.quality = dist_q2[cpos] ? (0x2f << RPLIDAR_RESP_MEASUREMENT_QUALITY_SHIFT) : 0;
840870
node.dist_mm_q2 = dist_q2[cpos];
@@ -873,7 +903,7 @@ void RPlidarDriverImplCommon::_dense_capsuleToNormal(const rplidar_response_
873903
const int dist = static_cast<const int>(_cached_previous_dense_capsuledata.cabins[pos].distance);
874904
dist_q2 = dist << 2;
875905
angle_q6 = (currentAngle_raw_q16 >> 10);
876-
syncBit = (((currentAngle_raw_q16 + angleInc_q16) % (360 << 16)) < angleInc_q16) ? 1 : 0;
906+
syncBit = _getSyncBitByAngle(currentAngle_raw_q16, angleInc_q16);
877907
currentAngle_raw_q16 += angleInc_q16;
878908

879909
if (angle_q6 < 0) angle_q6 += (360 << 6);
@@ -1163,7 +1193,7 @@ void RPlidarDriverImplCommon::_ultraCapsuleToNormal(const rplidar_response_ultra
11631193
{
11641194
int dist_q2[3];
11651195
int angle_q6[3];
1166-
int syncBit[3];
1196+
int syncBit[3] = {0};
11671197

11681198

11691199
_u32 combined_x3 = _cached_previous_ultracapsuledata.ultra_cabins[pos].combined_x3;
@@ -1223,8 +1253,8 @@ void RPlidarDriverImplCommon::_ultraCapsuleToNormal(const rplidar_response_ultra
12231253

12241254
for (int cpos = 0; cpos < 3; ++cpos)
12251255
{
1226-
1227-
syncBit[cpos] = (((currentAngle_raw_q16 + angleInc_q16) % (360 << 16)) < angleInc_q16) ? 1 : 0;
1256+
int syncBit_check_threshold = (int)((3 << 16) / angleInc_q16)+1;//find syncBit in 0~1 degree
1257+
syncBit[cpos] = _getSyncBitByAngle(currentAngle_raw_q16, angleInc_q16);
12281258

12291259
int offsetAngleMean_q16 = (int)(7.5 * 3.1415926535 * (1 << 16) / 180.0);
12301260

@@ -1248,7 +1278,6 @@ void RPlidarDriverImplCommon::_ultraCapsuleToNormal(const rplidar_response_ultra
12481278
node.quality = dist_q2[cpos] ? (0x2F << RPLIDAR_RESP_MEASUREMENT_QUALITY_SHIFT) : 0;
12491279
node.angle_z_q14 = _u16((angle_q6[cpos] << 8) / 90);
12501280
node.dist_mm_q2 = dist_q2[cpos];
1251-
12521281
nodebuffer[nodeCount++] = node;
12531282
}
12541283

@@ -1388,6 +1417,7 @@ u_result RPlidarDriverImplCommon::getLidarSampleDuration(float& sampleDurationRe
13881417
}
13891418
const _u32 *result = reinterpret_cast<const _u32*>(&answer[0]);
13901419
sampleDurationRes = (float)(*result >> 8);
1420+
_cached_current_us_per_sample = sampleDurationRes;
13911421
return ans;
13921422
}
13931423

@@ -1659,7 +1689,7 @@ u_result RPlidarDriverImplCommon::startScanExpress(bool force, _u16 scanMode, _u
16591689
{
16601690
return RESULT_INVALID_DATA;
16611691
}
1662-
1692+
16631693
ans = getMaxDistance(outUsedScanMode->max_distance, outUsedScanMode->id);
16641694
if (IS_FAIL(ans))
16651695
{
@@ -1767,6 +1797,7 @@ u_result RPlidarDriverImplCommon::startScanExpress(bool force, _u16 scanMode, _u
17671797
if (_cachethread.getHandle() == 0) {
17681798
return RESULT_OPERATION_FAIL;
17691799
}
1800+
17701801
}
17711802
return RESULT_OK;
17721803
}
@@ -2090,6 +2121,7 @@ u_result RPlidarDriverImplCommon::getSampleDuration_uS(rplidar_response_sample_r
20902121
}
20912122
_chanDev->recvdata(reinterpret_cast<_u8 *>(&rateInfo), sizeof(rateInfo));
20922123
}
2124+
_cached_current_us_per_sample = rateInfo.express_sample_duration_us;
20932125
return RESULT_OK;
20942126
}
20952127

‎sdk/sdk/src/rplidar_driver_impl.h‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -92,6 +92,7 @@ namespace rp { namespace standalone{ namespace rplidar {
9292
virtual u_result _waitNode(rplidar_response_measurement_node_t * node, _u32 timeout = DEFAULT_TIMEOUT);
9393
virtual u_result _cacheCapsuledScanData();
9494
virtual u_result _waitCapsuledNode(rplidar_response_capsule_measurement_nodes_t & node, _u32 timeout = DEFAULT_TIMEOUT);
95+
virtual int _getSyncBitByAngle(const int current_angle_q16, const int angleInc_q16);
9596
virtual void _capsuleToNormal(const rplidar_response_capsule_measurement_nodes_t & capsule, rplidar_response_measurement_node_hq_t *nodebuffer, size_t &nodeCount);
9697
virtual void _dense_capsuleToNormal(const rplidar_response_capsule_measurement_nodes_t & capsule, rplidar_response_measurement_node_hq_t *nodebuffer, size_t &nodeCount);
9798

@@ -117,13 +118,15 @@ namespace rp { namespace standalone{ namespace rplidar {
117118
_u16 _cached_sampleduration_std;
118119
_u16 _cached_sampleduration_express;
119120
_u8 _cached_express_flag;
121+
float _cached_current_us_per_sample;
120122

121123
rplidar_response_capsule_measurement_nodes_t _cached_previous_capsuledata;
122124
rplidar_response_dense_capsule_measurement_nodes_t _cached_previous_dense_capsuledata;
123125
rplidar_response_ultra_capsule_measurement_nodes_t _cached_previous_ultracapsuledata;
124126
rplidar_response_hq_capsule_measurement_nodes_t _cached_previous_Hqdata;
125127
bool _is_previous_capsuledataRdy;
126128
bool _is_previous_HqdataRdy;
129+
bool _syncBit_is_finded;
127130

128131

129132

‎sdk/workspaces/vc10/rplidar_driver/x64/Debug/CL.read.1.tlog‎

112 KB
Binary file not shown.

‎sdk/workspaces/vc10/rplidar_driver/x64/Debug/CL.write.1.tlog‎

3.02 KB
Binary file not shown.

‎sdk/workspaces/vc10/rplidar_driver/x64/Debug/Lib-link.read.1.tlog‎

1.93 KB
Binary file not shown.

‎sdk/workspaces/vc10/rplidar_driver/x64/Debug/Lib-link.write.1.tlog‎

1.04 KB
Binary file not shown.

‎sdk/workspaces/vc10/rplidar_driver/x64/Debug/cl.command.1.tlog‎

3.61 KB
Binary file not shown.

0 commit comments

Comments
 (0)