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.h | 5 + lib/src/main/cpp/driver_test.cpp | 44 ++++---- lib/src/main/cpp/test_common/car_sensor.h | 14 +- lib/src/main/cpp/utils/xconvert.h | 2 lib/src/main/cpp/utils/xconvert.cpp | 2 lib/src/main/cpp/test_items2/road_exam.cpp | 206 +++++++++++++++++++++++++++++++++++++---- lib/src/main/cpp/driver_test.h | 8 + 7 files changed, 229 insertions(+), 52 deletions(-) diff --git a/lib/src/main/cpp/driver_test.cpp b/lib/src/main/cpp/driver_test.cpp index 58c440f..8a9c7cd 100644 --- a/lib/src/main/cpp/driver_test.cpp +++ b/lib/src/main/cpp/driver_test.cpp @@ -55,17 +55,6 @@ #define MAP_LIST_SIZE 32 enum { - TEST_NONE, - TEST_PARK_EDGE, - TEST_PARK_BOTTOM, - TEST_TUNE_90, - TEST_S_CURVE, - TEST_SLOPE, - TEST_SIMPLE, - TEST_END -}; - -enum { TEST_TYPE_AREA = 2, TEST_TYPE_ROAD_DUMMY_LIGHT, TEST_TYPE_ROAD_TRUE_LIGHT @@ -258,8 +247,8 @@ if (map.triggerLine != NULL) { for (int j = 0; j < map.triggerLineNum; ++j) { - if (map.triggerLine[j].point != NULL) - free(map.triggerLine[j].point); + if (map.triggerLine[j].line.point != NULL) + free(map.triggerLine[j].line.point); } free(map.triggerLine); } @@ -305,6 +294,8 @@ newMap.redLine[i].point[j] = RoadMapPoints.point[redLines[i][j]]; } } + } else { + newMap.redLine = NULL; } if ((newMap.redAreaNum = redAreas.size()) > 0) { @@ -318,6 +309,8 @@ newMap.redArea[i].point[j] = RoadMapPoints.point[redAreas[i][j]]; } } + } else { + newMap.redArea = NULL; } if ((newMap.greenLineNum = greenLines.size()) > 0) { @@ -331,20 +324,25 @@ newMap.greenLine[i].point[j] = RoadMapPoints.point[greenLines[i][j]]; } } + } else { + newMap.greenLine = NULL; } if ((newMap.triggerLineNum = triggerLines.size()) > 0) { - newMap.triggerLine = (Polygon *) malloc(sizeof(Polygon) * newMap.triggerLineNum); + newMap.triggerLine = (struct trigger_line_t *) malloc(sizeof(struct trigger_line_t) * newMap.triggerLineNum); for (int i = 0; i < newMap.triggerLineNum; ++i) { - newMap.triggerLine[i].num = triggerLines[i].size(); - newMap.triggerLine[i].point = (PointF *) malloc(sizeof(PointF) * newMap.triggerLine[i].num); + newMap.triggerLine[i].line.num = triggerLines[i].size() - 1; + newMap.triggerLine[i].line.point = (PointF *) malloc(sizeof(PointF) * newMap.triggerLine[i].line.num); - for (int j = 0; j < newMap.triggerLine[i].num; ++j) { - newMap.triggerLine[i].point[j] = RoadMapPoints.point[triggerLines[i][j]]; + newMap.triggerLine[i].triggerMapId = triggerLines[i][0]; + for (int j = 0; j < newMap.triggerLine[i].line.num; ++j) { + newMap.triggerLine[i].line.point[j] = RoadMapPoints.point[triggerLines[i][j+1]]; } } - } + } else { + newMap.triggerLine = NULL; + }; RoadMapList.push_back(newMap); } @@ -480,7 +478,7 @@ return; } - if (MapNum == 0) { + if (MapNum == 0 && type == TEST_TYPE_AREA) { err = true; MA_SendExamStatus(0, -1); } @@ -491,6 +489,10 @@ if (DummyLightContent == NULL && type == TEST_TYPE_ROAD_DUMMY_LIGHT) { err = true; MA_SendExamStatus(0, -3); + } + if (type != TEST_TYPE_AREA && (RoadMapPoints.num == 0 || RoadMapPoints.point == NULL || RoadMapList.size() == 0)) { + err = true; + MA_SendExamStatus(0, -1); } if (!err) { @@ -657,7 +659,7 @@ if (ReadCarStatus(ENGINE_START) == ENGINE_START_ACTIVE) { if (!engineStart) { engineStart = true; - if (ReadCarStatus(SHIFT) != 'N') { + if (ReadCarStatus(GEAR) != 'N') { // 涓嶆槸绌烘尅鐐圭伀锛屼笉鍚堟牸 if (ExamType == TEST_TYPE_AREA) AddExamFault(3, rtkTime); diff --git a/lib/src/main/cpp/driver_test.h b/lib/src/main/cpp/driver_test.h index 85b0594..7bff0a9 100644 --- a/lib/src/main/cpp/driver_test.h +++ b/lib/src/main/cpp/driver_test.h @@ -84,6 +84,11 @@ int wrong_id; }; +struct trigger_line_t { + int triggerMapId; + Polygon line; +}; + struct road_exam_map { int id; int type; @@ -100,10 +105,11 @@ Polygon *redArea; int triggerLineNum; - Polygon *triggerLine; + trigger_line_t *triggerLine; Line startLine; Line endLine; + int flagStop; // 鍒拌揪寮�濮嬬嚎鍓嶏紝鏄惁闇�瑕佸仠杞� }; typedef vector<struct road_exam_map> LIST_ROAD_MAP; diff --git a/lib/src/main/cpp/test_common/car_sensor.h b/lib/src/main/cpp/test_common/car_sensor.h index 290f6bd..749dc26 100644 --- a/lib/src/main/cpp/test_common/car_sensor.h +++ b/lib/src/main/cpp/test_common/car_sensor.h @@ -10,7 +10,7 @@ enum { OBD_SPEED, ENGINE_RPM, - SHIFT, + GEAR, TURN_SIGNAL_LAMP, DIPPED_BEAM_LAMP, FOG_LAMP, @@ -36,13 +36,13 @@ FOG_LIGHT, INSERT_SEATBELT, ENGINE_START_ACTIVE, - SHIFT_N, + GEAR_N, SHIFT_R, - SHIFT_1, - SHIFT_2, - SHIFT_3, - SHIFT_4, - SHIFT_5 + GEAR_1, + GEAR_2, + GEAR_3, + GEAR_4, + GEAR_5 }; void CarSensorInit(void); 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; +} diff --git a/lib/src/main/cpp/test_items2/road_exam.h b/lib/src/main/cpp/test_items2/road_exam.h index 697d0c5..7cbff05 100644 --- a/lib/src/main/cpp/test_items2/road_exam.h +++ b/lib/src/main/cpp/test_items2/road_exam.h @@ -5,4 +5,9 @@ #ifndef MYAPPLICATION2_ROAD_EXAM_H #define MYAPPLICATION2_ROAD_EXAM_H +#include "../driver_test.h" + +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); + #endif //MYAPPLICATION2_ROAD_EXAM_H diff --git a/lib/src/main/cpp/utils/xconvert.cpp b/lib/src/main/cpp/utils/xconvert.cpp index a4e524b..0d07d85 100644 --- a/lib/src/main/cpp/utils/xconvert.cpp +++ b/lib/src/main/cpp/utils/xconvert.cpp @@ -23,7 +23,7 @@ return ((double)kmh) * 1000.0 / 3600.0; } -double ConvertMs2KMs(double ms) +double ConvertMs2KMh(double ms) { return ms * 3600.0 / 1000.0; } diff --git a/lib/src/main/cpp/utils/xconvert.h b/lib/src/main/cpp/utils/xconvert.h index dbbe7b4..9821df7 100644 --- a/lib/src/main/cpp/utils/xconvert.h +++ b/lib/src/main/cpp/utils/xconvert.h @@ -8,7 +8,7 @@ #include <cstdint> double ConvertKMh2Ms(int kmh); -double ConvertMs2KMs(double ms); +double ConvertMs2KMh(double ms); void ConvertPhoneNum(uint8_t *dst, int length, const char *src); void ConvertHex2String(char *str, const uint8_t *hex, int length); -- Gitblit v1.8.0