From 76859aa4b23ea8ebd90bd7553fd70e144bdc96ba Mon Sep 17 00:00:00 2001
From: yy1717 <fctom1215@outlook.com>
Date: 星期五, 15 五月 2020 15:53:20 +0800
Subject: [PATCH] 坐标
---
lib/src/main/cpp/test_items2/road_exam.cpp | 276 +++++++++++++++++++++++++++++++++++++++++++++++-------
1 files changed, 237 insertions(+), 39 deletions(-)
diff --git a/lib/src/main/cpp/test_items2/road_exam.cpp b/lib/src/main/cpp/test_items2/road_exam.cpp
index f712b10..a682012 100644
--- a/lib/src/main/cpp/test_items2/road_exam.cpp
+++ b/lib/src/main/cpp/test_items2/road_exam.cpp
@@ -89,7 +89,8 @@
int road;
int separate;
int lane;
-} CarOnLane;
+} CurrentLane;
+static bool laneChanging;
static const int MAX_ENGINE_RPM = 2500;
static const double START_CAR_MOVE_DISTANCE = 10.0;
@@ -100,6 +101,8 @@
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 NEXT_ROAD_TIP = 100.0; // 鍒拌揪璺彛鍓嶆彁绀轰笅涓�涓�庝箞璧�
static const int CRL_NONE = 0;
static const int CRL_SEP_DOTTED = 1;
@@ -127,6 +130,7 @@
static int CrashRoadLine(road_t &road, const car_model *car);
static bool LaneIsSame(struct car_on_lane lane1, struct car_on_lane lane2);
static bool LaneIsValid(struct car_on_lane lane);
+static void ArrivedRoadEnd(road_t &road, const car_model *car, LIST_CAR_MODEL &CarModelList);
void InitRoadExam(road_exam_map &RoadMap)
{
@@ -167,7 +171,8 @@
currCarOnRoadIndex = FIND_POSITION;
- CarOnLane.road = CarOnLane.separate = CarOnLane.lane = -1;
+ CurrentLane.road = CurrentLane.separate = CurrentLane.lane = -1;
+ laneChanging = false;
InitThroughSomething(RoadMap);
}
@@ -395,6 +400,16 @@
reportStopCarOnRedArea = false;
DEBUG("鍋滆溅浜� %d %d %d %d %d %d %d", rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss);
+
+ if (slideNormalDistance) {
+ // 鍚庢粦锛屾墸10鍒�
+ AddExamFault(18, rtkTime);
+ DEBUG("鍚庢粦瓒呰繃10鍘樼背, 浣嗕笉瓒呰繃30鍘樼背");
+ }
+
+ slideNormalDistance = false;
+ slideLongDistance = false;
+ occurSlide = false;
} else if (moveDirect == -1 && prevMoveDirect == 0) {
DEBUG("寮�濮嬪悗婊�");
stopPoint = car->basePoint;
@@ -402,6 +417,7 @@
}
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)) {
@@ -411,6 +427,7 @@
reportStopCarOnRedArea = true;
}*/
} else if (moveDirect == -1) {
+ // 鎸佺画鍚庢粦
if (occurSlide) {
double slideDistance = DistanceOf(stopPoint, car->basePoint);
@@ -423,18 +440,32 @@
AddExamFault(5, rtkTime);
DEBUG("鍚庢粦瓒呰繃30鍘樼背");
slideLongDistance = true;
+ slideNormalDistance = false;
+ occurSlide = false;
}
}
} 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;
}
// 妫�娴嬮�氳繃璺彛銆佷汉琛岄亾绛夊尯鍩熸椂锛岄噴鏀惧埞杞︽垨鍑忛��
@@ -563,32 +594,174 @@
}
}
+ // 妫�娴嬪帇绾跨姸鎬�
+ bool crashRedLineNow = false;
+ bool crashGreenLineNow = false;
+
if (currExamMapIndex >= 0) {
int crl = CrashRoadLine(RoadMap.roads[currExamMapIndex], car);
if (crl == CRL_NONE) {
- DEBUG("浠�涔堥兘娌″帇");
+// DEBUG("浠�涔堥兘娌″帇");
} else if (crl == CRL_SEP_DOTTED) {
- DEBUG("鍘嬪垎閬撹櫄绾�");
+// DEBUG("鍘嬪垎閬撹櫄绾�");
+ crashGreenLineNow = true;
} else if (crl == CRL_SEP_SOLID) {
- DEBUG("鍘嬪垎閬撳疄绾�");
+// DEBUG("鍘嬪垎閬撳疄绾�");
+ crashRedLineNow = true;
} else if (crl == CRL_EDGE_DOTTED) {
- DEBUG("鍘嬭竟娌胯櫄绾�");
+// DEBUG("鍘嬭竟娌胯櫄绾�");
+ crashGreenLineNow = true;
} else if (crl == CRL_EDGE_SOLID) {
- DEBUG("鍘嬭竟娌垮疄绾�");
+// DEBUG("鍘嬭竟娌垮疄绾�");
+ crashRedLineNow = true;
}
- if (crl != CRL_SEP_DOTTED || crl != CRL_SEP_SOLID) {
- struct car_on_lane lane;
- UpdateLane(lane, RoadMap.roads[currExamMapIndex], car);
- if (!LaneIsSame(lane, CarOnLane)) {
- if (LaneIsValid(CarOnLane)) {
- // 杞﹂亾鍙樻崲
- DEBUG("鍙樻洿杞﹂亾");
+ struct car_on_lane lane;
+ if (UpdateLane(lane, RoadMap.roads[currExamMapIndex], car)) {
+
+ if (lane.road == CurrentLane.road && lane.separate == CurrentLane.separate &&
+ lane.lane == CurrentLane.lane) {
+ laneChanging = false;
+ }
+
+ if (lane.road == CurrentLane.road && lane.separate == CurrentLane.separate &&
+ lane.lane != CurrentLane.lane) {
+ if (crl == CRL_SEP_DOTTED) {
+ // 鍙橀亾杩涜涓�
+ if (!laneChanging) {
+ laneChanging = true;
+ DEBUG("鍙戠敓鍙橀亾");
+ // 姣旇緝涓婃璺ㄧ嚎鏃堕棿
+ 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_ROAD_MIN_INTERVAL) {
+ DEBUG("===================== 杩炵画鍙橀亾 ============!!");
+ // 杩炵画鍙橀亾锛屼笉鍚堟牸
+ AddExamFault(15, rtkTime);
+ }
+ }
+
+ // 妫�鏌ュ彉閬撳墠锛屾槸鍚︽彁鍓嶈浆鍚戠伅
+ if (lane.lane < CurrentLane.lane) {
+ // 鍚戝乏渚у彉閬�
+ DEBUG("鍚戝乏渚у彉閬�");
+ if (currTurnSignalStatus != LEFT_TURN_LIGHT) {
+ DEBUG("鍙樿皟鏈墦鐏�!!");
+ // 娌℃墦鐏紝涓嶅悎鏍�
+ 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 {
+ // 鍚戝彸渚у彉閬�
+ DEBUG("鍚戝彸渚у彉閬�");
+ if (currTurnSignalStatus != RIGHT_TURN_LIGHT) {
+ DEBUG("鍙樿皟鏈墦鐏�!!");
+ // 娌℃墦鐏紝涓嶅悎鏍�
+ 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 {
+ // 鍙橀亾瀹屾垚
+ DEBUG("鍙橀亾瀹屾垚");
+ CurrentLane = lane;
+ laneChanging = false;
+
+ // 璁板綍鏈鍙橀亾鏃堕棿鐐�
+ Rtk2DriveTimer(crashGreenCmpTime, rtkTime);
}
- CarOnLane = lane;
+ }
+
+ if (lane.road != CurrentLane.road || lane.separate != CurrentLane.separate) {
+ // 璺垨娈靛彉鏇达紝鎾ら攢鍙橀亾璺熻釜
+ DEBUG("============== 璺垨娈靛彉鏇� CURR %d, %d NEW %d, %d", CurrentLane.road,
+ CurrentLane.separate,
+ lane.road, lane.separate);
+ CurrentLane = lane;
+ laneChanging = false;
}
}
+ }
+
+ // 鎾炵孩绾�
+ if (crashRedLineNow) {
+ if (!occurCrashRedLine) {
+ // 杞﹁締琛岄┒涓獞杞ц溅閬撲腑蹇冨疄绾挎垨鑰呰溅閬撹竟缂樺疄绾匡紝涓嶅悎鏍�
+ DEBUG("鎾為亾璺竟缂樼嚎");
+ AddExamFault(11, rtkTime);
+ occurCrashRedLine = true;
+ }
+ } else {
+ occurCrashRedLine = false;
+ }
+
+ // 鍘嬭櫄绾�
+ if (crashGreenLineNow) {
+ // 鍘嬭櫄绾�
+ 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;
+ }
+ } else {
+ // 涓嶅啀鍘嬭櫄绾�
+ occurCrashGreenLine = false;
+ checkCrashGreenTimeout = 0;
}
}
@@ -636,6 +809,8 @@
rightExt = true;
MakeLine(&rightExtLine, &car->carXY[car->axial[AXIAL_FRONT]], &vp);
goto RIGHT_EXT_CMP;
+ } else {
+ DEBUG("鍙充晶涓嶅瀭鐐� %d p1(%f,%f) p2(%f,%f) (%f,%f)", j, p1.X, p1.Y, p2.X, p2.Y, vp.X, vp.Y);
}
p1 = p2;
}
@@ -643,6 +818,7 @@
RIGHT_EXT_CMP:
if (!leftExt || !rightExt) {
+ DEBUG("宸﹀彸杈圭晫涓嶅尮閰�");
return false;
}
@@ -669,20 +845,20 @@
orthogonal.insert(pair<int, int>(j, 1));
orthogonalInSegment = true;
intersection = true;
- DEBUG("鍒嗛亾绾� %d 宸︽浜�", j);
+// DEBUG("鍒嗛亾绾� %d 宸︽浜�", j);
break;
} else if (IntersectionOf(rightExtLine, sep) == GM_Intersection) {
orthogonal.insert(pair<int, int>(j, 2));
orthogonalInSegment = true;
intersection = true;
- DEBUG("鍒嗛亾绾� %d 鍙虫浜�", j);
+// DEBUG("鍒嗛亾绾� %d 鍙虫浜�", j);
break;
}
p1 = p2;
}
}
}
- DEBUG("鐩爣 %d 褰撳墠 %d", road.separate[i].lines.size(), orthogonal.size());
+// DEBUG("鐩爣 %d 褰撳墠 %d", road.separate[i].lines.size(), orthogonal.size());
if (orthogonal.size() > 0) {
if (orthogonal.size() == road.separate[i].lines.size()) {
@@ -696,7 +872,7 @@
lane.separate = i;
lane.lane = itx->first;
- DEBUG("璺� %d 娈� %d 杞﹂亾 %d", lane.road, lane.separate, lane.lane);
+// DEBUG("璺� %d 娈� %d 杞﹂亾 %d", lane.road, lane.separate, lane.lane);
break;
}
}
@@ -706,16 +882,18 @@
lane.separate = i;
lane.lane = orthogonal.size();
- DEBUG("璺� %d 娈� %d 杞﹂亾 %d", lane.road, lane.separate, lane.lane);
+// DEBUG("璺� %d 娈� %d 杞﹂亾 %d", lane.road, lane.separate, lane.lane);
}
out = lane;
return true;
} else {
// 涓嶅畬鍏ㄦ浜わ紝鐩存帴閫�鍑�
}
+ DEBUG("鍒嗛亾绾块暱鐭笉鍚岋紝閫�鍑�");
return false;
}
}
+ DEBUG("娈靛尮閰嶅け璐�");
return false;
}
@@ -800,12 +978,12 @@
// 鍘嬪疄绾�
return CRL_SEP_SOLID;
} else if (road.separate[i].lines[j][k].character == LINE_HALF_SOLID_LEFT) {
- if (LaneIsValid(CarOnLane) && CarOnLane.lane <= j) {
+ if (LaneIsValid(CurrentLane) && CurrentLane.lane <= j) {
return CRL_SEP_SOLID;
}
return CRL_SEP_DOTTED;
} else if (road.separate[i].lines[j][k].character == LINE_HALF_SOLID_RIGHT) {
- if (LaneIsValid(CarOnLane) && CarOnLane.lane > j) {
+ if (LaneIsValid(CurrentLane) && CurrentLane.lane > j) {
return CRL_SEP_SOLID;
}
return CRL_SEP_DOTTED;
@@ -1164,11 +1342,11 @@
RoadMapList[currExamMapIndex].type <= TURN_AROUND_MAP) {
StartThroughExam(currExamMapIndex, RoadMapList);
} else if (RoadMapList[currExamMapIndex].type == DRIVE_STRAIGHT_MAP) {
- StartDriveStraightExam(currExamMapIndex, RoadMapList);
+
} else if (RoadMapList[currExamMapIndex].type == STOP_CAR_MAP) {
- StartStopCarExam(currExamMapIndex, RoadMapList);
+
} else if (RoadMapList[currExamMapIndex].type == OP_GEAER_MAP) {
- StartOperateGearExam(currExamMapIndex, RoadMapList, rtkTime);
+
}
}
} else if (startCar == START_CAR_DONE) {
@@ -1179,14 +1357,11 @@
CarModelList, speed, moveDirect, rtkTime);
}
else if (RoadMapList[currExamMapIndex].type == DRIVE_STRAIGHT_MAP) {
- currExamMapIndex = ExecuteDriveStraightExam(currExamMapIndex, RoadMapList, car,
- CarModelList, speed, moveDirect, rtkTime);
+
} else if (RoadMapList[currExamMapIndex].type == STOP_CAR_MAP) {
- currExamMapIndex = ExecuteStopCarExam(currExamMapIndex, RoadMapList, car,
- CarModelList, speed, moveDirect, rtkTime);
+
} else if (RoadMapList[currExamMapIndex].type == OP_GEAER_MAP) {
- currExamMapIndex = ExecuteOperateGearExam(currExamMapIndex, RoadMapList, car,
- CarModelList, speed, moveDirect, rtkTime);
+
}
if (currExamMapIndex == -1) {
@@ -1636,6 +1811,29 @@
return px;
}
+static void ArrivedRoadEnd(road_t &road, const car_model *car, LIST_CAR_MODEL &CarModelList)
+{
+ // 璁＄畻杞﹀墠杩涜建杩瑰欢闀跨嚎
+ double yaw = YawOf(car->carXY[ car->axial[AXIAL_FRONT] ], car->carXY[ car->axial[AXIAL_REAR] ]);
+
+ PointF extPoint = PointExtend(car->carXY[ car->axial[AXIAL_FRONT] ], NEXT_ROAD_TIP, yaw);
+ Line extLine;
+
+ MakeLine(&extLine, &car->carXY[ car->axial[AXIAL_FRONT] ], &extPoint);
+
+ if (IntersectionOf(extLine, road.stopLine) == GM_Intersection &&
+ IntersectionOfLine(extPoint, road.stopLine) == -1) {
+ if (DistanceOf(extPoint, road.stopLine) > 1.0 && !road.arrivedTail) {
+ // 鎺ヨ繎璺彛鍚庯紝瑕佹鏌ヨ溅杈嗘槸鍚﹁繘鍏ラ敊璇溅閬�
+ road.arrivedTail = true;
+ if (!road.tts.empty())
+ PlayTTS(road.tts.c_str());
+ }
+ } else if (road.arrivedTail) {
+ road.arrivedTail = false;
+ }
+}
+
#if 0
typedef struct {
--
Gitblit v1.8.0