Skip to content

Commit

Permalink
针对Tmini Pro雷达修改调转速
Browse files Browse the repository at this point in the history
  • Loading branch information
@zhanyiaini
zhanyiaini committed Sep 12, 2023
1 parent 1140d74 commit 6afed18
Show file tree
Hide file tree
Showing 6 changed files with 43 additions and 39 deletions.
2 changes: 1 addition & 1 deletion CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,7 +5,7 @@ project(ydlidar_sdk C CXX)
# version
set(YDLIDAR_SDK_VERSION_MAJOR 1)
set(YDLIDAR_SDK_VERSION_MINOR 1)
set(YDLIDAR_SDK_VERSION_PATCH 18)
set(YDLIDAR_SDK_VERSION_PATCH 19)
set(YDLIDAR_SDK_VERSION ${YDLIDAR_SDK_VERSION_MAJOR}.${YDLIDAR_SDK_VERSION_MINOR}.${YDLIDAR_SDK_VERSION_PATCH})

##########################################################
Expand Down
15 changes: 11 additions & 4 deletions core/common/ydlidar_help.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -351,17 +351,17 @@ inline bool hasZeroAngle(int model) {
* @param model lidar model
* @return true if supported, otherwise false.
*/
inline bool hasScanFrequencyCtrl(int model) {
inline bool hasScanFrequencyCtrl(int model)
{
bool ret = true;

if (model == DriverInterface::YDLIDAR_S4 ||
model == DriverInterface::YDLIDAR_S4B ||
model == DriverInterface::YDLIDAR_S2 ||
model == DriverInterface::YDLIDAR_X4 ||
model == DriverInterface::YDLIDAR_GS1 ||
model == DriverInterface::YDLIDAR_GS2 ||
model == DriverInterface::YDLIDAR_Tmini ||
model == DriverInterface::YDLIDAR_TminiPRO) {
model == DriverInterface::YDLIDAR_GS2)
{
ret = false;
}

Expand Down Expand Up @@ -457,6 +457,13 @@ inline bool isSupportScanFrequency(int model, double frequency)
ret = true;
}
}
else if (model == DriverInterface::YDLIDAR_Tmini)
{
if (5 <= frequency && frequency <= 12)
{
ret = true;
}
}
}
else
{
Expand Down
6 changes: 3 additions & 3 deletions samples/tmini_test.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -163,7 +163,7 @@ int main(int argc, char *argv[]) {

std::string input_frequency;

float frequency = 4.0;
float frequency = 10.0;

// while (ydlidar::os_isOk() && !isSingleChannel) {
// printf("Please enter the lidar scan frequency[5-12]:");
Expand Down Expand Up @@ -269,9 +269,9 @@ int main(int argc, char *argv[]) {
{
if (laser.doProcessSimple(scan))
{
printf("Scan received at [%lu] %u points is [%f]Hz\n",
scan.stamp / 1000000,
printf("[%u] points [%.02f(%.02f)]Hz\n",
(unsigned int)scan.points.size(),
scan.scanFreq,
1.0 / scan.config.scan_time);
// for (size_t i = 0; i < scan.points.size(); ++i)
// {
Expand Down
18 changes: 9 additions & 9 deletions src/CYdLidar.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1575,26 +1575,27 @@ bool CYdLidar::checkScanFrequency()

if (isSupportScanFrequency(lidar_model, m_ScanFrequency))
{
m_ScanFrequency += frequencyOffset;
//TODO: 此处为何要加上偏移量，待解释
// m_ScanFrequency += frequencyOffset;
ans = lidarPtr->getScanFrequency(_scan_frequency);

if (IS_OK(ans))
{
frequency = _scan_frequency.frequency / 100.f;
hz = m_ScanFrequency - frequency;

if (hz > 0)
{
//大调速
while (hz > 0.95)
{
lidarPtr->setScanFrequencyAdd(_scan_frequency);
hz = hz - 1.0;
hz -= 1.0;
}

//小调速
while (hz > 0.09)
{
lidarPtr->setScanFrequencyAddMic(_scan_frequency);
hz = hz - 0.1;
hz -= 0.1;
}

frequency = _scan_frequency.frequency / 100.0f;
Expand All @@ -1619,13 +1620,12 @@ bool CYdLidar::checkScanFrequency()
}
else
{
m_ScanFrequency += frequencyOffset;
// m_ScanFrequency += frequencyOffset;
fprintf(stderr, "current scan frequency[%f] is out of range.\n",
m_ScanFrequency - frequencyOffset);
m_ScanFrequency);
}

ans = lidarPtr->getScanFrequency(_scan_frequency);

if (IS_OK(ans))
{
frequency = _scan_frequency.frequency / 100.0f;
Expand All @@ -1639,7 +1639,7 @@ bool CYdLidar::checkScanFrequency()
// m_SampleRate = m_SampleRatebyD1;
// }

m_ScanFrequency -= frequencyOffset;
// m_ScanFrequency -= frequencyOffset;
m_FixedSize = m_SampleRate * 1000 / (m_ScanFrequency - 0.1);
printf("[YDLIDAR] Current scan frequency: %.02fHz\n", m_ScanFrequency);
// printf("[YDLIDAR] Fixed size: %d\n", m_FixedSize);
Expand Down
36 changes: 16 additions & 20 deletions src/SDMLidarDriver.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -275,23 +275,23 @@ result_t SDMLidarDriver::getData(uint8_t *data, size_t size)
return RESULT_OK;
}

result_t SDMLidarDriver::waitRes(
result_t SDMLidarDriver::waitResp(
uint8_t cmd,
uint32_t timeout)
{
std::vector<uint8_t> data;
return waitRes(cmd, data, timeout);
return waitResp(cmd, data, timeout);
}

result_t SDMLidarDriver::waitRes(
result_t SDMLidarDriver::waitResp(
uint8_t cmd,
std::vector<uint8_t> &data,
uint32_t timeout)
{
int pos = 0;
uint32_t st = getms();
uint32_t wt = 0;
std::vector<uint8_t> recvBuff(SDKSDMHEADSIZE, 0);
std::vector<uint8_t> recvBuff(SDK_BUFFER_MAXLEN, 0);
SdkSdmHead head;
uint8_t *buff = reinterpret_cast<uint8_t*>(&head);
uint8_t cs = 0;
Expand Down Expand Up @@ -401,7 +401,6 @@ result_t SDMLidarDriver::checkAutoConnecting()
{
{
ScopedLocker l(_cmd_lock);

if (_serial)
{
if (_serial->isOpen() || m_isConnected)
Expand All @@ -413,27 +412,25 @@ result_t SDMLidarDriver::checkAutoConnecting()
}
}
}
retryCount++;

if (retryCount > 100)
retryCount ++;
if (retryCount > 10)
{
retryCount = 100;
retryCount = 10;
}

delay(100 * retryCount);
int retryConnect = 0;

while (isAutoReconnect &&
connect(serial_port.c_str(), m_baudrate) != RESULT_OK)
connect(serial_port.c_str(), m_baudrate) != RESULT_OK)
{
retryConnect++;

if (retryConnect > 25)
if (retryConnect > 10)
{
retryConnect = 25;
retryConnect = 10;
}

delay(200 * retryConnect);
delay(100);
}

if (!isAutoReconnect)
Expand All @@ -446,7 +443,6 @@ result_t SDMLidarDriver::checkAutoConnecting()
{
delay(100);
{
ScopedLocker l(_cmd_lock);
ans = startAutoScan();
if (!IS_OK(ans))
{
Expand Down Expand Up @@ -757,7 +753,7 @@ result_t SDMLidarDriver::startScan(bool force, uint32_t timeout)
//双通雷达才发送启动命令
if (!m_SingleChannel)
{
ret = waitRes(SDK_CMD_STARTSCAN, timeout);
ret = waitResp(SDK_CMD_STARTSCAN, timeout);
if (!IS_OK(ret))
{
printf("[YDLIDAR] Response to start scan error!\n");
Expand All @@ -784,7 +780,7 @@ result_t SDMLidarDriver::stopScan(uint32_t timeout)
{
return ans;
}
if ((ans = waitRes(SDK_CMD_STOPSCAN, timeout)) != RESULT_OK)
if ((ans = waitResp(SDK_CMD_STOPSCAN, timeout)) != RESULT_OK)
{
return ans;
}
Expand Down Expand Up @@ -833,7 +829,7 @@ result_t SDMLidarDriver::startAutoScan(bool force, uint32_t timeout)
}
if (!m_SingleChannel)
{
if ((ans = waitRes(SDK_CMD_STARTSCAN, timeout)) != RESULT_OK)
if ((ans = waitResp(SDK_CMD_STARTSCAN, timeout)) != RESULT_OK)
{
return ans;
}
Expand Down Expand Up @@ -916,7 +912,7 @@ result_t SDMLidarDriver::setScanFreq(float sf, uint32_t timeout)
return ret;

std::vector<uint8_t> data;
ret = waitRes(SDK_CMD_SETFREQ, data, timeout);
ret = waitResp(SDK_CMD_SETFREQ, data, timeout);
if (!IS_OK(ret))
return ret;
if (!data.size())
Expand Down Expand Up @@ -968,7 +964,7 @@ result_t SDMLidarDriver::getDeviceInfo(device_info &info, uint32_t timeout)
return ret;

std::vector<uint8_t> data;
if ((ret = waitRes(SDK_CMD_GETVERSION, data, timeout)) != RESULT_OK)
if ((ret = waitResp(SDK_CMD_GETVERSION, data, timeout)) != RESULT_OK)
return ret;

// printf("%s %llu\n", __FUNCTION__, data.size());
Expand Down
5 changes: 3 additions & 2 deletions src/SDMLidarDriver.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -76,6 +76,7 @@
#define SDK_CMD_SETBAUDRATE 0x66 //设置串口波特率
#define SDK_CMD_SETOUTPUT 0x67 //设置输出的数据格式
#define SDK_CMD_RESET 0x68 //恢复出厂设置
#define SDK_BUFFER_MAXLEN 100 //缓存长度

//设置1字节对齐
#pragma pack(1)
Expand Down Expand Up @@ -339,9 +340,9 @@ class SDMLidarDriver : public DriverInterface
* @retval RESULT_FAILE 获取失败
* @note 当timeout = -1 时, 将一直等待
*/
result_t waitRes(uint8_t cmd,
result_t waitResp(uint8_t cmd,
uint32_t timeout = DEFAULT_TIMEOUT);
result_t waitRes(uint8_t cmd,
result_t waitResp(uint8_t cmd,
std::vector<uint8_t> &data,
uint32_t timeout = DEFAULT_TIMEOUT);
/*!
Expand Down

0 comments on commit 6afed18

Please sign in to comment.