From 611f51ed073de48e83f46d47c82cd5ebe61258d7 Mon Sep 17 00:00:00 2001
From: yy1717 <fctom1215@outlook.com>
Date: 星期三, 16 九月 2020 18:14:27 +0800
Subject: [PATCH] 加入地图标定模式
---
lib/src/main/cpp/test_items2/road_exam.cpp | 440 +++++++++++++++++++++++++++++++++---------------------
1 files changed, 267 insertions(+), 173 deletions(-)
diff --git a/lib/src/main/cpp/test_items2/road_exam.cpp b/lib/src/main/cpp/test_items2/road_exam.cpp
index 53eea6a..0ade5a9 100644
--- a/lib/src/main/cpp/test_items2/road_exam.cpp
+++ b/lib/src/main/cpp/test_items2/road_exam.cpp
@@ -155,7 +155,7 @@
static void DetectTurn(const car_model *car, int moveDirect, const struct RtkTime *rtkTime);
static bool StopOnRedArea(road_exam_map &RoadMap, const car_model *car);
-static int NearbyCrossingGuide(int &stopLineIndex, int roadIndex, road_t &road, const car_model *car);
+static int NearbyCrossingGuide(road_exam_map &RoadMap, int &stopLineIndex, int roadIndex, road_t &road, const car_model *car);
static int EntryItem(int index, road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList);
@@ -1109,7 +1109,7 @@
int stop_line_index;
PointF nearbyStopPoint;
- NearbyCrossingGuide(stop_line_index, roadIndex, RoadMap.roads[roadIndex], car);
+ NearbyCrossingGuide(RoadMap, stop_line_index, roadIndex, RoadMap.roads[roadIndex], car);
nearbyStopPoint.X = RoadMap.roads[roadIndex].stopLine[stop_line_index].line.X1;
nearbyStopPoint.Y = RoadMap.roads[roadIndex].stopLine[stop_line_index].line.Y1;
@@ -1159,6 +1159,20 @@
return rec;
}
+int ExamSchemeCrossing(road_exam_map &RoadMap, int roadIndex, int crossIndex)
+{
+ if (roadIndex < 0 || roadIndex >= RoadMap.roads.size() || RoadMap.examScheme.size() == 0) {
+ return -1;
+ }
+ for (int i = 0; i < RoadMap.examScheme[0].crossingActive.size(); ++i) {
+ if (RoadMap.examScheme[0].crossingActive[i].road_id == RoadMap.roads[roadIndex].id &&
+ RoadMap.examScheme[0].crossingActive[i].index == crossIndex) {
+ return RoadMap.examScheme[0].crossingActive[i].active;
+ }
+ }
+ return -1;
+}
+
static void HintCrossing(road_exam_map &RoadMap, int roadIndex, int stopIndex, double distance) {
if (roadIndex < 0 || roadIndex >= RoadMap.roads.size())
return;
@@ -1166,18 +1180,47 @@
if (distance > 5 && distance < 70) {
// 鎻愮ず璺彛鎬庝箞璧�
if (GetCrossingStatus(roadIndex, stopIndex) == CROSSING_NOT_HINT) {
- if (!RoadMap.roads[roadIndex].stopLine[stopIndex].tts.empty()) {
- DEBUG("璺彛鎻愮ず %s", RoadMap.roads[roadIndex].stopLine[stopIndex].tts.c_str());
- PlayTTS(RoadMap.roads[roadIndex].stopLine[stopIndex].tts.c_str(), NULL);
+ // 椤圭洰鏍囧畾妯″紡涓嬶紝杩欓噷鎻愮ず鏍囧畾鏈矾鍙ｅ姩浣�
+ if (RoadMap.calibrate) {
+ CrossingChange(RoadMap.roads[roadIndex].id, stopIndex, 1);
+ } else {
+ int act = ExamSchemeCrossing(RoadMap, roadIndex, stopIndex);
+
+ switch (act) {
+ case ROAD_ACTIVE_FORWARD:
+ DEBUG("璺彛鎻愮ず 鐩磋");
+ PlayTTS("鍓嶆柟璺彛璇风洿琛�", NULL);
+ break;
+ case ROAD_ACTIVE_TURN_LEFT:
+ DEBUG("璺彛鎻愮ず 宸﹁浆");
+ PlayTTS("鍓嶆柟璺彛璇峰乏杞�", NULL);
+ break;
+ case ROAD_ACTIVE_TURN_RIGHT:
+ DEBUG("璺彛鎻愮ず 鍙宠浆");
+ PlayTTS("鍓嶆柟璺彛璇峰彸杞�", NULL);
+ break;
+ case ROAD_ACTIVE_TURN_BACKWARD:
+ DEBUG("璺彛鎻愮ず 鎺夊ご");
+ PlayTTS("鍓嶆柟璺彛璇锋帀澶�", NULL);
+ break;
+ default:
+ DEBUG("璺彛鎻愮ず 鏈厤缃�");
+ PlayTTS("鍓嶆柟璺彛鍚暀缁冩寚浠�", NULL);
+ break;
+ }
}
ChangeCrossingStatus(roadIndex, stopIndex, CROSSING_HAS_HINT);
}
} else if (distance > 75 && GetCrossingStatus(roadIndex, stopIndex) != CROSSING_NOT_HINT) {
ChangeCrossingStatus(roadIndex, stopIndex, CROSSING_NOT_HINT);
+
+ if (RoadMap.calibrate) {
+ CrossingChange(RoadMap.roads[roadIndex].id, stopIndex, 0);
+ }
}
}
-static int NearbyCrossingGuide(int &stopLineIndex, int roadIndex, road_t &road, const car_model *car)
+static int NearbyCrossingGuide(road_exam_map &RoadMap, int &stopLineIndex, int roadIndex, road_t &road, const car_model *car)
{
int guide = 0, stopLine = 0;
double distance;
@@ -1203,7 +1246,7 @@
struct nearby_crossing temp;
temp.stopLine = i;
- temp.guide = road.stopLine[i].active;
+ temp.guide = ExamSchemeCrossing(RoadMap, roadIndex, i);
temp.distance = distance;
set.push_back(temp);
@@ -1253,157 +1296,169 @@
turnSignalStatus = ReadCarSensorValue(TURN_SIGNAL_LAMP);
}
- // 鍒硅溅鎻愮ず闊�
- RingBreak();
+ if (RoadMap.calibrate == 0) {
+ // 鍒硅溅鎻愮ず闊�
+ RingBreak();
- // 瓒呴�熸娴�
- if (speed > MAX_SPEED) {
- if (!occurOverSpeed) {
- occurOverSpeed = true;
- // 瓒呴�燂紝涓嶅悎鏍�
- DEBUG("瓒呴�� %f", ConvertMs2KMh(speed));
- AddExamFault(10, rtkTime);
+ // 瓒呴�熸娴�
+ if (speed > MAX_SPEED) {
+ if (!occurOverSpeed) {
+ occurOverSpeed = true;
+ // 瓒呴�燂紝涓嶅悎鏍�
+ DEBUG("瓒呴�� %f", ConvertMs2KMh(speed));
+ AddExamFault(10, rtkTime);
+ }
+ } else if (speed < DEC_MAX_SPEED) {
+ occurOverSpeed = false;
}
- } else if (speed < DEC_MAX_SPEED ) {
- occurOverSpeed = false;
- }
- // 鍓埞杞︽娴�
- if (ReadCarStatus(SECOND_BREAK) == BREAK_ACTIVE) {
- // 鍓埞杞﹁俯涓嬶紝涓嶅悎鏍�
- if (!occurSecondBreak) {
- DEBUG("鍓埞杞﹀姩浣滀簡");
- occurSecondBreak = true;
- AddExamFault(17, rtkTime);
+ // 鍓埞杞︽娴�
+ if (ReadCarStatus(SECOND_BREAK) == BREAK_ACTIVE) {
+ // 鍓埞杞﹁俯涓嬶紝涓嶅悎鏍�
+ if (!occurSecondBreak) {
+ DEBUG("鍓埞杞﹀姩浣滀簡");
+ occurSecondBreak = true;
+ AddExamFault(17, rtkTime);
+ }
+ } else {
+ occurSecondBreak = false;
}
- } else {
- occurSecondBreak = false;
- }
- // 鎸′綅鍖归厤妫�娴�
- bool currGearError = false;
- bool currGearNSlide = 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) {
- if (GearNSlideStatus == 0) {
- DEBUG("妫�娴嬪埌绌烘尅婊戣");
- GearNSlideStatus = 1;
- gearNSlideTimePoint = *rtkTime;
+ 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 (GearNSlideStatus == 1 && TimeGetDiff(rtkTime, &gearNSlideTimePoint) > GEAR_N_SLIDE_TIMEOUT) {
- // 绌烘。婊戣瓒�5绉掞紝涓嶅悎鏍�
- DEBUG("鎸′綅婊戣锛岃秴杩�5绉�");
- AddExamFault(8, rtkTime);
- GearNSlideStatus = 2;
- }
- } else if (GearNSlideStatus != 0) {
- GearNSlideStatus = 0;
- DEBUG("绌烘尅婊戣缁撴潫");
- }
-
- // 鎸′綅涓嶅尮閰嶈秴鏃�
- if (currGearError && prevGearError) {
- DEBUG("鎸′綅閿欒澧炲姞 %ld姣 褰撳墠鎸′綅 %d 鏃堕�� %f", TimeGetDiff(rtkTime, &gearErrorTimePoint), ReadCarStatus(GEAR), ConvertMs2KMh(speed));
- gearErrorTime += TimeGetDiff(rtkTime, &gearErrorTimePoint);
- }
- if (gearErrorTime > GEAR_ERROR_TIMEOUT) {
- // 绱15绉掞紝鎸′綅-杞﹂�熶笉鍖归厤锛屼笉鍚堟牸
- DEBUG("鎸′綅閿欒瓒呰繃15绉�");
- AddExamFault(6, rtkTime);
- gearErrorTime = 0;
- }
-
- prevGearError = currGearError;
- if (prevGearError) {
- gearErrorTimePoint = *rtkTime;
- }
-
- // 璧锋鍚庢粦
- if (moveDirect != prevMoveDirect) {
- if (moveDirect == 0) {
- stopTimepoint = *rtkTime;
- StopCarOnRedArea = 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鍘樼背");
+ // 绌烘。婊戣瓒呮椂
+ if (currGearNSlide) {
+ if (GearNSlideStatus == 0) {
+ DEBUG("妫�娴嬪埌绌烘尅婊戣");
+ GearNSlideStatus = 1;
+ gearNSlideTimePoint = *rtkTime;
}
-
- slideNormalDistance = false;
- slideLongDistance = false;
- occurSlide = false;
- } else if (moveDirect == -1 && prevMoveDirect == 0) {
- DEBUG("寮�濮嬪悗婊�");
- stopPoint = car->basePoint;
- occurSlide = true;
- }
- prevMoveDirect = moveDirect;
- } else if (moveDirect == 0) {
- // 鎸佺画鍋滆溅
- if (TimeGetDiff(rtkTime, &stopTimepoint) >= STOP_CAR_TIME && !StopCarOnRedArea && StopOnRedArea(RoadMap, car)) {
- // 鍋滆溅瓒�2绉掞紝鍋滃湪绾㈠尯锛屼笉鍚堟牸
- AddExamFault(16, rtkTime);
- DEBUG("绂佸仠鍖哄仠杞�");
- StopCarOnRedArea = true;
- }
- } else if (moveDirect == -1) {
- // 鎸佺画鍚庢粦
- if (occurSlide) {
- double slideDistance = DistanceOf(stopPoint, car->basePoint);
-
- if (slideDistance > SLIDE_DISTANCE_THRESHOLD_YELLOW) {
- slideNormalDistance = true;
+ if (GearNSlideStatus == 1 &&
+ TimeGetDiff(rtkTime, &gearNSlideTimePoint) > GEAR_N_SLIDE_TIMEOUT) {
+ // 绌烘。婊戣瓒�5绉掞紝涓嶅悎鏍�
+ DEBUG("鎸′綅婊戣锛岃秴杩�5绉�");
+ AddExamFault(8, rtkTime);
+ GearNSlideStatus = 2;
}
+ } else if (GearNSlideStatus != 0) {
+ GearNSlideStatus = 0;
+ DEBUG("绌烘尅婊戣缁撴潫");
+ }
- if (slideDistance > SLIDE_DISTANCE_THRESHOLD_RED && !slideLongDistance) {
- // 鍚庢粦瓒呰繃30鍘樼背, 涓嶅悎鏍�
- AddExamFault(5, rtkTime);
- DEBUG("鍚庢粦瓒呰繃30鍘樼背");
- slideLongDistance = true;
+ // 鎸′綅涓嶅尮閰嶈秴鏃�
+ if (currGearError && prevGearError) {
+ DEBUG("鎸′綅閿欒澧炲姞 %ld姣 褰撳墠鎸′綅 %d 鏃堕�� %f", TimeGetDiff(rtkTime, &gearErrorTimePoint),
+ ReadCarStatus(GEAR), ConvertMs2KMh(speed));
+ gearErrorTime += TimeGetDiff(rtkTime, &gearErrorTimePoint);
+ }
+ if (gearErrorTime > GEAR_ERROR_TIMEOUT) {
+ // 绱15绉掞紝鎸′綅-杞﹂�熶笉鍖归厤锛屼笉鍚堟牸
+ DEBUG("鎸′綅閿欒瓒呰繃15绉�");
+ AddExamFault(6, rtkTime);
+ gearErrorTime = 0;
+ }
+
+ prevGearError = currGearError;
+ if (prevGearError) {
+ gearErrorTimePoint = *rtkTime;
+ }
+
+ // 璧锋鍚庢粦
+ if (moveDirect != prevMoveDirect) {
+ if (moveDirect == 0) {
+ stopTimepoint = *rtkTime;
+ StopCarOnRedArea = 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;
+ occurSlide = true;
}
+ prevMoveDirect = moveDirect;
+ } else if (moveDirect == 0) {
+ // 鎸佺画鍋滆溅
+ if (TimeGetDiff(rtkTime, &stopTimepoint) >= STOP_CAR_TIME && !StopCarOnRedArea &&
+ StopOnRedArea(RoadMap, car)) {
+ // 鍋滆溅瓒�2绉掞紝鍋滃湪绾㈠尯锛屼笉鍚堟牸
+ AddExamFault(16, rtkTime);
+ DEBUG("绂佸仠鍖哄仠杞�");
+ StopCarOnRedArea = 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;
+ slideNormalDistance = false;
+ occurSlide = false;
+ }
+ }
+ } else {
+ // 鍓嶈繘
}
- } else {
- // 鍓嶈繘
}
// 妫�娴嬬寮�姝よ矾娈碉紝鍏ㄨ溅闇�涓嶅湪鑼冨洿鍐�
@@ -1415,6 +1470,15 @@
if (oldid >= 0) {
ResetCrossingStatus(oldid);
ResetErrorLaneRpt(oldid);
+
+ }
+
+ if (RoadMap.calibrate) {
+ if (currExamMapIndex < 0 && oldid >= 0) {
+ RoadChange(oldid, 0);
+ } else if (currExamMapIndex >= 0) {
+ RoadChange(currExamMapIndex, 1);
+ }
}
Lane.guide = 0;
}
@@ -1426,41 +1490,44 @@
}
ExitTarget(RoadMap, car, CarModelList, rtkTime);
- oldid = CrashLineType;
- // 妫�娴嬪帇绾跨姸鎬�
- DetectLine(currExamMapIndex, RoadMap, car, CarModelList, moveDirect, rtkTime);
+ if (RoadMap.calibrate == 0) {
+ oldid = CrashLineType;
+ // 妫�娴嬪帇绾跨姸鎬�
+ DetectLine(currExamMapIndex, RoadMap, car, CarModelList, moveDirect, rtkTime);
- if (oldid != CrashLineType) {
- DEBUG("鍘嬬嚎绫诲瀷鍒囨崲 %d", CrashLineType);
- }
+ if (oldid != CrashLineType) {
+ DEBUG("鍘嬬嚎绫诲瀷鍒囨崲 %d", CrashLineType);
+ }
-
- oldid = Lane.guide;
- DetectLane(RoadMap, car, currExamMapIndex, rtkTime);
+ oldid = Lane.guide;
+ DetectLane(RoadMap, car, currExamMapIndex, rtkTime);
// DEBUG("Lane淇℃伅 road %d sep %d total %d no %d guide %d", Lane.road, Lane.sep, Lane.total, Lane.no, Lane.guide);
- if (Lane.guide > 0 && currExamMapIndex >= 0) {
- int stop_line_index;
- int act = NearbyCrossingGuide(stop_line_index, currExamMapIndex, RoadMap.roads[currExamMapIndex], car);
+ if (Lane.guide > 0 && currExamMapIndex >= 0) {
+ int stop_line_index;
+ int act = NearbyCrossingGuide(RoadMap, stop_line_index, currExamMapIndex,
+ RoadMap.roads[currExamMapIndex], car);
- if (act != 0 && !(act & Lane.guide)) {
- if (!GetErrorLaneRpt(currExamMapIndex, stop_line_index)) {
- DEBUG("涓嶆寜瑙勫畾杞﹂亾鏍囧悜琛岄┒ %d: %d 鏈熸湜 = %d guide = %d", currExamMapIndex, stop_line_index, act, Lane.guide);
- AddExamFault(9, rtkTime);
- SetErrorLaneRpt(currExamMapIndex, stop_line_index, true);
+ if (act != 0 && !(act & Lane.guide)) {
+ if (!GetErrorLaneRpt(currExamMapIndex, stop_line_index)) {
+ DEBUG("涓嶆寜瑙勫畾杞﹂亾鏍囧悜琛岄┒ %d: %d 鏈熸湜 = %d guide = %d", currExamMapIndex,
+ stop_line_index, act, Lane.guide);
+ AddExamFault(9, rtkTime);
+ SetErrorLaneRpt(currExamMapIndex, stop_line_index, true);
+ }
}
}
- }
- if (Lane.guide != oldid) {
- DEBUG("瀵煎悜绫诲瀷鍒囨崲 %d", Lane.guide);
+ if (Lane.guide != oldid) {
+ DEBUG("瀵煎悜绫诲瀷鍒囨崲 %d", Lane.guide);
+ }
}
if (currExamMapIndex >= 0 && Lane.guide == 0) {
BigStraightRoadFree = AnalysisRoad(RoadMap, currExamMapIndex, Lane, car);
road_end_point_t ep = NearbyRoadEndPoint(currExamMapIndex, RoadMap, car);
-
+ // 鎻愮ず璺彛鎬庝箞璧�
HintCrossing(RoadMap, ep.road_index, ep.stop_line_index, ep.distance);
double freeSepDis = SeparateLength(RoadMap, Lane, car);
@@ -1476,10 +1543,15 @@
}
// 棰濆鐨勮浆鍚戞娴�
- DetectTurn(car, moveDirect, rtkTime);
- ItemExam(RoadMap, currExamMapIndex, car, CarModelList, speed, moveDirect, rtkTime, BigStraightRoadFree, TargetFree > RoadCrossingFree? RoadCrossingFree : TargetFree);
- ItemExam2(RoadMap, currExamMapIndex, car, CarModelList);
+ if (RoadMap.calibrate == 0) {
+ DetectTurn(car, moveDirect, rtkTime);
+
+ ItemExam(RoadMap, currExamMapIndex, car, CarModelList, speed, moveDirect, rtkTime,
+ BigStraightRoadFree,
+ TargetFree > RoadCrossingFree ? RoadCrossingFree : TargetFree);
+ ItemExam2(RoadMap, currExamMapIndex, car, CarModelList);
+ }
}
static void ItemExam(road_exam_map &RoadMap, int roadIndex, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime, double straight, double road_end)
@@ -1605,8 +1677,11 @@
}
}
-void CrossRoadCallback(int road, int stop_line, int active, const car_model *car)
+void CrossRoadCallback(road_exam_map &RoadMap, int road, int stop_line, int active, const car_model *car)
{
+ if (RoadMap.calibrate) {
+ CrossingChange(RoadMap.roads[road].id, stop_line, 0);
+ }
SetErrorLaneRpt(road, stop_line, false);
if (active != ROAD_ACTIVE_FORWARD) {
ResetTurnDetect(car);
@@ -1823,12 +1898,12 @@
static int EntryItem(int index, road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList)
{
- if (index < 0 || index >= RoadMap.roads.size())
+ if (index < 0 || index >= RoadMap.roads.size() || RoadMap.examScheme.size() == 0)
return -1;
- for (int i = 0; i < RoadMap.triggerLines.size(); ++i) {
- if (RoadMap.triggerLines[i].road == RoadMap.roads[index].id) {
- Line triggerLine;
+ for (int j = 0; j < RoadMap.examScheme[0].triggerLines.size(); ++j) {
+ if (RoadMap.examScheme[0].triggerLines[j].road == RoadMap.roads[index].id) {
+ /*Line triggerLine;
PointF p2 = CalcProjectionWithRoadEdge(RoadMap.roads[index].leftEdge, RoadMap.triggerLines[i].points[0]);
@@ -1837,6 +1912,25 @@
if (CrashTheLine(triggerLine, car, CarModelList)) {
DEBUG("瑙﹀彂椤圭洰 %d %s (%0.4f, %0.4f)-(%0.4f, %0.4f)", RoadMap.triggerLines[i].active, RoadMap.triggerLines[i].tts.c_str(), triggerLine.X1, triggerLine.Y1, triggerLine.X2, triggerLine.Y2);
return RoadMap.triggerLines[i].active;
+ }*/
+
+ vector<double> vec;
+
+ PointF p1 = CalcProjectionWithRoadEdge(RoadMap.roads[index].rightEdge,
+ car->carXY[car->axial[AXIAL_FRONT]]);
+ PointF p2 = CalcProjectionWithRoadEdge(RoadMap.roads[index].rightEdge,
+ RoadMap.examScheme[0].triggerLines[j].points[0]);
+
+ vec.push_back(DistanceOf(car->carXY[car->axial[AXIAL_FRONT]],
+ RoadMap.examScheme[0].triggerLines[j].points[0]));
+ vec.push_back(DistanceOf(p2, RoadMap.examScheme[0].triggerLines[j].points[0]));
+ vec.push_back(DistanceOf(p1, car->carXY[car->axial[AXIAL_FRONT]]));
+
+ sort(vec.begin(), vec.end());
+
+ if (fabs(vec[0] + vec[1] - vec[2]) < 0.1) {
+ DEBUG("瑙﹀彂椤圭洰 %d", RoadMap.examScheme[0].triggerLines[j].active);
+ return RoadMap.examScheme[0].triggerLines[j].active;
}
}
}
--
Gitblit v1.8.0