From 3d3a5aa436645e5af1a4877338319ff8274e0346 Mon Sep 17 00:00:00 2001
From: yy1717 <fctom1215@outlook.com>
Date: 星期四, 19 三月 2020 18:04:46 +0800
Subject: [PATCH] 通相检查

---
 lib/src/main/cpp/test_items2/road_exam.cpp |  206 ++++++++++++++++++++++++++++++++++++++++++++++-----
 1 files changed, 185 insertions(+), 21 deletions(-)

diff --git a/lib/src/main/cpp/test_items2/road_exam.cpp b/lib/src/main/cpp/test_items2/road_exam.cpp
index 80da3de..fd723b9 100644
--- a/lib/src/main/cpp/test_items2/road_exam.cpp
+++ b/lib/src/main/cpp/test_items2/road_exam.cpp
@@ -9,6 +9,7 @@
 #include "../jni_log.h"
 #include "../defs.h"
 #include "../test_common/car_sensor.h"
+#include "../native-lib.h"
 
 #include <vector>
 #include <list>
@@ -37,6 +38,8 @@
 static uint32_t stopTimepoint = 0;
 static bool reportStopCarOnRedArea;
 static PointF stopPoint;
+static bool prevGearError = false;
+static bool prevGearNSlide = false;
 
 static bool slideLongDistance;
 static bool slideNormalDistance;
@@ -49,22 +52,34 @@
     int msec;
 } crashGreenRunTime, crashGreenCmpTime, crashGreenStartTime, turnSignalChangeTime;
 
+static struct drive_timer gearErrorTimePoint;
+static struct drive_timer gearNSlideTimePoint;
+
+static int gearErrorTime;
+static int gearNSlideTime;
+
+static int currExamMapIndex;
+
+static const uint32_t GEAR_N_SLIDE_TIMEOUT = D_SEC(5);
+static const uint32_t GEAR_ERROR_TIMEOUT = D_SEC(15);
 static const uint32_t STOP_CAR_TIME = D_SEC(2);
 static const uint32_t CHANGE_ROAD_MIN_INTERVAL = D_SEC(10);
 static const uint32_t CRASH_DOTTED_LINE_TIMEOUT = D_SEC(10);
 static const uint32_t TURN_SIGNAL_LAMP_ADVANCE = D_SEC(3);
 
 static const double MAX_SPEED = 40.0 * 1000.0 / 3600.0;
-static const int SPEED_SHIFT_TABLE[][2] = {{0, 20}, {5, 30}, {15, 40}, {25, 10000}, {35, 10000}};
+static const int SPEED_GEAR_TABLE[][2] = {{0, 20}, {5, 30}, {15, 40}, {25, 10000}, {35, 10000}};
 
-static void Rtk2DirveTimer(struct drive_timer &tm, const struct RtkTime *rtkTime);
+static void Rtk2DriveTimer(struct drive_timer &tm, const struct RtkTime *rtkTime);
 static char isTurn(int currYaw, int prevYaw);
 static char CheckCarTurn(LIST_CAR_MODEL &CarModelList);
 static bool CrashRedLine(LIST_ROAD_MAP &RoadMapList, const car_model *car);
 static bool CrashRedArea(LIST_ROAD_MAP &RoadMapList, const car_model *car);
 static bool CrashGreenLine(LIST_ROAD_MAP &RoadMapList, const car_model *car, PointF &p1, PointF &p2);
+static int CrashTriggerLine(LIST_ROAD_MAP &RoadMapList, const car_model *car, LIST_CAR_MODEL &CarModelList);
+static int FindMapIndexById(int id, LIST_ROAD_MAP &RoadMapList);
 
-void Init(void)
+void InitRoadExam(void)
 {
     crashGreenCmpTime.hour = -1;
     occurCrashRedLine = false;
@@ -81,10 +96,17 @@
     occurSlide = false;
     slideLongDistance = false;
     slideNormalDistance = false;
+    prevGearError = false;
+    gearErrorTime = 0;
+    prevGearNSlide = false;
+    gearNSlideTime = 0;
+
+    currExamMapIndex = -1;
 }
 
 void TestRoadGeneral(LIST_ROAD_MAP &RoadMapList, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime)
 {
+    // 瓒呴�熸娴�
     if (moveDirect != 0 && speed > MAX_SPEED) {
         if (!occurOverSpeed) {
             occurOverSpeed = true;
@@ -96,28 +118,78 @@
     }
 
     // 鎸′綅鍖归厤妫�娴�
-    switch (ReadCarStatus(SHIFT)) {
-        case SHIFT_N:
+    bool currGearError = false;
+    bool currGearNSlide = false;
+
+    switch (ReadCarStatus(GEAR)) {
+        case GEAR_N:
             if (moveDirect != 0) {
-
+                // 绌烘。婊戣
+                currGearNSlide = true;
             }
             break;
-        case SHIFT_1:
-            if (ConvertMs2KMs(speed) < SPEED_SHIFT_TABLE[0][0] || ConvertMs2KMs(speed) > SPEED_SHIFT_TABLE[0][1]) {
-
+        case GEAR_1:
+            if (ConvertMs2KMh(speed) < SPEED_GEAR_TABLE[0][0] || ConvertMs2KMh(speed) > SPEED_GEAR_TABLE[0][1]) {
+                currGearError = true;
             }
             break;
-        case SHIFT_2:
+        case GEAR_2:
+            if (ConvertMs2KMh(speed) < SPEED_GEAR_TABLE[1][0] || ConvertMs2KMh(speed) > SPEED_GEAR_TABLE[1][1]) {
+                currGearError = true;
+            }
             break;
-        case SHIFT_3:
+        case GEAR_3:
+            if (ConvertMs2KMh(speed) < SPEED_GEAR_TABLE[2][0] || ConvertMs2KMh(speed) > SPEED_GEAR_TABLE[2][1]) {
+                currGearError = true;
+            }
             break;
-        case SHIFT_4:
+        case GEAR_4:
+            if (ConvertMs2KMh(speed) < SPEED_GEAR_TABLE[3][0] || ConvertMs2KMh(speed) > SPEED_GEAR_TABLE[3][1]) {
+                currGearError = true;
+            }
             break;
-        case SHIFT_5:
+        case GEAR_5:
+            if (ConvertMs2KMh(speed) < SPEED_GEAR_TABLE[4][0] || ConvertMs2KMh(speed) > SPEED_GEAR_TABLE[4][1]) {
+                currGearError = true;
+            }
             break;
         default:break;
     }
+    // 绌烘。婊戣瓒呮椂
+    if (currGearNSlide && prevGearNSlide) {
+        gearNSlideTime += TimeGetDiff(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10,
+                                      gearNSlideTimePoint.hour, gearNSlideTimePoint.min, gearNSlideTimePoint.sec, gearNSlideTimePoint.msec*10);
+    }
+    if (gearNSlideTime > GEAR_N_SLIDE_TIMEOUT) {
+        // 绌烘。婊戣瓒�5绉掞紝涓嶅悎鏍�
+        DEBUG("鎸′綅婊戣锛岃秴杩�5绉�");
+        AddExamFault(8, rtkTime);
+        gearNSlideTime = 0;
+    }
 
+    prevGearNSlide = currGearNSlide;
+    if (prevGearNSlide) {
+        Rtk2DriveTimer(gearNSlideTimePoint, rtkTime);
+    } else {
+        gearNSlideTime = 0;
+    }
+    // 鎸′綅涓嶅尮閰嶈秴鏃�
+    if (currGearError && prevGearError) {
+        gearErrorTime += TimeGetDiff(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10,
+                    gearErrorTimePoint.hour, gearErrorTimePoint.min, gearErrorTimePoint.sec, gearErrorTimePoint.msec*10);
+    }
+    if (gearErrorTime > GEAR_ERROR_TIMEOUT) {
+        // 绱15绉掞紝鎸′綅-杞﹂�熶笉鍖归厤锛屼笉鍚堟牸
+        DEBUG("鎸′綅閿欒瓒呰繃15绉�");
+        AddExamFault(6, rtkTime);
+        gearErrorTime = 0;
+    }
+
+    prevGearError = currGearError;
+    if (prevGearError) {
+        Rtk2DriveTimer(gearErrorTimePoint, rtkTime);
+    }
+    
     // 璧锋鍚庢粦
     if (moveDirect != prevMoveDirect) {
         if (moveDirect == 0) {
@@ -171,13 +243,13 @@
         case LEFT_TURN_LIGHT:
             if (currTurnSignalStatus != LEFT_TURN_LIGHT) {
                 currTurnSignalStatus = LEFT_TURN_LIGHT;
-                Rtk2DirveTimer(turnSignalChangeTime, rtkTime);
+                Rtk2DriveTimer(turnSignalChangeTime, rtkTime);
             }
             break;
         case RIGHT_TURN_LIGHT:
             if (currTurnSignalStatus != RIGHT_TURN_LIGHT) {
                 currTurnSignalStatus = RIGHT_TURN_LIGHT;
-                Rtk2DirveTimer(turnSignalChangeTime, rtkTime);
+                Rtk2DriveTimer(turnSignalChangeTime, rtkTime);
             }
             break;
         default:
@@ -226,7 +298,7 @@
         reportTurnSignalError = false;
     }
 
-
+    // 鎾炵孩绾�
     if (CrashRedLine(RoadMapList, car)) {
         if (!occurCrashRedLine) {
             // 杞﹁締琛岄┒涓獞杞ц溅閬撲腑蹇冨疄绾挎垨鑰呰溅閬撹竟缂樺疄绾匡紝涓嶅悎鏍�
@@ -237,14 +309,14 @@
         occurCrashRedLine = false;
     }
 
+    // 鎾炵豢绾�
     static PointF p1, p2;
-
     if (CrashGreenLine(RoadMapList, car, p1, p2)) {
         // 鍘嬭櫄绾�
         if (moveDirect != 0) {
             if (checkCrashGreenTimeout == 0) {
                 checkCrashGreenTimeout = 1;
-                Rtk2DirveTimer(crashGreenRunTime, rtkTime);         // 杩愬姩涓帇铏氱嚎鐨勫紑濮嬫椂闂寸偣
+                Rtk2DriveTimer(crashGreenRunTime, rtkTime);         // 杩愬姩涓帇铏氱嚎鐨勫紑濮嬫椂闂寸偣
             } else if (checkCrashGreenTimeout == 1) {
                 uint32_t diff = TimeGetDiff(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10,
                         crashGreenRunTime.hour, crashGreenRunTime.min, crashGreenRunTime.sec, crashGreenRunTime.msec*10);
@@ -265,7 +337,7 @@
         if (!occurCrashGreenLine) {
             occurCrashGreenLine = true;
             // 璁板綍寮�濮嬪帇绾跨殑鏃堕棿锛屼笉纭畾鏄惁鏈夊彉閬撴剰鍥撅紝寰呯‘璁ゅ彉閬撳悗鍐嶅鐞嗕箣
-            Rtk2DirveTimer(crashGreenStartTime, rtkTime);
+            Rtk2DriveTimer(crashGreenStartTime, rtkTime);
             turnSignalStatusWhenCrashGreenLine = currTurnSignalStatus;
         }
 
@@ -300,7 +372,7 @@
                 }
 
                 // 璁板綍鏈鍙橀亾鏃堕棿鐐�
-                Rtk2DirveTimer(crashGreenCmpTime, rtkTime);
+                Rtk2DriveTimer(crashGreenCmpTime, rtkTime);
 
                 // 妫�鏌ュ彉閬撳墠锛屾槸鍚︽彁鍓嶈浆鍚戠伅
                 if (inter == 1) {
@@ -323,9 +395,22 @@
         occurCrashGreenLine = false;
         checkCrashGreenTimeout = 0;
     }
+
+    // 瑙﹀彂绾挎娴�
+    if (currExamMapIndex == -1) {
+        currExamMapIndex = CrashTriggerLine(RoadMapList, car, CarModelList);
+        if (currExamMapIndex != -1) {
+            DEBUG("杩涘叆璺�冨瓙鍦板浘 index = %d id = %d item = %d", currExamMapIndex, RoadMapList[currExamMapIndex].id, RoadMapList[currExamMapIndex].type);
+            if (!RoadMapList[currExamMapIndex].tts.empty()) {
+                PlayTTS(RoadMapList[currExamMapIndex].tts.c_str(), 0);
+            }
+        }
+    } else {
+
+    }
 }
 
-static void Rtk2DirveTimer(struct drive_timer &tm, const struct RtkTime *rtkTime)
+static void Rtk2DriveTimer(struct drive_timer &tm, const struct RtkTime *rtkTime)
 {
     tm.hour = rtkTime->hh;
     tm.min = rtkTime->mm;
@@ -507,3 +592,82 @@
     }
     return false;
 }
+
+/************************************************************
+ * 妫�娴嬭溅杈嗘槸鍚﹁Е鍙戝瓙鑰冮」鍦板浘
+ * @param RoadMapList
+ * @param car
+ * @param CarModelList
+ * @return
+ */
+static int CrashTriggerLine(LIST_ROAD_MAP &RoadMapList, const car_model *car, LIST_CAR_MODEL &CarModelList)
+{
+    int map_idx = -1;
+
+    if (CarModelList.size() < 5)
+        return map_idx;
+
+    Polygon trace;
+
+    trace.num = 5;
+    trace.point = (PointF *) malloc(sizeof(PointF) * trace.num);
+
+    list<car_model *>::iterator iter = CarModelList.begin();
+
+    int pn = 0;
+    while (iter != CarModelList.end() && pn < trace.num) {
+        trace.point[pn++] = ((car_model *)(*iter))->carXY[((car_model *)(*iter))->left_front_tire[TIRE_OUTSIDE]];
+        ++iter;
+    }
+
+    for (int i = 0; i < RoadMapList.size(); ++i) {
+        if (RoadMapList[i].type == 100) {
+
+            // 姣忔潯绾块兘妫�娴�
+            for (int j = 0; j < RoadMapList[i].triggerLineNum; ++j) {
+                Line trigger_line;
+
+                int kp = 0;
+
+                // 瑙﹀彂绾夸竴鑸簲璇ュ彧鏈夐灏�2鐐癸紙id, p1, p2锛�
+                for (int k = 1; k < RoadMapList[i].triggerLine[j].line.num; ++k) {
+                    MakeLine(&trigger_line, &RoadMapList[i].triggerLine[j].line.point[kp], &RoadMapList[i].triggerLine[j].line.point[k]);
+
+                    int pp = 1;
+                    for (int p = 2; p < pn; ++p) {
+                        Line trace_line;
+                        MakeLine(&trace_line, &trace.point[pp], &trace.point[p]);
+
+                        if (IntersectionOf(trace_line, trigger_line) == GM_Intersection &&
+                            IntersectionOfLine(trace.point[pp], trace.point[p], car->carXY[car->left_front_tire[TIRE_OUTSIDE]]) == -1 &&
+                            DistanceOf(car->carXY[car->left_front_tire[TIRE_OUTSIDE]], trace_line) > 0.1) {
+                            // 纰板埌瑙﹀彂绾�
+                            map_idx =  FindMapIndexById(RoadMapList[i].triggerLine[j].triggerMapId, RoadMapList);
+                            goto SEARCH_TRIGGER_LINE_END;
+                        }
+
+                        pp = p;
+                    }
+
+                    kp = k;
+                }
+            }
+            break;
+        }
+    }
+
+SEARCH_TRIGGER_LINE_END:
+    free(trace.point);
+
+    return map_idx;
+}
+
+static int FindMapIndexById(int id, LIST_ROAD_MAP &RoadMapList)
+{
+    for (int i = 0; i < RoadMapList.size(); ++i) {
+        if (RoadMapList[i].id == id) {
+            return i;
+        }
+    }
+    return -1;
+}

--
Gitblit v1.8.0