1.new feature:support vs2019; 1.bugfix: grabscandata may miss the syn…

…cbit

sdk/app/frame_grabber/stdafx.h

@@ -75,4 +75,5 @@ using namespace WTL;
 
 //STL
 #include <vector>
-#include <map>
+#include <map>
+#include <string>

sdk/app/simple_grabber/main.cpp

@@ -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"

sdk/app/ultra_simple/main.cpp

@@ -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" 
@@ -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
@@ -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) {

sdk/sdk/src/rplidar_driver.cpp

@@ -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))) {
@@ -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));
 
@@ -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];
@@ -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);
@@ -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;
@@ -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);
 
@@ -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;
  }
 
@@ -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;
 }
 
@@ -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))
  {
@@ -1767,6 +1797,7 @@ u_result RPlidarDriverImplCommon::startScanExpress(bool force, _u16 scanMode, _u
  if (_cachethread.getHandle() == 0) {
  return RESULT_OPERATION_FAIL;
  }
+
  }
  return RESULT_OK;
 }
@@ -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;
 }
 

sdk/sdk/src/rplidar_driver_impl.h

@@ -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);
 
@@ -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;
 
 
 

sdk/workspaces/vc10/rplidar_driver/x64/Debug/rplidar_driver.lastbuildstate

@@ -0,0 +1,2 @@
+#v4.0:v100
+Debug|x64|D:\sdk\rplidar_sdk\rplidar_sdk\sdk\workspaces\vc10\|