diff --git a/src/autonomy/pl/src/pl.cpp b/src/autonomy/pl/src/pl.cpp index 3f2a7ac..16eb381 100644 --- a/src/autonomy/pl/src/pl.cpp +++ b/src/autonomy/pl/src/pl.cpp @@ -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.Longitude_degree = g_rtk.lon; g_NavInfo.Yaw_rad = g_rtk.direction; @@ -324,6 +337,7 @@ void PL_ProcThread() current_head); LOG_INFO_THROTTLE(1000, "目标位置: %d 经度: %.6f 纬度: %.6f 朝向: %.1f", des_pos, target_lon, target_lat, target_head); + LOG_INFO_THROTTLE(1000, "总点数: %d,当前点数: %d,目标点: %d", GpsPointNum, road_pos, des_pos); LOG_INFO_THROTTLE(1000, "############################################################"); // 计算当前定位点与目标点距离 @@ -331,16 +345,6 @@ void PL_ProcThread() int nl_within_head = (abs(target_head - current_head) < 30) ? 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)) { @@ -367,8 +371,6 @@ void PL_ProcThread() task_status = TaskStatus::RUNNING; } - // printf("x_cm:%lf y_cm:%lf speed:%d\n", x, y, pl_speed); - // 避免忙等待,增加延迟让 CPU 能够处理其他任务 auto now = std::chrono::steady_clock::now(); if (now > next_time)