Skip to content

Commit

Permalink
1.new feature:support vs2019; 1.bugfix: grabscandata may miss the syn…
Browse files Browse the repository at this point in the history 
…cbit
  • Loading branch information
@WubinXia
WubinXia committed Apr 9, 2020
1 parent a9ad2ce commit 504a2e5
Show file tree
Hide file tree
Showing 14 changed files with 839 additions and 28 deletions.
3 changes: 2 additions & 1 deletion sdk/app/frame_grabber/stdafx.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -75,4 +75,5 @@ using namespace WTL;

//STL
#include <vector>
#include <map>
#include <map>
#include <string>
2 changes: 1 addition & 1 deletion sdk/app/simple_grabber/main.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -54,7 +54,7 @@ using namespace rp::standalone::rplidar;
void print_usage(int argc, const char * argv[])
{
printf("Simple LIDAR data grabber for RPLIDAR.\n"
"Version: "RPLIDAR_SDK_VERSION"\n"
"Version: " RPLIDAR_SDK_VERSION "\n"
"Usage:\n"
"%s <com port> [baudrate]\n"
"The default baudrate is 115200(for A2) or 256000(for A3). Please refer to the datasheet for details.\n"
Expand Down
5 changes: 2 additions & 3 deletions sdk/app/ultra_simple/main.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -90,7 +90,7 @@ int main(int argc, const char * argv[]) {
bool useArgcBaudrate = false;

printf("Ultra simple LIDAR data grabber for RPLIDAR.\n"
"Version: "RPLIDAR_SDK_VERSION"\n");
"Version: " RPLIDAR_SDK_VERSION "\n");

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

op_result = drv->grabScanDataHq(nodes, count);

if (IS_OK(op_result)) {
drv->ascendScanData(nodes, count);
for (int pos = 0; pos < (int)count ; ++pos) {
Expand Down
78 changes: 55 additions & 23 deletions sdk/sdk/src/rplidar_driver.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -665,21 +665,49 @@ u_result RPlidarDriverImplCommon::checkExpressScanSupported(bool & support, _u32
return RESULT_OK;
}


int RPlidarDriverImplCommon::_getSyncBitByAngle(const int current_angle_q16, const int angleInc_q16)
{
static int last_angleInc_q16 = 0;
int current_angleInc_q16 = angleInc_q16;
int syncBit_check_threshold = (int)((5 << 16) / angleInc_q16) + 1;//find syncBit in 0~3 degree
int syncBit = 0;
int predict_angle_q16 = (current_angle_q16 + angleInc_q16) % (360 << 16);

if (predict_angle_q16 < 0) {
predict_angle_q16 += (360 << 16);
}
if (!_syncBit_is_finded)
{
if (0 < predict_angle_q16 && predict_angle_q16 < (90 << 16))
syncBit = 1;
if (syncBit)
_syncBit_is_finded = true;
}
else
{
if(predict_angle_q16 > (270<<16))
_syncBit_is_finded = false;
//if (predict_angle_q16 > (syncBit_check_threshold * angleInc_q16)) {
// _is_previous_syncBit = false;
//}
}
last_angleInc_q16 = current_angleInc_q16;
return syncBit;
}

u_result RPlidarDriverImplCommon::_cacheCapsuledScanData()
{
rplidar_response_capsule_measurement_nodes_t capsule_node;
rplidar_response_measurement_node_hq_t local_buf[128];
size_t count = 128;
rplidar_response_measurement_node_hq_t local_buf[512];
size_t count = 512;
rplidar_response_measurement_node_hq_t local_scan[MAX_SCAN_NODES];
size_t scan_count = 0;
u_result ans;
memset(local_scan, 0, sizeof(local_scan));

_waitCapsuledNode(capsule_node); // // always discard the first data since it may be incomplete




while(_isScanning)
{
if (IS_FAIL(ans=_waitCapsuledNode(capsule_node))) {
Expand Down Expand Up @@ -736,10 +764,11 @@ u_result RPlidarDriverImplCommon::_cacheCapsuledScanData()
u_result RPlidarDriverImplCommon::_cacheUltraCapsuledScanData()
{
rplidar_response_ultra_capsule_measurement_nodes_t ultra_capsule_node;
rplidar_response_measurement_node_hq_t local_buf[128];
size_t count = 128;
rplidar_response_measurement_node_hq_t local_buf[512];
size_t count = 512;
rplidar_response_measurement_node_hq_t local_scan[MAX_SCAN_NODES];
size_t scan_count = 0;
size_t last_scan_count = 0;
u_result ans;
memset(local_scan, 0, sizeof(local_scan));

Expand Down Expand Up @@ -809,32 +838,33 @@ void RPlidarDriverImplCommon::_capsuleToNormal(const rplidar_response_capsul
for (size_t pos = 0; pos < _countof(_cached_previous_capsuledata.cabins); ++pos)
{
int dist_q2[2];
int angle_q6[2];
int syncBit[2];
int angle_q16[2];
int syncBit[2] = { 0,0 };

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

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

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

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

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

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

if (angle_q6[cpos] < 0) angle_q6[cpos] += (360<<6);
if (angle_q6[cpos] >= (360<<6)) angle_q6[cpos] -= (360<<6);
if (angle_q16[cpos] < 0) angle_q16[cpos] += (360<<16);
if (angle_q16[cpos] >= (360<<16)) angle_q16[cpos] -= (360<<16);

rplidar_response_measurement_node_hq_t node;

node.angle_z_q14 = _u16((angle_q6[cpos] << 8) / 90);
node.angle_z_q14 = _u16((angle_q16[cpos] >> 2) / 90);
node.flag = (syncBit[cpos] | ((!syncBit[cpos]) << 1));
node.quality = dist_q2[cpos] ? (0x2f << RPLIDAR_RESP_MEASUREMENT_QUALITY_SHIFT) : 0;
node.dist_mm_q2 = dist_q2[cpos];
Expand Down Expand Up @@ -873,7 +903,7 @@ void RPlidarDriverImplCommon::_dense_capsuleToNormal(const rplidar_response_
const int dist = static_cast<const int>(_cached_previous_dense_capsuledata.cabins[pos].distance);
dist_q2 = dist << 2;
angle_q6 = (currentAngle_raw_q16 >> 10);
syncBit = (((currentAngle_raw_q16 + angleInc_q16) % (360 << 16)) < angleInc_q16) ? 1 : 0;
syncBit = _getSyncBitByAngle(currentAngle_raw_q16, angleInc_q16);
currentAngle_raw_q16 += angleInc_q16;

if (angle_q6 < 0) angle_q6 += (360 << 6);
Expand Down Expand Up @@ -1163,7 +1193,7 @@ void RPlidarDriverImplCommon::_ultraCapsuleToNormal(const rplidar_response_ultra
{
int dist_q2[3];
int angle_q6[3];
int syncBit[3];
int syncBit[3] = {0};


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

for (int cpos = 0; cpos < 3; ++cpos)
{

syncBit[cpos] = (((currentAngle_raw_q16 + angleInc_q16) % (360 << 16)) < angleInc_q16) ? 1 : 0;
int syncBit_check_threshold = (int)((3 << 16) / angleInc_q16)+1;//find syncBit in 0~1 degree
syncBit[cpos] = _getSyncBitByAngle(currentAngle_raw_q16, angleInc_q16);

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

Expand All @@ -1248,7 +1278,6 @@ void RPlidarDriverImplCommon::_ultraCapsuleToNormal(const rplidar_response_ultra
node.quality = dist_q2[cpos] ? (0x2F << RPLIDAR_RESP_MEASUREMENT_QUALITY_SHIFT) : 0;
node.angle_z_q14 = _u16((angle_q6[cpos] << 8) / 90);
node.dist_mm_q2 = dist_q2[cpos];

nodebuffer[nodeCount++] = node;
}

Expand Down Expand Up @@ -1388,6 +1417,7 @@ u_result RPlidarDriverImplCommon::getLidarSampleDuration(float& sampleDurationRe
}
const _u32 *result = reinterpret_cast<const _u32*>(&answer[0]);
sampleDurationRes = (float)(*result >> 8);
_cached_current_us_per_sample = sampleDurationRes;
return ans;
}

Expand Down Expand Up @@ -1659,7 +1689,7 @@ u_result RPlidarDriverImplCommon::startScanExpress(bool force, _u16 scanMode, _u
{
return RESULT_INVALID_DATA;
}

ans = getMaxDistance(outUsedScanMode->max_distance, outUsedScanMode->id);
if (IS_FAIL(ans))
{
Expand Down Expand Up @@ -1767,6 +1797,7 @@ u_result RPlidarDriverImplCommon::startScanExpress(bool force, _u16 scanMode, _u
if (_cachethread.getHandle() == 0) {
return RESULT_OPERATION_FAIL;
}

}
return RESULT_OK;
}
Expand Down Expand Up @@ -2090,6 +2121,7 @@ u_result RPlidarDriverImplCommon::getSampleDuration_uS(rplidar_response_sample_r
}
_chanDev->recvdata(reinterpret_cast<_u8 *>(&rateInfo), sizeof(rateInfo));
}
_cached_current_us_per_sample = rateInfo.express_sample_duration_us;
return RESULT_OK;
}

Expand Down
3 changes: 3 additions & 0 deletions sdk/sdk/src/rplidar_driver_impl.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -92,6 +92,7 @@ namespace rp { namespace standalone{ namespace rplidar {
virtual u_result _waitNode(rplidar_response_measurement_node_t * node, _u32 timeout = DEFAULT_TIMEOUT);
virtual u_result _cacheCapsuledScanData();
virtual u_result _waitCapsuledNode(rplidar_response_capsule_measurement_nodes_t & node, _u32 timeout = DEFAULT_TIMEOUT);
virtual int _getSyncBitByAngle(const int current_angle_q16, const int angleInc_q16);
virtual void _capsuleToNormal(const rplidar_response_capsule_measurement_nodes_t & capsule, rplidar_response_measurement_node_hq_t *nodebuffer, size_t &nodeCount);
virtual void _dense_capsuleToNormal(const rplidar_response_capsule_measurement_nodes_t & capsule, rplidar_response_measurement_node_hq_t *nodebuffer, size_t &nodeCount);

Expand All @@ -117,13 +118,15 @@ namespace rp { namespace standalone{ namespace rplidar {
_u16 _cached_sampleduration_std;
_u16 _cached_sampleduration_express;
_u8 _cached_express_flag;
float _cached_current_us_per_sample;

rplidar_response_capsule_measurement_nodes_t _cached_previous_capsuledata;
rplidar_response_dense_capsule_measurement_nodes_t _cached_previous_dense_capsuledata;
rplidar_response_ultra_capsule_measurement_nodes_t _cached_previous_ultracapsuledata;
rplidar_response_hq_capsule_measurement_nodes_t _cached_previous_Hqdata;
bool _is_previous_capsuledataRdy;
bool _is_previous_HqdataRdy;
bool _syncBit_is_finded;



Expand Down
Loading

0 comments on commit 504a2e5

Please sign in to comment.