From cff87234c496f0a4522cd2ac57941f14c729b6ad Mon Sep 17 00:00:00 2001
From: fctom1215 <fctom1215@outlook.com>
Date: 星期五, 28 八月 2020 01:05:09 +0800
Subject: [PATCH] 坐标
---
lib/src/main/cpp/test_items2/road_exam.cpp | 173 ++++++++++++++++++++++++++++++---------------------------
1 files changed, 91 insertions(+), 82 deletions(-)
diff --git a/lib/src/main/cpp/test_items2/road_exam.cpp b/lib/src/main/cpp/test_items2/road_exam.cpp
index 8c56d03..c9fc0d2 100644
--- a/lib/src/main/cpp/test_items2/road_exam.cpp
+++ b/lib/src/main/cpp/test_items2/road_exam.cpp
@@ -83,7 +83,7 @@
static bool StopCarOnRedArea;
static PointF stopPoint;
static bool prevGearError = false;
-static bool prevGearNSlide = false;
+static int GearNSlideStatus = 0;
static bool slideLongDistance;
static bool slideNormalDistance;
@@ -91,11 +91,10 @@
static struct RtkTime crashGreenRunTime, crashGreenStartTime, stopTimepoint;
-static struct drive_timer gearErrorTimePoint;
-static struct drive_timer gearNSlideTimePoint;
+static struct RtkTime gearErrorTimePoint;
+static struct RtkTime gearNSlideTimePoint;
static int gearErrorTime;
-static int gearNSlideTime;
static int currExamMapIndex;
@@ -147,6 +146,7 @@
static const int SPEED_GEAR_TABLE[][2] = {{0, 20}, {5, 30}, {15, 40}, {25, 10000}, {35, 10000}};
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);
+static void ItemExam2(road_exam_map &RoadMap, int roadIndex, const car_model *car, LIST_CAR_MODEL &CarModelList);
static int isTurn(int currYaw, int prevYaw, int thres);
static void ResetTurnDetect(const car_model *car);
static void DetectTurn(const car_model *car, int moveDirect, const struct RtkTime *rtkTime);
@@ -154,7 +154,7 @@
static int NearbyCrossingGuide(int &stopLineIndex, int roadIndex, road_t &road, const car_model *car);
-static trigger_line_t * EntryItem(int index, road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList);
+static int EntryItem(int index, road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList);
static int CalcRoadIndex(int currRoadIndex, road_exam_map &RoadMap, const car_model *car);
@@ -176,8 +176,7 @@
slideNormalDistance = false;
prevGearError = false;
gearErrorTime = 0;
- prevGearNSlide = false;
- gearNSlideTime = 0;
+ GearNSlideStatus = 0;
currExamMapIndex = -1;
@@ -880,6 +879,8 @@
checkCrashGreenTimeout = 0;
// 璁板綍寮�濮嬪帇绾跨殑鏃堕棿锛屼笉纭畾鏄惁鏈夊彉閬撴剰鍥撅紝寰呯‘璁ゅ彉閬撳悗鍐嶅鐞嗕箣
crashGreenStartTime = *rtkTime;
+
+ DEBUG("寮�濮嬪帇铏氱嚎 %02d-%02d-%02d %02d:%02d:%02d", crashGreenStartTime.YY, crashGreenStartTime.MM, crashGreenStartTime.DD, crashGreenStartTime.hh, crashGreenStartTime.mm, crashGreenStartTime.ss, crashGreenStartTime.mss * 10);
}
crashDottedLine = true;
} else {
@@ -939,8 +940,11 @@
DEBUG("鍙樿皟鏈墦鐏�!!");
// 娌℃墦鐏紝涓嶅悎鏍�
AddExamFault(13, rtkTime);
- } else if (TimeGetDiff(&crashGreenStartTime, &lamp.time) >= TURN_SIGNAL_LAMP_ADVANCE) {
- DEBUG("杞悜鐏椂闂翠笉瓒�");
+ } else if (TimeGetDiff(&crashGreenStartTime, &lamp.time) < TURN_SIGNAL_LAMP_ADVANCE) {
+ DEBUG("杞悜鐏椂闂翠笉瓒� %02d-%02d-%02d %02d:%02d:%02d.%03d -> %02d-%02d-%02d %02d:%02d:%02d.%03d",
+ crashGreenStartTime.YY, crashGreenStartTime.MM, crashGreenStartTime.DD, crashGreenStartTime.hh, crashGreenStartTime.mm, crashGreenStartTime.ss, crashGreenStartTime.mss * 10,
+ lamp.time.YY, lamp.time.MM, lamp.time.DD, lamp.time.hh, lamp.time.mm, lamp.time.ss, lamp.time.mss * 10);
+
// 涓嶈冻3绉掞紝涓嶅悎鏍�
AddExamFault(14, rtkTime);
}
@@ -949,8 +953,10 @@
DEBUG("鍙樿皟鏈墦鐏�!!");
// 娌℃墦鐏紝涓嶅悎鏍�
AddExamFault(13, rtkTime);
- } else if (TimeGetDiff(&crashGreenStartTime, &lamp.time) >= TURN_SIGNAL_LAMP_ADVANCE) {
- DEBUG("杞悜鐏椂闂翠笉瓒�");
+ } else if (TimeGetDiff(&crashGreenStartTime, &lamp.time) < TURN_SIGNAL_LAMP_ADVANCE) {
+ DEBUG("杞悜鐏椂闂翠笉瓒� %02d-%02d-%02d %02d:%02d:%02d.%03d -> %02d-%02d-%02d %02d:%02d:%02d.%03d",
+ crashGreenStartTime.YY, crashGreenStartTime.MM, crashGreenStartTime.DD, crashGreenStartTime.hh, crashGreenStartTime.mm, crashGreenStartTime.ss, crashGreenStartTime.mss * 10,
+ lamp.time.YY, lamp.time.MM, lamp.time.DD, lamp.time.hh, lamp.time.mm, lamp.time.ss, lamp.time.mss * 10);
// 涓嶈冻3绉掞紝涓嶅悎鏍�
AddExamFault(14, rtkTime);
}
@@ -1298,27 +1304,26 @@
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 (currGearNSlide) {
+ if (GearNSlideStatus == 0) {
+ DEBUG("妫�娴嬪埌绌烘尅婊戣");
+ GearNSlideStatus = 1;
+ gearNSlideTimePoint = *rtkTime;
+ }
+ 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 (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);
+ gearErrorTime += TimeGetDiff(rtkTime, &gearErrorTimePoint);
}
if (gearErrorTime > GEAR_ERROR_TIMEOUT) {
// 绱15绉掞紝鎸′綅-杞﹂�熶笉鍖归厤锛屼笉鍚堟牸
@@ -1329,7 +1334,7 @@
prevGearError = currGearError;
if (prevGearError) {
- Rtk2DriveTimer(gearErrorTimePoint, rtkTime);
+ gearErrorTimePoint = *rtkTime;
}
// 璧锋鍚庢粦
@@ -1395,6 +1400,7 @@
ResetCrossingStatus(oldid);
ResetErrorLaneRpt(oldid);
}
+ Lane.guide = 0;
}
if (currExamMapIndex >= 0) {
@@ -1422,7 +1428,7 @@
if (!(NearbyCrossingGuide(stop_line_index, currExamMapIndex, RoadMap.roads[currExamMapIndex], car) & Lane.guide)) {
if (!GetErrorLaneRpt(currExamMapIndex, stop_line_index)) {
- DEBUG("涓嶆寜瑙勫畾杞﹂亾鏍囧悜琛岄┒");
+ DEBUG("涓嶆寜瑙勫畾杞﹂亾鏍囧悜琛岄┒ %d: %d guide = %d", currExamMapIndex, stop_line_index, Lane.guide);
AddExamFault(9, rtkTime);
SetErrorLaneRpt(currExamMapIndex, stop_line_index, true);
}
@@ -1456,6 +1462,7 @@
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)
@@ -1474,41 +1481,41 @@
if (RoadExamItem[ROAD_EXAM_ITEM_STRAIGHT] == ROAD_EXAM_ITEM_NOT_EXEC) {
not_complete = true;
- if (straight > 170 && road_end > 170) {
- StartDriveStraightExam();
- RoadExamStatus = ROAD_EXAM_ITEM_STRAIGHT;
- return;
- }
+// if (straight > 170 && road_end > 170) {
+// StartDriveStraightExam();
+// RoadExamStatus = ROAD_EXAM_ITEM_STRAIGHT;
+// return;
+// }
}
if (RoadExamItem[ROAD_EXAM_ITEM_OP_GEAR] == ROAD_EXAM_ITEM_NOT_EXEC) {
not_complete = true;
- if (road_end > 170) {
- StartOperateGearExam();
- RoadExamStatus = ROAD_EXAM_ITEM_OP_GEAR;
- return;
- }
+// if (road_end > 170) {
+// StartOperateGearExam();
+// RoadExamStatus = ROAD_EXAM_ITEM_OP_GEAR;
+// return;
+// }
}
if (RoadExamItem[ROAD_EXAM_ITEM_CHANGE_LANE] == ROAD_EXAM_ITEM_NOT_EXEC) {
not_complete = true;
- if (road_end > 150 && Lane.total > 1) {
- StartChaneLaneExam(Lane.no);
- RoadExamStatus = ROAD_EXAM_ITEM_CHANGE_LANE;
- return;
- }
+// if (road_end > 150 && Lane.total > 1) {
+// StartChaneLaneExam(Lane.no);
+// RoadExamStatus = ROAD_EXAM_ITEM_CHANGE_LANE;
+// return;
+// }
}
if (RoadExamItem[ROAD_EXAM_ITEM_OVER_TAKE] == ROAD_EXAM_ITEM_NOT_EXEC) {
not_complete = true;
- if (road_end > 200 && Lane.total > 1) {
- if (Lane.no == 0) {
- // 宸插湪鏈�宸﹁溅閬擄紝鍏堝彉鏇磋溅閬擄紵
- StartChaneLaneExam(Lane.no);
- RoadExamStatus = ROAD_EXAM_ITEM_CHANGE_LANE;
- } else {
- StartOvertakeExam(Lane.no);
- RoadExamStatus = ROAD_EXAM_ITEM_OVER_TAKE;
- }
- return;
- }
+// if (road_end > 200 && Lane.total > 1) {
+// if (Lane.no == 0) {
+// // 宸插湪鏈�宸﹁溅閬擄紝鍏堝彉鏇磋溅閬擄紵
+// StartChaneLaneExam(Lane.no);
+// RoadExamStatus = ROAD_EXAM_ITEM_CHANGE_LANE;
+// } else {
+// StartOvertakeExam(Lane.no);
+// RoadExamStatus = ROAD_EXAM_ITEM_OVER_TAKE;
+// }
+// return;
+// }
}
if (!not_complete) {
@@ -1522,7 +1529,6 @@
} else if (RoadExamStatus == ROAD_EXAM_FREE_RUN) {
if (ReadOdo() - freeRunDistance > freeRunExceptDistance) {
RoadExamStatus = ROAD_EXAM_READY_NEXT;
-
}
} else {
bool testing = false;
@@ -1554,7 +1560,7 @@
if (RoadExamStatus == ROAD_EXAM_ITEM_CAR_START) {
freeRunExceptDistance = 60.0;
} else {
- freeRunExceptDistance = 250.0;
+ freeRunExceptDistance = 2;//250.0;
}
RoadExamStatus = ROAD_EXAM_FREE_RUN;
@@ -1563,12 +1569,23 @@
}
}
-void Rtk2DriveTimer(struct drive_timer &tm, const struct RtkTime *rtkTime)
+static void ItemExam2(road_exam_map &RoadMap, int roadIndex, const car_model *car, LIST_CAR_MODEL &CarModelList)
{
- tm.hour = rtkTime->hh;
- tm.min = rtkTime->mm;
- tm.sec = rtkTime->ss;
- tm.msec = rtkTime->mss;
+ int item = EntryItem(roadIndex, RoadMap, car, CarModelList);
+
+ if (RoadExamStatus == ROAD_EXAM_READY_NEXT && item == 3) {
+ StartDriveStraightExam();
+ RoadExamStatus = ROAD_EXAM_ITEM_STRAIGHT;
+ } else if (RoadExamStatus == ROAD_EXAM_READY_NEXT && item == 4) {
+ StartOperateGearExam();
+ RoadExamStatus = ROAD_EXAM_ITEM_OP_GEAR;
+ } else if (RoadExamStatus == ROAD_EXAM_READY_NEXT && item == 1) {
+ StartChaneLaneExam(Lane.no);
+ RoadExamStatus = ROAD_EXAM_ITEM_CHANGE_LANE;
+ } else if (RoadExamStatus == ROAD_EXAM_READY_NEXT && item == 2) {
+ StartOvertakeExam(Lane.no);
+ RoadExamStatus = ROAD_EXAM_ITEM_OVER_TAKE;
+ }
}
void CrossRoadCallback(int road, int stop_line, int active, const car_model *car)
@@ -1666,7 +1683,7 @@
DEBUG("鍙樿皟鏈墦鐏�!!");
// 娌℃墦鐏紝涓嶅悎鏍�
AddExamFault(13, rtkTime);
- } else if (TimeGetDiff(&beginTurnTime, &lamp.time) >= TURN_SIGNAL_LAMP_ADVANCE) {
+ } else if (TimeGetDiff(&beginTurnTime, &lamp.time) < TURN_SIGNAL_LAMP_ADVANCE) {
DEBUG("杞悜鐏椂闂翠笉瓒�");
// 涓嶈冻3绉掞紝涓嶅悎鏍�
AddExamFault(14, rtkTime);
@@ -1676,7 +1693,7 @@
DEBUG("鍙樿皟鏈墦鐏�!!");
// 娌℃墦鐏紝涓嶅悎鏍�
AddExamFault(13, rtkTime);
- } else if (TimeGetDiff(&beginTurnTime, &lamp.time) >= TURN_SIGNAL_LAMP_ADVANCE) {
+ } else if (TimeGetDiff(&beginTurnTime, &lamp.time) < TURN_SIGNAL_LAMP_ADVANCE) {
DEBUG("杞悜鐏椂闂翠笉瓒�");
// 涓嶈冻3绉掞紝涓嶅悎鏍�
AddExamFault(14, rtkTime);
@@ -1784,35 +1801,27 @@
return false;
}
-static trigger_line_t * EntryItem(int index, road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList)
+static int EntryItem(int index, road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList)
{
+ if (index < 0 || index >= RoadMap.roads.size())
+ return -1;
+
for (int i = 0; i < RoadMap.triggerLines.size(); ++i) {
if (RoadMap.triggerLines[i].road == RoadMap.roads[index].id) {
Line triggerLine;
- if (RoadMap.triggerLines[i].leftPoints.size() != RoadMap.triggerLines[i].points.size()) {
- RoadMap.triggerLines[i].leftPoints.clear();
+ PointF p2 = CalcProjectionWithRoadEdge(RoadMap.roads[index].leftEdge, RoadMap.triggerLines[i].points[0]);
- for (int j = 0; j < RoadMap.triggerLines[i].points.size(); ++j) {
- RoadMap.triggerLines[i].leftPoints.push_back(CalcProjectionWithRoadEdge(RoadMap.roads[index].leftEdge, RoadMap.triggerLines[i].points[j]));
- }
-
- for (int j = 0; j < RoadMap.triggerLines[i].points.size(); ++j) {
- DEBUG("瑙﹀彂绾胯ˉ榻� road %d id %d type %d (%0.4f, %0.4f)-(%0.4f, %0.4f)", RoadMap.roads[index].id, RoadMap.triggerLines[i].id, RoadMap.triggerLines[i].active,
- RoadMap.triggerLines[i].points[0].X, RoadMap.triggerLines[i].points[0].Y, RoadMap.triggerLines[i].leftPoints[0].X, RoadMap.triggerLines[i].leftPoints[0].Y);
- }
- }
-
- MakeLine(&triggerLine, &RoadMap.triggerLines[i].points[0], &RoadMap.triggerLines[i].leftPoints[0]);
+ MakeLine(&triggerLine, &RoadMap.triggerLines[i].points[0], &p2);
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];
+ return RoadMap.triggerLines[i].active;
}
}
}
- return NULL;
+ return -1;
}
static double AnalysisRoad(road_exam_map &RoadMap, int roadIndex, lane_t lane, const car_model *car)
--
Gitblit v1.8.0