From 2d0741f8e56e4f24126ea8448551e64560c45b4e Mon Sep 17 00:00:00 2001
From: yy1717 <fctom1215@outlook.com>
Date: 星期三, 18 三月 2020 18:41:57 +0800
Subject: [PATCH] 通用测试

---
 lib/src/main/cpp/driver_test.cpp           |   22 ++-
 lib/src/main/cpp/test_items2/road_exam.cpp |  273 ++++++++++++++++++++++++++++++++++++++++++--
 lib/src/main/cpp/driver_test.h             |   24 ++-
 3 files changed, 286 insertions(+), 33 deletions(-)

diff --git a/lib/src/main/cpp/driver_test.cpp b/lib/src/main/cpp/driver_test.cpp
index 99eac50..58c440f 100644
--- a/lib/src/main/cpp/driver_test.cpp
+++ b/lib/src/main/cpp/driver_test.cpp
@@ -102,8 +102,6 @@
 
 static car_model *CarModel = NULL;
 
-typedef list<car_model *> LIST_CAR_MODEL;
-
 static LIST_CAR_MODEL CarModelList;             // 涓�娈垫椂闂寸殑杞﹁締杞ㄨ抗闆嗗悎
 
 static struct dummy_light_exam *DummyLightContent;
@@ -126,7 +124,7 @@
 static void ExecuteExam(double speed, int move, double azimuth, const struct RtkTime* rtkTime);
 static uint32_t CalcTimeDiff(const rtk_info *a, const rtk_info *b);
 static void ReadDriverExamPrimerTimeout(union sigval sig);
-static void UpdateCarBodyCoord(double azimuth, double pitch, double roll, PointF main_ant, car_model *carModel);
+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);
@@ -843,10 +841,20 @@
     main_ant_coord.X = RtkBuffer[p1].x;
     main_ant_coord.Y = RtkBuffer[p1].y;
 
-    UpdateCarBodyCoord(RtkBuffer[p1].heading, RtkBuffer[p1].pitch, RtkBuffer[p1].roll, main_ant_coord, CarModel);
+    struct RtkTime tm;
+
+    tm.YY = RtkBuffer[p1].YY;
+    tm.MM = RtkBuffer[p1].MM;
+    tm.DD = RtkBuffer[p1].DD;
+    tm.hh = RtkBuffer[p1].hh;
+    tm.mm = RtkBuffer[p1].mm;
+    tm.mss = RtkBuffer[p1].dss;
+
+    UpdateCarBodyCoord(&tm, RtkBuffer[p1].heading, RtkBuffer[p1].pitch, RtkBuffer[p1].roll, main_ant_coord, CarModel);
 
     car_model *newModel = (car_model *)malloc(sizeof(car_model));
 
+    newModel->tm = CarModel->tm;
     newModel->basePoint = CarModel->basePoint;
     newModel->axial[0] = CarModel->axial[0];
     newModel->axial[1] = CarModel->axial[1];
@@ -874,7 +882,8 @@
     newModel->pitch = CarModel->pitch;
 
     CarModelList.push_front(newModel);
-    while (CarModelList.size() > 25) {
+
+    while (CarModelList.size() > 100) {
         DEBUG("鍒犻櫎灏剧粨鐐�");
         car_model *ptr = CarModelList.back();
 
@@ -1159,12 +1168,13 @@
  * @param azimuth
  * @param coord
  */
-static void UpdateCarBodyCoord(double azimuth, double pitch, double roll, PointF main_ant, car_model *carModel)
+static void UpdateCarBodyCoord(struct RtkTime *rtkTime, double azimuth, double pitch, double roll, PointF main_ant, car_model *carModel)
 {
     carModel->basePoint = main_ant;
 
     carModel->yaw = azimuth;
     carModel->pitch = pitch;
+    carModel->tm = *rtkTime;
 
     pitch = pitch - carModel->antPitch;
 
diff --git a/lib/src/main/cpp/driver_test.h b/lib/src/main/cpp/driver_test.h
index 8a0ff4a..85b0594 100644
--- a/lib/src/main/cpp/driver_test.h
+++ b/lib/src/main/cpp/driver_test.h
@@ -11,6 +11,7 @@
 
 #include <vector>
 #include <string>
+#include <list>
 
 #define EXAM_AREA_NONE          0
 #define EXAM_AREA_START         1
@@ -29,7 +30,18 @@
     double angle;               // 浠庝腑杞寸嚎閫嗘椂閽堝舰鎴愮殑瑙掑害
 };
 
+struct RtkTime {
+    int YY;
+    int MM;
+    int DD;
+    int hh;
+    int mm;
+    int ss;
+    int mss;
+};
+
 typedef struct {
+    struct RtkTime tm;
     PointF basePoint;               // 鐜板満娴嬮噺鐨勪富澶╃嚎鍧愭爣
     int axial[2];
     int left_front_tire[2];
@@ -66,16 +78,6 @@
     carModelDesc_t *desc;
 } car_model_cache_t;
 
-struct RtkTime {
-    int YY;
-    int MM;
-    int DD;
-    int hh;
-    int mm;
-    int ss;
-    int mss;
-};
-
 struct ExamFault {
     int sn;
     char utc[32];
@@ -106,6 +108,8 @@
 
 typedef vector<struct road_exam_map> LIST_ROAD_MAP;
 
+typedef list<car_model *> LIST_CAR_MODEL;
+
 //vector<ExamFault> ExamFaultList;
 
 void DriverTestInit(void);
diff --git a/lib/src/main/cpp/test_items2/road_exam.cpp b/lib/src/main/cpp/test_items2/road_exam.cpp
index 6826a10..c0b82e3 100644
--- a/lib/src/main/cpp/test_items2/road_exam.cpp
+++ b/lib/src/main/cpp/test_items2/road_exam.cpp
@@ -7,27 +7,54 @@
 #include "../utils/xconvert.h"
 #include "../common/apptimer.h"
 #include "../jni_log.h"
+#include "../defs.h"
+#include "../test_common/car_sensor.h"
+
+#include <vector>
+#include <list>
+#include <string>
 
 #define DEBUG(fmt, args...)     LOGD("<road_exam> <%s>: " fmt, __func__, ##args)
+
+using namespace std;
+
+#define TURN_CHECK_CNT          5
+
+static const int TURN_THRESHOLD = 10;
+static const int TURN_CHECK_INTERVAL = D_SEC(1);
 
 static bool occurCrashRedLine;
 static bool occurCrashGreenLine;
 static bool occurOverSpeed;
 static int checkCrashGreenTimeout;
 static char carIntersectionOfGreenLine;
+static int currTurnSignalStatus;
+static int turnSignalStatusWhenCrashGreenLine;
+static bool reportTurnSignalError;
+static int prevMoveDirect;
+static uint32_t stopTimepoint = 0;
+static bool reportStopCarOnRedArea;
+static PointF stopPoint;
 
-struct {
+static struct drive_timer {
     int hour;
     int min;
     int sec;
     int msec;
-} crashGreenRunTime, crashGreenCmpTime;
+} crashGreenRunTime, crashGreenCmpTime, crashGreenStartTime, turnSignalChangeTime;
 
+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 void Rtk2DirveTimer(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);
 
 void Init(void)
@@ -38,11 +65,17 @@
     occurOverSpeed = false;
     checkCrashGreenTimeout = 0;
     carIntersectionOfGreenLine = 0;
+
+    reportTurnSignalError = false;
+    currTurnSignalStatus = OFF_LIGHT;
+    prevMoveDirect = 0;
+
+    reportStopCarOnRedArea = false;
 }
 
-void TestRoadGeneral(LIST_ROAD_MAP &RoadMapList, const car_model *car, double speed, int moveStatus, const struct RtkTime *rtkTime)
+void TestRoadGeneral(LIST_ROAD_MAP &RoadMapList, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime)
 {
-    if (speed > MAX_SPEED) {
+    if (moveDirect != 0 && speed > MAX_SPEED) {
         if (!occurOverSpeed) {
             occurOverSpeed = true;
             // 瓒呴�燂紝涓嶅悎鏍�
@@ -51,6 +84,89 @@
     } else {
         occurOverSpeed = false;
     }
+
+    if (moveDirect != prevMoveDirect) {
+        if (moveDirect == 0) {
+            stopTimepoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
+            reportStopCarOnRedArea = false;
+
+            DEBUG("鍋滆溅浜� %d %d %d %d %d %d %d", rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss);
+        } else if (moveDirect == -1) {
+
+        }
+        prevMoveDirect = moveDirect;
+    } else if (moveDirect == 0) {
+        uint32_t tp = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
+
+        if (tp - stopTimepoint >= STOP_CAR_TIME && !reportStopCarOnRedArea && CrashRedArea(RoadMapList, car)) {
+            // 鍋滆溅瓒�2绉掞紝鍋滃湪绾㈠尯
+            AddExamFault(16, rtkTime);
+            DEBUG("涓�斿仠杞�");
+            reportStopCarOnRedArea = true;
+        }
+
+        stopPoint = car->basePoint;
+    }
+
+    switch (ReadCarStatus(TURN_SIGNAL_LAMP)) {
+        case LEFT_TURN_LIGHT:
+            if (currTurnSignalStatus != LEFT_TURN_LIGHT) {
+                currTurnSignalStatus = LEFT_TURN_LIGHT;
+                Rtk2DirveTimer(turnSignalChangeTime, rtkTime);
+            }
+            break;
+        case RIGHT_TURN_LIGHT:
+            if (currTurnSignalStatus != RIGHT_TURN_LIGHT) {
+                currTurnSignalStatus = RIGHT_TURN_LIGHT;
+                Rtk2DirveTimer(turnSignalChangeTime, rtkTime);
+            }
+            break;
+        default:
+            currTurnSignalStatus = ReadCarStatus(TURN_SIGNAL_LAMP);
+            break;
+    }
+
+
+    // 妫�鏌ユ槸鍚︽寔缁浆鍚�
+    char turnDirect = CheckCarTurn(CarModelList);
+    if (turnDirect == 'L') {
+        if (currTurnSignalStatus != LEFT_TURN_LIGHT) {
+            if (!reportTurnSignalError) {
+                DEBUG("娌℃墦宸﹁浆鐏�");
+                // 娌℃墦宸﹁浆鐏紝涓嶅悎鏍�
+                AddExamFault(13, rtkTime);
+                reportTurnSignalError = true;
+            }
+        } else if (TimeGetDiff(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10,
+                               turnSignalChangeTime.hour, turnSignalChangeTime.min, turnSignalChangeTime.sec, turnSignalChangeTime.msec*10) < TURN_SIGNAL_LAMP_ADVANCE) {
+            if (!reportTurnSignalError) {
+                DEBUG("杞悜鐏椂闂翠笉瓒�");
+                // 涓嶈冻3绉掞紝涓嶅悎鏍�
+                AddExamFault(14, rtkTime);
+                reportTurnSignalError = true;
+            }
+        }
+    } else if (turnDirect == 'R') {
+        if (currTurnSignalStatus != RIGHT_TURN_LIGHT) {
+            if (!reportTurnSignalError) {
+                DEBUG("娌℃墦鍙宠浆鐏�");
+                // 娌℃墦鍙宠浆鐏紝涓嶅悎鏍�
+                AddExamFault(13, rtkTime);
+                reportTurnSignalError = true;
+            }
+        } else if (TimeGetDiff(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10,
+                               turnSignalChangeTime.hour, turnSignalChangeTime.min, turnSignalChangeTime.sec, turnSignalChangeTime.msec*10) < TURN_SIGNAL_LAMP_ADVANCE) {
+            if (!reportTurnSignalError) {
+                DEBUG("杞悜鐏椂闂翠笉瓒�");
+                // 涓嶈冻3绉掞紝涓嶅悎鏍�
+                AddExamFault(14, rtkTime);
+                reportTurnSignalError = true;
+            }
+        }
+    } else {
+        reportTurnSignalError = false;
+    }
+
 
     if (CrashRedLine(RoadMapList, car)) {
         if (!occurCrashRedLine) {
@@ -65,13 +181,11 @@
     static PointF p1, p2;
 
     if (CrashGreenLine(RoadMapList, car, p1, p2)) {
-        if (moveStatus != 0) {
+        // 鍘嬭櫄绾�
+        if (moveDirect != 0) {
             if (checkCrashGreenTimeout == 0) {
                 checkCrashGreenTimeout = 1;
-                crashGreenRunTime.hour = rtkTime->hh;
-                crashGreenRunTime.min = rtkTime->mm;
-                crashGreenRunTime.sec = rtkTime->ss;
-                crashGreenRunTime.msec = rtkTime->mss;
+                Rtk2DirveTimer(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);
@@ -91,7 +205,9 @@
         // 妫�娴嬫槸鍚�3绉掑墠鏈夊紑鍚搴斾箣杞悜鐏�
         if (!occurCrashGreenLine) {
             occurCrashGreenLine = true;
-
+            // 璁板綍寮�濮嬪帇绾跨殑鏃堕棿锛屼笉纭畾鏄惁鏈夊彉閬撴剰鍥撅紝寰呯‘璁ゅ彉閬撳悗鍐嶅鐞嗕箣
+            Rtk2DirveTimer(crashGreenStartTime, rtkTime);
+            turnSignalStatusWhenCrashGreenLine = currTurnSignalStatus;
         }
 
         // p1 ---------------> p2
@@ -104,14 +220,14 @@
             carIntersectionOfGreenLine = 'L';
         }
     } else {
+        // 涓嶅啀鍘嬭櫄绾�
         if (occurCrashGreenLine) {
             int inter = IntersectionOfLine(p1, p2, car->basePoint);
 
-            // 鏈夎繃绾垮姩浣�
+            // 瀹屾垚璺ㄧ嚎鍔ㄤ綔
             if ((inter == 1 && carIntersectionOfGreenLine == 'R') ||
                     (inter == -1 && carIntersectionOfGreenLine == 'L')) {
-                DEBUG("璺ㄨ秺铏氱嚎");
-
+                // 姣旇緝涓婃璺ㄧ嚎鏃堕棿
                 if (crashGreenCmpTime.hour >= 0) {
                     uint32_t diff = TimeGetDiff(rtkTime->hh, rtkTime->mm, rtkTime->ss,
                                                 rtkTime->mss * 10,
@@ -124,15 +240,102 @@
                     }
                 }
 
-                crashGreenCmpTime.hour = rtkTime->hh;
-                crashGreenCmpTime.min = rtkTime->mm;
-                crashGreenCmpTime.sec = rtkTime->ss;
-                crashGreenCmpTime.msec = rtkTime->mss;
+                // 璁板綍鏈鍙橀亾鏃堕棿鐐�
+                Rtk2DirveTimer(crashGreenCmpTime, rtkTime);
+
+                // 妫�鏌ュ彉閬撳墠锛屾槸鍚︽彁鍓嶈浆鍚戠伅
+                if (inter == 1) {
+                    // 鍚戝乏渚у彉閬�
+                    DEBUG("鍚戝乏渚у彉閬�");
+                    if (turnSignalStatusWhenCrashGreenLine != LEFT_TURN_LIGHT) {
+                        // 娌℃墦鐏紝涓嶅悎鏍�
+                        AddExamFault(13, rtkTime);
+                    }
+                } else {
+                    // 鍚戝彸渚у彉閬�
+                    DEBUG("鍚戝彸渚у彉閬�");
+                    if (turnSignalStatusWhenCrashGreenLine != RIGHT_TURN_LIGHT) {
+                        // 娌℃墦鐏紝涓嶅悎鏍�
+                        AddExamFault(13, rtkTime);
+                    }
+                }
             }
         }
         occurCrashGreenLine = false;
         checkCrashGreenTimeout = 0;
     }
+}
+
+static void Rtk2DirveTimer(struct drive_timer &tm, const struct RtkTime *rtkTime)
+{
+    tm.hour = rtkTime->hh;
+    tm.min = rtkTime->mm;
+    tm.sec = rtkTime->ss;
+    tm.msec = rtkTime->mss;
+}
+
+static char isTurn(int currYaw, int prevYaw)
+{
+    if (ABS(currYaw - prevYaw) > 180) {
+        currYaw = 360 - ABS(currYaw-prevYaw);
+    } else {
+        currYaw = ABS(currYaw - prevYaw);
+    }
+
+    if (currYaw >= TURN_THRESHOLD) {
+        if((( currYaw + 360 - prevYaw) % 360) < 180) {
+            DEBUG("鍙宠浆");
+            return 'R';
+        } else {
+            DEBUG("宸﹁浆");
+            return 'L';
+        }
+    }
+
+    return 0;
+}
+
+static char CheckCarTurn(LIST_CAR_MODEL &CarModelList)
+{
+    // 鏈�杩�3绉掑唴锛屾瘡绉掔殑瑙掑害宸ぇ浜�10搴︼紝涓旀柟鍚戠浉鍚岋紝杩炵画3绉掞紝璁や负杞悜
+    if (CarModelList.size() < 1)
+        return false;
+
+    list<car_model *>::iterator iter = CarModelList.begin();
+
+    car_model *c1 = *iter, *c2;
+
+    ++iter;
+
+    char turn[TURN_CHECK_CNT] = {0};
+    int checkCnt = 0;
+
+    while (iter != CarModelList.end()) {
+        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)
+                break;
+        }
+
+        ++iter;
+    }
+
+    int i = 0;
+    for (; checkCnt == TURN_CHECK_CNT && i < TURN_CHECK_CNT-1; ++i) {
+        if (turn[i] != turn[i+1])
+            break;
+    }
+
+    if (i == TURN_CHECK_CNT-1)
+        return turn[0];
+
+    return 0;
 }
 
 /*****************************************************
@@ -170,6 +373,42 @@
     return false;
 }
 
+/**********************************************************
+ * 鎸夋暣涓溅韬槸鍚﹁鐩栬绠�
+ * @param RoadMapList
+ * @param car
+ * @return
+ */
+static bool CrashRedArea(LIST_ROAD_MAP &RoadMapList, const car_model *car)
+{
+    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]];
+    }
+
+    for (int i = 0; i < RoadMapList.size(); ++i) {
+        if (RoadMapList[i].type == 100) {
+
+            // 姣忔潯绾㈠尯閮芥娴�
+            for (int j = 0; j < RoadMapList[i].redAreaNum; ++j) {
+                if (IntersectionOf(&carBody, &RoadMapList[i].redArea[j]) != GM_None) {
+                    ret = true;
+                }
+            }
+            break;
+        }
+    }
+
+    free(carBody.point);
+
+    return ret;
+}
+
 /**************************************************
  * 杞﹁疆瑙︾閬撹矾铏氱嚎銆傛娴嬭椹舵椂闂磋秴闀匡紱鍙橀亾鎯呭喌锛�
  * @param RoadMapList

--
Gitblit v1.8.0