From 682b17ff66dff23e03c6a57de276ea0c3e670c0e Mon Sep 17 00:00:00 2001
From: yy1717 <fctom1215@outlook.com>
Date: 星期一, 23 三月 2020 18:44:47 +0800
Subject: [PATCH] 起步和直线

---
 lib/src/main/cpp/test_items2/road_exam.cpp |  127 ++++++++++++++++++++++++++++++++++++------
 1 files changed, 109 insertions(+), 18 deletions(-)

diff --git a/lib/src/main/cpp/test_items2/road_exam.cpp b/lib/src/main/cpp/test_items2/road_exam.cpp
index ae82561..b158dd5 100644
--- a/lib/src/main/cpp/test_items2/road_exam.cpp
+++ b/lib/src/main/cpp/test_items2/road_exam.cpp
@@ -12,6 +12,7 @@
 #include "../native-lib.h"
 #include "through_something.h"
 #include "../master/comm_if.h"
+#include "drive_straight.h"
 
 #include <vector>
 #include <list>
@@ -23,6 +24,12 @@
 using namespace std;
 
 #define TURN_CHECK_CNT          4
+
+enum {
+    START_CAR_NOT_DO,
+    START_CAR_DOING,
+    START_CAR_DONE
+};
 
 static const int TURN_THRESHOLD = 5;
 static const int TURN_CHECK_INTERVAL = 500;
@@ -40,7 +47,7 @@
 static int prevMoveDirect;
 static uint32_t stopTimepoint = 0;
 static bool reportStopCarOnRedArea;
-static PointF stopPoint;
+static PointF stopPoint, startPoint;
 static bool prevGearError = false;
 static bool prevGearNSlide = false;
 
@@ -61,8 +68,12 @@
 static int gearErrorTime;
 static int gearNSlideTime;
 
+static int startCar;
 static int currExamMapIndex;
 
+static const int MAX_ENGINE_RPM = 2500;
+static const double START_CAR_MOVE_DISTANCE = 0.5;//10.0;
+static const double START_CAR_CHECK_DOOR_DISTANCE = 0.1;//1.0;
 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);
@@ -73,8 +84,9 @@
 static const double MAX_SPEED = 40.0 * 1000.0 / 3600.0;
 static const int SPEED_GEAR_TABLE[][2] = {{0, 20}, {5, 30}, {15, 40}, {25, 10000}, {35, 10000}};
 
+static void TestRoadStartCar(const car_model *car, double speed, int moveDirect, const struct RtkTime *rtkTime);
 static void Rtk2DriveTimer(struct drive_timer &tm, const struct RtkTime *rtkTime);
-static char isTurn(int currYaw, int prevYaw);
+static char isTurn(int currYaw, int prevYaw, int &ang);
 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);
@@ -107,10 +119,69 @@
     gearNSlideTime = 0;
 
     currExamMapIndex = -1;
+
+    startCar = START_CAR_NOT_DO;
+}
+
+static void TestRoadStartCar(const car_model *car, double speed, int moveDirect, const struct RtkTime *rtkTime)
+{
+    double moveDistance;
+    static bool checkDoor = false;
+    static bool handBreakActive = false;
+    static bool reportRPMOver = false;
+
+    if (startCar == START_CAR_NOT_DO) {
+        startPoint = car->basePoint;
+        reportRPMOver = false;
+        startCar = START_CAR_DOING;
+
+        PlayTTS("璇疯捣姝�");
+    } else if (startCar == START_CAR_DOING) {
+        moveDistance = DistanceOf(startPoint, car->basePoint);
+        if (moveDistance > START_CAR_MOVE_DISTANCE) {
+            
+            if (ReadCarStatus(HAND_BREAK) == BREAK_ACTIVE) {
+                DEBUG("Handbreak active move over 10m");
+                // 鎵嬪埞鎷夎捣鐘舵�佷笅锛岃椹朵簡10绫充互涓婏紝涓嶅悎鏍�
+                AddExamFault(25, rtkTime);
+            } else if (handBreakActive) {
+                // 鎵嬪埞鎷夎捣鐘舵�佷笅锛岃椹朵簡1绫充互涓婏紝鎵�10鍒�
+                DEBUG("Handbreak active move over 1M");
+                AddExamFault(26, rtkTime);
+            }
+            startCar = START_CAR_DONE;
+        } else if (moveDistance >= START_CAR_CHECK_DOOR_DISTANCE) {
+            if (!checkDoor) {
+                checkDoor = true;
+
+                if (ReadCarStatus(DOOR) == DOOR_OPEN) {
+                    // 杞﹂棬鏈畬鍏ㄥ叧闂紝涓嶅悎鏍�
+                    DEBUG("杞﹂棬鏈叧闂�");
+                    AddExamFault(23, rtkTime);
+                }
+
+                if (ReadCarStatus(HAND_BREAK) == BREAK_ACTIVE) {
+                    handBreakActive = true;
+                }
+            }
+        }
+
+        if (ReadCarStatus(ENGINE_RPM) > MAX_ENGINE_RPM && !reportRPMOver) {
+            // 杞�熻秴鏍囷紝涓嶅悎鏍�
+            DEBUG("杞�熻秴鏍�");
+            AddExamFault(29, rtkTime);
+            reportRPMOver = true;
+        }
+    } else {
+
+    }
 }
 
 void TestRoadGeneral(LIST_ROAD_MAP &RoadMapList, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime)
 {
+    // 璧锋妫�娴�
+    TestRoadStartCar(car, speed, moveDirect, rtkTime);
+
     // 瓒呴�熸娴�
     if (moveDirect != 0 && speed > MAX_SPEED) {
         if (!occurOverSpeed) {
@@ -266,7 +337,7 @@
     // 妫�鏌ユ槸鍚︽寔缁浆鍚�
     char turnDirect = CheckCarTurn(CarModelList);
     if (turnDirect == 'L') {
-        PlayTTS("宸�1", 5692);
+//        PlayTTS("宸�1");
         if (currTurnSignalStatus != LEFT_TURN_LIGHT) {
             if (!reportTurnSignalError) {
                 DEBUG("娌℃墦宸﹁浆鐏�");
@@ -284,7 +355,7 @@
             }
         }
     } else if (turnDirect == 'R') {
-        PlayTTS("鍙�1", 5692);
+//        PlayTTS("鍙�1");
         if (currTurnSignalStatus != RIGHT_TURN_LIGHT) {
             if (!reportTurnSignalError) {
                 DEBUG("娌℃墦鍙宠浆鐏�");
@@ -386,7 +457,7 @@
 
                 // 妫�鏌ュ彉閬撳墠锛屾槸鍚︽彁鍓嶈浆鍚戠伅
                 if (inter == 1) {
-                    PlayTTS("宸�2", 5698);
+//                    PlayTTS("宸�2");
                     // 鍚戝乏渚у彉閬�
                     DEBUG("鍚戝乏渚у彉閬�");
                     if (turnSignalStatusWhenCrashGreenLine != LEFT_TURN_LIGHT) {
@@ -395,7 +466,7 @@
                         AddExamFault(13, rtkTime);
                     }
                 } else {
-                    PlayTTS("鍙�2", 5698);
+//                    PlayTTS("鍙�2");
                     // 鍚戝彸渚у彉閬�
                     DEBUG("鍚戝彸渚у彉閬�");
                     if (turnSignalStatusWhenCrashGreenLine != RIGHT_TURN_LIGHT) {
@@ -410,20 +481,32 @@
         checkCrashGreenTimeout = 0;
     }
 
-    // 瑙﹀彂绾挎娴�
-    if (currExamMapIndex == -1) {
+    // 瀹屾垚璧锋鍚庯紝瑙﹀彂绾挎娴�
+    if (currExamMapIndex == -1 && startCar == START_CAR_DONE) {
         currExamMapIndex = CrashTriggerLine(RoadMapList, car, CarModelList);
         if (currExamMapIndex != -1) {
             DEBUG("纰版挒瑙﹀彂绾�");
 
             MA_EnterMap(RoadMapList[currExamMapIndex].id, RoadMapList[currExamMapIndex].type, 1);
-            StartThroughExam(currExamMapIndex, RoadMapList);
+
+            if (RoadMapList[currExamMapIndex].type >= THROUGH_INTERSECTION_MAP &&
+                    RoadMapList[currExamMapIndex].type <= TURN_AROUND_MAP) {
+                StartThroughExam(currExamMapIndex, RoadMapList);
+            } else if (RoadMapList[currExamMapIndex].type == DRIVE_STRAIGHT_MAP) {
+                StartDriveStraightExam(currExamMapIndex, RoadMapList);
+            }
         }
-    } else {
+    } else if (startCar == START_CAR_DONE) {
         int prevIdx = currExamMapIndex;
 
-        currExamMapIndex = ExecuteThroughExam(currExamMapIndex, RoadMapList, car,
-        CarModelList, speed, moveDirect, rtkTime);
+        if (currExamMapIndex >= THROUGH_INTERSECTION_MAP && currExamMapIndex <= TURN_AROUND_MAP) {
+            currExamMapIndex = ExecuteThroughExam(currExamMapIndex, RoadMapList, car,
+                                                  CarModelList, speed, moveDirect, rtkTime);
+        }
+        else if (currExamMapIndex == DRIVE_STRAIGHT_MAP) {
+            currExamMapIndex = ExecuteDriveStraightExam(currExamMapIndex, RoadMapList, car,
+                                     CarModelList, speed, moveDirect, rtkTime);
+        }
 
         if (currExamMapIndex == -1) {
             MA_EnterMap(RoadMapList[prevIdx].id, RoadMapList[prevIdx].type, 1);
@@ -521,10 +604,9 @@
     tm.msec = rtkTime->mss;
 }
 
-static char isTurn(int currYaw, int prevYaw)
+static char isTurn(int currYaw, int prevYaw, int &ang)
 {
 //    DEBUG("currYaw %d prevYaw %d", currYaw, prevYaw);
-
     int deltaAng = 0;
 
     if (ABS(currYaw - prevYaw) > 180) {
@@ -532,6 +614,8 @@
     } else {
         deltaAng = ABS(currYaw - prevYaw);
     }
+
+    ang = deltaAng;
 
 //    DEBUG("瑙掑害宸�� %d", deltaAng);
 
@@ -545,12 +629,12 @@
         }
     }
 
-    return 0;
+    return 'N';
 }
 
 static char CheckCarTurn(LIST_CAR_MODEL &CarModelList)
 {
-    // 鏈�杩�3绉掑唴锛屾瘡绉掔殑瑙掑害宸ぇ浜�10搴︼紝涓旀柟鍚戠浉鍚岋紝杩炵画3绉掞紝璁や负杞悜
+    // 鏈�杩�2绉掑唴锛屾瘡0.5绉掔殑瑙掑害宸ぇ浜�5搴︼紝涓旀柟鍚戠浉鍚岋紝杩炵画4娆★紱鎴栫獊鐜拌秴30搴︾殑杞悜锛涜涓鸿浆鍚戙��
     if (CarModelList.size() < 1)
         return false;
 
@@ -571,10 +655,15 @@
         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);
+            int ang = 0;
+            turn[checkCnt] = isTurn((int)c1->yaw, (int)c2->yaw, ang);
 //            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) {
+
+            if (turn[checkCnt] == 'N') {
                 break;
+            } else if (ang >= 30) {
+                DEBUG("宸﹀彸杞‘璁� %c", turn[checkCnt]);
+                return turn[checkCnt];
             }
 
             c1 = c2;
@@ -790,3 +879,5 @@
     }
     return -1;
 }
+
+

--
Gitblit v1.8.0