Skip to content

Commit

Permalink
修改TG系列新版固件不支持A5D0命令导致启动时间较长的问题
Browse files Browse the repository at this point in the history
  • Loading branch information
@zhanyiaini
zhanyiaini committed Mar 3, 2022
1 parent 03e0cc7 commit 472a17e
Show file tree
Hide file tree
Showing 3 changed files with 178 additions and 17 deletions.
140 changes: 140 additions & 0 deletions .gitignore
View file
Original file line number Diff line number Diff line change
Expand Up @@ -31,3 +31,143 @@
# IDE
.vscode/*
CMakeLists.txt.*
build/cmake_install.cmake
build/cmake_uninstall.cmake
build/CMakeCache.txt
build/compile_commands.json
build/CPackConfig.cmake
build/CPackSourceConfig.cmake
build/CTestTestfile.cmake
build/etlidar_test
build/FindYDLIDAR_SDK.cmake
build/lidar_c_api_test
build/Makefile
build/tof_test
build/ydlidar_config.h
build/YDLIDAR_SDK.pc
build/ydlidar_sdkConfig.cmake
build/ydlidar_sdkConfigVersion.cmake
build/ydlidar_sdkTargets.cmake
build/ydlidar_test
build/CMakeFiles/cmake.check_cache
build/CMakeFiles/CMakeDirectoryInformation.cmake
build/CMakeFiles/CMakeOutput.log
build/CMakeFiles/CMakeRuleHashes.txt
build/CMakeFiles/feature_tests.bin
build/CMakeFiles/feature_tests.c
build/CMakeFiles/feature_tests.cxx
build/CMakeFiles/Makefile.cmake
build/CMakeFiles/Makefile2
build/CMakeFiles/progress.marks
build/CMakeFiles/TargetDirectories.txt
build/CMakeFiles/3.10.2/CMakeCCompiler.cmake
build/CMakeFiles/3.10.2/CMakeCXXCompiler.cmake
build/CMakeFiles/3.10.2/CMakeDetermineCompilerABI_C.bin
build/CMakeFiles/3.10.2/CMakeDetermineCompilerABI_CXX.bin
build/CMakeFiles/3.10.2/CMakeSystem.cmake
build/CMakeFiles/3.10.2/CompilerIdC/CMakeCCompilerId.c
build/CMakeFiles/3.10.2/CompilerIdCXX/CMakeCXXCompilerId.cpp
build/CMakeFiles/uninstall.dir/build.make
build/CMakeFiles/uninstall.dir/cmake_clean.cmake
build/CMakeFiles/uninstall.dir/DependInfo.cmake
build/CMakeFiles/uninstall.dir/progress.make
build/CMakeFiles/ydlidar_sdk.dir/build.make
build/CMakeFiles/ydlidar_sdk.dir/C.includecache
build/CMakeFiles/ydlidar_sdk.dir/cmake_clean_target.cmake
build/CMakeFiles/ydlidar_sdk.dir/cmake_clean.cmake
build/CMakeFiles/ydlidar_sdk.dir/CXX.includecache
build/CMakeFiles/ydlidar_sdk.dir/depend.internal
build/CMakeFiles/ydlidar_sdk.dir/depend.make
build/CMakeFiles/ydlidar_sdk.dir/DependInfo.cmake
build/CMakeFiles/ydlidar_sdk.dir/flags.make
build/CMakeFiles/ydlidar_sdk.dir/link.txt
build/CMakeFiles/ydlidar_sdk.dir/progress.make
build/core/cmake_install.cmake
build/core/Makefile
build/core/CMakeFiles/CMakeDirectoryInformation.cmake
build/core/CMakeFiles/progress.marks
build/core/base/cmake_install.cmake
build/core/base/Makefile
build/core/base/CMakeFiles/CMakeDirectoryInformation.cmake
build/core/base/CMakeFiles/progress.marks
build/core/common/cmake_install.cmake
build/core/common/Makefile
build/core/common/CMakeFiles/CMakeDirectoryInformation.cmake
build/core/common/CMakeFiles/progress.marks
build/core/math/cmake_install.cmake
build/core/math/Makefile
build/core/math/CMakeFiles/CMakeDirectoryInformation.cmake
build/core/math/CMakeFiles/progress.marks
build/core/network/cmake_install.cmake
build/core/network/Makefile
build/core/network/CMakeFiles/CMakeDirectoryInformation.cmake
build/core/network/CMakeFiles/progress.marks
build/core/serial/cmake_install.cmake
build/core/serial/Makefile
build/core/serial/CMakeFiles/CMakeDirectoryInformation.cmake
build/core/serial/CMakeFiles/progress.marks
build/core/serial/impl/cmake_install.cmake
build/core/serial/impl/Makefile
build/core/serial/impl/CMakeFiles/CMakeDirectoryInformation.cmake
build/core/serial/impl/CMakeFiles/progress.marks
build/core/serial/impl/unix/cmake_install.cmake
build/core/serial/impl/unix/Makefile
build/core/serial/impl/unix/CMakeFiles/CMakeDirectoryInformation.cmake
build/core/serial/impl/unix/CMakeFiles/progress.marks
build/samples/cmake_install.cmake
build/samples/Makefile
build/samples/CMakeFiles/CMakeDirectoryInformation.cmake
build/samples/CMakeFiles/progress.marks
build/samples/CMakeFiles/etlidar_test.dir/build.make
build/samples/CMakeFiles/etlidar_test.dir/cmake_clean.cmake
build/samples/CMakeFiles/etlidar_test.dir/CXX.includecache
build/samples/CMakeFiles/etlidar_test.dir/depend.internal
build/samples/CMakeFiles/etlidar_test.dir/depend.make
build/samples/CMakeFiles/etlidar_test.dir/DependInfo.cmake
build/samples/CMakeFiles/etlidar_test.dir/flags.make
build/samples/CMakeFiles/etlidar_test.dir/link.txt
build/samples/CMakeFiles/etlidar_test.dir/progress.make
build/samples/CMakeFiles/lidar_c_api_test.dir/build.make
build/samples/CMakeFiles/lidar_c_api_test.dir/C.includecache
build/samples/CMakeFiles/lidar_c_api_test.dir/cmake_clean.cmake
build/samples/CMakeFiles/lidar_c_api_test.dir/depend.internal
build/samples/CMakeFiles/lidar_c_api_test.dir/depend.make
build/samples/CMakeFiles/lidar_c_api_test.dir/DependInfo.cmake
build/samples/CMakeFiles/lidar_c_api_test.dir/flags.make
build/samples/CMakeFiles/lidar_c_api_test.dir/link.txt
build/samples/CMakeFiles/lidar_c_api_test.dir/progress.make
build/samples/CMakeFiles/tof_test.dir/build.make
build/samples/CMakeFiles/tof_test.dir/cmake_clean.cmake
build/samples/CMakeFiles/tof_test.dir/CXX.includecache
build/samples/CMakeFiles/tof_test.dir/depend.internal
build/samples/CMakeFiles/tof_test.dir/depend.make
build/samples/CMakeFiles/tof_test.dir/DependInfo.cmake
build/samples/CMakeFiles/tof_test.dir/flags.make
build/samples/CMakeFiles/tof_test.dir/link.txt
build/samples/CMakeFiles/tof_test.dir/progress.make
build/samples/CMakeFiles/ydlidar_test.dir/build.make
build/samples/CMakeFiles/ydlidar_test.dir/cmake_clean.cmake
build/samples/CMakeFiles/ydlidar_test.dir/CXX.includecache
build/samples/CMakeFiles/ydlidar_test.dir/depend.internal
build/samples/CMakeFiles/ydlidar_test.dir/depend.make
build/samples/CMakeFiles/ydlidar_test.dir/DependInfo.cmake
build/samples/CMakeFiles/ydlidar_test.dir/flags.make
build/samples/CMakeFiles/ydlidar_test.dir/link.txt
build/samples/CMakeFiles/ydlidar_test.dir/progress.make
build/src/cmake_install.cmake
build/src/Makefile
build/src/CMakeFiles/CMakeDirectoryInformation.cmake
build/src/CMakeFiles/progress.marks
build/src/filters/cmake_install.cmake
build/src/filters/Makefile
build/src/filters/CMakeFiles/CMakeDirectoryInformation.cmake
build/src/filters/CMakeFiles/progress.marks
YDLidar-SDK.cflags
YDLidar-SDK.config
YDLidar-SDK.creator
YDLidar-SDK.creator.user
.gitignore
YDLidar-SDK.cxxflags
YDLidar-SDK.files
.gitignore
YDLidar-SDK.includes
2 changes: 1 addition & 1 deletion samples/tof_test.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -225,7 +225,7 @@ int main(int argc, char *argv[]) {
b_optvalue = isSingleChannel;
laser.setlidaropt(LidarPropSingleChannel, &b_optvalue, sizeof(bool));
/// intensity
b_optvalue = false;
b_optvalue = true;
laser.setlidaropt(LidarPropIntenstiy, &b_optvalue, sizeof(bool));
/// Motor DTR
laser.setlidaropt(LidarPropSupportMotorDtrCtrl, &b_optvalue, sizeof(bool));
Expand Down
53 changes: 37 additions & 16 deletions src/CYdLidar.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -384,24 +384,29 @@ bool CYdLidar::getlidaropt(int optname, void *optval, int optlen) {
/*-------------------------------------------------------------
initialize
-------------------------------------------------------------*/
bool CYdLidar::initialize() {
if (!checkCOMMs()) {
bool CYdLidar::initialize()
{
uint32_t t = getms();
if (!checkCOMMs())
{
fprintf(stderr,
"[CYdLidar::initialize] Error initializing YDLIDAR check Comms.\n");
fflush(stderr);
return false;
}

if (!checkStatus()) {
if (!checkStatus())
{
fprintf(stderr,
"[CYdLidar::initialize] Error initializing YDLIDAR check status under [%s] and [%d].\n",
m_SerialPort.c_str(), m_SerialBaudrate);
fflush(stderr);
return false;
}

printf("LiDAR init success!\n");
printf("LiDAR init success, Elapsed time %u ms\n", getms() - t);
fflush(stdout);

return true;
}

Expand All @@ -420,6 +425,7 @@ bool CYdLidar::turnOn() {
return true;
}

uint32_t t = getms();
// start scan...
result_t op_result = lidarPtr->startScan();

Expand All @@ -434,6 +440,9 @@ bool CYdLidar::turnOn() {
}
}

printf("[CYdLidar] Successed to start scan mode, Elapsed time %u ms\n", getms() - t);
fflush(stdout);

m_PointTime = lidarPtr->getPointTime();

if (checkLidarAbnormal()) {
Expand Down Expand Up @@ -1211,7 +1220,7 @@ bool CYdLidar::getDeviceInfo() {
}
}

frequencyOffset = 0.4;
frequencyOffset = 0.4;
lidar_model = devinfo.model;
bool intensity = hasIntensity(devinfo.model);
defalutSampleRate = lidarModelDefaultSampleRate(devinfo.model);
Expand Down Expand Up @@ -1242,13 +1251,17 @@ bool CYdLidar::getDeviceInfo() {
devinfo.firmware_version >> 8, devinfo.firmware_version & 0x00ff);
}

// uint32_t t = getms();

if (hasSampleRate(devinfo.model)) {
checkSampleRate();
} else {
m_PointTime = 1e9 / (defalutSampleRate * 1000);
lidarPtr->setPointTime(m_PointTime);
}

// printf("LiDAR get device info finished, Elapsed time %u ms\n", getms() - t);

if (hasScanFrequencyCtrl(devinfo.model) || ((isTOFLidar(m_LidarType)) &&
!m_SingleChannel) || isNetTOFLidar(m_LidarType)) {
checkScanFrequency();
Expand Down Expand Up @@ -1317,15 +1330,22 @@ void CYdLidar::checkSampleRate() {

m_SampleRatebyD1 = _rate.rate;

if (IS_OK(ans)) {
if (IS_OK(ans))
{
_samp_rate = ConvertUserToLidarSmaple(lidar_model, m_SampleRate, _rate.rate);

while (_samp_rate != _rate.rate) {
ans = lidarPtr->setSamplingRate(_rate);
try_count++;
if (!isTOFLidarByModel(lidar_model))
{
//非TOF雷达通过设备信息获取
while (_samp_rate != _rate.rate)
{
ans = lidarPtr->setSamplingRate(_rate);
try_count++;

if (try_count > 3) {
break;
if (try_count > 3)
{
break;
}
}
}

Expand Down Expand Up @@ -1544,17 +1564,18 @@ bool CYdLidar::checkCOMMs() {
/*-------------------------------------------------------------
checkStatus
-------------------------------------------------------------*/
bool CYdLidar::checkStatus() {

if (!checkCOMMs()) {
bool CYdLidar::checkStatus()
{
if (!checkCOMMs())
{
return false;
}

getDeviceHealth();

if (!getDeviceInfo()) {
if (!getDeviceInfo())
{
return false;

}

return true;
Expand Down

0 comments on commit 472a17e

Please sign in to comment.