From 21834035950feaadac9d4c7cad82b90ee624c984 Mon Sep 17 00:00:00 2001 From: yy1717 <fctom1215@outlook.com> Date: 星期一, 17 八月 2020 19:30:06 +0800 Subject: [PATCH] 传感器变更 --- lib/src/main/cpp/test_items2/road_exam.cpp | 226 +++++++++++++++++++++++++++++++++++++++++++++++++++----- 1 files changed, 204 insertions(+), 22 deletions(-) diff --git a/lib/src/main/cpp/test_items2/road_exam.cpp b/lib/src/main/cpp/test_items2/road_exam.cpp index 5196886..a1e3357 100644 --- a/lib/src/main/cpp/test_items2/road_exam.cpp +++ b/lib/src/main/cpp/test_items2/road_exam.cpp @@ -74,7 +74,7 @@ static const int INVALID_ROAD = -1; -static const int TURN_THRESHOLD = 1; +static const int TURN_THRESHOLD = 3; static const int TURN_CHECK_INTERVAL = 500; const double SLIDE_DISTANCE_THRESHOLD_RED = 0.3; const double SLIDE_DISTANCE_THRESHOLD_YELLOW = 0.1; @@ -94,7 +94,7 @@ static char carIntersectionOfGreenLine; static int currTurnSignalStatus; static int turnSignalStatusWhenCrashGreenLine; -static bool reportTurnSignalError; + static int prevMoveDirect; static uint32_t stopTimepoint = 0; static bool reportStopCarOnRedArea; @@ -135,6 +135,12 @@ static map<int, car_sensor_value_t> CarSensorValue; static map<int, int> CrossingHint; +static map<int, bool> ErrorLaneReport; + +static struct RtkTime beginTurnTime, prevDetectTurnTime; +static int startTurnYaw, prevYaw; +static int turnCnt, turnTimeCnt; +static int prevTurnWise; static const int MAX_ENGINE_RPM = 2500; static const double START_CAR_MOVE_DISTANCE = 10.0; @@ -157,8 +163,9 @@ static int TestRoadStartCar(const car_model *car, double speed, int moveDirect, const struct RtkTime *rtkTime); -static char isTurn(int currYaw, int prevYaw, int &ang); -static char CheckCarTurn(LIST_CAR_MODEL &CarModelList); +static int isTurn(int currYaw, int prevYaw, int thres); +static void ResetTurnDetect(const car_model *car); +static void DetectTurn(const car_model *car, int moveDirect, const struct RtkTime *rtkTime); static trigger_line_t * EntryItem(int index, road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList); @@ -179,7 +186,6 @@ carIntersectionOfGreenLine = 0; - reportTurnSignalError = false; currTurnSignalStatus = OFF_LIGHT; prevMoveDirect = 0; @@ -211,6 +217,7 @@ checkTurn = false; CrashLineType = -1; + turnCnt = -1; ResetOdo(); ResetTarget(RoadMap); @@ -973,7 +980,6 @@ } else if (TimeGetDiff(&crashGreenStartTime, &lamp.time) >= D_SEC(3)) { DEBUG("杞悜鐏椂闂翠笉瓒�"); // 涓嶈冻3绉掞紝涓嶅悎鏍� - reportTurnSignalError = true; AddExamFault(14, rtkTime); } } else { @@ -984,7 +990,6 @@ } else if (TimeGetDiff(&crashGreenStartTime, &lamp.time) >= D_SEC(3)) { DEBUG("杞悜鐏椂闂翠笉瓒�"); // 涓嶈冻3绉掞紝涓嶅悎鏍� - reportTurnSignalError = true; AddExamFault(14, rtkTime); } } @@ -1027,6 +1032,62 @@ return CROSSING_NOT_HINT; } +static void ResetCrossingStatus(int roadIndex) +{ + while (true) { + bool w = false; + for (auto it = CrossingHint.begin(); it != CrossingHint.end(); ++it) { + if (it->first / 100 == roadIndex && it->second != CROSSING_NOT_HINT) { + ChangeCrossingStatus(it->first / 100, it->first % 100, CROSSING_NOT_HINT); + w = true; + break; + } + } + if (!w) + break; + } +} + +static void SetErrorLaneRpt(int roadIndex, int index, bool status) +{ + int key = roadIndex * 100 + index; + + auto it = ErrorLaneReport.find(key); + + if (it != ErrorLaneReport.end()) { + ErrorLaneReport.erase(it); + } + ErrorLaneReport.insert(pair<int, bool>(key, status)); +} + +static bool GetErrorLaneRpt(int roadIndex, int index) +{ + int key = roadIndex * 100 + index; + + auto it = ErrorLaneReport.find(key); + + if (it != ErrorLaneReport.end()) { + return it->second; + } + return false; +} + +static void ResetErrorLaneRpt(int roadIndex) +{ + while (true) { + bool w = false; + for (auto it = ErrorLaneReport.begin(); it != ErrorLaneReport.end(); ++it) { + if (it->first / 100 == roadIndex && it->second == true) { + SetErrorLaneRpt(it->first / 100, it->first % 100, false); + w = true; + break; + } + } + if (!w) + break; + } +} + static void HintCrossing(int roadIndex, road_t &road, const car_model *car, LIST_CAR_MODEL &CarModelList) { for (int i = 0; i < road.stopLine.size(); ++i) { @@ -1053,12 +1114,13 @@ } } -static int NearbyCrossingGuide(int roadIndex, road_t &road, const car_model *car, LIST_CAR_MODEL &CarModelList) +static int NearbyCrossingGuide(int &stopLineIndex, int roadIndex, road_t &road, const car_model *car, LIST_CAR_MODEL &CarModelList) { - int guide = 0; + int guide = 0, stopLine = 0; double distance; struct nearby_crossing { + int stopLine; int guide; double distance; }; @@ -1077,6 +1139,7 @@ if (distance > 1e-3) { struct nearby_crossing temp; + temp.stopLine = i; temp.guide = road.stopLine[i].active; temp.distance = distance; @@ -1092,10 +1155,12 @@ if (distance > set[i].distance) { distance = set[i].distance; guide = set[i].guide; + stopLine = set[i].stopLine; } } } + stopLineIndex = stopLine; return guide; } @@ -1286,6 +1351,15 @@ // 妫�娴嬬寮�姝よ矾娈碉紝鍏ㄨ溅闇�涓嶅湪鑼冨洿鍐� int oldid = currExamMapIndex; currExamMapIndex = CalcRoadIndex(currExamMapIndex, RoadMap, car); + if (oldid != currExamMapIndex) { + DEBUG("閬撹矾ID鍒囨崲 %d ======> %d", oldid, currExamMapIndex); + + if (oldid >= 0) { + ResetCrossingStatus(oldid); + ResetErrorLaneRpt(oldid); + } + } + if (currExamMapIndex >= 0) { car_sensor_value_t brk = ReadCarSensorValue(BREAK); HintCrossing(currExamMapIndex, RoadMap.roads[currExamMapIndex], car, CarModelList); @@ -1294,10 +1368,6 @@ ApproachTarget(RoadMap, car, currExamMapIndex, (brk.value == BREAK_ACTIVE), speed, moveDirect, rtkTime); } ExitTarget(RoadMap, car, CarModelList, rtkTime); - - if (oldid != currExamMapIndex) { - DEBUG("閬撹矾ID鍒囨崲 %d", currExamMapIndex); - } oldid = CrashLineType; @@ -1311,9 +1381,14 @@ oldid = Lane.guide; DetectLane(RoadMap, car, currExamMapIndex, rtkTime); if (Lane.guide > 0 && currExamMapIndex >= 0) { - if (!(NearbyCrossingGuide(currExamMapIndex, RoadMap.roads[currExamMapIndex], car, CarModelList) & Lane.guide)) { - DEBUG("涓嶆寜瑙勫畾杞﹂亾鏍囧悜琛岄┒"); - AddExamFault(9, rtkTime); + int stop_line; + + if (!(NearbyCrossingGuide(stop_line, currExamMapIndex, RoadMap.roads[currExamMapIndex], car, CarModelList) & Lane.guide)) { + if (!GetErrorLaneRpt(currExamMapIndex, stop_line)) { + DEBUG("涓嶆寜瑙勫畾杞﹂亾鏍囧悜琛岄┒"); + AddExamFault(9, rtkTime); + SetErrorLaneRpt(currExamMapIndex, stop_line, true); + } } } @@ -1323,6 +1398,9 @@ // 妫�娴嬪帇绾跨姸鎬� TestRoadStartCar(car, speed, moveDirect, rtkTime); + + // 棰濆鐨勮浆鍚戞娴� + DetectTurn(car, moveDirect, rtkTime); if (startCar != START_CAR_DONE) return; @@ -1399,7 +1477,15 @@ tm.msec = rtkTime->mss; } -static char isTurn(int currYaw, int prevYaw, int &ang) +void CrossRoadCallback(int road, int stop_line, int active, const car_model *car) +{ + SetErrorLaneRpt(road, stop_line, false); + if (active != ROAD_ACTIVE_FORWARD) { + ResetTurnDetect(car); + } +} + +static int isTurn(int currYaw, int prevYaw, int thres) { // DEBUG("currYaw %d prevYaw %d", currYaw, prevYaw); int deltaAng = 0; @@ -1410,21 +1496,117 @@ deltaAng = ABS(currYaw - prevYaw); } - ang = deltaAng; // DEBUG("瑙掑害宸�� %d", deltaAng); - if (deltaAng >= TURN_THRESHOLD) { + if (deltaAng >= thres) { if((( currYaw + 360 - prevYaw) % 360) < 180) { // DEBUG("鍙宠浆"); - return 'R'; + return deltaAng; } else { // DEBUG("宸﹁浆"); - return 'L'; + return 0 - deltaAng; } } - return 'N'; + return 0; +} + +static void ResetTurnDetect(const car_model *car) +{ + turnCnt = 0; + turnTimeCnt = 0; + prevDetectTurnTime = car->tm; + prevYaw = car->yaw; +} + +static void DetectTurn(const car_model *car, int moveDirect, const struct RtkTime *rtkTime) +{ + int angle; + + if (turnCnt < 0 || TimeGetDiff(rtkTime, &prevDetectTurnTime) < 500) { + return; + } + + angle = isTurn((int) car->yaw, prevYaw, TURN_THRESHOLD); + + prevYaw = (int) car->yaw; + prevDetectTurnTime = *rtkTime; + + if (turnCnt == 0) { + if (angle != 0 && moveDirect != 0) { + DEBUG("鏈夎浆鍚戣抗璞�"); + turnCnt++; + beginTurnTime = *rtkTime; + startTurnYaw = (int) car->yaw; + } + } else if (turnCnt == 1) { + if (angle != 0 && moveDirect != 0) { + if (angle * prevTurnWise > 0) { + DEBUG("纭杞悜"); + // 鍚屽悜杞姩 + turnCnt++; + turnTimeCnt = 0; + } else { + beginTurnTime = *rtkTime; + startTurnYaw = (int) car->yaw; + } + } else { + turnCnt = 0; + } + } else if (turnCnt >= 2) { + if (moveDirect == 0) { + // 鏆傚仠 + } else { + turnTimeCnt += TimeGetDiff(rtkTime, &prevDetectTurnTime); + int wise = isTurn((int) car->yaw, startTurnYaw, TURN_THRESHOLD); + DEBUG("杞姩瑙掑害 %d", wise); + if (ABS(wise) > 60) { + // 纭杞集琛屼负锛屾娴嬪紑濮嬪垰杞集鏃惰浆鍚戠伅鎯呭喌 + turnCnt = -1; + + car_sensor_value_t lamp = ReadCarSensorValue(TURN_SIGNAL_LAMP); + if (lamp.name == TURN_SIGNAL_LAMP) { + if (wise < 0) { + if (lamp.value != LEFT_TURN_LIGHT) { + DEBUG("鍙樿皟鏈墦鐏�!!"); + // 娌℃墦鐏紝涓嶅悎鏍� + AddExamFault(13, rtkTime); + } else if (TimeGetDiff(&beginTurnTime, &lamp.time) >= D_SEC(3)) { + DEBUG("杞悜鐏椂闂翠笉瓒�"); + // 涓嶈冻3绉掞紝涓嶅悎鏍� + AddExamFault(14, rtkTime); + } + } else { + if (lamp.value != RIGHT_TURN_LIGHT) { + DEBUG("鍙樿皟鏈墦鐏�!!"); + // 娌℃墦鐏紝涓嶅悎鏍� + AddExamFault(13, rtkTime); + } else if (TimeGetDiff(&beginTurnTime, &lamp.time) >= D_SEC(3)) { + DEBUG("杞悜鐏椂闂翠笉瓒�"); + // 涓嶈冻3绉掞紝涓嶅悎鏍� + AddExamFault(14, rtkTime); + } + } + } + } else if (turnTimeCnt > D_SEC(10)) { + // 鍙栨秷杞悜妫�娴� + DEBUG("鍙栨秷杞悜妫�娴�1"); + turnCnt = -1; + } + } + } + + if (turnCnt < 2 && moveDirect != 0) { + turnTimeCnt += TimeGetDiff(rtkTime, &prevDetectTurnTime); + if (turnTimeCnt > D_SEC(15)) { + // 鍙栨秷杞悜妫�娴� + DEBUG("鍙栨秷杞悜妫�娴�2"); + turnCnt = -1; + } + } + + prevTurnWise = angle; } static char CheckCarTurn(LIST_CAR_MODEL &CarModelList) -- Gitblit v1.8.0