From bb7622ee9632eb25a58d4371a900aea9a26651ad Mon Sep 17 00:00:00 2001
From: fctom1215 <fctom1215@outlook.com>
Date: 星期一, 27 七月 2020 17:03:42 +0800
Subject: [PATCH] 坐标
---
lib/src/main/cpp/test_items2/road_exam.cpp | 464 ++-------------------------------------------------------
1 files changed, 20 insertions(+), 444 deletions(-)
diff --git a/lib/src/main/cpp/test_items2/road_exam.cpp b/lib/src/main/cpp/test_items2/road_exam.cpp
index f0b3010..1b77684 100644
--- a/lib/src/main/cpp/test_items2/road_exam.cpp
+++ b/lib/src/main/cpp/test_items2/road_exam.cpp
@@ -48,7 +48,7 @@
const double SLIDE_DISTANCE_THRESHOLD_YELLOW = 0.1;
const double CHANGE_LANE_RANGE = 100.0;
const double OVERTAKE_RANGE = 150.0;
-const int OVERTAKE_HOLD_TIME = D_SEC(11); // 鍦ㄨ秴杞﹂亾琛岄┒鐨勪竴娈垫椂闂�
+const int OVERTAKE_HOLD_TIME = D_SEC(3); // 鍦ㄨ秴杞﹂亾琛岄┒鐨勪竴娈垫椂闂�
const double EXAM_RANGE = 3000.0; // 鑷冲皯椹鹃┒璺濈
static const double LASTEST_BREAK_POINT = 30.0;
@@ -112,6 +112,8 @@
int type; // 瀹炵嚎锛岃櫄绾�
} CurrentLane;
static bool laneChanging;
+static int changeLaneDirect;
+
static double odoGraph;
static struct drive_timer odoTimer;
static double odoPrevSpeed;
@@ -199,6 +201,7 @@
CurrentLane.road = CurrentLane.separate = CurrentLane.lane = -1;
laneChanging = false;
+ changeLaneDirect = 0;
nextRoadId = -1;
checkTurn = false;
@@ -704,8 +707,15 @@
crashGreenCmpTime.min,
crashGreenCmpTime.sec,
crashGreenCmpTime.msec * 10);
+ int laneDirect = 0;
- if (diff < CHANGE_LANE_MIN_INTERVAL) {
+ if (CurrentLane.lane > lane.lane) {
+ laneDirect = 1;
+ } else {
+ laneDirect = -1;
+ }
+
+ if (diff < CHANGE_LANE_MIN_INTERVAL && laneDirect == changeLaneDirect) {
DEBUG("===================== 杩炵画鍙橀亾 ============!!");
// 杩炵画鍙橀亾锛屼笉鍚堟牸
AddExamFault(15, rtkTime);
@@ -764,16 +774,24 @@
if (currRoadItem != NULL && currRoadItem->active == ROAD_ITEM_CHANGE_LANE) {
DEBUG("鍙樻洿杞﹂亾椤圭洰瀹屾垚");
currRoadItem = NULL;
+ PlayTTS("瀹屾垚鍙橀亾");
} else if (currRoadItem != NULL && currRoadItem->active == ROAD_ITEM_OVERTAKE) {
if (CurrentLane.lane > lane.lane) {
DEBUG("瓒呰溅鍙橀亾瀹屾垚");
overtake = true;
Rtk2DriveTimer(overTakeCmpTime, rtkTime);
+ PlayTTS("瀹屾垚瓒呰溅");
} else {
DEBUG("鍙抽亾瓒呰溅锛岄敊璇�");
AddExamFault(3, rtkTime);
currRoadItem = NULL;
}
+ }
+
+ if (CurrentLane.lane > lane.lane) {
+ changeLaneDirect = -1;
+ } else {
+ changeLaneDirect = 1;
}
CurrentLane = lane;
@@ -1256,448 +1274,6 @@
}
return false;
}
-
-#if 0
-void TestRoadGeneral(LIST_ROAD_MAP &RoadMapList, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime)
-{
- // 璧锋妫�娴�
- TestRoadStartCar(car, speed, moveDirect, rtkTime);
-
- // 瓒呴�熸娴�
- if (moveDirect != 0 && speed > MAX_SPEED) {
- if (!occurOverSpeed) {
- occurOverSpeed = true;
- // 瓒呴�燂紝涓嶅悎鏍�
- AddExamFault(10, rtkTime);
- }
- } else {
- occurOverSpeed = false;
- }
-
- // 鍓埞杞︽娴�
- if (ReadCarStatus(SECOND_BREAK) == BREAK_ACTIVE) {
- // 鍓埞杞﹁俯涓嬶紝涓嶅悎鏍�
- if (!occurSecondBreak) {
- DEBUG("鍓埞杞﹀姩浣滀簡");
- occurSecondBreak = true;
- AddExamFault(17, rtkTime);
- }
- } else {
- occurSecondBreak = false;
- }
-
- // 鎸′綅鍖归厤妫�娴�
- bool currGearError = false;
- bool currGearNSlide = false;
-
- switch (ReadCarStatus(GEAR)) {
- case GEAR_N:
- if (moveDirect != 0) {
- // 绌烘。婊戣
- currGearNSlide = true;
- }
- break;
- case GEAR_1:
- if (ConvertMs2KMh(speed) < SPEED_GEAR_TABLE[0][0] || ConvertMs2KMh(speed) > SPEED_GEAR_TABLE[0][1]) {
- currGearError = true;
- }
- break;
- case GEAR_2:
- if (ConvertMs2KMh(speed) < SPEED_GEAR_TABLE[1][0] || ConvertMs2KMh(speed) > SPEED_GEAR_TABLE[1][1]) {
- currGearError = true;
- }
- break;
- case GEAR_3:
- if (ConvertMs2KMh(speed) < SPEED_GEAR_TABLE[2][0] || ConvertMs2KMh(speed) > SPEED_GEAR_TABLE[2][1]) {
- currGearError = true;
- }
- break;
- case GEAR_4:
- if (ConvertMs2KMh(speed) < SPEED_GEAR_TABLE[3][0] || ConvertMs2KMh(speed) > SPEED_GEAR_TABLE[3][1]) {
- currGearError = true;
- }
- break;
- 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) {
- 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 == 0) {
- DEBUG("寮�濮嬪悗婊�");
- stopPoint = car->basePoint;
- occurSlide = true;
- }
- 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;
- }
- } else if (moveDirect == -1) {
- if (occurSlide) {
- double slideDistance = DistanceOf(stopPoint, car->basePoint);
-
- if (slideDistance > SLIDE_DISTANCE_THRESHOLD_YELLOW) {
- slideNormalDistance = true;
- }
-
- if (slideDistance > SLIDE_DISTANCE_THRESHOLD_RED && !slideLongDistance) {
- // 鍚庢粦瓒呰繃30鍘樼背, 涓嶅悎鏍�
- AddExamFault(5, rtkTime);
- DEBUG("鍚庢粦瓒呰繃30鍘樼背");
- slideLongDistance = true;
- }
- }
- } else {
- if (slideNormalDistance) {
- // 鍚庢粦锛屾墸10鍒�
- AddExamFault(18, rtkTime);
- DEBUG("鍚庢粦瓒呰繃10鍘樼背, 浣嗕笉瓒呰繃30鍘樼背");
- }
-
- slideNormalDistance = false;
- slideLongDistance = false;
- occurSlide = false;
- }
-
- switch (ReadCarStatus(TURN_SIGNAL_LAMP)) {
- case LEFT_TURN_LIGHT:
- if (currTurnSignalStatus != LEFT_TURN_LIGHT) {
- currTurnSignalStatus = LEFT_TURN_LIGHT;
- Rtk2DriveTimer(turnSignalChangeTime, rtkTime);
- }
- break;
- case RIGHT_TURN_LIGHT:
- if (currTurnSignalStatus != RIGHT_TURN_LIGHT) {
- currTurnSignalStatus = RIGHT_TURN_LIGHT;
- Rtk2DriveTimer(turnSignalChangeTime, rtkTime);
- }
- break;
- default:
- currTurnSignalStatus = ReadCarStatus(TURN_SIGNAL_LAMP);
- break;
- }
-
- // 妫�鏌ユ槸鍚︽寔缁浆鍚�
- char turnDirect = CheckCarTurn(CarModelList);
- if (turnDirect == 'L') {
-// PlayTTS("宸�1");
- if (currTurnSignalStatus != LEFT_TURN_LIGHT) {
- if (!reportTurnSignalError) {
- DEBUG("娌℃墦宸﹁浆鐏�");
- // 娌℃墦宸﹁浆鐏紝涓嶅悎鏍�
- reportTurnSignalError = true;
- ReportTurnSignalError(13, rtkTime);
- }
- } 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绉掞紝涓嶅悎鏍�
- reportTurnSignalError = true;
- ReportTurnSignalError(14, rtkTime);
- }
- }
- } else if (turnDirect == 'R') {
-// PlayTTS("鍙�1");
- if (currTurnSignalStatus != RIGHT_TURN_LIGHT) {
- if (!reportTurnSignalError) {
- DEBUG("娌℃墦鍙宠浆鐏�");
- // 娌℃墦鍙宠浆鐏紝涓嶅悎鏍�
- reportTurnSignalError = true;
- ReportTurnSignalError(13, rtkTime);
- }
- } 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绉掞紝涓嶅悎鏍�
- reportTurnSignalError = true;
- ReportTurnSignalError(14, rtkTime);
- }
- }
- } else {
- reportTurnSignalError = false;
- }
-
- // 鎾炵孩绾�
- if (CrashRedLine(RoadMapList, car)) {
- if (!occurCrashRedLine) {
- // 杞﹁締琛岄┒涓獞杞ц溅閬撲腑蹇冨疄绾挎垨鑰呰溅閬撹竟缂樺疄绾匡紝涓嶅悎鏍�
- DEBUG("鎾為亾璺竟缂樼嚎");
- AddExamFault(11, rtkTime);
- occurCrashRedLine = true;
- }
- } else {
- occurCrashRedLine = false;
- }
-
- // 鎾炵豢绾�
- static PointF p1, p2;
- if (CrashGreenLine(RoadMapList, car, p1, p2)) {
- // 鍘嬭櫄绾�
- if (moveDirect != 0) {
- if (checkCrashGreenTimeout == 0) {
- checkCrashGreenTimeout = 1;
- 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);
-
- if (diff >= CRASH_DOTTED_LINE_TIMEOUT) {
- DEBUG("闀挎椂闂村帇铏氱嚎");
- checkCrashGreenTimeout = 2;
- // 闀挎椂闂撮獞杞ц溅閬撳垎鐣岀嚎琛岄┒锛屼笉鍚堟牸
- AddExamFault(12, rtkTime);
- }
- }
- } else {
- // 鍋滆溅灏变笉璁℃椂浜�
- checkCrashGreenTimeout = 0;
- }
-
- // 妫�娴嬪綋鍓嶈溅杈嗕簬铏氱嚎鐨勪綅缃紝鍋氬彉閬撴娴�;
- // 妫�娴嬫槸鍚�3绉掑墠鏈夊紑鍚搴斾箣杞悜鐏�
- if (!occurCrashGreenLine) {
- occurCrashGreenLine = true;
- // 璁板綍寮�濮嬪帇绾跨殑鏃堕棿锛屼笉纭畾鏄惁鏈夊彉閬撴剰鍥撅紝寰呯‘璁ゅ彉閬撳悗鍐嶅鐞嗕箣
- Rtk2DriveTimer(crashGreenStartTime, rtkTime);
- turnSignalStatusWhenCrashGreenLine = currTurnSignalStatus;
- }
-
- // p1 ---------------> p2
- double angle = car->yaw - YawOf(p2, p1);
- if (angle < 0 || angle > 180) {
- // 鍙充晶
- carIntersectionOfGreenLine = 'R';
- } else {
- // 宸︿晶
- carIntersectionOfGreenLine = 'L';
- }
- } else {
- // 涓嶅啀鍘嬭櫄绾�
- if (occurCrashGreenLine) {
- int inter = IntersectionOfLine(p1, p2, car->basePoint);
-
- // 瀹屾垚璺ㄧ嚎鍔ㄤ綔
- if ((inter == 1 && carIntersectionOfGreenLine == 'R') ||
- (inter == -1 && carIntersectionOfGreenLine == 'L')) {
- // 姣旇緝涓婃璺ㄧ嚎鏃堕棿
- if (crashGreenCmpTime.hour >= 0) {
- uint32_t diff = TimeGetDiff(rtkTime->hh, rtkTime->mm, rtkTime->ss,
- rtkTime->mss * 10,
- crashGreenCmpTime.hour, crashGreenCmpTime.min,
- crashGreenCmpTime.sec, crashGreenCmpTime.msec * 10);
-
- if (diff < CHANGE_LANE_MIN_INTERVAL) {
- DEBUG("===================== 杩炵画鍙橀亾 ============!!");
- // 杩炵画鍙橀亾锛屼笉鍚堟牸
- AddExamFault(15, rtkTime);
- }
- }
-
- // 璁板綍鏈鍙橀亾鏃堕棿鐐�
- Rtk2DriveTimer(crashGreenCmpTime, rtkTime);
-
- // 妫�鏌ュ彉閬撳墠锛屾槸鍚︽彁鍓嶈浆鍚戠伅
- if (inter == 1) {
-// PlayTTS("宸�2");
- // 鍚戝乏渚у彉閬�
- DEBUG("鍚戝乏渚у彉閬�");
- if (turnSignalStatusWhenCrashGreenLine != LEFT_TURN_LIGHT) {
- DEBUG("鍙樿皟鏈墦鐏�!!");
- // 娌℃墦鐏紝涓嶅悎鏍�
- ReportTurnSignalError(13, rtkTime);
- }
- } else {
-// PlayTTS("鍙�2");
- // 鍚戝彸渚у彉閬�
- DEBUG("鍚戝彸渚у彉閬�");
- if (turnSignalStatusWhenCrashGreenLine != RIGHT_TURN_LIGHT) {
- DEBUG("鍙樿皟鏈墦鐏�!!");
- // 娌℃墦鐏紝涓嶅悎鏍�
- ReportTurnSignalError(14, rtkTime);
- }
- }
- }
- }
- occurCrashGreenLine = false;
- checkCrashGreenTimeout = 0;
- }
-
- // 瀹屾垚璧锋鍚庯紝瑙﹀彂绾挎娴�
- if (currExamMapIndex == -1 && startCar == START_CAR_DONE) {
- currExamMapIndex = CrashTriggerLine(RoadMapList, car, CarModelList);
- if (currExamMapIndex != -1) {
- DEBUG("纰版挒瑙﹀彂绾�");
-
- MA_EnterMap(RoadMapList[currExamMapIndex].id, RoadMapList[currExamMapIndex].type, 1);
-
- if (RoadMapList[currExamMapIndex].type >= THROUGH_INTERSECTION_MAP &&
- RoadMapList[currExamMapIndex].type <= TURN_AROUND_MAP) {
- StartThroughExam(currExamMapIndex, RoadMapList);
- } else if (RoadMapList[currExamMapIndex].type == DRIVE_STRAIGHT_MAP) {
-
- } else if (RoadMapList[currExamMapIndex].type == STOP_CAR_MAP) {
-
- } else if (RoadMapList[currExamMapIndex].type == OP_GEAER_MAP) {
-
- }
- }
- } else if (startCar == START_CAR_DONE) {
- int prevIdx = currExamMapIndex;
-
- if (RoadMapList[currExamMapIndex].type >= THROUGH_INTERSECTION_MAP && currExamMapIndex <= TURN_AROUND_MAP) {
- currExamMapIndex = ExecuteThroughExam(currExamMapIndex, RoadMapList, car,
- CarModelList, speed, moveDirect, rtkTime);
- }
- else if (RoadMapList[currExamMapIndex].type == DRIVE_STRAIGHT_MAP) {
-
- } else if (RoadMapList[currExamMapIndex].type == STOP_CAR_MAP) {
-
- } else if (RoadMapList[currExamMapIndex].type == OP_GEAER_MAP) {
-
- }
-
- if (currExamMapIndex == -1) {
- DEBUG("绂诲紑鍖哄煙 index %d id %d type %d", prevIdx, RoadMapList[prevIdx].id, RoadMapList[prevIdx].type);
-
- MA_EnterMap(RoadMapList[prevIdx].id, RoadMapList[prevIdx].type, 0);
- }
- }
-}
-
-
-bool ExitSonArea(int index, LIST_ROAD_MAP &RoadMapList, const car_model *car)
-{
- bool ret = false;
-
- if (index < 0 || index > RoadMapList.size())
- return true;
-
- if (RoadMapList[index].area.point != NULL) {
- // 闇�瑕佽溅韬叏閮ㄧ寮�
- 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]];
- }
-
- if (IntersectionOf(&carBody, &RoadMapList[index].area) == GM_None) {
- ret = true;
- }
-
- free(carBody.point);
- }
-
- return ret;
-}
-
-bool CrashSonRedLine(int index, LIST_ROAD_MAP &RoadMapList, const car_model *car, LIST_CAR_MODEL &CarModelList)
-{
- bool ret = false;
-
- if (CarModelList.size() < 5 || index < 0 || index >= RoadMapList.size())
- return ret;
-
- Polygon trace;
-
- trace.num = 5; // 鏈�杩�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 j = 0; j < RoadMapList[index].redLineNum; ++j) {
- Line red_line;
-
- int kp = 0;
-
- // 瑙﹀彂绾夸竴鑸簲璇ュ彧鏈夐灏�2鐐癸紙id, p1, p2锛�
- for (int k = 1; k < RoadMapList[index].redLine[j].num; ++k) {
- MakeLine(&red_line, &RoadMapList[index].redLine[j].point[kp],
- &RoadMapList[index].redLine[j].point[k]);
-
- int pp = 0;
- for (int p = 1; p < pn; ++p) {
- Line trace_line;
- MakeLine(&trace_line, &trace.point[pp], &trace.point[p]);
-
- if (IntersectionOf(trace_line, red_line) == GM_Intersection) {
- // 纰板埌瑙﹀彂绾�
- ret = true;
- goto SEARCH_SON_RED_LINE_END;
- }
-
- pp = p;
- }
-
- kp = k;
- }
- }
-
- SEARCH_SON_RED_LINE_END:
- free(trace.point);
-
- return ret;
-}
-#endif
void Rtk2DriveTimer(struct drive_timer &tm, const struct RtkTime *rtkTime)
{
--
Gitblit v1.8.0