From 6e0f29b08a040d14576d7053c1206a8439936570 Mon Sep 17 00:00:00 2001
From: yy1717 <fctom1215@outlook.com>
Date: 星期二, 24 十一月 2020 14:18:03 +0800
Subject: [PATCH] 坐标

---
 lib/src/main/cpp/test_items2/road_exam.cpp |  165 +++++++++++++++++++++++++++++++++++++++++++++++++++---
 1 files changed, 155 insertions(+), 10 deletions(-)

diff --git a/lib/src/main/cpp/test_items2/road_exam.cpp b/lib/src/main/cpp/test_items2/road_exam.cpp
index f3164f5..22102d0 100644
--- a/lib/src/main/cpp/test_items2/road_exam.cpp
+++ b/lib/src/main/cpp/test_items2/road_exam.cpp
@@ -106,6 +106,12 @@
 static map<int, int> CrossingHint;
 static map<int, bool> ErrorLaneReport;
 
+typedef struct {
+    int road_id;
+    double distance;
+} trigger_detect_t;
+static map<int, trigger_detect_t> TriggerDetect;           // 璺濈鏈矾娈靛悇瑙﹀彂鐐圭殑璺濈
+
 #define ROAD_EXAM_READY_NEXT            0
 #define ROAD_EXAM_FREE_RUN              1
 #define ROAD_EXAM_ITEM_CAR_START            2
@@ -134,7 +140,7 @@
 static int prevTurnWise;
 
 static void ItemExam(road_exam_map &RoadMap, int roadIndex, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime, double straight, double road_end);
-static void ItemExam2(road_exam_map &RoadMap, int roadIndex, const car_model *car, LIST_CAR_MODEL &CarModelList);
+static void ItemExam2(road_exam_map &RoadMap, int roadIndex, const car_model *car, LIST_CAR_MODEL &CarModelList, int forward, int moveDirect);
 static int isTurn(int currYaw, int prevYaw, int thres);
 static void ResetTurnDetect(const car_model *car);
 static void DetectTurn(const car_model *car, int moveDirect, const struct RtkTime *rtkTime);
@@ -142,7 +148,7 @@
 
 static int NearbyCrossingGuide(road_exam_map &RoadMap, int &stopLineIndex, int roadIndex, road_t &road, const car_model *car);
 
-static int EntryItem(int index, road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList);
+static int EntryItem(int index, road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList, int forward, int moveDirect);
 
 static int CalcRoadIndex(int currRoadIndex, road_exam_map &RoadMap, const car_model *car);
 
@@ -187,6 +193,8 @@
     RoadExamItem[ROAD_EXAM_ITEM_CAR_STOP] = ROAD_EXAM_ITEM_NOT_EXEC;
 
     RoadExamStatus = ROAD_EXAM_READY_NEXT;
+
+    TriggerDetect.clear();
 
     win = false;
 }
@@ -1276,7 +1284,7 @@
 void TestRoadGeneral(road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime)
 {
     double BigStraightRoadFree = 0, RoadCrossingFree = 0, TargetFree = 0;
-
+    int forward = 0;                    // 杞﹁締鐩稿褰撳墠閬撹矾鐨�
     UpdateCarSensor(rtkTime);
 
     UpdataOdo(speed, moveDirect, rtkTime);
@@ -1436,7 +1444,7 @@
             prevMoveDirect = moveDirect;
         } else if (moveDirect == 0) {
             // 鎸佺画鍋滆溅
-            if (TimeGetDiff(rtkTime, &stopTimepoint) >= examParam.road_pause_criteria && !StopCarOnRedArea &&
+            if (TimeGetDiff(rtkTime, &stopTimepoint) >= CorrectPauseCriteria(examParam.road_pause_criteria) && !StopCarOnRedArea &&
                 StopOnRedArea(RoadMap, car)) {
                 // 鍋滆溅瓒�2绉掞紝鍋滃湪绾㈠尯锛屼笉鍚堟牸
                 AddExamFault(16, rtkTime);
@@ -1491,6 +1499,34 @@
         car_sensor_value_t brk = ReadCarSensorValue(BREAK);
         // 妫�娴嬮�氳繃璺彛銆佷汉琛岄亾绛夊尯鍩熸椂锛岄噴鏀惧埞杞︽垨鍑忛��
         TargetFree = ApproachTarget(RoadMap, car, currExamMapIndex, (brk.value == BREAK_ACTIVE), speed, moveDirect, rtkTime);
+        // 妫�鏌�
+        projection_t projection;
+
+        uint32_t tm1 = AppTimer_GetTickCount();
+
+        projection = CalcProjectionWithRoadEdgeEx(RoadMap.roads[currExamMapIndex].rightEdge, car->carXY[car->axial[AXIAL_FRONT]]);
+
+        if (projection.edgeIndex < RoadMap.roads[currExamMapIndex].rightEdge.size()) {
+            if (projection.pointIndex < RoadMap.roads[currExamMapIndex].rightEdge[projection.edgeIndex].points.size() - 1) {
+                double roadYaw = YawOf(RoadMap.roads[currExamMapIndex].rightEdge[projection.edgeIndex].points[projection.pointIndex + 1],
+                      RoadMap.roads[currExamMapIndex].rightEdge[projection.edgeIndex].points[projection.pointIndex]);
+
+                int deltaAng;
+
+                if (ABS((int)roadYaw - (int)car->yaw) > 180) {
+                    deltaAng = 360 - ABS((int)roadYaw - (int)car->yaw);
+                } else {
+                    deltaAng = ABS((int)roadYaw - (int)car->yaw);
+                }
+
+                if (deltaAng < 90) {
+                    forward = 1;
+                } else {
+                    forward = -1;
+                }
+//                DEBUG("璺鏂瑰悜 %f 杞﹁締鏂瑰悜 %f 宸�� %d 鑰楁椂 %ld", roadYaw, car->yaw, deltaAng, AppTimer_GetTickCount() - tm1);
+            }
+        }
     }
     ExitTarget(RoadMap, car, CarModelList, rtkTime);
 
@@ -1554,7 +1590,7 @@
         ItemExam(RoadMap, currExamMapIndex, car, CarModelList, speed, moveDirect, rtkTime,
                  BigStraightRoadFree,
                  TargetFree > RoadCrossingFree ? RoadCrossingFree : TargetFree);
-        ItemExam2(RoadMap, currExamMapIndex, car, CarModelList);
+        ItemExam2(RoadMap, currExamMapIndex, car, CarModelList, forward, moveDirect);
     }
 }
 
@@ -1621,6 +1657,7 @@
         }
     } else if (RoadExamStatus == ROAD_EXAM_FREE_RUN) {
         if (ReadOdo() - freeRunDistance > freeRunExceptDistance) {
+            DEBUG("瀵绘壘涓嬩竴涓瓙椤圭洰");
             RoadExamStatus = ROAD_EXAM_READY_NEXT;
         }
     } else {
@@ -1649,6 +1686,7 @@
         }
 
         if (!testing) {
+            DEBUG("褰撳墠瀛愰」缁撴潫");
             RoadExamItem[RoadExamStatus] = ROAD_EXAM_ITEM_EXECED;
             if (RoadExamStatus == ROAD_EXAM_ITEM_CAR_START) {
                 freeRunExceptDistance = 60.0;
@@ -1665,9 +1703,13 @@
     }
 }
 
-static void ItemExam2(road_exam_map &RoadMap, int roadIndex, const car_model *car, LIST_CAR_MODEL &CarModelList)
+static void ItemExam2(road_exam_map &RoadMap, int roadIndex, const car_model *car, LIST_CAR_MODEL &CarModelList, int forward, int moveDirect)
 {
-    int item = EntryItem(roadIndex, RoadMap, car, CarModelList);
+    int item = 0;
+
+    if (RoadExamStatus == ROAD_EXAM_READY_NEXT) {
+        item = EntryItem(roadIndex, RoadMap, car, CarModelList, forward, moveDirect);
+    }
 
     if (RoadExamStatus == ROAD_EXAM_READY_NEXT && item == 3) {
         StartDriveStraightExam();
@@ -1919,13 +1961,35 @@
     return false;
 }
 
-static int EntryItem(int index, road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList)
+static int EntryItem(int index, road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList, int forward, int moveDirect)
 {
-    if (index < 0 || index >= RoadMap.roads.size() || RoadMap.examScheme.size() == 0)
+    if (index < 0 || index >= RoadMap.roads.size() || RoadMap.examScheme.size() == 0 || CarModelList.size() < 2)
         return -1;
+
+    list<car_model *>::iterator iter = CarModelList.begin();
+
+    Line trace;
+    PointF p1, p2;
+
+    p1 = ((car_model *)(*iter))->carXY[((car_model *)(*iter))->axial[AXIAL_FRONT]];
+    ++iter;
+    p2 = ((car_model *)(*iter))->carXY[((car_model *)(*iter))->axial[AXIAL_FRONT]];
+    MakeLine(&trace, &p1, &p2);
+
+    double trace_length = DistanceOf(p1, p2);
+
+    if (TriggerDetect.size() > 0) {
+        auto it = TriggerDetect.begin();
+
+        if (it->second.road_id != RoadMap.roads[index].id) {
+            TriggerDetect.clear();
+        }
+    }
 
     for (int j = 0; j < RoadMap.examScheme[0].triggerLines.size(); ++j) {
         if (RoadMap.examScheme[0].triggerLines[j].road == RoadMap.roads[index].id) {
+
+
             /*Line triggerLine;
 
             PointF p2 = CalcProjectionWithRoadEdge(RoadMap.roads[index].leftEdge, RoadMap.triggerLines[i].points[0]);
@@ -1937,7 +2001,7 @@
                 return RoadMap.triggerLines[i].active;
             }*/
 
-            vector<double> vec;
+           /* vector<double> vec;
 
             PointF p1 = CalcProjectionWithRoadEdge(RoadMap.roads[index].rightEdge,
                                                    car->carXY[car->axial[AXIAL_FRONT]]);
@@ -1954,6 +2018,87 @@
             if (fabs(vec[0] + vec[1] - vec[2]) < 0.1) {
                 DEBUG("瑙﹀彂椤圭洰 %d", RoadMap.examScheme[0].triggerLines[j].active);
                 return RoadMap.examScheme[0].triggerLines[j].active;
+            }*/
+
+//            double angleOf = DeltaYaw(YawOf(RoadMap.examScheme[0].triggerLines[j].points[0], car->carXY[car->axial[AXIAL_FRONT]]), car->yaw);
+//            auto it = TriggerDetect.find(j);
+//
+//            if (it != TriggerDetect.end()) {
+//                if (fabs(angleOf - it->second.angleOf) > 2.0) {
+//                    bool trigger = false;
+//
+//                    trigger_detect_t up;
+//
+//                    if (moveDirect == 1 && forward == 1 && it->second.angleOf <= 90 && angleOf > 90) {
+//                        trigger = true;
+//                    }
+//                    DEBUG("瑙傚療瀛�<%d> 鏇存柊 %f -> %f", j, it->second.angleOf, angleOf);
+//
+//                    up.angleOf = angleOf;
+//                    up.road_id = RoadMap.roads[index].id;
+//
+//                    TriggerDetect[j] = up;
+//
+//
+//
+//                    if (trigger) {
+//                        DEBUG("pos %d 瑙﹀彂椤圭洰 %d", j, RoadMap.examScheme[0].triggerLines[j].active);
+//                        return RoadMap.examScheme[0].triggerLines[j].active;
+//                    }
+//                }
+//            } else {
+//                trigger_detect_t up;
+//
+//                up.road_id = RoadMap.roads[index].id;
+//                up.angleOf = angleOf;
+//
+//                TriggerDetect.insert(pair<int, trigger_detect_t>(j, up));
+//
+//                DEBUG("瑙傚療瀛�<%d> 鍔犲叆 %f", j, angleOf);
+//            }
+
+//            double dist = DistanceOf(car->carXY[car->axial[AXIAL_FRONT]],
+//                       RoadMap.examScheme[0].triggerLines[j].points[0]);
+            auto it = TriggerDetect.find(j);
+
+            if (it != TriggerDetect.end()) {
+                PointF verticalPoint;
+
+                if (moveDirect == 1 &&
+                    forward == 1 &&
+                    trace_length > 0.01 &&
+                    VerticalPointOnLine(RoadMap.examScheme[0].triggerLines[j].points[0], trace, verticalPoint) &&
+                    DistanceOf(verticalPoint, RoadMap.examScheme[0].triggerLines[j].points[0]) < it->second.distance) {
+                    DEBUG("pos %d 瑙﹀彂椤圭洰 %d", j, RoadMap.examScheme[0].triggerLines[j].active);
+
+                    DEBUG("(%f, %f) ON (%f, %f)-(%f, %f) IS (%f, %f) trace %f",
+                          RoadMap.examScheme[0].triggerLines[j].points[0].X,
+                          RoadMap.examScheme[0].triggerLines[j].points[0].Y,
+                          trace.X1,
+                          trace.Y1,
+                          trace.X2,
+                          trace.Y2,
+                          verticalPoint.X,
+                          verticalPoint.Y,
+                          trace_length);
+
+                    return RoadMap.examScheme[0].triggerLines[j].active;
+                }
+            } else {
+                PointF p1 = CalcProjectionWithRoadEdge(RoadMap.roads[index].leftEdge, RoadMap.examScheme[0].triggerLines[j].points[0]);
+                PointF p2 = CalcProjectionWithRoadEdge(RoadMap.roads[index].rightEdge, RoadMap.examScheme[0].triggerLines[j].points[0]);
+
+                double dist1 = DistanceOf(p1, RoadMap.examScheme[0].triggerLines[j].points[0]);
+                double dist2 = DistanceOf(p2, RoadMap.examScheme[0].triggerLines[j].points[0]);
+
+                trigger_detect_t up;
+
+                up.road_id = RoadMap.roads[index].id;
+                up.distance = MAX(dist1, dist2);
+
+                TriggerDetect.insert(pair<int, trigger_detect_t>(j, up));
+
+                DEBUG("瑙傚療瀛�<%d> 鍔犲叆 %f", j, MAX(dist1, dist2));
             }
         }
     }

--
Gitblit v1.8.0