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