From 24428a1b69afcf113883e9cede1159443201a1b8 Mon Sep 17 00:00:00 2001
From: yy1717 <fctom1215@outlook.com>
Date: 星期四, 02 四月 2020 18:57:07 +0800
Subject: [PATCH] 进出科目二区域调整

---
 lib/src/main/cpp/test_items/driving_curve.cpp   |   30 ++++
 lib/src/main/cpp/driver_test.cpp                |  157 ++++++++++++++++---------
 lib/src/main/cpp/test_items/park_edge.h         |    2 
 lib/src/main/cpp/test_items/park_bottom.h       |    2 
 lib/src/main/cpp/test_items/stop_and_start.cpp  |   40 +++++-
 lib/src/main/cpp/driver_test.h                  |    8 +
 lib/src/main/cpp/test_items/driving_curve.h     |    2 
 lib/src/main/cpp/master/comm_if.h               |    2 
 lib/src/main/cpp/test_items/park_bottom.cpp     |   38 +++++-
 lib/src/main/cpp/test_items/turn_a90.cpp        |   12 +
 lib/src/main/cpp/test_items2/drive_straight.cpp |    9 +
 lib/src/main/cpp/test_items/park_edge.cpp       |   20 ++
 lib/src/main/cpp/master/comm_if.cpp             |   18 +++
 lib/src/main/cpp/test_items/turn_a90.h          |    2 
 lib/src/main/cpp/test_items/stop_and_start.h    |    2 
 15 files changed, 256 insertions(+), 88 deletions(-)

diff --git a/lib/src/main/cpp/driver_test.cpp b/lib/src/main/cpp/driver_test.cpp
index 0af9a65..9f47645 100644
--- a/lib/src/main/cpp/driver_test.cpp
+++ b/lib/src/main/cpp/driver_test.cpp
@@ -40,12 +40,6 @@
 
 using namespace std;
 
-#define MAP_TYPE_PARK_BUTTOM        1
-#define MAP_TYPE_STOP_START         2
-#define MAP_TYPE_PART_EDGE         3
-#define MAP_TYPE_CURVE         4
-#define MAP_TYPE_TURN_90         5
-
 #define RTK_INVALID         0
 #define RTK_SINGLE_POINT   1
 #define RTK_DIFF            2
@@ -117,7 +111,8 @@
 static void UpdateCarBodyCoord(struct RtkTime *rtkTime, double azimuth, double pitch, double roll, PointF main_ant, car_model *carModel);
 static bool UpdateCarCoord(double &spd, int &mov, int &idx);
 
-static int EnterMap(const car_model *car, const struct map_list *mapList, int mapNum);
+static bool CrashTriggerLine(Line triggerLine, const car_model *car, LIST_CAR_MODEL &CarModelList);
+static int EnterMap(const car_model *car, LIST_CAR_MODEL &CarModelList, const struct map_list *mapList, int mapNum);
 static bool ExitMap(const car_model *car, int mapId, const struct map_list *mapList, int mapNum);
 static int GetMapId(int index, const struct map_list *mapList, int mapNum);
 static int GetMapType(int index, const struct map_list *mapList, int mapNum);
@@ -650,7 +645,7 @@
                 RtkBuffer[index].mm, RtkBuffer[index].ss, RtkBuffer[index].dss);
 
         brief.qf = RtkBuffer[index].qf;
-        brief.map_id = GetMapId(CurrExamMapIndex, MapList, MapNum);
+        brief.map_id = -1;//GetMapId(CurrExamMapIndex, MapList, MapNum);
         brief.move = move;
         brief.speed = speed * 3.6;
         brief.heading = RtkBuffer[index].heading;
@@ -712,18 +707,18 @@
     }
     if (CurrExamMapIndex < 0) {
         if (CurrEnterMapIndex < 0) {
-            CurrEnterMapIndex = EnterMap(CarModel, MapList, MapNum);
+            CurrEnterMapIndex = EnterMap(CarModel, CarModelList, MapList, MapNum);
             if (CurrEnterMapIndex >= 0) {
-                DEBUG("鍙戦�佽繘鍏ュ満鍦版姤鍛� %d", GetMapId(CurrEnterMapIndex, MapList, MapNum));
-                MA_EnterMap(GetMapId(CurrEnterMapIndex, MapList, MapNum), GetMapType(CurrEnterMapIndex, MapList, MapNum), 1);
+//                DEBUG("鍙戦�佽繘鍏ュ満鍦版姤鍛� %d", GetMapId(CurrEnterMapIndex, MapList, MapNum));
+//                MA_EnterMap(GetMapId(CurrEnterMapIndex, MapList, MapNum), GetMapType(CurrEnterMapIndex, MapList, MapNum), 1);
 
                 CurrExamMapIndex = CurrEnterMapIndex;
                 CurrExamStatus = EXAM_AREA_START;
             }
         } else {
             if (ExitMap(CarModel, CurrEnterMapIndex, MapList, MapNum)) {
-                DEBUG("鍙戦�佺寮�鍦哄湴鎶ュ憡 %d", GetMapId(CurrEnterMapIndex, MapList, MapNum));
-                MA_EnterMap(GetMapId(CurrEnterMapIndex, MapList, MapNum), GetMapType(CurrEnterMapIndex, MapList, MapNum), 0);
+//                DEBUG("鍙戦�佺寮�鍦哄湴鎶ュ憡 %d", GetMapId(CurrEnterMapIndex, MapList, MapNum));
+//                MA_EnterMap(GetMapId(CurrEnterMapIndex, MapList, MapNum), GetMapType(CurrEnterMapIndex, MapList, MapNum), 0);
                 CurrEnterMapIndex = -1;
             }
         }
@@ -894,33 +889,33 @@
                     DEBUG("杩涘叆鍊掕溅鍏ュ簱鍦哄湴");
                     MA_SendDebugInfo("杩涘叆鍊掕溅鍏ュ簱鍦哄湴 %d", GetMapId(CurrExamMapIndex, MapList, MapNum));
 
-                    StartParkBottom(move, rtkTime);
+                    StartParkBottom(CurrExamMapIndex, move, rtkTime);
                     CurrExamStatus = EXAM_AREA_RUN;
                     break;
                 case MAP_TYPE_STOP_START:
                     DEBUG("杩涘叆涓婂潯璧锋鍦哄湴");
                     MA_SendDebugInfo("杩涘叆涓婂潯璧锋鍦哄湴 %d", GetMapId(CurrExamMapIndex, MapList, MapNum));
-                    StartSAS(move, rtkTime);
+                    StartSAS(CurrExamMapIndex, move, rtkTime);
                     CurrExamStatus = EXAM_AREA_RUN;
                     break;
                 case MAP_TYPE_PART_EDGE:
                     DEBUG("杩涘叆渚ф柟浣嶅仠杞﹀満鍦�");
                     MA_SendDebugInfo("杩涘叆渚ф柟浣嶅仠杞﹀満鍦� %d", GetMapId(CurrExamMapIndex, MapList, MapNum));
-                    StartParkEdge(move, rtkTime);
+                    StartParkEdge(CurrExamMapIndex, move, rtkTime);
 
                     CurrExamStatus = EXAM_AREA_RUN;
                     break;
                 case MAP_TYPE_CURVE:
                     DEBUG("杩涘叆鏇茬嚎琛岄┒鍦哄湴");
                     MA_SendDebugInfo("杩涘叆鏇茬嚎琛岄┒鍦哄湴 %d", GetMapId(CurrExamMapIndex, MapList, MapNum));
-                    StartDrivingCurve(move, rtkTime);
+                    StartDrivingCurve(CurrExamMapIndex, move, rtkTime);
 
                     CurrExamStatus = EXAM_AREA_RUN;
                     break;
                 case MAP_TYPE_TURN_90:
                     DEBUG("杩涘叆鐩磋杞集鍦哄湴");
                     MA_SendDebugInfo("杩涘叆鐩磋杞集鍦哄湴 %d", GetMapId(CurrExamMapIndex, MapList, MapNum));
-                    StartTurnA90(move, azimuth, rtkTime);
+                    StartTurnA90(CurrExamMapIndex, move, azimuth, rtkTime);
                     CurrExamStatus = EXAM_AREA_RUN;
                     break;
                 default:break;
@@ -1145,15 +1140,79 @@
     return true;
 }
 
-static int EnterMap(const car_model *car, const struct map_list *mapList, int mapNum)
+static bool CrashTriggerLine(Line triggerLine, const car_model *car, LIST_CAR_MODEL &CarModelList)
 {
-    // 杞︾殑鏈�鍓嶇偣鏄惁杩涘叆鍦板浘
+    bool trigger = false;
+
+    if (CarModelList.size() < 5)
+        return trigger;
+
+    Polygon trace, trace2;
+    int pn = 0;
+
+    trace2.num = trace.num = 5;
+    trace.point = (PointF *) malloc(sizeof(PointF) * trace.num);
+    trace2.point = (PointF *) malloc(sizeof(PointF) * trace2.num);
+
+    list<car_model *>::iterator iter = CarModelList.begin();
+
+    car_model *c1 = *iter;
+
+    trace.point[pn] = c1->carXY[c1->left_front_tire[TIRE_OUTSIDE]];
+    trace2.point[pn++] = c1->carXY[c1->left_rear_tire[TIRE_OUTSIDE]];
+
+    ++iter;
+
+    while (iter != CarModelList.end() && pn < trace.num) {
+        car_model *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 >= D_SEC(1)) {
+            trace.point[pn] = c2->carXY[c2->left_front_tire[TIRE_OUTSIDE]];
+            trace2.point[pn++] = c2->carXY[c2->left_rear_tire[TIRE_OUTSIDE]];
+            c1 = c2;
+        }
+        ++iter;
+    }
+
+    PointF p1, p2;
+
+    p1.X = triggerLine.X1;
+    p1.Y = triggerLine.Y1;
+    p2.X = triggerLine.X2;
+    p2.Y = triggerLine.Y2;
+
+    int pp = 0;
+    for (int p = 1; p < pn; ++p) {
+        Line trace_line, trace2_line;
+
+        MakeLine(&trace_line, &trace.point[pp], &trace.point[p]);
+        MakeLine(&trace2_line, &trace2.point[pp], &trace2.point[p]);
+
+        if ((IntersectionOf(trace_line, triggerLine) == GM_Intersection || IntersectionOf(trace2_line, triggerLine) == GM_Intersection) &&
+            IntersectionOfLine(p1, p2, car->carXY[car->left_front_tire[TIRE_OUTSIDE]]) == -1 &&
+            DistanceOf(car->carXY[car->left_front_tire[TIRE_OUTSIDE]], triggerLine) > 0.1) {
+            // 纰板埌瑙﹀彂绾�
+            DEBUG("纰版挒瑙﹀彂绾� 寮曞彂鍦板浘");
+            trigger = true;
+            goto SEARCH_TRIGGER_LINE_END;
+        }
+    }
+
+SEARCH_TRIGGER_LINE_END:
+    free(trace.point);
+
+    return trigger;
+}
+
+static int EnterMap(const car_model *car, LIST_CAR_MODEL &CarModelList, const struct map_list *mapList, int mapNum)
+{
     for (int i = 0; i < mapNum && car != NULL; ++i) {
+        // 杞﹀墠杞垨鍚庤疆杞ㄨ抗瓒婅繃瑙﹀彂绾�
         if (mapList[i].type == MAP_TYPE_STOP_START) {
             // 鏋勯�犺櫄鎷熺殑宸︿笂瑙掔偣
             double x9, y9, xo, yo;
-
-            bool enter = false;
 
             xo = (mapList[i].map.point[0].X + mapList[i].map.point[7].X) / 2;
             yo = (mapList[i].map.point[0].Y + mapList[i].map.point[7].Y) / 2;
@@ -1161,31 +1220,18 @@
             x9 = 2*xo - mapList[i].map.point[8].X;
             y9 = 2*yo - mapList[i].map.point[8].Y;
 
-            Polygon map;
+            Line triggerLine;
 
-            map.num = 4;
-            map.point = (PointF *) malloc(map.num * sizeof(PointF));
+            triggerLine.X1 = mapList[i].map.point[0].X;
+            triggerLine.Y1 = mapList[i].map.point[0].Y;
+            triggerLine.X2 = x9;
+            triggerLine.Y2 = y9;
 
-            map.point[0] = mapList[i].map.point[0];
-            map.point[1] = mapList[i].map.point[8];
-            map.point[2] = mapList[i].map.point[7];
-            map.point[3].X = x9;
-            map.point[3].Y = y9;
-
-            if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &map) == GM_Containment) {
-                Line enterLine1;
-
-                MakeLine(&enterLine1, &(map.point[0]), &(map.point[3]));
-
-                if (DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], enterLine1) > 0.1)
-                    enter = true;
-            }
-
-            free(map.point);
-
-            if (enter) return i;
+            if (CrashTriggerLine(triggerLine, car, CarModelList))
+                return i;
         }
         if (mapList[i].type == MAP_TYPE_PARK_BUTTOM) {
+            // 杞﹀ご椤剁偣鍦ㄥ満鍦板唴
             if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &mapList[i].map) == GM_Containment) {
                 Line enterLine1, enterLine2;
 
@@ -1198,6 +1244,7 @@
             }
         }
         if (mapList[i].type == MAP_TYPE_PART_EDGE) {
+            // 杞﹀ご椤剁偣鍦ㄥ満鍦板唴
             if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &mapList[i].map) == GM_Containment) {
                 Line enterLine;
 
@@ -1208,26 +1255,20 @@
             }
         }
         if (mapList[i].type == MAP_TYPE_TURN_90) {
-            if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &mapList[i].map) == GM_Containment) {
-                Line enterLine1;
+            // 杞﹀墠杞垨鍚庤疆杞ㄨ抗瓒婅繃瑙﹀彂绾�
+            Line triggerLine;
 
-                MakeLine(&enterLine1, &(mapList[i].map.point[0]), &(mapList[i].map.point[1]));
+            MakeLine(&triggerLine, &(mapList[i].map.point[0]), &(mapList[i].map.point[1]));
 
-                if (DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], enterLine1) > 0.1)
-                    return i;
-            }
+            if (CrashTriggerLine(triggerLine, car, CarModelList))
+                return i;
         }
         if (mapList[i].type == MAP_TYPE_CURVE) {
-            Line startLine;
-            Line carAxial;
+            Line triggerLine;
 
-            MakeLine(&startLine, &mapList[i].map.point[0], &mapList[i].map2.point[0]);
-            MakeLine(&carAxial, &car->carXY[car->left_front_tire[TIRE_OUTSIDE]], &car->carXY[car->left_rear_tire[TIRE_OUTSIDE]]);
-
-            if (IntersectionOf(startLine, carAxial) == GM_Intersection) {
-                if (DistanceOf(car->carXY[car->left_front_tire[TIRE_OUTSIDE]], startLine) > 0.1)
-                    return i;
-            }
+            MakeLine(&triggerLine, &mapList[i].map2.point[0], &mapList[i].map.point[0]);
+            if (CrashTriggerLine(triggerLine, car, CarModelList))
+                return i;
         }
     }
     return -1;
diff --git a/lib/src/main/cpp/driver_test.h b/lib/src/main/cpp/driver_test.h
index 684e996..0cc5aa0 100644
--- a/lib/src/main/cpp/driver_test.h
+++ b/lib/src/main/cpp/driver_test.h
@@ -21,7 +21,13 @@
 #define TIRE_OUTSIDE    0
 #define TIRE_INSIDE     1
 #define AXIAL_FRONT     0
-#define AXIAL_REAR     1
+#define AXIAL_REAR      1
+
+#define MAP_TYPE_PARK_BUTTOM        1
+#define MAP_TYPE_STOP_START         2
+#define MAP_TYPE_PART_EDGE          3
+#define MAP_TYPE_CURVE              4
+#define MAP_TYPE_TURN_90            5
 
 using namespace std;
 
diff --git a/lib/src/main/cpp/master/comm_if.cpp b/lib/src/main/cpp/master/comm_if.cpp
index ceaf567..90b4b1a 100644
--- a/lib/src/main/cpp/master/comm_if.cpp
+++ b/lib/src/main/cpp/master/comm_if.cpp
@@ -60,12 +60,15 @@
 #define ID_MS_SYS_SHUTDOWN      0x8010
 #define ID_SM_LIGHT_EXAM_REQ    0x0012
 #define ID_MS_LIGHT_EXAM_RES    0x8012
+#define ID_SM_DISTANCE              0x0020
+
 
 #define MA_OUT_GPS_BRIEF        0x0001
 #define MA_OUT_RTK_BRIEF        0x0002
 #define MA_OUT_CAR_BRIEF        0x0004
 #define MA_OUT_RTCM_IND        0x0008
 #define MA_OUT_DBG_INFO        0x0010
+
 
 static int OnOff = 0;//0xFFFF;
 
@@ -352,6 +355,21 @@
     SendMsgToMainProcIndep(ID_SM_RTK_BRIEF, sb.GetString());
 }
 
+void MA_SendDistance(double l, double r)
+{
+    StringBuffer sb;
+    Writer<StringBuffer> writer(sb);
+
+    writer.StartObject();
+    writer.Key("left");
+    writer.Double(l);
+    writer.Key("right");
+    writer.Double(r);
+    writer.EndObject();
+
+    SendMsgToMainProcIndep(ID_SM_DISTANCE, sb.GetString());
+}
+
 void MA_SendExamWrong(vector<ExamFault> &ExamFaultList)
 {
     StringBuffer sb;
diff --git a/lib/src/main/cpp/master/comm_if.h b/lib/src/main/cpp/master/comm_if.h
index 8e23f95..80d93a9 100644
--- a/lib/src/main/cpp/master/comm_if.h
+++ b/lib/src/main/cpp/master/comm_if.h
@@ -97,4 +97,6 @@
 void MA_EnterMap(int map_id, int type, int enter);
 void MA_ExamLight(void);
 
+void MA_SendDistance(double l, double r);
+
 #endif //MYAPPLICATION2_COMM_IF_H
diff --git a/lib/src/main/cpp/test_items/driving_curve.cpp b/lib/src/main/cpp/test_items/driving_curve.cpp
index 7a33a68..24ad383 100644
--- a/lib/src/main/cpp/test_items/driving_curve.cpp
+++ b/lib/src/main/cpp/test_items/driving_curve.cpp
@@ -7,6 +7,8 @@
 #include "../driver_test.h"
 #include "../common/apptimer.h"
 #include "../utils/xconvert.h"
+#include "../master/comm_if.h"
+#include "../native-lib.h"
 
 #include <vector>
 #include <cstdlib>
@@ -18,6 +20,7 @@
 const uint32_t STOP_CAR_TIME = D_SEC(2);
 
 static bool testing = false;
+static int mapIndex = 0;
 static uint32_t stopTimepoint = 0;
 
 static bool reportStopCarTimeout;
@@ -32,13 +35,14 @@
 
 static bool UpdateStartLine(struct scan_window_t *zone, const Polygon *map, const Polygon *map2, const Polygon *tireRect);
 static bool UpdateEndLine(bool mode, struct scan_window_t *zone, const Polygon *map, const Polygon *map2, const Polygon *tireRect);
-static bool CrashRedLine(const Polygon *map, const Polygon *map2, const car_model *car, struct scan_window_t *zone);
+static bool CrashRedLine(const Polygon *map, const Polygon *map2, const car_model *car, struct scan_window_t *zone, int &who);
 
-void StartDrivingCurve(int moveDirect, const struct RtkTime *rtkTime)
+void StartDrivingCurve(int index, int moveDirect, const struct RtkTime *rtkTime)
 {
     DEBUG("杩涘叆鏇茬嚎琛岄┒鍦哄湴");
 
     testing = true;
+    mapIndex = index;
 
     prevMoveDirect = moveDirect;
     if (moveDirect == 0) {
@@ -48,12 +52,14 @@
     crashRedLine = false;
 
     scanWindow.leftStart = scanWindow.leftEnd = scanWindow.rightStart = scanWindow.rightEnd = 0;
+
+    MA_EnterMap(mapIndex, MAP_TYPE_CURVE, 1);
 }
 
 int TestDrivingCurve(const Polygon *map, const Polygon *map2, const car_model *car, const car_model *carPrev, double speed, int moveDirect, const struct RtkTime *rtkTime)
 {
     Polygon tireRect;
-
+    int who = 0;
     MakePolygon(&tireRect, {car->carXY[car->left_front_tire[TIRE_OUTSIDE]],
                             car->carXY[car->right_front_tire[TIRE_OUTSIDE]],
                             car->carXY[car->right_rear_tire[TIRE_OUTSIDE]],
@@ -71,12 +77,17 @@
 
     DEBUG("scanWindow leftStart %d leftEnd %d rightStart %d rightEnd %d", scanWindow.leftStart, scanWindow.leftEnd, scanWindow.rightStart, scanWindow.rightEnd);
 
-    if (CrashRedLine(map, map2, car, &scanWindow)) {
+    if (CrashRedLine(map, map2, car, &scanWindow, who)) {
         if (!crashRedLine) {
             crashRedLine = true;
             // 杞﹁疆鍘嬭竟绾匡紝涓嶅悎鏍�
             AddExamFault(27, rtkTime);
             DEBUG("杞﹁疆鍘嬭竟绾�");
+            if (who == 1) {
+                PlayTTS("鍘嬪乏鏇茬嚎");
+            } else if (who == 2) {
+                PlayTTS("鍘嬪彸鏇茬嚎");
+            }
         }
     } else {
         crashRedLine = false;
@@ -105,6 +116,11 @@
 
 TEST_END:
     CleanPolygon(&tireRect);
+
+    if (!testing) {
+        MA_EnterMap(mapIndex, MAP_TYPE_CURVE, 0);
+    }
+
     return testing ? 1 : 0;
 }
 
@@ -276,7 +292,7 @@
 }
 
 // 杞﹁疆鏄惁鍘嬭竟绾�
-static bool CrashRedLine(const Polygon *map, const Polygon *map2, const car_model *car, struct scan_window_t *zone)
+static bool CrashRedLine(const Polygon *map, const Polygon *map2, const car_model *car, struct scan_window_t *zone, int &who)
 {
     bool ret = false;
 
@@ -290,9 +306,11 @@
     for (int e = zone->leftStart - 1; e >= zone->leftEnd; --e) {
         MakeLine(&redLine, &map->point[s], &map->point[e]);
         if (IntersectionOf(redLine, frontTireAxial) != GM_None) {
+            who = 1;
             return true;
         }
         if (IntersectionOf(redLine, rearTireAxial) != GM_None) {
+            who = 1;
             return true;
         }
         s = e;
@@ -302,9 +320,11 @@
     for (int e = zone->rightStart - 1; e >= zone->rightEnd; --e) {
         MakeLine(&redLine, &map2->point[s], &map2->point[e]);
         if (IntersectionOf(redLine, frontTireAxial) != GM_None) {
+            who = 2;
             return true;
         }
         if (IntersectionOf(redLine, rearTireAxial) != GM_None) {
+            who = 2;
             return true;
         }
         s = e;
diff --git a/lib/src/main/cpp/test_items/driving_curve.h b/lib/src/main/cpp/test_items/driving_curve.h
index 22c7181..e9f39fc 100644
--- a/lib/src/main/cpp/test_items/driving_curve.h
+++ b/lib/src/main/cpp/test_items/driving_curve.h
@@ -10,7 +10,7 @@
 
 using namespace std;
 
-void StartDrivingCurve(int moveDirect, const struct RtkTime *rtkTime);
+void StartDrivingCurve(int index, int moveDirect, const struct RtkTime *rtkTime);
 bool ExitDrivingCurveArea(const Polygon *map, const Polygon *map2, const car_model *car);
 
 int TestDrivingCurve(const Polygon *map, const Polygon *map2, const car_model *car, const car_model *carPrev, double speed, int moveDirect, const struct RtkTime *rtkTime);
diff --git a/lib/src/main/cpp/test_items/park_bottom.cpp b/lib/src/main/cpp/test_items/park_bottom.cpp
index b0a598b..3a59252 100644
--- a/lib/src/main/cpp/test_items/park_bottom.cpp
+++ b/lib/src/main/cpp/test_items/park_bottom.cpp
@@ -9,6 +9,7 @@
 #include "../jni_log.h"
 #include "../driver_test.h"
 #include "../utils/xconvert.h"
+#include "../master/comm_if.h"
 #include <vector>
 #include <cstdlib>
 
@@ -27,7 +28,8 @@
 
 const int PARK_TIMEOUT = D_SEC(210);
 
-static bool testing = false;
+static bool testing = false, reverseCar = false;
+static int mapIndex = 0;
 
 const uint32_t STOP_CAR_TIME = D_SEC(2);
 static uint32_t stopTimepoint;
@@ -44,13 +46,15 @@
 
 static char CrossCtrlLine(const Polygon *map, const car_model *car, const car_model *prev_car);
 static bool EnterParking(const Polygon *map, const car_model *car);
-static bool CrashRedLine(const Polygon *map, const car_model *car);
+static bool CrashRedLine(const Polygon *map, const car_model *car, int &who);
 static bool ExitParkArea(const Polygon *map, const car_model *car);
 
-void StartParkBottom(int moveDirect, const struct RtkTime *rtkTime)
+void StartParkBottom(int index, int moveDirect, const struct RtkTime *rtkTime)
 {
     DEBUG("StartParkBottom");
     testing = true;
+    reverseCar = false;
+    mapIndex = index;
     memset(carray, 0, sizeof(carray));
     memset(darray, 0, sizeof(darray));
     memset(parkStatus, 0, sizeof(parkStatus));
@@ -69,12 +73,13 @@
 {
     char crossCtrlLine = 0;
     uint32_t tp = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
+    int who = 0;
 
     if (ExitParkArea(map, car)) {
         DEBUG("绂诲紑鍦哄湴");
         // 绂诲紑鍦哄湴
         testing = false;
-        if ((parkStatus[0] != 1 || parkStatus[1] != 1) && !reportParkFail) {
+        if ((parkStatus[0] != 1 || parkStatus[1] != 1) && !reportParkFail && reverseCar) {
             // 鍊掑簱涓嶅叆锛屼笉鍚堟牸
             reportParkFail = true;
             AddExamFault(8, rtkTime);
@@ -83,12 +88,18 @@
         goto TEST_END;
     }
 
-    if (CrashRedLine(map, car)) {
+
+    if (CrashRedLine(map, car, who)) {
         if (!occurCrashRedLine) {
             occurCrashRedLine = true;
             // 杞﹁韩鍑虹嚎锛屼笉鍚堟牸
             AddExamFault(7, rtkTime);
             DEBUG("杞﹁疆鍘嬬嚎");
+            if (who == 1) {
+                PlayTTS("鍘嬪乏搴撲綅绾�");
+            } else if (who == 2) {
+                PlayTTS("鍘嬪彸搴撲綅绾�");
+            }
         }
     } else {
         occurCrashRedLine = false;
@@ -153,6 +164,10 @@
                 }
             } else if (moveDirect == -1) {
                 // 鍒囨崲涓哄�掕溅
+                if (!reverseCar) {
+                    reverseCar = true;
+                    MA_EnterMap(mapIndex, MAP_TYPE_PARK_BUTTOM, 1);
+                }
                 if (darray[parkCount] == 0) {
                     if (!crossCtrlLineSw) {
                         // 鍊掕溅鍓嶏紝鍓嶈疆鏈┒杩囨帶鍒剁嚎
@@ -160,7 +175,7 @@
                         DEBUG("鍊掕溅鍓嶏紝鍓嶈疆鏈┒杩囨帶鍒剁嚎");
                     }
                     darray[parkCount] = 1;
-                    firstReverseTimepoint = tp;
+                    firstReverseTimepoint = tp;         // 棣栨鍊掕溅锛屾墠鎰忓懗鐫�姝ら」鐩紑濮�
                 }
             } else {
                 // 鍒囨崲涓哄墠杩�
@@ -209,6 +224,9 @@
     }
 
 TEST_END:
+    if (!testing && reverseCar) {
+        MA_EnterMap(mapIndex, MAP_TYPE_PARK_BUTTOM, 0);
+    }
     return testing ? 1 : 0;
 }
 
@@ -258,7 +276,7 @@
     return succ;
 }
 
-static bool CrashRedLine(const Polygon *map, const car_model *car)
+static bool CrashRedLine(const Polygon *map, const car_model *car, int &who)
 {
     bool ret = false;
 
@@ -276,6 +294,12 @@
     for (int i = 0; i < sizeof(red_lines) / sizeof(red_lines[0]); ++i) {
         MakeLine(&red_line, &(map->point[red_lines[i][0]]), &(map->point[red_lines[i][1]]));
         if (IntersectionOf(red_line, &car_body) != GM_None) {
+            if (i == 2 || i == 1) {
+                who = 1;
+            } else if (i == 4 || i == 5) {
+                who = 2;
+            }
+
             ret = true;
             break;
         }
diff --git a/lib/src/main/cpp/test_items/park_bottom.h b/lib/src/main/cpp/test_items/park_bottom.h
index be36967..69cd14e 100644
--- a/lib/src/main/cpp/test_items/park_bottom.h
+++ b/lib/src/main/cpp/test_items/park_bottom.h
@@ -11,7 +11,7 @@
 
 using namespace std;
 
-void StartParkBottom(int moveStatus, const struct RtkTime *rtkTime);
+void StartParkBottom(int index, int moveStatus, const struct RtkTime *rtkTime);
 int TestParkBottom(const Polygon *map, const car_model *car, const car_model *carPrev, double speed, int moveStatus, const struct RtkTime *rtkTime);
 
 #endif //RTKDRIVERTEST_PARK_BOTTOM_H
diff --git a/lib/src/main/cpp/test_items/park_edge.cpp b/lib/src/main/cpp/test_items/park_edge.cpp
index 207d9e7..921ce34 100644
--- a/lib/src/main/cpp/test_items/park_edge.cpp
+++ b/lib/src/main/cpp/test_items/park_edge.cpp
@@ -10,6 +10,7 @@
 #include "../native-lib.h"
 #include "../utils/xconvert.h"
 #include "../test_common/car_sensor.h"
+#include "../master/comm_if.h"
 
 #include <vector>
 #include <cstdlib>
@@ -21,7 +22,10 @@
 const int PARK_TIMEOUT = D_SEC(90);
 const uint32_t STOP_CAR_TIME = D_SEC(2);
 
+static int mapIndex = 0;
 static bool reportExamTimeout;
+static bool reportParkFail;
+
 static uint32_t stopTimepoint = 0;
 static bool occurCrashRedLine1, occurCrashRedLine2;
 static int prevMoveStatus, storeMoveStatusBeforeStop;
@@ -36,15 +40,16 @@
 static bool ExitParkArea(const Polygon *map, const car_model *car);
 static bool ExitParkArea2(const Polygon *map, const car_model *car);
 
-void StartParkEdge(int moveStatus, const struct RtkTime *rtkTime)
+void StartParkEdge(int index, int moveStatus, const struct RtkTime *rtkTime)
 {
     DEBUG("杩涘叆渚ф柟鍋滆溅鍦哄湴");
 
     testing = true;
+    mapIndex = index;
 
     occurCrashRedLine1 = occurCrashRedLine2 = false;        // 杩欎釜绉戠洰瑙勫畾鐗规畩鐐癸紝鍙戠敓涓�娆℃墸10鍒嗭紝鑰屼笉鐩存帴娣樻卑
     reportExamTimeout = false;
-
+    reportParkFail = false;
     prevMoveStatus = moveStatus;
     parkSuccess = false;
     parkStatus = 0;
@@ -80,9 +85,10 @@
     }
 
     if (ExitParkArea(map, car) || ExitParkArea2(map, car)) {
-        if (!parkSuccess || !occurMoveBack) {
+        if (!parkSuccess && occurMoveBack && !reportParkFail) {
             // 鐩存帴椹剁娴嬭瘯鍖猴紝璁や负绉诲簱涓嶅叆
             AddExamFault(21, rtkTime);
+            reportParkFail = true;
             DEBUG("鐩存帴椹剁娴嬭瘯鍖�,绉诲簱涓嶅叆");
         }
         testing = false;
@@ -130,9 +136,10 @@
                         }
                     }
 
-                    if (!parkSuccess) {
+                    if (!parkSuccess && !reportParkFail) {
                         // 鍋滄鍚庯紝杞﹁韩鍑虹嚎锛屼笉鍚堟牸
                         AddExamFault(21, rtkTime);
+                        reportParkFail = true;
                         DEBUG("绉诲簱涓嶅叆");
                     }
 
@@ -149,6 +156,7 @@
                 DEBUG("寮�濮嬪�掕溅");
                 occurMoveBack = true;
                 moveBackTimePoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
+                MA_EnterMap(mapIndex, MAP_TYPE_PART_EDGE, 1);
             }
         }
 
@@ -165,7 +173,11 @@
             }
         }
     }
+
 TEST_END:
+    if (!testing && occurMoveBack) {
+        MA_EnterMap(mapIndex, MAP_TYPE_PART_EDGE, 0);
+    }
     return testing ? 1 : 0;
 }
 
diff --git a/lib/src/main/cpp/test_items/park_edge.h b/lib/src/main/cpp/test_items/park_edge.h
index 752e909..8916ff8 100644
--- a/lib/src/main/cpp/test_items/park_edge.h
+++ b/lib/src/main/cpp/test_items/park_edge.h
@@ -11,7 +11,7 @@
 
 using namespace std;
 
-void StartParkEdge(int moveStatus, const struct RtkTime *rtkTime);
+void StartParkEdge(int index, int moveStatus, const struct RtkTime *rtkTime);
 int TestParkEdge(const Polygon *map, const car_model *car, const car_model *carPrev, double speed, int moveStatus, const struct RtkTime *rtkTime);
 
 #endif //RTKDRIVERTEST_PARK_EDGE_H
diff --git a/lib/src/main/cpp/test_items/stop_and_start.cpp b/lib/src/main/cpp/test_items/stop_and_start.cpp
index 91a9b0b..b57ceec 100644
--- a/lib/src/main/cpp/test_items/stop_and_start.cpp
+++ b/lib/src/main/cpp/test_items/stop_and_start.cpp
@@ -12,6 +12,7 @@
 #include "../common/apptimer.h"
 #include "../utils/xconvert.h"
 #include "../test_common/car_sensor.h"
+#include "../master/comm_if.h"
 
 #define DEBUG(fmt, args...)     LOGD("<stop_and_start> <%s>: " fmt, __func__, ##args)
 
@@ -31,6 +32,7 @@
 
 static PointF stopPoint;
 
+static int mapIndex = 0;
 static int prevMoveDirect;
 static uint32_t stopTimepoint = 0;
 static bool stopCar = false;
@@ -46,11 +48,11 @@
 static double DistanceOfTire2Edge(const Polygon *map, const car_model *car);
 static bool ExitTestArea(const Polygon *map, const car_model *car);
 
-void StartSAS(int moveDirect, const struct RtkTime *rtkTime)
+void StartSAS(int index, int moveDirect, const struct RtkTime *rtkTime)
 {
     DEBUG("杩涘叆鍧¤捣椤圭洰");
     testing = true;
-
+    mapIndex = index;
     prevMoveDirect = moveDirect;
 
     if (moveDirect == 0) {
@@ -63,6 +65,8 @@
     slideNormalDistance = false;
     reportSlideFault = false;
     reportStartTimeout = false;
+
+    MA_EnterMap(mapIndex, MAP_TYPE_STOP_START, 1);
 }
 
 int TestSAS(const Polygon *map, const car_model *car, const car_model *carPrev, double speed, int moveDirect, const struct RtkTime *rtkTime)
@@ -81,6 +85,12 @@
     if (ExitTestArea(map, car)) {
         // 椹剁娴嬭瘯鍖�
         testing = false;
+    }
+
+    {
+        double dis2 = DistanceOfTire2Edge(map, car);
+
+        MA_SendDistance(-1, dis2);
     }
 
     if (prevMoveDirect != moveDirect) {
@@ -172,6 +182,10 @@
         }
     }
 
+    if (!testing) {
+        MA_EnterMap(mapIndex, MAP_TYPE_STOP_START, 0);
+    }
+
     return testing ? 1 : 0;
 }
 
@@ -245,10 +259,22 @@
 // 鏁翠釜杞﹁締閮借椹剁璇ユ祴璇曞尯鍩�
 static bool ExitTestArea(const Polygon *map, const car_model *car)
 {
-    // 鍦�8->7绾跨殑鍙充晶
-    for (int i = 0; i < car->bodyNum; ++i) {
-        if (IntersectionOfLine(map->point[8], map->point[7], car->carXY[car->body[i]]) != -1)
-            return false;
+    // 鍏ㄨ溅閮介渶涓嶅湪鍦板浘涓�
+    bool ret = false;
+
+    Polygon carBody;
+
+    carBody.num = car->bodyNum;
+    carBody.point = (PointF *)malloc(carBody.num * sizeof(PointF));
+    for (int i = 0; i < carBody.num; ++i) {
+        carBody.point[i] = car->carXY[car->body[i]];
     }
-    return true;
+
+    if (IntersectionOf(&carBody, map) == GM_None) {
+        ret = true;
+    }
+
+    free(carBody.point);
+
+    return ret;
 }
diff --git a/lib/src/main/cpp/test_items/stop_and_start.h b/lib/src/main/cpp/test_items/stop_and_start.h
index b3f69cb..894a7e3 100644
--- a/lib/src/main/cpp/test_items/stop_and_start.h
+++ b/lib/src/main/cpp/test_items/stop_and_start.h
@@ -9,7 +9,7 @@
 
 using namespace std;
 
-void StartSAS(int moveDirect, const struct RtkTime *rtkTime);
+void StartSAS(int index, int moveDirect, const struct RtkTime *rtkTime);
 int TestSAS(const Polygon *map, const car_model *car, const car_model *carPrev, double speed, int moveStatus, const struct RtkTime *rtkTime);
 
 #endif //RTKDRIVERTEST_STOP_AND_START_H
diff --git a/lib/src/main/cpp/test_items/turn_a90.cpp b/lib/src/main/cpp/test_items/turn_a90.cpp
index 6ee18df..4b3ba68 100644
--- a/lib/src/main/cpp/test_items/turn_a90.cpp
+++ b/lib/src/main/cpp/test_items/turn_a90.cpp
@@ -10,6 +10,7 @@
 #include "../utils/xconvert.h"
 #include "../defs.h"
 #include "../test_common/car_sensor.h"
+#include "../master/comm_if.h"
 
 #include <vector>
 #include <cstdlib>
@@ -21,6 +22,7 @@
 const uint32_t STOP_CAR_TIME = D_SEC(2);
 
 static bool testing;
+static int mapIndex;
 
 static int enterAreaHeading;
 static bool turnLeftFinished;
@@ -33,7 +35,7 @@
 static bool CrashRedLine(const Polygon *map, const car_model *car);
 static bool ExitTestArea(const Polygon *map, const car_model *car);
 
-void StartTurnA90(int moveDirect, double heading, const struct RtkTime *rtkTime)
+void StartTurnA90(int index, int moveDirect, double heading, const struct RtkTime *rtkTime)
 {
     DEBUG("杩涘叆鐩磋杞集鍦哄湴");
     testing = true;
@@ -45,6 +47,9 @@
     reportStopCarTimeout = false;
     crashRedLine = false;
     turnLeftFinished = false;
+    mapIndex = index;
+
+    MA_EnterMap(mapIndex, MAP_TYPE_TURN_90, 1);
 }
 
 int TestTurnA90(const Polygon *map, const car_model *car, const car_model *carPrev, double heading, double speed, int moveDirect, const struct RtkTime *rtkTime)
@@ -118,6 +123,11 @@
     if (turnLeftFinished) {
 
     }
+
+    if (!testing) {
+        MA_EnterMap(mapIndex, MAP_TYPE_TURN_90, 0);
+    }
+
 TEST_END:
     return testing? 1:0;
 }
diff --git a/lib/src/main/cpp/test_items/turn_a90.h b/lib/src/main/cpp/test_items/turn_a90.h
index f6e88b2..857285d 100644
--- a/lib/src/main/cpp/test_items/turn_a90.h
+++ b/lib/src/main/cpp/test_items/turn_a90.h
@@ -10,7 +10,7 @@
 
 using namespace std;
 
-void StartTurnA90(int moveDirect, double heading, const struct RtkTime *rtkTime);
+void StartTurnA90(int index, int moveDirect, double heading, const struct RtkTime *rtkTime);
 int TestTurnA90(const Polygon *map, const car_model *car, const car_model *carPrev, double heading, double speed, int moveDirect, const struct RtkTime *rtkTime);
 
 #endif //RTKDRIVERTEST_TURN_A90_H
diff --git a/lib/src/main/cpp/test_items2/drive_straight.cpp b/lib/src/main/cpp/test_items2/drive_straight.cpp
index 16a8832..8db23e0 100644
--- a/lib/src/main/cpp/test_items2/drive_straight.cpp
+++ b/lib/src/main/cpp/test_items2/drive_straight.cpp
@@ -7,6 +7,7 @@
 #include "../native-lib.h"
 #include "../jni_log.h"
 #include "road_exam.h"
+#include "../master/comm_if.h"
 #include <cmath>
 
 #define DEBUG(fmt, args...)     LOGD("<drive_straight> <%s>: " fmt, __func__, ##args)
@@ -43,6 +44,14 @@
 
     MakeLine(&road_edge, &RoadMapList[index].roadEdgeLine[0].point[0], &RoadMapList[index].roadEdgeLine[0].point[1]);
 
+    {
+        double l1 = DistanceOf(car->carXY[car->right_front_tire[TIRE_OUTSIDE]], road_edge);
+
+        double l2 = DistanceOf(car->carXY[car->right_rear_tire[TIRE_OUTSIDE]], road_edge);
+
+        MA_SendDistance(-1, (l1+l2)/2.0);
+    }
+
     if (!crossStartLine) {
         PointF p1, p2;
 

--
Gitblit v1.8.0