From 21834035950feaadac9d4c7cad82b90ee624c984 Mon Sep 17 00:00:00 2001
From: yy1717 <fctom1215@outlook.com>
Date: 星期一, 17 八月 2020 19:30:06 +0800
Subject: [PATCH] 传感器变更

---
 lib/src/main/cpp/test_items2/road_exam.cpp |  226 +++++++++++++++++++++++++++++++++++++++++++++++++++-----
 1 files changed, 204 insertions(+), 22 deletions(-)

diff --git a/lib/src/main/cpp/test_items2/road_exam.cpp b/lib/src/main/cpp/test_items2/road_exam.cpp
index 5196886..a1e3357 100644
--- a/lib/src/main/cpp/test_items2/road_exam.cpp
+++ b/lib/src/main/cpp/test_items2/road_exam.cpp
@@ -74,7 +74,7 @@
 
 static const int INVALID_ROAD = -1;
 
-static const int TURN_THRESHOLD = 1;
+static const int TURN_THRESHOLD = 3;
 static const int TURN_CHECK_INTERVAL = 500;
 const double SLIDE_DISTANCE_THRESHOLD_RED = 0.3;
 const double SLIDE_DISTANCE_THRESHOLD_YELLOW = 0.1;
@@ -94,7 +94,7 @@
 static char carIntersectionOfGreenLine;
 static int currTurnSignalStatus;
 static int turnSignalStatusWhenCrashGreenLine;
-static bool reportTurnSignalError;
+
 static int prevMoveDirect;
 static uint32_t stopTimepoint = 0;
 static bool reportStopCarOnRedArea;
@@ -135,6 +135,12 @@
 
 static map<int, car_sensor_value_t> CarSensorValue;
 static map<int, int> CrossingHint;
+static map<int, bool> ErrorLaneReport;
+
+static struct RtkTime beginTurnTime, prevDetectTurnTime;
+static int startTurnYaw, prevYaw;
+static int turnCnt, turnTimeCnt;
+static int prevTurnWise;
 
 static const int MAX_ENGINE_RPM = 2500;
 static const double START_CAR_MOVE_DISTANCE = 10.0;
@@ -157,8 +163,9 @@
 
 static int TestRoadStartCar(const car_model *car, double speed, int moveDirect, const struct RtkTime *rtkTime);
 
-static char isTurn(int currYaw, int prevYaw, int &ang);
-static char CheckCarTurn(LIST_CAR_MODEL &CarModelList);
+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);
 
 
 static trigger_line_t * EntryItem(int index, road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList);
@@ -179,7 +186,6 @@
 
     carIntersectionOfGreenLine = 0;
 
-    reportTurnSignalError = false;
     currTurnSignalStatus = OFF_LIGHT;
     prevMoveDirect = 0;
 
@@ -211,6 +217,7 @@
     checkTurn = false;
 
     CrashLineType = -1;
+    turnCnt = -1;
 
     ResetOdo();
     ResetTarget(RoadMap);
@@ -973,7 +980,6 @@
                     } else if (TimeGetDiff(&crashGreenStartTime, &lamp.time) >= D_SEC(3)) {
                         DEBUG("杞悜鐏椂闂翠笉瓒�");
                         // 涓嶈冻3绉掞紝涓嶅悎鏍�
-                        reportTurnSignalError = true;
                         AddExamFault(14, rtkTime);
                     }
                 } else {
@@ -984,7 +990,6 @@
                     } else if (TimeGetDiff(&crashGreenStartTime, &lamp.time) >= D_SEC(3)) {
                         DEBUG("杞悜鐏椂闂翠笉瓒�");
                         // 涓嶈冻3绉掞紝涓嶅悎鏍�
-                        reportTurnSignalError = true;
                         AddExamFault(14, rtkTime);
                     }
                 }
@@ -1027,6 +1032,62 @@
     return CROSSING_NOT_HINT;
 }
 
+static void ResetCrossingStatus(int roadIndex)
+{
+    while (true) {
+        bool w = false;
+        for (auto it = CrossingHint.begin(); it != CrossingHint.end(); ++it) {
+            if (it->first / 100 == roadIndex && it->second != CROSSING_NOT_HINT) {
+                ChangeCrossingStatus(it->first / 100, it->first % 100, CROSSING_NOT_HINT);
+                w = true;
+                break;
+            }
+        }
+        if (!w)
+            break;
+    }
+}
+
+static void SetErrorLaneRpt(int roadIndex, int index, bool status)
+{
+    int key = roadIndex * 100 + index;
+
+    auto it = ErrorLaneReport.find(key);
+
+    if (it != ErrorLaneReport.end()) {
+        ErrorLaneReport.erase(it);
+    }
+    ErrorLaneReport.insert(pair<int, bool>(key, status));
+}
+
+static bool GetErrorLaneRpt(int roadIndex, int index)
+{
+    int key = roadIndex * 100 + index;
+
+    auto it = ErrorLaneReport.find(key);
+
+    if (it != ErrorLaneReport.end()) {
+        return it->second;
+    }
+    return false;
+}
+
+static void ResetErrorLaneRpt(int roadIndex)
+{
+    while (true) {
+        bool w = false;
+        for (auto it = ErrorLaneReport.begin(); it != ErrorLaneReport.end(); ++it) {
+            if (it->first / 100 == roadIndex && it->second == true) {
+                SetErrorLaneRpt(it->first / 100, it->first % 100, false);
+                w = true;
+                break;
+            }
+        }
+        if (!w)
+            break;
+    }
+}
+
 static void HintCrossing(int roadIndex, road_t &road, const car_model *car, LIST_CAR_MODEL &CarModelList)
 {
     for (int i = 0; i < road.stopLine.size(); ++i) {
@@ -1053,12 +1114,13 @@
     }
 }
 
-static int NearbyCrossingGuide(int roadIndex, road_t &road, const car_model *car, LIST_CAR_MODEL &CarModelList)
+static int NearbyCrossingGuide(int &stopLineIndex, int roadIndex, road_t &road, const car_model *car, LIST_CAR_MODEL &CarModelList)
 {
-    int guide = 0;
+    int guide = 0, stopLine = 0;
     double distance;
 
     struct nearby_crossing {
+        int stopLine;
         int guide;
         double distance;
     };
@@ -1077,6 +1139,7 @@
         if (distance > 1e-3) {
             struct nearby_crossing temp;
 
+            temp.stopLine = i;
             temp.guide = road.stopLine[i].active;
             temp.distance = distance;
 
@@ -1092,10 +1155,12 @@
             if (distance > set[i].distance) {
                 distance = set[i].distance;
                 guide = set[i].guide;
+                stopLine = set[i].stopLine;
             }
         }
     }
 
+    stopLineIndex = stopLine;
     return guide;
 }
 
@@ -1286,6 +1351,15 @@
     // 妫�娴嬬寮�姝よ矾娈碉紝鍏ㄨ溅闇�涓嶅湪鑼冨洿鍐�
     int oldid = currExamMapIndex;
     currExamMapIndex = CalcRoadIndex(currExamMapIndex, RoadMap, car);
+    if (oldid != currExamMapIndex) {
+        DEBUG("閬撹矾ID鍒囨崲 %d ======> %d", oldid, currExamMapIndex);
+
+        if (oldid >= 0) {
+            ResetCrossingStatus(oldid);
+            ResetErrorLaneRpt(oldid);
+        }
+    }
+
     if (currExamMapIndex >= 0) {
         car_sensor_value_t brk = ReadCarSensorValue(BREAK);
         HintCrossing(currExamMapIndex, RoadMap.roads[currExamMapIndex], car, CarModelList);
@@ -1294,10 +1368,6 @@
         ApproachTarget(RoadMap, car, currExamMapIndex, (brk.value == BREAK_ACTIVE), speed, moveDirect, rtkTime);
     }
     ExitTarget(RoadMap, car, CarModelList, rtkTime);
-
-    if (oldid != currExamMapIndex) {
-        DEBUG("閬撹矾ID鍒囨崲 %d", currExamMapIndex);
-    }
 
     oldid = CrashLineType;
 
@@ -1311,9 +1381,14 @@
     oldid = Lane.guide;
     DetectLane(RoadMap, car, currExamMapIndex, rtkTime);
     if (Lane.guide > 0 && currExamMapIndex >= 0) {
-        if (!(NearbyCrossingGuide(currExamMapIndex, RoadMap.roads[currExamMapIndex], car, CarModelList) & Lane.guide)) {
-            DEBUG("涓嶆寜瑙勫畾杞﹂亾鏍囧悜琛岄┒");
-            AddExamFault(9, rtkTime);
+        int stop_line;
+
+        if (!(NearbyCrossingGuide(stop_line, currExamMapIndex, RoadMap.roads[currExamMapIndex], car, CarModelList) & Lane.guide)) {
+            if (!GetErrorLaneRpt(currExamMapIndex, stop_line)) {
+                DEBUG("涓嶆寜瑙勫畾杞﹂亾鏍囧悜琛岄┒");
+                AddExamFault(9, rtkTime);
+                SetErrorLaneRpt(currExamMapIndex, stop_line, true);
+            }
         }
     }
 
@@ -1323,6 +1398,9 @@
 
     // 妫�娴嬪帇绾跨姸鎬�
     TestRoadStartCar(car, speed, moveDirect, rtkTime);
+
+    // 棰濆鐨勮浆鍚戞娴�
+    DetectTurn(car, moveDirect, rtkTime);
 
     if (startCar != START_CAR_DONE)
         return;
@@ -1399,7 +1477,15 @@
     tm.msec = rtkTime->mss;
 }
 
-static char isTurn(int currYaw, int prevYaw, int &ang)
+void CrossRoadCallback(int road, int stop_line, int active, const car_model *car)
+{
+    SetErrorLaneRpt(road, stop_line, false);
+    if (active != ROAD_ACTIVE_FORWARD) {
+        ResetTurnDetect(car);
+    }
+}
+
+static int isTurn(int currYaw, int prevYaw, int thres)
 {
 //    DEBUG("currYaw %d prevYaw %d", currYaw, prevYaw);
     int deltaAng = 0;
@@ -1410,21 +1496,117 @@
         deltaAng = ABS(currYaw - prevYaw);
     }
 
-    ang = deltaAng;
 
 //    DEBUG("瑙掑害宸�� %d", deltaAng);
 
-    if (deltaAng >= TURN_THRESHOLD) {
+    if (deltaAng >= thres) {
         if((( currYaw + 360 - prevYaw) % 360) < 180) {
 //            DEBUG("鍙宠浆");
-            return 'R';
+            return deltaAng;
         } else {
 //            DEBUG("宸﹁浆");
-            return 'L';
+            return 0 - deltaAng;
         }
     }
 
-    return 'N';
+    return 0;
+}
+
+static void ResetTurnDetect(const car_model *car)
+{
+    turnCnt = 0;
+    turnTimeCnt = 0;
+    prevDetectTurnTime = car->tm;
+    prevYaw = car->yaw;
+}
+
+static void DetectTurn(const car_model *car, int moveDirect, const struct RtkTime *rtkTime)
+{
+    int angle;
+
+    if (turnCnt < 0 || TimeGetDiff(rtkTime, &prevDetectTurnTime) < 500) {
+        return;
+    }
+
+    angle = isTurn((int) car->yaw, prevYaw, TURN_THRESHOLD);
+
+    prevYaw = (int) car->yaw;
+    prevDetectTurnTime = *rtkTime;
+
+    if (turnCnt == 0) {
+        if (angle != 0 && moveDirect != 0) {
+            DEBUG("鏈夎浆鍚戣抗璞�");
+            turnCnt++;
+            beginTurnTime = *rtkTime;
+            startTurnYaw = (int) car->yaw;
+        }
+    } else if (turnCnt == 1) {
+        if (angle != 0 && moveDirect != 0) {
+            if (angle * prevTurnWise > 0) {
+                DEBUG("纭杞悜");
+                // 鍚屽悜杞姩
+                turnCnt++;
+                turnTimeCnt = 0;
+            } else {
+                beginTurnTime = *rtkTime;
+                startTurnYaw = (int) car->yaw;
+            }
+        } else {
+            turnCnt = 0;
+        }
+    } else if (turnCnt >= 2) {
+        if (moveDirect == 0) {
+            // 鏆傚仠
+        } else {
+            turnTimeCnt += TimeGetDiff(rtkTime, &prevDetectTurnTime);
+            int wise = isTurn((int) car->yaw, startTurnYaw, TURN_THRESHOLD);
+            DEBUG("杞姩瑙掑害 %d", wise);
+            if (ABS(wise) > 60) {
+                // 纭杞集琛屼负锛屾娴嬪紑濮嬪垰杞集鏃惰浆鍚戠伅鎯呭喌
+                turnCnt = -1;
+
+                car_sensor_value_t lamp = ReadCarSensorValue(TURN_SIGNAL_LAMP);
+                if (lamp.name == TURN_SIGNAL_LAMP) {
+                    if (wise < 0) {
+                        if (lamp.value != LEFT_TURN_LIGHT) {
+                            DEBUG("鍙樿皟鏈墦鐏�!!");
+                            // 娌℃墦鐏紝涓嶅悎鏍�
+                            AddExamFault(13, rtkTime);
+                        } else if (TimeGetDiff(&beginTurnTime, &lamp.time) >= D_SEC(3)) {
+                            DEBUG("杞悜鐏椂闂翠笉瓒�");
+                            // 涓嶈冻3绉掞紝涓嶅悎鏍�
+                            AddExamFault(14, rtkTime);
+                        }
+                    } else {
+                        if (lamp.value != RIGHT_TURN_LIGHT) {
+                            DEBUG("鍙樿皟鏈墦鐏�!!");
+                            // 娌℃墦鐏紝涓嶅悎鏍�
+                            AddExamFault(13, rtkTime);
+                        } else if (TimeGetDiff(&beginTurnTime, &lamp.time) >= D_SEC(3)) {
+                            DEBUG("杞悜鐏椂闂翠笉瓒�");
+                            // 涓嶈冻3绉掞紝涓嶅悎鏍�
+                            AddExamFault(14, rtkTime);
+                        }
+                    }
+                }
+            } else if (turnTimeCnt > D_SEC(10)) {
+                // 鍙栨秷杞悜妫�娴�
+                DEBUG("鍙栨秷杞悜妫�娴�1");
+                turnCnt = -1;
+            }
+        }
+    }
+
+    if (turnCnt < 2 && moveDirect != 0) {
+        turnTimeCnt += TimeGetDiff(rtkTime, &prevDetectTurnTime);
+        if (turnTimeCnt > D_SEC(15)) {
+            // 鍙栨秷杞悜妫�娴�
+            DEBUG("鍙栨秷杞悜妫�娴�2");
+            turnCnt = -1;
+        }
+    }
+
+    prevTurnWise = angle;
 }
 
 static char CheckCarTurn(LIST_CAR_MODEL &CarModelList)

--
Gitblit v1.8.0