From d05c25aa4823e9d2aa812e9c0e3d1d66bb9861dd Mon Sep 17 00:00:00 2001
From: fctom1215 <fctom1215@outlook.com>
Date: 星期六, 29 八月 2020 10:47:17 +0800
Subject: [PATCH] 坐标
---
lib/src/main/cpp/test_items2/drive_straight.cpp | 33 +++++++++++++++++++++++++++------
1 files changed, 27 insertions(+), 6 deletions(-)
diff --git a/lib/src/main/cpp/test_items2/drive_straight.cpp b/lib/src/main/cpp/test_items2/drive_straight.cpp
index d6a9db2..8b7d1cf 100644
--- a/lib/src/main/cpp/test_items2/drive_straight.cpp
+++ b/lib/src/main/cpp/test_items2/drive_straight.cpp
@@ -20,6 +20,7 @@
static int setup;
static double beginOdo;
static int yaw_stat;
+static double offsetBase;
static void TtsBack(int seq)
{
@@ -33,9 +34,13 @@
PlayTTS("璇蜂繚鎸佺洿绾胯椹�", TtsBack);
}
-bool TestDriveStraight(const car_model *car, const struct RtkTime *rtkTime) {
+bool TestDriveStraight(road_exam_map &RoadMap, int roadIndex, const car_model *car, const struct RtkTime *rtkTime) {
static double yaw;
static Line baseLine;
+ static double offset1, offset2;
+
+ if (roadIndex < 0 || roadIndex >= RoadMap.roads.size())
+ return false;
if (setup == 1) {
// 鍋忚埅瑙掑钩鍧囧��
@@ -47,7 +52,7 @@
yaws.push_back(car->yaw);
yaw_stat++;
- if (yaw_stat == 5) {
+ if (yaw_stat == 15) {
yaw = AvgYaw(yaws);
vector<double>().swap(yaws);
@@ -55,13 +60,28 @@
PointF extPoint = PointExtend(car->basePoint, 100, yaw);
MakeLine(&baseLine, &car->basePoint, &extPoint);
beginOdo = ReadOdo();
+
+ PointF px = CalcProjectionWithRoadEdge(RoadMap.roads[roadIndex].rightEdge, car->carXY[car->axial[AXIAL_FRONT]]);
+ offsetBase = DistanceOf(px, car->carXY[car->axial[AXIAL_FRONT]]);
+ DEBUG("璧风偣鍋忕 %f", offsetBase);
+
setup = 2;
}
} else if (setup == 2) {
- double offset = DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], baseLine);
+ offset1 = DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], baseLine);
- if (offset > MAX_OFFSET_DISTANCE) {
- DEBUG("鐩寸嚎鍋忕Щ澶т簬30鍘樼背 offset = %f", offset);
+ if (offset1 > MAX_OFFSET_DISTANCE) {
+ DEBUG("铏氭嫙鐩寸嚎鍋忕Щ澶т簬30鍘樼背 offset1 = %f", offset1);
+// // 鍋忕Щ澶т簬30鍘樼背锛屼笉鍚堟牸
+// AddExamFault(30, rtkTime);
+// return false;
+ }
+
+ PointF px = CalcProjectionWithRoadEdge(RoadMap.roads[roadIndex].rightEdge, car->carXY[car->axial[AXIAL_FRONT]]);
+ offset2 = DistanceOf(px, car->carXY[car->axial[AXIAL_FRONT]]);
+
+ if (fabs(offset2 - offsetBase) > MAX_OFFSET_DISTANCE) {
+ DEBUG("鐩寸嚎鍋忕Щ澶т簬30鍘樼背 offset2 = %f", fabs(offset2 - offsetBase));
// 鍋忕Щ澶т簬30鍘樼背锛屼笉鍚堟牸
AddExamFault(30, rtkTime);
return false;
@@ -69,7 +89,8 @@
}
if (setup == 2 && ReadOdo() - beginOdo > CHECK_STAGE_DISTANCE) {
- DEBUG("鐩寸嚎琛岄┒缁撴潫");
+ DEBUG("鐩寸嚎琛岄┒缁撴潫 offset1 = %f offset2 = %f", offset1, fabs(offset2 - offsetBase));
+
PlayTTS("鐩寸嚎琛岄┒缁撴潫", NULL);
return false;
}
--
Gitblit v1.8.0