From 539bdbbebc2410bbad25af01d9088594d02c95b0 Mon Sep 17 00:00:00 2001
From: yy1717 <fctom1215@outlook.com>
Date: 星期三, 12 八月 2020 18:45:58 +0800
Subject: [PATCH] 坐标
---
lib/src/main/cpp/test_items2/road_exam.cpp | 837 +++++++++++++++--------------------------------------------
1 files changed, 219 insertions(+), 618 deletions(-)
diff --git a/lib/src/main/cpp/test_items2/road_exam.cpp b/lib/src/main/cpp/test_items2/road_exam.cpp
index 674f248..e476e9a 100644
--- a/lib/src/main/cpp/test_items2/road_exam.cpp
+++ b/lib/src/main/cpp/test_items2/road_exam.cpp
@@ -42,6 +42,26 @@
STOP_CAR_DONE
};
+typedef struct {
+ int road;
+ int sep;
+ int no;
+ int guide;
+} lane_t;
+
+typedef struct {
+ int edgeIndex;
+ int pointIndex;
+ PointF point;
+} projection_t;
+
+typedef struct
+{
+ int name;
+ int value;
+ struct RtkTime time;
+} car_sensor_value_t;
+
static const int INVALID_ROAD = -1;
static const int TURN_THRESHOLD = 1;
@@ -65,7 +85,7 @@
static bool occurCrashGreenLine;
static bool occurOverSpeed;
static bool occurSecondBreak;
-static int checkCrashGreenTimeout;
+
static char carIntersectionOfGreenLine;
static int currTurnSignalStatus;
static int turnSignalStatusWhenCrashGreenLine;
@@ -82,7 +102,7 @@
static bool occurSlide;
static bool startCarLeftTurnSignal, checkStartCarSignal;
-static struct drive_timer crashGreenRunTime, crashGreenCmpTime, crashGreenStartTime, turnSignalChangeTime;
+static struct RtkTime crashGreenRunTime, crashGreenStartTime;
static struct drive_timer gearErrorTimePoint;
static struct drive_timer gearNSlideTimePoint;
static struct drive_timer startCarLeftTurnSignalTime;
@@ -106,30 +126,14 @@
static const uint32_t TURN_ERROR_COLD_TIME = D_SEC(10);
static bool turnError13Cold, turnError14Cold;
-static struct car_on_lane {
- int road;
- int separate;
- int lane;
- int direct; // 杞﹂亾鏂瑰悜闄愬埗
- int type; // 瀹炵嚎锛岃櫄绾�
-} CurrentLane;
+
+static lane_t Lane;
+
static bool laneChanging;
static int changeLaneDirect;
+static int CrashLineType;
-
-
-typedef struct {
- int road;
- int sep;
- int lane;
- int guide;
-} lane_t;
-
-typedef struct {
- int edgeIndex;
- int pointIndex;
- PointF point;
-} projection_t;
+static map<int, car_sensor_value_t> CarSensorValue;
static const int MAX_ENGINE_RPM = 2500;
static const double START_CAR_MOVE_DISTANCE = 10.0;
@@ -147,7 +151,7 @@
static const int CRL_EDGE_DOTTED = 3;
static const int CRL_EDGE_SOLID = 4;
-static const double MAX_SPEED = 40.0 * 1000.0 / 3600.0;
+static const double MAX_SPEED = 60.0 * 1000.0 / 3600.0;
static const int SPEED_GEAR_TABLE[][2] = {{0, 20}, {5, 30}, {15, 40}, {25, 10000}, {35, 10000}};
static int TestRoadStartCar(const car_model *car, double speed, int moveDirect, const struct RtkTime *rtkTime);
@@ -158,9 +162,8 @@
static void TurnSignalError13ColdTimer(union sigval sig);
static void TurnSignalError14ColdTimer(union sigval sig);
static void ReportTurnSignalError(int err, const struct RtkTime *rtkTime);
-static bool UpdateLane(struct car_on_lane &out, road_t &road, const car_model *car);
-static int CrashRoadLine(road_t &road, const car_model *car);
-static void ArrivedRoadEnd(road_t &road, const car_model *car, LIST_CAR_MODEL &CarModelList);
+
+
static trigger_line_t * EntryItem(int index, road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList);
static void ClearAll(road_exam_map &map);
static bool AllCmp(road_exam_map &map);
@@ -174,13 +177,12 @@
{
DEBUG("Start road_exam");
- crashGreenCmpTime.hour = -1;
occurCrashRedLine = false;
occurCrashGreenLine = false;
occurOverSpeed = false;
occurSecondBreak = false;
- checkCrashGreenTimeout = 0;
+
carIntersectionOfGreenLine = 0;
reportTurnSignalError = false;
@@ -211,13 +213,15 @@
turnError13Cold = turnError14Cold = true;
- CurrentLane.road = CurrentLane.separate = CurrentLane.lane = -1;
+ Lane.road = Lane.sep = Lane.no = -1;
laneChanging = false;
changeLaneDirect = 0;
nextRoadId = -1;
checkTurn = false;
ClearAll(RoadMap);
+
+ CrashLineType = -1;
ResetOdo();
@@ -489,29 +493,26 @@
/************************************************
* 杞﹁疆鍘嬪疄绾匡紝鍓嶅悗杞翠氦鍙夛紝鍓嶅悗杞抗浜ゅ弶
- * @param mode 1 - 閬撹矾瀹炵嚎/鍒嗛亾瀹炵嚎 2 - 鍒嗛亾铏氱嚎/鍗婂疄鍗婅櫄绾�
+ * @param currCrashLineType
* @param RoadMap
* @param car
* @param CarModelList
* @return
*/
-static bool CrashRedLine(int mode, int roadIndex, road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList)
+static int CrashRedLine(int currCrashLineType, int roadIndex, road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList)
{
Line frontTireAxle, rearTireAxle;
Line frontLeftTireTrack, frontRightTireTrack;
Line rearLeftTireTrack, rearRightTireTrack;
bool track = false;
int lineType = -1;
-
+ PointF p11, p12, p21, p22, p31, p32, p41, p42;
MakeLine(&frontTireAxle, &car->carXY[car->left_front_tire[TIRE_OUTSIDE]], &car->carXY[car->right_front_tire[TIRE_OUTSIDE]]);
MakeLine(&rearTireAxle, &car->carXY[car->left_rear_tire[TIRE_OUTSIDE]], &car->carXY[car->right_rear_tire[TIRE_OUTSIDE]]);
if (CarModelList.size() >= 2) {
list<car_model *>::iterator iter = CarModelList.begin();
-
- PointF p11, p12, p21, p22, p31, p32, p41, p42;
-
p11 = ((car_model *)(*iter))->carXY[((car_model *)(*iter))->left_front_tire[TIRE_OUTSIDE]];
p21 = ((car_model *)(*iter))->carXY[((car_model *)(*iter))->right_front_tire[TIRE_OUTSIDE]];
p31 = ((car_model *)(*iter))->carXY[((car_model *)(*iter))->left_rear_tire[TIRE_OUTSIDE]];
@@ -528,7 +529,7 @@
struct RtkTime time2 = ((car_model *)(*iter))->tm;
- if (TimeGetDiff(time1.hh, time1.mm, time1.ss, time1.mss, time2.hh, time2.mm, time2.ss, time2.mss) <= D_SEC(1)) {
+ if (TimeGetDiff(&time1, &time2) <= D_SEC(1)) {
MakeLine(&frontLeftTireTrack, &p11, &p12);
MakeLine(&frontRightTireTrack, &p21, &p22);
MakeLine(&rearLeftTireTrack, &p31, &p32);
@@ -635,24 +636,19 @@
}
CRASH_LINE_CONFIRM:
- if (lineType == LINE_HALF_SOLID_LEFT) {
- if (currCrashLineType != LINE_HALF_SOLID_LEFT && track) {
- if (IntersectionOfLine(car->carXY[car->right_front_tire[TIRE_OUTSIDE]], redLine) == -1) {
- // 闈炴硶璺ㄧ嚎
- }
+ if (lineType == LINE_HALF_SOLID_LEFT && currCrashLineType != lineType && track) {
+ if (IntersectionOfLine(p21, redLine) == -1 && IntersectionOfLine(p22, redLine) == 1) {
+ // 闈炴硶璺ㄧ嚎
+ lineType += 100;
}
- } else if (lineType == LINE_HALF_SOLID_RIGHT) {
- if (currCrashLineType != LINE_HALF_SOLID_RIGHT && track) {
- if (IntersectionOfLine(car->carXY[car->left_front_tire[TIRE_OUTSIDE]], redLine) == 1) {
- // 闈炴硶璺ㄧ嚎
- }
+ } else if (lineType == LINE_HALF_SOLID_RIGHT && currCrashLineType != lineType && track) {
+ if (IntersectionOfLine(p11, redLine) == 1 && IntersectionOfLine(p12, redLine) == -1) {
+ // 闈炴硶璺ㄧ嚎
+ lineType += 100;
}
}
-
- return false;
+ return lineType;
}
-
-
static int GetGuideDirect(road_exam_map &RoadMap, PointF point, int roadIndex, int sepIndex, int laneNo)
{
@@ -673,7 +669,7 @@
* @param roadIndex
* @return
*/
-static bool GetLane(lane_t &theLane, PointF point, road_exam_map &RoadMap, int roadIndex)
+static bool GetLane(lane_t &lane, PointF point, road_exam_map &RoadMap, int roadIndex)
{
Line leftProjLine, rightProjLine;
Line sep;
@@ -694,7 +690,7 @@
char pos; // 鐐瑰湪杞﹂亾鐨勫乏鍙充綅缃�
bool operator == (const int &x) {
- return(this->lane_no == x);
+ return (this->lane_no == x);
}
};
@@ -770,22 +766,65 @@
}
}
- theLane.road = roadIndex;
- theLane.sep = i;
- theLane.lane = CLP.size();
+ lane.road = roadIndex;
+ lane.sep = i;
+ lane.no = CLP.size();
for (int x = 0; x < CLP.size(); ++x) {
if (CLP[x].pos == 'L') {
- theLane.lane = x;
+ lane.no = x;
break;
}
}
- theLane.guide = GetGuideDirect(RoadMap, p0, theLane.road, theLane.sep, theLane.lane);
+ lane.guide = GetGuideDirect(RoadMap, p0, lane.road, lane.sep, lane.no);
return true;
}
}
return false;
+}
+
+static void WriteCarSensorValue(int name, int value, const struct RtkTime *rtkTime)
+{
+ car_sensor_value_t nv;
+
+ nv.name = name;
+ nv.value = value;
+ nv.time = *rtkTime;
+
+ auto it = CarSensorValue.find(name);
+
+ if (it != CarSensorValue.end()) {
+ if (it->second.value != value || name == ENGINE_RPM) {
+ CarSensorValue[name] = nv;
+ }
+ } else {
+ CarSensorValue.insert(pair<int, car_sensor_value_t>(name, nv));
+ }
+}
+
+static car_sensor_value_t ReadCarSensorValue(int name)
+{
+ car_sensor_value_t nv = {.name = -1};
+
+ auto it = CarSensorValue.find(name);
+
+ if (it != CarSensorValue.end()) {
+ nv = it->second;
+ }
+
+ return nv;
+}
+
+static void UpdateCarSensor(const struct RtkTime *rtkTime)
+{
+ WriteCarSensorValue(TURN_SIGNAL_LAMP, ReadCarStatus(TURN_SIGNAL_LAMP), rtkTime);
+ WriteCarSensorValue(HAND_BREAK, ReadCarStatus(HAND_BREAK), rtkTime);
+ WriteCarSensorValue(DOOR, ReadCarStatus(DOOR), rtkTime);
+ WriteCarSensorValue(ENGINE_RPM, ReadCarStatus(ENGINE_RPM), rtkTime);
+ WriteCarSensorValue(SECOND_BREAK, ReadCarStatus(SECOND_BREAK), rtkTime);
+ WriteCarSensorValue(GEAR, ReadCarStatus(GEAR), rtkTime);
+ WriteCarSensorValue(BREAK, ReadCarStatus(BREAK), rtkTime);
}
static void TurnSignalError13ColdTimer(union sigval sig)
@@ -893,17 +932,119 @@
return startCar;
}
+static void DetectLine(int roadIndex, road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList, int moveDirect, const struct RtkTime *rtkTime)
+{
+ static int checkCrashGreenTimeout = 0;
+ static bool crashDottedLine = false;
+ int newLineType = CrashRedLine(CrashLineType, roadIndex, RoadMap, car, CarModelList);
+
+ if (newLineType >= 100) {
+ newLineType -= 100;
+ DEBUG("闈炴硶璺ㄨ秺鍒嗛亾绾�");
+ AddExamFault(11, rtkTime);
+ }
+ if (newLineType == LINE_SOLID && CrashLineType != LINE_SOLID) {
+ // 杞﹁締琛岄┒涓獞杞ц溅閬撲腑蹇冨疄绾挎垨鑰呰溅閬撹竟缂樺疄绾匡紝涓嶅悎鏍�
+ DEBUG("鎾為亾璺竟缂樼嚎");
+ AddExamFault(11, rtkTime);
+ }
+ if (newLineType == LINE_DOTTED || newLineType == LINE_HALF_SOLID_LEFT || newLineType == LINE_HALF_SOLID_RIGHT) {
+ if (!crashDottedLine) {
+ checkCrashGreenTimeout = 0;
+ // 璁板綍寮�濮嬪帇绾跨殑鏃堕棿锛屼笉纭畾鏄惁鏈夊彉閬撴剰鍥撅紝寰呯‘璁ゅ彉閬撳悗鍐嶅鐞嗕箣
+ crashGreenStartTime = *rtkTime;
+ }
+ crashDottedLine = true;
+ } else {
+ crashDottedLine = false;
+ }
+
+ if (crashDottedLine) {
+ // 鍘嬭櫄绾�
+ if (moveDirect != 0) {
+ if (checkCrashGreenTimeout == 0) {
+ checkCrashGreenTimeout = 1;
+ crashGreenRunTime = *rtkTime; // 杩愬姩涓帇铏氱嚎鐨勫紑濮嬫椂闂寸偣
+ } else if (checkCrashGreenTimeout == 1) {
+ if (TimeGetDiff(rtkTime, &crashGreenRunTime) >= CRASH_DOTTED_LINE_TIMEOUT) {
+ DEBUG("闀挎椂闂村帇铏氱嚎");
+ checkCrashGreenTimeout = 2;
+ // 闀挎椂闂撮獞杞ц溅閬撳垎鐣岀嚎琛岄┒锛屼笉鍚堟牸
+ AddExamFault(12, rtkTime);
+ }
+ }
+ } else {
+ // 鍋滆溅灏变笉璁℃椂浜�
+ checkCrashGreenTimeout = 0;
+ }
+ } else {
+ // 涓嶅啀鍘嬭櫄绾�
+ checkCrashGreenTimeout = 0;
+ }
+
+ CrashLineType = newLineType;
+}
+
+int DetectLane(road_exam_map &RoadMap, const car_model *car, int roadIndex, const struct RtkTime *rtkTime)
+{
+ lane_t newLane;
+ int gain = 0;
+
+ if (CrashLineType == LINE_DOTTED || CrashLineType == LINE_HALF_SOLID_LEFT || CrashLineType == LINE_HALF_SOLID_RIGHT) {
+ // 鎺掗櫎璺ㄧ嚎涓�
+ return 0;
+ }
+ if (GetLane(newLane, car->carXY[car->axial[AXIAL_FRONT]], RoadMap, roadIndex)) {
+ if (newLane.road == Lane.road && newLane.sep == Lane.sep) {
+ gain = newLane.no - Lane.no;
+ }
+
+ // 妫�鏌ヨ浆鍚戠伅
+ if (gain != 0) {
+ car_sensor_value_t lamp = ReadCarSensorValue(TURN_SIGNAL_LAMP);
+ if (lamp.name == TURN_SIGNAL_LAMP) {
+ if (gain < 0) {
+ if (lamp.value != LEFT_TURN_LIGHT) {
+ DEBUG("鍙樿皟鏈墦鐏�!!");
+ // 娌℃墦鐏紝涓嶅悎鏍�
+ ReportTurnSignalError(13, rtkTime);
+ } else if (TimeGetDiff(&crashGreenStartTime, &lamp.time) >= D_SEC(3)) {
+ DEBUG("杞悜鐏椂闂翠笉瓒�");
+ // 涓嶈冻3绉掞紝涓嶅悎鏍�
+ reportTurnSignalError = true;
+ ReportTurnSignalError(14, rtkTime);
+ }
+ } else {
+ if (lamp.value != RIGHT_TURN_LIGHT) {
+ DEBUG("鍙樿皟鏈墦鐏�!!");
+ // 娌℃墦鐏紝涓嶅悎鏍�
+ ReportTurnSignalError(13, rtkTime);
+ } else {
+ DEBUG("杞悜鐏椂闂翠笉瓒�");
+ // 涓嶈冻3绉掞紝涓嶅悎鏍�
+ reportTurnSignalError = true;
+ ReportTurnSignalError(14, rtkTime);
+ }
+ }
+ }
+ }
+
+ Lane = newLane;
+ }
+ return gain;
+}
+
void TestRoadGeneral(road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime)
{
uint32_t cts = AppTimer_GetTickCount();
int ri = CalcRoadIndex(-1, RoadMap, car);
- bool crash = CrashRedLine(0, 0, RoadMap, car, CarModelList);
+ bool crash = CrashRedLine(CrashLineType, 0, RoadMap, car, CarModelList);
lane_t laneInfo;
double redist = -1;
laneInfo.road = -1;
laneInfo.sep = -1;
- laneInfo.lane = -1;
+ laneInfo.no = -1;
if (ri >= 0) {
GetLane(laneInfo, car->carXY[car->axial[AXIAL_FRONT]], RoadMap, ri);
@@ -919,7 +1060,9 @@
RoadMap.roads[ri].rightEdge);
}
- DEBUG("褰撳墠閬撹矾绱㈠紩 %d, 瑙﹀彂绾㈢嚎 %d lane %d 璺濈 %f %ld", ri, crash, laneInfo.lane, redist, AppTimer_GetTickCount() - cts);
+ DEBUG("褰撳墠閬撹矾绱㈠紩 %d, 瑙﹀彂绾㈢嚎 %d lane %d 璺濈 %f %ld", ri, crash, laneInfo.no, redist, AppTimer_GetTickCount() - cts);
+
+ UpdateCarSensor(rtkTime);
UpdataOdo(speed, moveDirect, rtkTime);
@@ -1076,24 +1219,6 @@
}
- // 杞悜鐏鍙�
- 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;
- }
// 妫�娴嬮�氳繃璺彛銆佷汉琛岄亾绛夊尯鍩熸椂锛岄噴鏀惧埞杞︽垨鍑忛��
CheckBreakActive(RoadMap, car, CarModelList,
@@ -1102,268 +1227,12 @@
// 妫�娴嬬寮�姝よ矾娈碉紝鍏ㄨ溅闇�涓嶅湪鑼冨洿鍐�
currExamMapIndex = CalcRoadIndex(currExamMapIndex, RoadMap, car);
+ DetectLine(currExamMapIndex, RoadMap, car, CarModelList, moveDirect, rtkTime);
+
+ DetectLane(RoadMap, car, currExamMapIndex, rtkTime);
+
// 妫�娴嬪帇绾跨姸鎬�
- bool crashRedLineNow = false;
- bool crashGreenLineNow = false;
- if (currExamMapIndex >= 0) {
- int crl = CrashRoadLine(RoadMap.roads[currExamMapIndex], car);
-
- if (crl == CRL_NONE) {
-// DEBUG("浠�涔堥兘娌″帇");
- } else if (crl == CRL_SEP_DOTTED) {
- DEBUG("鍘嬪垎閬撹櫄绾�");
- crashGreenLineNow = true;
- } else if (crl == CRL_SEP_SOLID) {
- DEBUG("鍘嬪垎閬撳疄绾�");
- crashRedLineNow = true;
- } else if (crl == CRL_EDGE_DOTTED) {
- DEBUG("鍘嬭竟娌胯櫄绾�");
- crashGreenLineNow = true;
- } else if (crl == CRL_EDGE_SOLID) {
- DEBUG("鍘嬭竟娌垮疄绾�");
- crashRedLineNow = true;
- }
-
- // 妫�鏌ュ綋鍓嶈溅閬�
- 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);
- int laneDirect = 0;
-
- if (CurrentLane.lane > lane.lane) {
- laneDirect = -1;
- } else {
- laneDirect = 1;
- }
-
- if (diff < CHANGE_LANE_MIN_INTERVAL && laneDirect == changeLaneDirect) {
- DEBUG("===================== 杩炵画鍙橀亾 %d -> %d ============", CurrentLane.lane, lane.lane);
- // 杩炵画鍙橀亾锛屼笉鍚堟牸
- 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("鍙橀亾瀹屾垚 %d -> %d", CurrentLane.lane, lane.lane);
-
- 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;
- laneChanging = false;
- // 璁板綍鏈鍙橀亾鏃堕棿鐐�
- Rtk2DriveTimer(crashGreenCmpTime, rtkTime);
- }
- }
-
- if (lane.road != CurrentLane.road || lane.separate != CurrentLane.separate || lane.direct != CurrentLane.direct) {
- // 璺垨娈靛彉鏇达紝鎾ら攢鍙橀亾璺熻釜
- DEBUG("============== 璺垨娈靛彉鏇� CURR %d, %d dir %d NEW %d, %d, dir %d", CurrentLane.road,
- CurrentLane.separate, CurrentLane.direct,
- lane.road, lane.separate, lane.direct);
-
- CurrentLane = lane;
- laneChanging = false;
- }
-
- if (CurrentLane.direct != 0 && !(CurrentLane.direct & RoadMap.roads[currExamMapIndex].active)) {
- if (!RoadMap.roads[currExamMapIndex].errorLane) {
- RoadMap.roads[currExamMapIndex].errorLane = true;
- DEBUG("涓嶆寜瑙勫畾杞﹂亾鏍囩嚎鏂瑰悜琛岄┒ <%d> %d %d", RoadMap.roads[currExamMapIndex].id, CurrentLane.direct, RoadMap.roads[currExamMapIndex].active);
- AddExamFault(9, rtkTime);
- }
- }
- }
-
- ArrivedRoadEnd(RoadMap.roads[currExamMapIndex], car, CarModelList);
-
-// if (CrashTheLine(RoadMap.roads[currExamMapIndex].stopLine, car, CarModelList)) {
-// DEBUG("涓嬩竴涓洰鏍囪矾 id = %d", RoadMap.roads[currExamMapIndex].targetRoad);
-// nextRoadId = RoadMap.roads[currExamMapIndex].targetRoad;
-// checkTurn = true;
-// }
-
- if (checkTurn) {
- // 妫�鏌ユ槸鍚︽寔缁浆鍚�
- 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 (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;
- }
TestRoadStartCar(car, speed, moveDirect, rtkTime);
@@ -1432,274 +1301,6 @@
}
}
}
-}
-
-/*************************************************************
- * 妫�娴嬪綋鍓嶈溅閬擄紝浠ヨ溅澶翠腑鐐逛负鍩哄噯
- * @param road
- * @param car
- */
-static bool UpdateLane(struct car_on_lane &out, road_t &road, const car_model *car)
-{
- bool leftExt = false, rightExt = false;
- Line leftExtLine, rightExtLine;
- struct car_on_lane lane;
-
- for (int i = 0; i < road.leftEdge.size(); ++i) {
- PointF p1, p2;
- Line edge;
-
- p1 = road.leftEdge[i].points[0];
- for (int j = 1; j < road.leftEdge[i].points.size(); ++j) {
- p2 = road.leftEdge[i].points[j];
- MakeLine(&edge, &p1, &p2);
-
- PointF vp;
- if (VerticalPointOnLine(car->carXY[car->axial[AXIAL_FRONT]], edge, vp)) {
- leftExt = true;
- MakeLine(&leftExtLine, &car->carXY[car->axial[AXIAL_FRONT]], &vp);
- goto LEFT_EXT_CMP;
- }
- p1 = p2;
- }
- }
-LEFT_EXT_CMP:
- for (int i = 0; i < road.rightEdge.size(); ++i) {
- PointF p1, p2;
- Line edge;
-
- p1 = road.rightEdge[i].points[0];
- for (int j = 1; j < road.rightEdge[i].points.size(); ++j) {
- p2 = road.rightEdge[i].points[j];
- MakeLine(&edge, &p1, &p2);
-
- PointF vp;
- if (VerticalPointOnLine(car->carXY[car->axial[AXIAL_FRONT]], edge, vp)) {
- 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;
- }
- }
-RIGHT_EXT_CMP:
-
- if (!leftExt || !rightExt) {
- DEBUG("宸﹀彸杈圭晫涓嶅尮閰�");
- return false;
- }
-
- bool orthogonalInSegment = false;
-
- for (int i = 0; i < road.separate.size(); ++i) { // 娈�
- PointF p1, p2;
- Line sep;
-
- map<int, vector<int>> orthogonal;
-
- // 涓�娈靛垎閬撶粍鍐咃紝鏈変竴鏉℃浜わ紝灏卞繀椤讳繚璇佸悓缁勭殑鍏ㄩ兘姝ｄ氦锛屽惁鍒欑洿鎺ラ��鍑�
- for (int j = 0; j < road.separate[i].lines.size(); ++j) { // 绾跨粍
- bool intersection = false;
-
- for (int k = 0; !intersection && k < road.separate[i].lines[j].size(); ++k) { // 鑺�
-
- p1 = road.separate[i].lines[j][k].points[0];
- for (int m = 1; m < road.separate[i].lines[j][k].points.size(); ++m) {
- p2 = road.separate[i].lines[j][k].points[m];
- MakeLine(&sep, &p1, &p2);
-
- if (IntersectionOf(leftExtLine, sep) == GM_Intersection) {
- vector<int> stor(4);
-
- stor[0] = 1;
- stor[1] = road.separate[i].lines[j][k].character;
- stor[2] = road.separate[i].lines[j][k].left_lane_direct;
- stor[3] = road.separate[i].lines[j][k].right_lane_direct;
-
- orthogonal.insert(pair<int, vector<int>>(j, stor));
- orthogonalInSegment = true;
- intersection = true;
-
- break;
- } else if (IntersectionOf(rightExtLine, sep) == GM_Intersection) {
- vector<int> stor(4);
-
- stor[0] = 2;
- stor[1] = road.separate[i].lines[j][k].character;
- stor[2] = road.separate[i].lines[j][k].left_lane_direct;
- stor[3] = road.separate[i].lines[j][k].right_lane_direct;
-
- orthogonal.insert(pair<int, vector<int>>(j, stor));
- orthogonalInSegment = true;
- intersection = true;
-
- break;
- }
- p1 = p2;
- }
- }
- }
-// DEBUG("鐩爣 %d 褰撳墠 %d", road.separate[i].lines.size(), orthogonal.size());
-
- if (orthogonal.size() > 0) {
- if (orthogonal.size() == road.separate[i].lines.size()) {
- // 寰楀埌褰撳墠鍦ㄧ鍑犱釜杞﹂亾
- int x = 0;
- int left_char = LINE_SOLID; // 閬撹矾宸﹁竟绾�
- int right_direct = 0;
-
- for (x = 0; x < orthogonal.size(); ++x) {
- auto itx = orthogonal.find(x);
- if (itx != orthogonal.end()) {
- if (itx->second[0] != 1) { // 鍦ㄨ溅杈嗗彸渚�
- lane.road = road.id;
- lane.separate = i;
- lane.lane = itx->first;
-
-// DEBUG("left_char %d second %d", left_char, itx->second[1]);
-
- if ((left_char == LINE_SOLID || left_char == LINE_HALF_SOLID_RIGHT) &&
- (itx->second[1] == LINE_SOLID || itx->second[1] == LINE_HALF_SOLID_LEFT)) // 杞﹂亾宸﹀彸鍧囨槸瀹炵嚎
- lane.direct = itx->second[2];
- else
- lane.direct = 0;
-// DEBUG("璺� %d 娈� %d 杞﹂亾 %d 闄愬畾 %d", lane.road, lane.separate, lane.lane, lane.direct);
- break;
- } else {
- right_direct = itx->second[3];
- left_char = itx->second[2];
- }
- }
- }
- if (x >= orthogonal.size()) {
- lane.road = road.id;
- lane.separate = i;
- lane.lane = orthogonal.size();
-
-// DEBUG("left_char %d right_direct %d", left_char, right_direct);
-
- // 鏈�鍚庤溅閬撶殑鍙充晶闄愬畾
- if (left_char == LINE_SOLID || left_char == LINE_HALF_SOLID_RIGHT)
- lane.direct = right_direct;
- else
- lane.direct = 0;
-// DEBUG("璺� %d 娈� %d 杞﹂亾 %d 闄愬畾 %d", lane.road, lane.separate, lane.lane, lane.direct);
- }
- out = lane;
- return true;
- } else {
- // 涓嶅畬鍏ㄦ浜わ紝鐩存帴閫�鍑�
- }
- DEBUG("鍒嗛亾绾块暱鐭笉鍚岋紝閫�鍑�");
- return false;
- }
- }
- DEBUG("娈靛尮閰嶅け璐�");
- return false;
-}
-
-/*************************************************************************
- * 纰拌Е杞﹂亾绾垮瀷
- * @param road
- * @param car
- */
-static int CrashRoadLine(road_t &road, const car_model *car)
-{
- Line frontAxle, rearAxle;
-
- MakeLine(&frontAxle, &car->carXY[car->left_front_tire[TIRE_OUTSIDE]], &car->carXY[car->right_front_tire[TIRE_OUTSIDE]]);
- MakeLine(&rearAxle, &car->carXY[car->left_rear_tire[TIRE_OUTSIDE]], &car->carXY[car->right_rear_tire[TIRE_OUTSIDE]]);
-
- // 妫�鏌ラ亾璺竟缂�
- for (int i = 0; i < road.leftEdge.size(); ++i) {
- PointF p1, p2;
- Line edge;
-
- p1 = road.leftEdge[i].points[0];
- for (int j = 1; j < road.leftEdge[i].points.size(); ++j) {
- p2 = road.leftEdge[i].points[j];
- MakeLine(&edge, &p1, &p2);
-
- if (IntersectionOf(edge, frontAxle) == GM_Intersection ||
- IntersectionOf(edge, rearAxle) == GM_Intersection) {
- // 鍘嬮亾璺乏杈圭嚎
- if (road.leftEdge[i].character != LINE_SOLID) {
- return CRL_EDGE_DOTTED;
- }
- return CRL_EDGE_SOLID;
- }
- p1 = p2;
- }
- }
-
- for (int i = 0; i < road.rightEdge.size(); ++i) {
- PointF p1, p2;
- Line edge;
-
- p1 = road.rightEdge[i].points[0];
- for (int j = 1; j < road.rightEdge[i].points.size(); ++j) {
- p2 = road.rightEdge[i].points[j];
- MakeLine(&edge, &p1, &p2);
-
- if (IntersectionOf(edge, frontAxle) == GM_Intersection ||
- IntersectionOf(edge, rearAxle) == GM_Intersection) {
- // 鍘嬮亾璺彸杈圭嚎
- if (road.rightEdge[i].character != LINE_SOLID) {
- return CRL_EDGE_DOTTED;
- }
- return CRL_EDGE_SOLID;
- }
- p1 = p2;
- }
- }
-
- // 妫�鏌ュ垎閬撶嚎
- for (int i = 0; i < road.separate.size(); ++i) {
- // 娈�
- PointF p1, p2;
- Line sep;
-
- for (int j = 0; j < road.separate[i].lines.size(); ++j) {
- // 绾�
- for (int k = 0; k < road.separate[i].lines[j].size(); ++k) {
- // 鑺�
- p1 = road.separate[i].lines[j][k].points[0];
-
- for (int m = 1; m < road.separate[i].lines[j][k].points.size(); ++m) {
- p2 = road.separate[i].lines[j][k].points[m];
- MakeLine(&sep, &p1, &p2);
-
- if (IntersectionOf(sep, frontAxle) == GM_Intersection ||
- IntersectionOf(sep, rearAxle) == GM_Intersection) {
- // 妫�鏌ラ亾璺垎闅旂嚎绫诲瀷
- if (road.separate[i].lines[j][k].character == LINE_DOTTED) {
- // 鍘嬭櫄绾�
- return CRL_SEP_DOTTED;
- } else if (road.separate[i].lines[j][k].character == LINE_SOLID) {
- // 鍘嬪疄绾�
- return CRL_SEP_SOLID;
- } else if (road.separate[i].lines[j][k].character == LINE_HALF_SOLID_LEFT) {
- 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(CurrentLane) && CurrentLane.lane > j) {
- return CRL_SEP_SOLID;
- }
- return CRL_SEP_DOTTED;
- }
- }
-
- p1 = p2;
- }
- }
- }
- }
-
- return CRL_NONE;
}
void Rtk2DriveTimer(struct drive_timer &tm, const struct RtkTime *rtkTime)
@@ -2006,17 +1607,17 @@
*/
static void ClearAll(road_exam_map &map)
{
- for (int i = 0; i < map.roads.size(); ++i) {
- map.roads[i].activeStop = map.roads[i].activeBreak = false;
- map.roads[i].arrivedTail = false;
- map.roads[i].errorLane = false;
- }
- for (int i = 0; i < map.specialAreas.size(); i++) {
- map.specialAreas[i].overSpeed = map.specialAreas[i].activeBreak = false;
- }
- for (int i = 0; i < map.triggerLines.size(); ++i) {
- map.triggerLines[i].cmp = false;
- }
+// for (int i = 0; i < map.roads.size(); ++i) {
+// map.roads[i].activeStop = map.roads[i].activeBreak = false;
+// map.roads[i].arrivedTail = false;
+// map.roads[i].errorLane = false;
+// }
+// for (int i = 0; i < map.specialAreas.size(); i++) {
+// map.specialAreas[i].overSpeed = map.specialAreas[i].activeBreak = false;
+// }
+// for (int i = 0; i < map.triggerLines.size(); ++i) {
+// map.triggerLines[i].cmp = false;
+// }
}
static void CheckBreakActive(road_exam_map &map, const car_model *car, LIST_CAR_MODEL &CarModelList,
--
Gitblit v1.8.0