From 21834035950feaadac9d4c7cad82b90ee624c984 Mon Sep 17 00:00:00 2001
From: yy1717 <fctom1215@outlook.com>
Date: 星期一, 17 八月 2020 19:30:06 +0800
Subject: [PATCH] 传感器变更
---
lib/src/main/cpp/test_items2/road_exam.cpp | 226 +++++++++++++++++++++++++++++++++++++++++++++++++++-----
1 files changed, 204 insertions(+), 22 deletions(-)
diff --git a/lib/src/main/cpp/test_items2/road_exam.cpp b/lib/src/main/cpp/test_items2/road_exam.cpp
index 5196886..a1e3357 100644
--- a/lib/src/main/cpp/test_items2/road_exam.cpp
+++ b/lib/src/main/cpp/test_items2/road_exam.cpp
@@ -74,7 +74,7 @@
static const int INVALID_ROAD = -1;
-static const int TURN_THRESHOLD = 1;
+static const int TURN_THRESHOLD = 3;
static const int TURN_CHECK_INTERVAL = 500;
const double SLIDE_DISTANCE_THRESHOLD_RED = 0.3;
const double SLIDE_DISTANCE_THRESHOLD_YELLOW = 0.1;
@@ -94,7 +94,7 @@
static char carIntersectionOfGreenLine;
static int currTurnSignalStatus;
static int turnSignalStatusWhenCrashGreenLine;
-static bool reportTurnSignalError;
+
static int prevMoveDirect;
static uint32_t stopTimepoint = 0;
static bool reportStopCarOnRedArea;
@@ -135,6 +135,12 @@
static map<int, car_sensor_value_t> CarSensorValue;
static map<int, int> CrossingHint;
+static map<int, bool> ErrorLaneReport;
+
+static struct RtkTime beginTurnTime, prevDetectTurnTime;
+static int startTurnYaw, prevYaw;
+static int turnCnt, turnTimeCnt;
+static int prevTurnWise;
static const int MAX_ENGINE_RPM = 2500;
static const double START_CAR_MOVE_DISTANCE = 10.0;
@@ -157,8 +163,9 @@
static int TestRoadStartCar(const car_model *car, double speed, int moveDirect, const struct RtkTime *rtkTime);
-static char isTurn(int currYaw, int prevYaw, int &ang);
-static char CheckCarTurn(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);
static trigger_line_t * EntryItem(int index, road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList);
@@ -179,7 +186,6 @@
carIntersectionOfGreenLine = 0;
- reportTurnSignalError = false;
currTurnSignalStatus = OFF_LIGHT;
prevMoveDirect = 0;
@@ -211,6 +217,7 @@
checkTurn = false;
CrashLineType = -1;
+ turnCnt = -1;
ResetOdo();
ResetTarget(RoadMap);
@@ -973,7 +980,6 @@
} else if (TimeGetDiff(&crashGreenStartTime, &lamp.time) >= D_SEC(3)) {
DEBUG("杞悜鐏椂闂翠笉瓒�");
// 涓嶈冻3绉掞紝涓嶅悎鏍�
- reportTurnSignalError = true;
AddExamFault(14, rtkTime);
}
} else {
@@ -984,7 +990,6 @@
} else if (TimeGetDiff(&crashGreenStartTime, &lamp.time) >= D_SEC(3)) {
DEBUG("杞悜鐏椂闂翠笉瓒�");
// 涓嶈冻3绉掞紝涓嶅悎鏍�
- reportTurnSignalError = true;
AddExamFault(14, rtkTime);
}
}
@@ -1027,6 +1032,62 @@
return CROSSING_NOT_HINT;
}
+static void ResetCrossingStatus(int roadIndex)
+{
+ while (true) {
+ bool w = false;
+ for (auto it = CrossingHint.begin(); it != CrossingHint.end(); ++it) {
+ if (it->first / 100 == roadIndex && it->second != CROSSING_NOT_HINT) {
+ ChangeCrossingStatus(it->first / 100, it->first % 100, CROSSING_NOT_HINT);
+ w = true;
+ break;
+ }
+ }
+ if (!w)
+ break;
+ }
+}
+
+static void SetErrorLaneRpt(int roadIndex, int index, bool status)
+{
+ int key = roadIndex * 100 + index;
+
+ auto it = ErrorLaneReport.find(key);
+
+ if (it != ErrorLaneReport.end()) {
+ ErrorLaneReport.erase(it);
+ }
+ ErrorLaneReport.insert(pair<int, bool>(key, status));
+}
+
+static bool GetErrorLaneRpt(int roadIndex, int index)
+{
+ int key = roadIndex * 100 + index;
+
+ auto it = ErrorLaneReport.find(key);
+
+ if (it != ErrorLaneReport.end()) {
+ return it->second;
+ }
+ return false;
+}
+
+static void ResetErrorLaneRpt(int roadIndex)
+{
+ while (true) {
+ bool w = false;
+ for (auto it = ErrorLaneReport.begin(); it != ErrorLaneReport.end(); ++it) {
+ if (it->first / 100 == roadIndex && it->second == true) {
+ SetErrorLaneRpt(it->first / 100, it->first % 100, false);
+ w = true;
+ break;
+ }
+ }
+ if (!w)
+ break;
+ }
+}
+
static void HintCrossing(int roadIndex, road_t &road, const car_model *car, LIST_CAR_MODEL &CarModelList)
{
for (int i = 0; i < road.stopLine.size(); ++i) {
@@ -1053,12 +1114,13 @@
}
}
-static int NearbyCrossingGuide(int roadIndex, road_t &road, const car_model *car, LIST_CAR_MODEL &CarModelList)
+static int NearbyCrossingGuide(int &stopLineIndex, int roadIndex, road_t &road, const car_model *car, LIST_CAR_MODEL &CarModelList)
{
- int guide = 0;
+ int guide = 0, stopLine = 0;
double distance;
struct nearby_crossing {
+ int stopLine;
int guide;
double distance;
};
@@ -1077,6 +1139,7 @@
if (distance > 1e-3) {
struct nearby_crossing temp;
+ temp.stopLine = i;
temp.guide = road.stopLine[i].active;
temp.distance = distance;
@@ -1092,10 +1155,12 @@
if (distance > set[i].distance) {
distance = set[i].distance;
guide = set[i].guide;
+ stopLine = set[i].stopLine;
}
}
}
+ stopLineIndex = stopLine;
return guide;
}
@@ -1286,6 +1351,15 @@
// 妫�娴嬬寮�姝よ矾娈碉紝鍏ㄨ溅闇�涓嶅湪鑼冨洿鍐�
int oldid = currExamMapIndex;
currExamMapIndex = CalcRoadIndex(currExamMapIndex, RoadMap, car);
+ if (oldid != currExamMapIndex) {
+ DEBUG("閬撹矾ID鍒囨崲 %d ======> %d", oldid, currExamMapIndex);
+
+ if (oldid >= 0) {
+ ResetCrossingStatus(oldid);
+ ResetErrorLaneRpt(oldid);
+ }
+ }
+
if (currExamMapIndex >= 0) {
car_sensor_value_t brk = ReadCarSensorValue(BREAK);
HintCrossing(currExamMapIndex, RoadMap.roads[currExamMapIndex], car, CarModelList);
@@ -1294,10 +1368,6 @@
ApproachTarget(RoadMap, car, currExamMapIndex, (brk.value == BREAK_ACTIVE), speed, moveDirect, rtkTime);
}
ExitTarget(RoadMap, car, CarModelList, rtkTime);
-
- if (oldid != currExamMapIndex) {
- DEBUG("閬撹矾ID鍒囨崲 %d", currExamMapIndex);
- }
oldid = CrashLineType;
@@ -1311,9 +1381,14 @@
oldid = Lane.guide;
DetectLane(RoadMap, car, currExamMapIndex, rtkTime);
if (Lane.guide > 0 && currExamMapIndex >= 0) {
- if (!(NearbyCrossingGuide(currExamMapIndex, RoadMap.roads[currExamMapIndex], car, CarModelList) & Lane.guide)) {
- DEBUG("涓嶆寜瑙勫畾杞﹂亾鏍囧悜琛岄┒");
- AddExamFault(9, rtkTime);
+ int stop_line;
+
+ if (!(NearbyCrossingGuide(stop_line, currExamMapIndex, RoadMap.roads[currExamMapIndex], car, CarModelList) & Lane.guide)) {
+ if (!GetErrorLaneRpt(currExamMapIndex, stop_line)) {
+ DEBUG("涓嶆寜瑙勫畾杞﹂亾鏍囧悜琛岄┒");
+ AddExamFault(9, rtkTime);
+ SetErrorLaneRpt(currExamMapIndex, stop_line, true);
+ }
}
}
@@ -1323,6 +1398,9 @@
// 妫�娴嬪帇绾跨姸鎬�
TestRoadStartCar(car, speed, moveDirect, rtkTime);
+
+ // 棰濆鐨勮浆鍚戞娴�
+ DetectTurn(car, moveDirect, rtkTime);
if (startCar != START_CAR_DONE)
return;
@@ -1399,7 +1477,15 @@
tm.msec = rtkTime->mss;
}
-static char isTurn(int currYaw, int prevYaw, int &ang)
+void CrossRoadCallback(int road, int stop_line, int active, const car_model *car)
+{
+ SetErrorLaneRpt(road, stop_line, false);
+ if (active != ROAD_ACTIVE_FORWARD) {
+ ResetTurnDetect(car);
+ }
+}
+
+static int isTurn(int currYaw, int prevYaw, int thres)
{
// DEBUG("currYaw %d prevYaw %d", currYaw, prevYaw);
int deltaAng = 0;
@@ -1410,21 +1496,117 @@
deltaAng = ABS(currYaw - prevYaw);
}
- ang = deltaAng;
// DEBUG("瑙掑害宸�� %d", deltaAng);
- if (deltaAng >= TURN_THRESHOLD) {
+ if (deltaAng >= thres) {
if((( currYaw + 360 - prevYaw) % 360) < 180) {
// DEBUG("鍙宠浆");
- return 'R';
+ return deltaAng;
} else {
// DEBUG("宸﹁浆");
- return 'L';
+ return 0 - deltaAng;
}
}
- return 'N';
+ return 0;
+}
+
+static void ResetTurnDetect(const car_model *car)
+{
+ turnCnt = 0;
+ turnTimeCnt = 0;
+ prevDetectTurnTime = car->tm;
+ prevYaw = car->yaw;
+}
+
+static void DetectTurn(const car_model *car, int moveDirect, const struct RtkTime *rtkTime)
+{
+ int angle;
+
+ if (turnCnt < 0 || TimeGetDiff(rtkTime, &prevDetectTurnTime) < 500) {
+ return;
+ }
+
+ angle = isTurn((int) car->yaw, prevYaw, TURN_THRESHOLD);
+
+ prevYaw = (int) car->yaw;
+ prevDetectTurnTime = *rtkTime;
+
+ if (turnCnt == 0) {
+ if (angle != 0 && moveDirect != 0) {
+ DEBUG("鏈夎浆鍚戣抗璞�");
+ turnCnt++;
+ beginTurnTime = *rtkTime;
+ startTurnYaw = (int) car->yaw;
+ }
+ } else if (turnCnt == 1) {
+ if (angle != 0 && moveDirect != 0) {
+ if (angle * prevTurnWise > 0) {
+ DEBUG("纭杞悜");
+ // 鍚屽悜杞姩
+ turnCnt++;
+ turnTimeCnt = 0;
+ } else {
+ beginTurnTime = *rtkTime;
+ startTurnYaw = (int) car->yaw;
+ }
+ } else {
+ turnCnt = 0;
+ }
+ } else if (turnCnt >= 2) {
+ if (moveDirect == 0) {
+ // 鏆傚仠
+ } else {
+ turnTimeCnt += TimeGetDiff(rtkTime, &prevDetectTurnTime);
+ int wise = isTurn((int) car->yaw, startTurnYaw, TURN_THRESHOLD);
+ DEBUG("杞姩瑙掑害 %d", wise);
+ if (ABS(wise) > 60) {
+ // 纭杞集琛屼负锛屾娴嬪紑濮嬪垰杞集鏃惰浆鍚戠伅鎯呭喌
+ 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) >= D_SEC(3)) {
+ DEBUG("杞悜鐏椂闂翠笉瓒�");
+ // 涓嶈冻3绉掞紝涓嶅悎鏍�
+ AddExamFault(14, rtkTime);
+ }
+ } else {
+ if (lamp.value != RIGHT_TURN_LIGHT) {
+ DEBUG("鍙樿皟鏈墦鐏�!!");
+ // 娌℃墦鐏紝涓嶅悎鏍�
+ AddExamFault(13, rtkTime);
+ } else if (TimeGetDiff(&beginTurnTime, &lamp.time) >= D_SEC(3)) {
+ DEBUG("杞悜鐏椂闂翠笉瓒�");
+ // 涓嶈冻3绉掞紝涓嶅悎鏍�
+ AddExamFault(14, rtkTime);
+ }
+ }
+ }
+ } else if (turnTimeCnt > D_SEC(10)) {
+ // 鍙栨秷杞悜妫�娴�
+ DEBUG("鍙栨秷杞悜妫�娴�1");
+ turnCnt = -1;
+ }
+ }
+ }
+
+ if (turnCnt < 2 && moveDirect != 0) {
+ turnTimeCnt += TimeGetDiff(rtkTime, &prevDetectTurnTime);
+ if (turnTimeCnt > D_SEC(15)) {
+ // 鍙栨秷杞悜妫�娴�
+ DEBUG("鍙栨秷杞悜妫�娴�2");
+ turnCnt = -1;
+ }
+ }
+
+ prevTurnWise = angle;
}
static char CheckCarTurn(LIST_CAR_MODEL &CarModelList)
--
Gitblit v1.8.0