From 1101dc614acb3cd794c13cd68c9a24d7c353cb28 Mon Sep 17 00:00:00 2001
From: yy1717 <fctom1215@outlook.com>
Date: 星期六, 21 三月 2020 15:56:03 +0800
Subject: [PATCH] 坐标

---
 lib/src/main/cpp/test_items2/road_exam.h           |    1 
 lib/src/main/cpp/driver_test.cpp                   |   27 +++++-
 lib/src/main/cpp/master/comm_if.cpp                |    2 
 lib/src/main/cpp/native-lib.cpp                    |    2 
 lib/src/main/cpp/test_items2/through_something.cpp |   24 +++++
 lib/src/main/cpp/test_items2/road_exam.cpp         |  133 ++++++++++++++++++++++++++++-----
 lib/src/main/cpp/driver_test.h                     |    2 
 7 files changed, 159 insertions(+), 32 deletions(-)

diff --git a/lib/src/main/cpp/driver_test.cpp b/lib/src/main/cpp/driver_test.cpp
index 8601f17..fae8d3b 100644
--- a/lib/src/main/cpp/driver_test.cpp
+++ b/lib/src/main/cpp/driver_test.cpp
@@ -277,7 +277,7 @@
     }
 }
 
-void AddRoadMapParent(int id, int type, string tts,
+void AddRoadMapParent(int id, int type, string tts, int stopFlag,
         vector<vector<int>> &redLines,
                       vector<vector<int>> &redAreas,
                       vector<vector<int>> &greenLines,
@@ -290,8 +290,13 @@
     newMap.id = id;
     newMap.type = type;
     newMap.tts = tts;
+    newMap.flagStop = stopFlag;
 
     DEBUG("鍔犲叆璺�冨湴鍥句俊鎭� id = %d type = %d", id, type);
+
+    if (!tts.empty()) {
+        DEBUG("TTS 淇℃伅 = %s", tts.c_str());
+    }
 
     if ((newMap.redLineNum = redLines.size()) > 0) {
         newMap.redLine = (Polygon *) malloc(sizeof(Polygon) * newMap.redLineNum);
@@ -357,7 +362,7 @@
             newMap.triggerLine[i].line.num = triggerLines[i].size() - 1;
             newMap.triggerLine[i].line.point = (PointF *) malloc(sizeof(PointF) * newMap.triggerLine[i].line.num);
 
-            DEBUG("    缁撶偣 %d 涓�", newMap.triggerLine[i].line.num);
+            DEBUG("    缁撶偣 %d 涓�, 瑙﹀彂 id = %d", newMap.triggerLine[i].line.num, triggerLines[i][0]);
 
             newMap.triggerLine[i].triggerMapId = triggerLines[i][0];
             for (int j = 0; j < newMap.triggerLine[i].line.num; ++j) {
@@ -497,7 +502,7 @@
 {
     DEBUG("鑾峰彇妯℃嫙璺�冪伅鍏夋祴璇曢」鐩� N = %d %d", n, ExamStart);
 
-//    if (ExamStart) return;
+    if (ExamStart) return;
 
     if (DummyLightContent != NULL) {
         free(DummyLightContent);
@@ -538,8 +543,8 @@
     }
     if (DummyLightContent == NULL && type == TEST_TYPE_ROAD_DUMMY_LIGHT) {
         DEBUG("娌℃湁妯℃嫙鐏厜");
-        err = true;
-        MA_SendExamStatus(0, -3);
+//        err = true;
+//        MA_SendExamStatus(0, -3);
     }
     if (type != TEST_TYPE_AREA && (RoadMapPoints.num == 0 || RoadMapPoints.point == NULL || RoadMapList.size() == 0)) {
         DEBUG("娌℃湁璺�冨湴鍥�");
@@ -559,6 +564,8 @@
 
             if (type == TEST_TYPE_ROAD_DUMMY_LIGHT) {
                 exam_dummy_light = 0;
+
+//                InitRoadExam();         ////////////////////
             }
         }
         MA_SendExamStatus(1, 0);
@@ -733,16 +740,18 @@
         if (ExamType == TEST_TYPE_AREA)
             AddExamFault(4, rtkTime);
     }
-DEBUG("22222222222222222222");
+
     if (ExamType == TEST_TYPE_ROAD_DUMMY_LIGHT) {
         if (exam_dummy_light == 0) {
             StartDummyLightExam(DummyLightContent, DummyLightContentSize, rtkTime);
             exam_dummy_light = 1;
             // 姹囨姤鐏厜鑰冭瘯寮�濮�
+            DEBUG("鐏厜鑰冭瘯寮�濮�");
         } else if (exam_dummy_light == 1) {
             exam_dummy_light = ExecuteDummyLightExam(rtkTime);
             // 姹囨姤鐏厜鑰冭瘯缁撴潫
             if (exam_dummy_light == 2) {
+                DEBUG("鐏厜鑰冭瘯缁撴潫");
                 InitRoadExam();
             }
         }
@@ -912,6 +921,7 @@
     tm.DD = RtkBuffer[p1].DD;
     tm.hh = RtkBuffer[p1].hh;
     tm.mm = RtkBuffer[p1].mm;
+    tm.ss = RtkBuffer[p1].ss;
     tm.mss = RtkBuffer[p1].dss;
 
     UpdateCarBodyCoord(&tm, RtkBuffer[p1].heading, RtkBuffer[p1].pitch, RtkBuffer[p1].roll, main_ant_coord, CarModel);
@@ -1210,6 +1220,11 @@
     fault.sn = examFaultIndex++;
     sprintf(fault.utc, "%04d%02d%02d%02d%02d%02d.%02d", 2000 + rtkTime->YY,
             rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss);
+
+    if (ExamType != TEST_TYPE_AREA) {
+        wrong += 1000;
+    }
+
     fault.wrong_id = wrong;
 
     DEBUG("鑰冭瘯鍙戠敓閿欒 %d %s", wrong, fault.utc);
diff --git a/lib/src/main/cpp/driver_test.h b/lib/src/main/cpp/driver_test.h
index ca11dee..b590054 100644
--- a/lib/src/main/cpp/driver_test.h
+++ b/lib/src/main/cpp/driver_test.h
@@ -124,7 +124,7 @@
 void AddMap(int id, int type, const double (*map)[2], int pointNum, const double (*map2)[2], int pointNum2);
 void CleanRoadMap(void);
 void SetRoadMapPoints(vector<double> &mapPoints);
-void AddRoadMapParent(int id, int type, string tts, vector<vector<int>> &redLines,
+void AddRoadMapParent(int id, int type, string tts, int stopFlag, vector<vector<int>> &redLines,
                       vector<vector<int>> &redAreas, vector<vector<int>> &greenLines, vector<vector<int>> &triggerLines,
                       vector<int> area, vector<int> stopLine);
 
diff --git a/lib/src/main/cpp/master/comm_if.cpp b/lib/src/main/cpp/master/comm_if.cpp
index cf9abe8..fddeb5c 100644
--- a/lib/src/main/cpp/master/comm_if.cpp
+++ b/lib/src/main/cpp/master/comm_if.cpp
@@ -633,7 +633,7 @@
                                 tts = s.GetString();
                             }
 
-                            AddRoadMapParent(id, type, tts, redLines, redAreas, greenLines, triggerLines, area, stopLine);
+                            AddRoadMapParent(id, type, tts, stop_flag, redLines, redAreas, greenLines, triggerLines, area, stopLine);
                         }
                     }
                 }
diff --git a/lib/src/main/cpp/native-lib.cpp b/lib/src/main/cpp/native-lib.cpp
index 9cc7f3c..e19a097 100644
--- a/lib/src/main/cpp/native-lib.cpp
+++ b/lib/src/main/cpp/native-lib.cpp
@@ -26,7 +26,7 @@
 const int RTK_PLATFORM_PORT = 12125;
 const uint8_t phone[] = {0x20,0x19,0x10,0x15,0x00,0x00,0x00,0x01};
 
-const char *VIRTUAL_RTK_IP = "192.168.3.52";
+const char *VIRTUAL_RTK_IP = "192.168.16.100";
 const int VIRTUAL_RTK_PORT = 9001;
 
 static void SendBootIndicate(union sigval sig);
diff --git a/lib/src/main/cpp/test_items2/road_exam.cpp b/lib/src/main/cpp/test_items2/road_exam.cpp
index 51fdf57..ae82561 100644
--- a/lib/src/main/cpp/test_items2/road_exam.cpp
+++ b/lib/src/main/cpp/test_items2/road_exam.cpp
@@ -11,6 +11,7 @@
 #include "../test_common/car_sensor.h"
 #include "../native-lib.h"
 #include "through_something.h"
+#include "../master/comm_if.h"
 
 #include <vector>
 #include <list>
@@ -21,10 +22,10 @@
 
 using namespace std;
 
-#define TURN_CHECK_CNT          5
+#define TURN_CHECK_CNT          4
 
-static const int TURN_THRESHOLD = 10;
-static const int TURN_CHECK_INTERVAL = D_SEC(1);
+static const int TURN_THRESHOLD = 5;
+static const int TURN_CHECK_INTERVAL = 500;
 const double SLIDE_DISTANCE_THRESHOLD_RED = 0.3;
 const double SLIDE_DISTANCE_THRESHOLD_YELLOW = 0.1;
 
@@ -83,6 +84,8 @@
 
 void InitRoadExam(void)
 {
+    DEBUG("Start road_exam");
+
     crashGreenCmpTime.hour = -1;
     occurCrashRedLine = false;
     occurCrashGreenLine = false;
@@ -209,9 +212,9 @@
         uint32_t tp = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
 
         if (tp - stopTimepoint >= STOP_CAR_TIME && !reportStopCarOnRedArea && CrashRedArea(RoadMapList, car)) {
-            // 鍋滆溅瓒�2绉掞紝鍋滃湪绾㈠尯
+            // 鍋滆溅瓒�2绉掞紝鍋滃湪绾㈠尯锛屼笉鍚堟牸
             AddExamFault(16, rtkTime);
-            DEBUG("涓�斿仠杞�");
+            DEBUG("绂佸仠鍖哄仠杞�");
             reportStopCarOnRedArea = true;
         }
     } else if (moveDirect == -1) {
@@ -263,6 +266,7 @@
     // 妫�鏌ユ槸鍚︽寔缁浆鍚�
     char turnDirect = CheckCarTurn(CarModelList);
     if (turnDirect == 'L') {
+        PlayTTS("宸�1", 5692);
         if (currTurnSignalStatus != LEFT_TURN_LIGHT) {
             if (!reportTurnSignalError) {
                 DEBUG("娌℃墦宸﹁浆鐏�");
@@ -280,6 +284,7 @@
             }
         }
     } else if (turnDirect == 'R') {
+        PlayTTS("鍙�1", 5692);
         if (currTurnSignalStatus != RIGHT_TURN_LIGHT) {
             if (!reportTurnSignalError) {
                 DEBUG("娌℃墦鍙宠浆鐏�");
@@ -304,6 +309,7 @@
     if (CrashRedLine(RoadMapList, car)) {
         if (!occurCrashRedLine) {
             // 杞﹁締琛岄┒涓獞杞ц溅閬撲腑蹇冨疄绾挎垨鑰呰溅閬撹竟缂樺疄绾匡紝涓嶅悎鏍�
+            DEBUG("鎾為亾璺竟缂樼嚎");
             AddExamFault(11, rtkTime);
             occurCrashRedLine = true;
         }
@@ -324,6 +330,7 @@
                         crashGreenRunTime.hour, crashGreenRunTime.min, crashGreenRunTime.sec, crashGreenRunTime.msec*10);
 
                 if (diff >= CRASH_DOTTED_LINE_TIMEOUT) {
+                    DEBUG("闀挎椂闂村帇铏氱嚎");
                     checkCrashGreenTimeout = 2;
                     // 闀挎椂闂撮獞杞ц溅閬撳垎鐣岀嚎琛岄┒锛屼笉鍚堟牸
                     AddExamFault(12, rtkTime);
@@ -368,6 +375,7 @@
                                                 crashGreenCmpTime.sec, crashGreenCmpTime.msec * 10);
 
                     if (diff < CHANGE_ROAD_MIN_INTERVAL) {
+                        DEBUG("===================== 杩炵画鍙橀亾 ============!!");
                         // 杩炵画鍙橀亾锛屼笉鍚堟牸
                         AddExamFault(15, rtkTime);
                     }
@@ -378,16 +386,20 @@
 
                 // 妫�鏌ュ彉閬撳墠锛屾槸鍚︽彁鍓嶈浆鍚戠伅
                 if (inter == 1) {
+                    PlayTTS("宸�2", 5698);
                     // 鍚戝乏渚у彉閬�
                     DEBUG("鍚戝乏渚у彉閬�");
                     if (turnSignalStatusWhenCrashGreenLine != LEFT_TURN_LIGHT) {
+                        DEBUG("鍙樿皟鏈墦鐏�!!");
                         // 娌℃墦鐏紝涓嶅悎鏍�
                         AddExamFault(13, rtkTime);
                     }
                 } else {
+                    PlayTTS("鍙�2", 5698);
                     // 鍚戝彸渚у彉閬�
                     DEBUG("鍚戝彸渚у彉閬�");
                     if (turnSignalStatusWhenCrashGreenLine != RIGHT_TURN_LIGHT) {
+                        DEBUG("鍙樿皟鏈墦鐏�!!");
                         // 娌℃墦鐏紝涓嶅悎鏍�
                         AddExamFault(13, rtkTime);
                     }
@@ -402,10 +414,20 @@
     if (currExamMapIndex == -1) {
         currExamMapIndex = CrashTriggerLine(RoadMapList, car, CarModelList);
         if (currExamMapIndex != -1) {
+            DEBUG("纰版挒瑙﹀彂绾�");
+
+            MA_EnterMap(RoadMapList[currExamMapIndex].id, RoadMapList[currExamMapIndex].type, 1);
             StartThroughExam(currExamMapIndex, RoadMapList);
         }
     } else {
+        int prevIdx = currExamMapIndex;
 
+        currExamMapIndex = ExecuteThroughExam(currExamMapIndex, RoadMapList, car,
+        CarModelList, speed, moveDirect, rtkTime);
+
+        if (currExamMapIndex == -1) {
+            MA_EnterMap(RoadMapList[prevIdx].id, RoadMapList[prevIdx].type, 1);
+        }
     }
 }
 
@@ -436,6 +458,61 @@
     return ret;
 }
 
+bool CrashSonRedLine(int index, LIST_ROAD_MAP &RoadMapList, const car_model *car, LIST_CAR_MODEL &CarModelList)
+{
+    bool ret = false;
+
+    if (CarModelList.size() < 5 || index < 0 || index >= RoadMapList.size())
+        return ret;
+
+    Polygon trace;
+
+    trace.num = 5;          // 鏈�杩�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 j = 0; j < RoadMapList[index].redLineNum; ++j) {
+        Line red_line;
+
+        int kp = 0;
+
+        // 瑙﹀彂绾夸竴鑸簲璇ュ彧鏈夐灏�2鐐癸紙id, p1, p2锛�
+        for (int k = 1; k < RoadMapList[index].redLine[j].num; ++k) {
+            MakeLine(&red_line, &RoadMapList[index].redLine[j].point[kp],
+                     &RoadMapList[index].redLine[j].point[k]);
+
+            int pp = 0;
+            for (int p = 1; p < pn; ++p) {
+                Line trace_line;
+                MakeLine(&trace_line, &trace.point[pp], &trace.point[p]);
+
+                if (IntersectionOf(trace_line, red_line) == GM_Intersection) {
+                    // 纰板埌瑙﹀彂绾�
+                    ret = true;
+                    goto SEARCH_SON_RED_LINE_END;
+                }
+
+                pp = p;
+            }
+
+            kp = k;
+        }
+    }
+
+    SEARCH_SON_RED_LINE_END:
+    free(trace.point);
+
+    return ret;
+}
+
 static void Rtk2DriveTimer(struct drive_timer &tm, const struct RtkTime *rtkTime)
 {
     tm.hour = rtkTime->hh;
@@ -446,18 +523,24 @@
 
 static char isTurn(int currYaw, int prevYaw)
 {
+//    DEBUG("currYaw %d prevYaw %d", currYaw, prevYaw);
+
+    int deltaAng = 0;
+
     if (ABS(currYaw - prevYaw) > 180) {
-        currYaw = 360 - ABS(currYaw-prevYaw);
+        deltaAng = 360 - ABS(currYaw-prevYaw);
     } else {
-        currYaw = ABS(currYaw - prevYaw);
+        deltaAng = ABS(currYaw - prevYaw);
     }
 
-    if (currYaw >= TURN_THRESHOLD) {
+//    DEBUG("瑙掑害宸�� %d", deltaAng);
+
+    if (deltaAng >= TURN_THRESHOLD) {
         if((( currYaw + 360 - prevYaw) % 360) < 180) {
-            DEBUG("鍙宠浆");
+//            DEBUG("鍙宠浆");
             return 'R';
         } else {
-            DEBUG("宸﹁浆");
+//            DEBUG("宸﹁浆");
             return 'L';
         }
     }
@@ -480,17 +563,22 @@
     char turn[TURN_CHECK_CNT] = {0};
     int checkCnt = 0;
 
-    while (iter != CarModelList.end()) {
+//    DEBUG("CheckCarTurn.........");
+
+    while (iter != CarModelList.end() && checkCnt < TURN_CHECK_CNT) {
         c2 = *iter;
 
         uint32_t tdiff = TimeGetDiff(c1->tm.hh, c1->tm.mm, c1->tm.ss, c1->tm.mss * 10, c2->tm.hh, c2->tm.mm, c2->tm.ss, c2->tm.mss*10);
 
         if (tdiff >= TURN_CHECK_INTERVAL) {
-            turn[checkCnt++] = isTurn((int)c1->yaw, (int)c2->yaw);
-            c1 = c2;
-
-            if (checkCnt == TURN_CHECK_CNT)
+            turn[checkCnt] = isTurn((int)c1->yaw, (int)c2->yaw);
+//            DEBUG("%c  瑙掑害姣旇緝 %02d:%02d:%02d.%03d  %02d:%02d:%02d.%03d", turn[checkCnt], c1->tm.hh, c1->tm.mm, c1->tm.ss, c1->tm.mss * 10, c2->tm.hh, c2->tm.mm, c2->tm.ss, c2->tm.mss*10);
+            if (turn[checkCnt] == 0) {
                 break;
+            }
+
+            c1 = c2;
+            checkCnt++;
         }
 
         ++iter;
@@ -502,8 +590,10 @@
             break;
     }
 
-    if (i == TURN_CHECK_CNT-1)
+    if (i == TURN_CHECK_CNT-1) {
+        DEBUG("宸﹀彸杞‘璁� %c", turn[0]);
         return turn[0];
+    }
 
     return 0;
 }
@@ -659,15 +749,18 @@
                 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) {
+                    int pp = 0;
+                    for (int p = 1; 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) {
+                            IntersectionOfLine(RoadMapList[i].triggerLine[j].line.point[kp],
+                                    RoadMapList[i].triggerLine[j].line.point[k],
+                                    car->carXY[car->left_front_tire[TIRE_OUTSIDE]]) == -1 &&
+                            DistanceOf(car->carXY[car->left_front_tire[TIRE_OUTSIDE]], trigger_line) > 0.1) {
                             // 纰板埌瑙﹀彂绾�
+                            DEBUG("纰版挒瑙﹀彂绾� 寮曞彂鍦板浘 id = %d", RoadMapList[i].triggerLine[j].triggerMapId);
                             map_idx =  FindMapIndexById(RoadMapList[i].triggerLine[j].triggerMapId, RoadMapList);
                             goto SEARCH_TRIGGER_LINE_END;
                         }
diff --git a/lib/src/main/cpp/test_items2/road_exam.h b/lib/src/main/cpp/test_items2/road_exam.h
index 16d0fd2..2b9d29c 100644
--- a/lib/src/main/cpp/test_items2/road_exam.h
+++ b/lib/src/main/cpp/test_items2/road_exam.h
@@ -22,5 +22,6 @@
 void InitRoadExam(void);
 void TestRoadGeneral(LIST_ROAD_MAP &RoadMapList, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime);
 bool ExitSonArea(int index, LIST_ROAD_MAP &RoadMapList, const car_model *car);
+bool CrashSonRedLine(int index, LIST_ROAD_MAP &RoadMapList, const car_model *car, LIST_CAR_MODEL &CarModelList);
 
 #endif //MYAPPLICATION2_ROAD_EXAM_H
diff --git a/lib/src/main/cpp/test_items2/through_something.cpp b/lib/src/main/cpp/test_items2/through_something.cpp
index 33fe453..f0fa865 100644
--- a/lib/src/main/cpp/test_items2/through_something.cpp
+++ b/lib/src/main/cpp/test_items2/through_something.cpp
@@ -17,17 +17,22 @@
 
 static int breakActive;
 static int stopActive;
+static bool crashRedLine;
 
 void StartThroughExam(int index, LIST_ROAD_MAP &RoadMapList)
 {
     if (index == -1)
         return;
-    DEBUG("杩涘叆璺�冨瓙鍦板浘 index = %d id = %d item = %d", index, RoadMapList[index].id, RoadMapList[index].type);
+    DEBUG("杩涘叆璺�冮�氳繃something鍦板浘 index = %d id = %d item = %d", index, RoadMapList[index].id, RoadMapList[index].type);
     if (!RoadMapList[index].tts.empty()) {
+        DEBUG("鎾斁TTS");
         PlayTTS(RoadMapList[index].tts.c_str(), 0);
+    } else {
+        DEBUG("娌℃湁TTS");
     }
     breakActive = 0;
     stopActive = 0;
+    crashRedLine = false;
 }
 
 int ExecuteThroughExam(int index, LIST_ROAD_MAP &RoadMapList, const car_model *car,
@@ -99,6 +104,7 @@
                 default:
                     break;
             }
+            stopActive = -1;
         } else if (distance2StopLine < DISTANCE_STOP_CAR_TO_STOP_LINE) {
             if (moveDirect == 0) {
                 stopActive = 1;
@@ -106,9 +112,21 @@
         }
     }
 
-    if (ExitSonArea(index, RoadMapList, car))
+     if (CrashSonRedLine(index, RoadMapList, car, CarModelList)) {
+         if (!crashRedLine) {
+             DEBUG("涓嶆寜鑰冭瘯鍛樻寚浠よ椹�");
+             crashRedLine = true;
+             // 涓嶆寜鑰冭瘯鍛樻寚浠よ椹�
+             AddExamFault(3, rtkTime);
+         }
+     } else {
+         crashRedLine = false;
+     }
+
+    if (ExitSonArea(index, RoadMapList, car)) {
+        DEBUG("绂诲紑閫氳繃something鍖哄煙");
         return -1;
+    }
 
     return index;
 }
-

--
Gitblit v1.8.0