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