lyq/sweeper_200
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

fix bug 01

Browse Source
This commit is contained in:
Alvin-lyq 2026-04-23 17:37:31 +08:00
parent f3fc9be86b
commit 75394b1226
1 changed files with 14 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
src/autonomy/pl/src/pl.cpp
View File

@ -301,6 +301,19 @@ void PL_ProcThread()
} }
} }
// 检查 GPS 点数，确保至少有几个点才开始寻迹
if (GpsPointNum < 5)
{
// 如果点数不够，等待文件更新
x = 0.0;
y = 1.0;
pl_speed = 0;
// 不设置 COMPLETED 状态
LOG_INFO_THROTTLE(1000, "等待 GPS 路径点加载完成，当前点数: %d", GpsPointNum);
usleep(10000);
continue;
}
g_NavInfo.Latitude_degree = g_rtk.lat; g_NavInfo.Latitude_degree = g_rtk.lat;
g_NavInfo.Longitude_degree = g_rtk.lon; g_NavInfo.Longitude_degree = g_rtk.lon;
g_NavInfo.Yaw_rad = g_rtk.direction; g_NavInfo.Yaw_rad = g_rtk.direction;
@ -324,6 +337,7 @@ void PL_ProcThread()
current_head); current_head);
LOG_INFO_THROTTLE(1000, "目标位置: %d 经度: %.6f 纬度: %.6f 朝向: %.1f", des_pos, target_lon, target_lat, LOG_INFO_THROTTLE(1000, "目标位置: %d 经度: %.6f 纬度: %.6f 朝向: %.1f", des_pos, target_lon, target_lat,
target_head); target_head);
LOG_INFO_THROTTLE(1000, "总点数: %d，当前点数: %d，目标点: %d", GpsPointNum, road_pos, des_pos);
LOG_INFO_THROTTLE(1000, "############################################################"); LOG_INFO_THROTTLE(1000, "############################################################");
// 计算当前定位点与目标点距离 // 计算当前定位点与目标点距离
@ -331,16 +345,6 @@ void PL_ProcThread()
int nl_within_head = (abs(target_head - current_head) < 30) ? 1 : 0; int nl_within_head = (abs(target_head - current_head) < 30) ? 1 : 0;
nl_within_radius = ((distance <= nl_radius) && nl_within_head) ? 1 : 0; nl_within_radius = ((distance <= nl_radius) && nl_within_head) ? 1 : 0;
//
// cout << " * * * * * * * * * * * * * * * * * * * * *" << endl;
// cout << endl;
// cout << "--- -target_head " << target_head << "--- current_head " << current_head << " " <<
// abs(target_head - current_head) << endl; cout << endl; cout << "--- -distance " << distance << "- -nl_radius
// " << nl_radius << endl;
// cout << endl;
// cout << " * * * * * * * * * * * * * * * * * * * * *" << endl;
if (road_pos >= (GpsPointNum - 1)) if (road_pos >= (GpsPointNum - 1))
{ {
@ -367,8 +371,6 @@ void PL_ProcThread()
task_status = TaskStatus::RUNNING; task_status = TaskStatus::RUNNING;
} }
// printf("x_cm:%lf y_cm:%lf speed:%d\n", x, y, pl_speed);
// 避免忙等待，增加延迟让 CPU 能够处理其他任务 // 避免忙等待，增加延迟让 CPU 能够处理其他任务
auto now = std::chrono::steady_clock::now(); auto now = std::chrono::steady_clock::now();
if (now > next_time) if (now > next_time)