From 76859aa4b23ea8ebd90bd7553fd70e144bdc96ba Mon Sep 17 00:00:00 2001
From: yy1717 <fctom1215@outlook.com>
Date: 星期五, 15 五月 2020 15:53:20 +0800
Subject: [PATCH] 坐标
---
lib/src/main/cpp/test_items2/drive_straight.cpp | 137 +++++++++++++++++++++++++++------------------
1 files changed, 83 insertions(+), 54 deletions(-)
diff --git a/lib/src/main/cpp/test_items2/drive_straight.cpp b/lib/src/main/cpp/test_items2/drive_straight.cpp
index 5e2b12f..3b9f8d3 100644
--- a/lib/src/main/cpp/test_items2/drive_straight.cpp
+++ b/lib/src/main/cpp/test_items2/drive_straight.cpp
@@ -15,88 +15,117 @@
static const double CHECK_STAGE_DISTANCE = 100.0;
static const double MAX_OFFSET_DISTANCE = 0.3;
+static int ttsPlayEnd;
static bool crossStartLine;
static bool reportOffsetOver;
static double edgeDistance;
+static double distanceToStartSum;
-void StartDriveStraightExam(int index, LIST_ROAD_MAP &RoadMapList) {
- if (index == -1)
- return;
+static double CalcDistance2Edge(road_t &road, const car_model *car);
- DEBUG("杩涘叆璺�冪洿绾胯椹跺湴鍥� index = %d id = %d item = %d", index, RoadMapList[index].id, RoadMapList[index].type);
+void StartDriveStraightExam(std::string tts) {
+ DEBUG("寮�濮嬬洿绾胯椹�");
- if (!RoadMapList[index].tts.empty()) {
- DEBUG("鎾斁TTS");
- PlayTTS(RoadMapList[index].tts.c_str());
+ if (!tts.empty()) {
+ PlayTTS(tts.c_str());
} else {
DEBUG("娌℃湁TTS");
}
- crossStartLine = false;
+ distanceToStartSum = 0;
reportOffsetOver = false;
}
-int ExecuteDriveStraightExam(int index, LIST_ROAD_MAP &RoadMapList, const car_model *car,
+int ExecuteDriveStraightExam(road_t &road, const car_model *car,
LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime) {
- Line road_edge;
-
static PointF startPoint;
- MakeLine(&road_edge, &RoadMapList[index].roadEdgeLine[0].point[0], &RoadMapList[index].roadEdgeLine[0].point[1]);
+ double dis2roadEdge = 0;
- {
- double l1 = DistanceOf(car->carXY[car->right_front_tire[TIRE_OUTSIDE]], road_edge);
+ if (ttsPlayEnd == 1) {
+ ttsPlayEnd = 2;
+ startPoint = car->basePoint;
+ edgeDistance = CalcDistance2Edge(road, car); // 鍩哄噯杈硅窛
- double l2 = DistanceOf(car->carXY[car->right_rear_tire[TIRE_OUTSIDE]], road_edge);
-
- MA_SendDistance(6 - (l1+l2)/2.0, (l1+l2)/2.0);
+ DEBUG("褰撳墠鍩哄噯璺竟闂磋窛 %f", edgeDistance);
}
- if (!crossStartLine) {
- PointF p1, p2;
+ if (ttsPlayEnd != 2)
+ return 1;
- p1.X = RoadMapList[index].stopLine.X1;
- p1.Y = RoadMapList[index].stopLine.Y1;
- p2.X = RoadMapList[index].stopLine.X2;
- p2.Y = RoadMapList[index].stopLine.Y2;
+ double distanceToStart = DistanceOf(car->basePoint, startPoint);
+ dis2roadEdge = CalcDistance2Edge(road, car);
- if (IntersectionOfLine(p1, p2, car->carXY[car->left_front_tire[TIRE_OUTSIDE]]) == -1) {
- DEBUG("璺ㄨ繃鏍囪绾匡紝寮�濮嬬洿绾挎祴璇�");
- crossStartLine = true;
- startPoint = car->basePoint;
- edgeDistance = DistanceOf(car->basePoint, road_edge);
+ DEBUG("璺竟闂磋窛 %f --- %f", dis2roadEdge, edgeDistance);
- DEBUG("褰撳墠鍩哄噯璺竟闂磋窛 %f", edgeDistance);
- }
- } else {
- double distanceToStart = DistanceOf(car->basePoint, startPoint);
+ if (!reportOffsetOver && fabs(dis2roadEdge - edgeDistance) > MAX_OFFSET_DISTANCE) {
+ DEBUG("鐩寸嚎鍋忕Щ澶т簬30鍘樼背");
+ // 鍋忕Щ澶т簬30鍘樼背锛屼笉鍚堟牸
+ AddExamFault(30, rtkTime);
+ reportOffsetOver = true;
- DEBUG("璺竟闂磋窛 %f --- %f", DistanceOf(car->basePoint, road_edge), edgeDistance);
-
- if (!reportOffsetOver && fabs(DistanceOf(car->basePoint, road_edge) - edgeDistance) > MAX_OFFSET_DISTANCE) {
- DEBUG("鐩寸嚎鍋忕Щ澶т簬30鍘樼背");
- // 鍋忕Щ澶т簬30鍘樼背锛屼笉鍚堟牸
- AddExamFault(30, rtkTime);
- reportOffsetOver = true;
-
- //////////////////////////////////////////////
- startPoint = car->basePoint;
- edgeDistance = DistanceOf(car->basePoint, road_edge);
- reportOffsetOver = false;
- }
-
- if (distanceToStart > CHECK_STAGE_DISTANCE) {
- DEBUG("澶嶄綅杈硅窛鍋忕Щ閲�");
- startPoint = car->basePoint;
- edgeDistance = DistanceOf(car->basePoint, road_edge);
- reportOffsetOver = false;
- }
+ //////////////////////////////////////////////
+ startPoint = car->basePoint;
+ edgeDistance = dis2roadEdge;
+ reportOffsetOver = false;
}
- if (ExitSonArea(index, RoadMapList, car)) {
+ if (distanceToStart > CHECK_STAGE_DISTANCE) {
+ DEBUG("澶嶄綅杈硅窛鍋忕Щ閲�");
+ startPoint = car->basePoint;
+ edgeDistance = dis2roadEdge;
+ reportOffsetOver = false;
+ distanceToStartSum += distanceToStart;
+ distanceToStart = 0;
+ }
+
+ if (distanceToStart + distanceToStartSum > 150) {
DEBUG("绂诲紑鐩寸嚎琛岄┒鍖哄煙");
return -1;
}
+ return 1;
+}
- return index;
+static double CalcDistance2Edge(road_t &road, const car_model *car)
+{
+ PointF vp;
+ bool get_vp = false;
+ double distance = 0;
+
+ // 妫�娴嬮亾璺竟缂樼嚎
+ for (int i = 0; i < road.leftEdge.size(); ++i) {
+ PointF p1, p2;
+ Line edge;
+
+ p1 = road.leftEdge[i].points[0];
+ for (int j = 1; j < road.leftEdge[i].points.size(); ++j) {
+ p2 = road.leftEdge[i].points[j];
+ MakeLine(&edge, &p1, &p2);
+
+ if (VerticalPointOnLine(car->basePoint, edge, vp)) {
+ get_vp = true;
+ goto FIND_VP_END;
+ }
+
+ p1 = p2;
+ }
+ }
+
+FIND_VP_END:
+ if (get_vp) {
+ distance = DistanceOf(car->basePoint, vp);
+ } else {
+ // 娌℃湁鎵惧埌鍖归厤绾跨锛屾寜鏈�灏忚窛绂婚《鐐硅绠�
+ distance = 100;
+ for (int i = 0; i < road.leftEdge.size(); ++i) {
+ for (int j = 0; j < road.leftEdge[i].points.size(); ++j) {
+ double x;
+ if (distance > (x = DistanceOf(car->basePoint, road.leftEdge[i].points[j]))) {
+ distance = x;
+ }
+ }
+ }
+ }
+
+ return distance;
}
--
Gitblit v1.8.0