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/test_items2/road_exam.cpp | 273 +++++++++++++++++++++++++++++++++++++++++++++++++++--- 1 files changed, 256 insertions(+), 17 deletions(-) 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