From d05c25aa4823e9d2aa812e9c0e3d1d66bb9861dd Mon Sep 17 00:00:00 2001
From: fctom1215 <fctom1215@outlook.com>
Date: 星期六, 29 八月 2020 10:47:17 +0800
Subject: [PATCH] 坐标
---
lib/src/main/cpp/test_items2/road_exam.cpp | 130 +++++++++++++++++++++++++------------------
1 files changed, 75 insertions(+), 55 deletions(-)
diff --git a/lib/src/main/cpp/test_items2/road_exam.cpp b/lib/src/main/cpp/test_items2/road_exam.cpp
index b4e0483..53eea6a 100644
--- a/lib/src/main/cpp/test_items2/road_exam.cpp
+++ b/lib/src/main/cpp/test_items2/road_exam.cpp
@@ -72,7 +72,7 @@
const double SLIDE_DISTANCE_THRESHOLD_RED = 0.3;
const double SLIDE_DISTANCE_THRESHOLD_YELLOW = 0.1;
-const double EXAM_RANGE = 3000.0; // 鑷冲皯椹鹃┒璺濈
+const double EXAM_RANGE = 2700.0; // 鑷冲皯椹鹃┒璺濈
static bool occurOverSpeed;
static bool occurSecondBreak;
@@ -93,6 +93,8 @@
static struct RtkTime gearErrorTimePoint;
static struct RtkTime gearNSlideTimePoint;
+
+static car_sensor_value_t turnSignalStatus;
static int gearErrorTime;
@@ -127,13 +129,14 @@
} RoadExamItem_t;
static map<int, int> RoadExamItem;
static int RoadExamStatus;
+static bool win;
static struct RtkTime beginTurnTime, prevDetectTurnTime;
static int startTurnYaw, prevYaw;
static int turnCnt, turnTimeCnt;
static int prevTurnWise;
-static const uint32_t GEAR_N_SLIDE_TIMEOUT = D_SEC(5);
+static const uint32_t GEAR_N_SLIDE_TIMEOUT = D_SEC(10);
static const uint32_t GEAR_ERROR_TIMEOUT = D_SEC(15);
static const uint32_t STOP_CAR_TIME = D_SEC(2);
static const uint32_t CHANGE_LANE_MIN_INTERVAL = D_SEC(10);
@@ -199,6 +202,8 @@
RoadExamItem[ROAD_EXAM_ITEM_CAR_STOP] = ROAD_EXAM_ITEM_NOT_EXEC;
RoadExamStatus = ROAD_EXAM_READY_NEXT;
+
+ win = false;
}
void TerminateRoadExam(void)
@@ -879,7 +884,7 @@
checkCrashGreenTimeout = 0;
// 璁板綍寮�濮嬪帇绾跨殑鏃堕棿锛屼笉纭畾鏄惁鏈夊彉閬撴剰鍥撅紝寰呯‘璁ゅ彉閬撳悗鍐嶅鐞嗕箣
crashGreenStartTime = *rtkTime;
-
+ turnSignalStatus = ReadCarSensorValue(TURN_SIGNAL_LAMP);
DEBUG("寮�濮嬪帇铏氱嚎 %02d-%02d-%02d %02d:%02d:%02d", crashGreenStartTime.YY, crashGreenStartTime.MM, crashGreenStartTime.DD, crashGreenStartTime.hh, crashGreenStartTime.mm, crashGreenStartTime.ss, crashGreenStartTime.mss * 10);
}
crashDottedLine = true;
@@ -926,40 +931,45 @@
if (newLane.road == Lane.road && newLane.sep == Lane.sep) {
gain = newLane.no - Lane.no;
} else {
- DEBUG("杞﹂亾鍒嗘鍒囨崲");
+ DEBUG("杞﹂亾鍒嗘鍒囨崲 Old Lane road = %d sep = %d no = %d New Lane road = %d sep = %d no = %d", Lane.road, Lane.sep, Lane.no, newLane.road, newLane.sep, newLane.no);
ChangeLane.gain = 0;
}
// 妫�鏌ヨ浆鍚戠伅
if (gain != 0) {
DEBUG("鍙橀亾 new lane %d, gain %d", newLane.no, gain);
- 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("鍙樿皟鏈墦鐏�!!");
- // 娌℃墦鐏紝涓嶅悎鏍�
- AddExamFault(13, rtkTime);
- } 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);
+ if (gain < 0) {
+ if (turnSignalStatus.value != LEFT_TURN_LIGHT) {
+ DEBUG("鍙樿皟鏈墦鐏�!!");
+ // 娌℃墦鐏紝涓嶅悎鏍�
+ AddExamFault(13, rtkTime);
+ } else if (TimeGetDiff(&crashGreenStartTime, &turnSignalStatus.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,
+ turnSignalStatus.time.YY, turnSignalStatus.time.MM, turnSignalStatus.time.DD, turnSignalStatus.time.hh,
+ turnSignalStatus.time.mm, turnSignalStatus.time.ss, turnSignalStatus.time.mss * 10);
- // 涓嶈冻3绉掞紝涓嶅悎鏍�
- AddExamFault(14, rtkTime);
- }
- } else {
- if (lamp.value != RIGHT_TURN_LIGHT) {
- DEBUG("鍙樿皟鏈墦鐏�!!");
- // 娌℃墦鐏紝涓嶅悎鏍�
- AddExamFault(13, rtkTime);
- } 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);
- }
+ // 涓嶈冻3绉掞紝涓嶅悎鏍�
+ AddExamFault(14, rtkTime);
+ }
+ } else {
+ if (turnSignalStatus.value != RIGHT_TURN_LIGHT) {
+ DEBUG("鍙樿皟鏈墦鐏�!!");
+ // 娌℃墦鐏紝涓嶅悎鏍�
+ AddExamFault(13, rtkTime);
+ } else if (TimeGetDiff(&crashGreenStartTime, &turnSignalStatus.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,
+ turnSignalStatus.time.YY, turnSignalStatus.time.MM, turnSignalStatus.time.DD, turnSignalStatus.time.hh,
+ turnSignalStatus.time.mm, turnSignalStatus.time.ss, turnSignalStatus.time.mss * 10);
+ // 涓嶈冻3绉掞紝涓嶅悎鏍�
+ AddExamFault(14, rtkTime);
}
}
@@ -1238,6 +1248,11 @@
UpdataOdo(speed, moveDirect, rtkTime);
+ if (!win) {
+ win = true;
+ turnSignalStatus = ReadCarSensorValue(TURN_SIGNAL_LAMP);
+ }
+
// 鍒硅溅鎻愮ず闊�
RingBreak();
@@ -1323,6 +1338,7 @@
// 鎸′綅涓嶅尮閰嶈秴鏃�
if (currGearError && prevGearError) {
+ DEBUG("鎸′綅閿欒澧炲姞 %ld姣 褰撳墠鎸′綅 %d 鏃堕�� %f", TimeGetDiff(rtkTime, &gearErrorTimePoint), ReadCarStatus(GEAR), ConvertMs2KMh(speed));
gearErrorTime += TimeGetDiff(rtkTime, &gearErrorTimePoint);
}
if (gearErrorTime > GEAR_ERROR_TIMEOUT) {
@@ -1425,10 +1441,11 @@
// 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 (!(NearbyCrossingGuide(stop_line_index, currExamMapIndex, RoadMap.roads[currExamMapIndex], car) & Lane.guide)) {
+ if (act != 0 && !(act & Lane.guide)) {
if (!GetErrorLaneRpt(currExamMapIndex, stop_line_index)) {
- DEBUG("涓嶆寜瑙勫畾杞﹂亾鏍囧悜琛岄┒ %d: %d guide = %d", currExamMapIndex, stop_line_index, Lane.guide);
+ DEBUG("涓嶆寜瑙勫畾杞﹂亾鏍囧悜琛岄┒ %d: %d 鏈熸湜 = %d guide = %d", currExamMapIndex, stop_line_index, act, Lane.guide);
AddExamFault(9, rtkTime);
SetErrorLaneRpt(currExamMapIndex, stop_line_index, true);
}
@@ -1547,7 +1564,7 @@
testing = TestOvertake(Lane.no, rtkTime);
break;
case ROAD_EXAM_ITEM_STRAIGHT:
- testing = TestDriveStraight(car, rtkTime);
+ testing = TestDriveStraight(RoadMap, roadIndex, car, rtkTime);
break;
case ROAD_EXAM_ITEM_CAR_STOP:
testing = TestStopCar(RoadMap, roadIndex, car, moveDirect, rtkTime);
@@ -1650,6 +1667,8 @@
turnCnt++;
beginTurnTime = *rtkTime;
startTurnYaw = (int) car->yaw;
+
+ turnSignalStatus = ReadCarSensorValue(TURN_SIGNAL_LAMP);
}
} else if (turnCnt == 1) {
if (angle != 0 && moveDirect != 0) {
@@ -1661,6 +1680,8 @@
} else {
beginTurnTime = *rtkTime;
startTurnYaw = (int) car->yaw;
+
+ turnSignalStatus = ReadCarSensorValue(TURN_SIGNAL_LAMP);
}
} else {
turnCnt = 0;
@@ -1676,28 +1697,27 @@
// 纭杞集琛屼负锛屾娴嬪紑濮嬪垰杞集鏃惰浆鍚戠伅鎯呭喌
turnCnt = -1;
- car_sensor_value_t lamp = ReadCarSensorValue(TURN_SIGNAL_LAMP);
- if (lamp.name == TURN_SIGNAL_LAMP) {
- if (wise < 0) {
- if (lamp.value != LEFT_TURN_LIGHT) {
- DEBUG("鍙樿皟鏈墦鐏�!!");
- // 娌℃墦鐏紝涓嶅悎鏍�
- AddExamFault(13, rtkTime);
- } else if (TimeGetDiff(&beginTurnTime, &lamp.time) < TURN_SIGNAL_LAMP_ADVANCE) {
- DEBUG("杞悜鐏椂闂翠笉瓒�");
- // 涓嶈冻3绉掞紝涓嶅悎鏍�
- AddExamFault(14, rtkTime);
- }
- } else {
- if (lamp.value != RIGHT_TURN_LIGHT) {
- DEBUG("鍙樿皟鏈墦鐏�!!");
- // 娌℃墦鐏紝涓嶅悎鏍�
- AddExamFault(13, rtkTime);
- } else if (TimeGetDiff(&beginTurnTime, &lamp.time) < TURN_SIGNAL_LAMP_ADVANCE) {
- DEBUG("杞悜鐏椂闂翠笉瓒�");
- // 涓嶈冻3绉掞紝涓嶅悎鏍�
- AddExamFault(14, rtkTime);
- }
+ if (wise < 0) {
+ if (turnSignalStatus.value != LEFT_TURN_LIGHT) {
+ DEBUG("鍙樿皟鏈墦鐏�!!");
+ // 娌℃墦鐏紝涓嶅悎鏍�
+ AddExamFault(13, rtkTime);
+ } else if (TimeGetDiff(&beginTurnTime, &turnSignalStatus.time) <
+ TURN_SIGNAL_LAMP_ADVANCE) {
+ DEBUG("杞悜鐏椂闂翠笉瓒�");
+ // 涓嶈冻3绉掞紝涓嶅悎鏍�
+ AddExamFault(14, rtkTime);
+ }
+ } else {
+ if (turnSignalStatus.value != RIGHT_TURN_LIGHT) {
+ DEBUG("鍙樿皟鏈墦鐏�!!");
+ // 娌℃墦鐏紝涓嶅悎鏍�
+ AddExamFault(13, rtkTime);
+ } else if (TimeGetDiff(&beginTurnTime, &turnSignalStatus.time) <
+ TURN_SIGNAL_LAMP_ADVANCE) {
+ DEBUG("杞悜鐏椂闂翠笉瓒�");
+ // 涓嶈冻3绉掞紝涓嶅悎鏍�
+ AddExamFault(14, rtkTime);
}
}
} else if (turnTimeCnt > D_SEC(10)) {
--
Gitblit v1.8.0