From 372659e4d884f390f2f5031c55ba7b3fa9fd5912 Mon Sep 17 00:00:00 2001
From: yy1717 <fctom1215@outlook.com>
Date: 星期四, 13 八月 2020 18:21:13 +0800
Subject: [PATCH] 坐标
---
lib/src/main/cpp/test_items2/road_exam.cpp | 412 ++++++++++++++++++++++++++++++++++++----------------------
1 files changed, 257 insertions(+), 155 deletions(-)
diff --git a/lib/src/main/cpp/test_items2/road_exam.cpp b/lib/src/main/cpp/test_items2/road_exam.cpp
index e476e9a..d75355d 100644
--- a/lib/src/main/cpp/test_items2/road_exam.cpp
+++ b/lib/src/main/cpp/test_items2/road_exam.cpp
@@ -42,6 +42,11 @@
STOP_CAR_DONE
};
+enum {
+ CROSSING_NOT_HINT,
+ CROSSING_HAS_HINT,
+};
+
typedef struct {
int road;
int sep;
@@ -61,6 +66,11 @@
int value;
struct RtkTime time;
} car_sensor_value_t;
+
+typedef struct {
+ int gain;
+ struct RtkTime time;
+} change_lane_t;
static const int INVALID_ROAD = -1;
@@ -103,6 +113,7 @@
static bool startCarLeftTurnSignal, checkStartCarSignal;
static struct RtkTime crashGreenRunTime, crashGreenStartTime;
+
static struct drive_timer gearErrorTimePoint;
static struct drive_timer gearNSlideTimePoint;
static struct drive_timer startCarLeftTurnSignalTime;
@@ -123,17 +134,13 @@
static bool handBreakActive = false;
static bool reportRPMOver = false;
-static const uint32_t TURN_ERROR_COLD_TIME = D_SEC(10);
-
-static bool turnError13Cold, turnError14Cold;
-
static lane_t Lane;
+static change_lane_t ChangeLane;
-static bool laneChanging;
-static int changeLaneDirect;
static int CrashLineType;
static map<int, car_sensor_value_t> CarSensorValue;
+static map<int, int> CrossingHint;
static const int MAX_ENGINE_RPM = 2500;
static const double START_CAR_MOVE_DISTANCE = 10.0;
@@ -158,10 +165,6 @@
static char isTurn(int currYaw, int prevYaw, int &ang);
static char CheckCarTurn(LIST_CAR_MODEL &CarModelList);
-
-static void TurnSignalError13ColdTimer(union sigval sig);
-static void TurnSignalError14ColdTimer(union sigval sig);
-static void ReportTurnSignalError(int err, const struct RtkTime *rtkTime);
static trigger_line_t * EntryItem(int index, road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList);
@@ -211,11 +214,7 @@
checkStartCarSignal = startCarLeftTurnSignal = false;
- turnError13Cold = turnError14Cold = true;
-
Lane.road = Lane.sep = Lane.no = -1;
- laneChanging = false;
- changeLaneDirect = 0;
nextRoadId = -1;
checkTurn = false;
@@ -234,9 +233,6 @@
TerminateStopCarExam();
TerminateOperateGearExam();
TerminateDriveStraightExam();
-
- AppTimer_delete(TurnSignalError13ColdTimer);
- AppTimer_delete(TurnSignalError14ColdTimer);
}
/*********************************************************************
@@ -598,10 +594,16 @@
}
// 鍒嗛亾瀹炵嚎
+// DEBUG("%d 鍒嗛亾娈� %d", n, RoadMap.roads[n].separate.size());
+
for (int m = 0; m < RoadMap.roads[n].separate.size(); ++m) {
// 涓�缁勮溅閬�
+// DEBUG("%d %d 杞﹂亾鏁� %d", n, m, RoadMap.roads[n].separate[m].lines.size());
+
for (int l = 0; l < RoadMap.roads[n].separate[m].lines.size(); ++l) {
// 澶氭牴鍒嗛亾绾�
+
+// DEBUG("%d %d %d 绾挎鏍� %d", n, m, l, RoadMap.roads[n].separate[m].lines[l].size());
for (int a = 0; a < RoadMap.roads[n].separate[m].lines[l].size(); ++a) {
// 涓�鏍瑰垎閬撶嚎涓嚎鍨嬬浉鍚岀殑
int character = RoadMap.roads[n].separate[m].lines[l][a].character;
@@ -827,39 +829,6 @@
WriteCarSensorValue(BREAK, ReadCarStatus(BREAK), rtkTime);
}
-static void TurnSignalError13ColdTimer(union sigval sig)
-{
- AppTimer_delete(TurnSignalError13ColdTimer);
-
- turnError13Cold = true;
-}
-
-static void TurnSignalError14ColdTimer(union sigval sig)
-{
- AppTimer_delete(TurnSignalError14ColdTimer);
-
- turnError14Cold = true;
-}
-
-static void ReportTurnSignalError(int err, const struct RtkTime *rtkTime)
-{
- if (err == 13 && turnError13Cold) {
- DEBUG("璧锋涓嶅紑杞悜鐏�");
- AddExamFault(13, rtkTime);
-
- turnError13Cold = false;
- AppTimer_delete(TurnSignalError13ColdTimer);
- AppTimer_add(TurnSignalError13ColdTimer, TURN_ERROR_COLD_TIME);
- } else if (err == 14 && turnError14Cold) {
- DEBUG("璧锋杞悜鐏笉瓒�3绉�");
- AddExamFault(14, rtkTime);
-
- turnError14Cold = false;
- AppTimer_delete(TurnSignalError14ColdTimer);
- AppTimer_add(TurnSignalError14ColdTimer, TURN_ERROR_COLD_TIME);
- }
-}
-
static int TestRoadStartCar(const car_model *car, double speed, int moveDirect, const struct RtkTime *rtkTime)
{
double moveDistance;
@@ -882,10 +851,10 @@
if (!checkStartCarSignal && moveDirect == 1) {
checkStartCarSignal = true;
if (!startCarLeftTurnSignal) {
- ReportTurnSignalError(13, rtkTime);
+ AddExamFault(13, rtkTime);
} else if (TimeGetDiff(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10,
startCarLeftTurnSignalTime.hour, startCarLeftTurnSignalTime.min, startCarLeftTurnSignalTime.sec, startCarLeftTurnSignalTime.msec*10) < TURN_SIGNAL_LAMP_ADVANCE) {
- ReportTurnSignalError(14, rtkTime);
+ AddExamFault(14, rtkTime);
}
}
@@ -997,36 +966,46 @@
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;
+ } else {
+ ChangeLane.gain = 0;
}
// 妫�鏌ヨ浆鍚戠伅
if (gain != 0) {
+ DEBUG("鍙橀亾 gain %d", 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("鍙樿皟鏈墦鐏�!!");
// 娌℃墦鐏紝涓嶅悎鏍�
- ReportTurnSignalError(13, rtkTime);
+ AddExamFault(13, rtkTime);
} else if (TimeGetDiff(&crashGreenStartTime, &lamp.time) >= D_SEC(3)) {
DEBUG("杞悜鐏椂闂翠笉瓒�");
// 涓嶈冻3绉掞紝涓嶅悎鏍�
reportTurnSignalError = true;
- ReportTurnSignalError(14, rtkTime);
+ AddExamFault(14, rtkTime);
}
} else {
if (lamp.value != RIGHT_TURN_LIGHT) {
DEBUG("鍙樿皟鏈墦鐏�!!");
// 娌℃墦鐏紝涓嶅悎鏍�
- ReportTurnSignalError(13, rtkTime);
- } else {
+ AddExamFault(13, rtkTime);
+ } else if (TimeGetDiff(&crashGreenStartTime, &lamp.time) >= D_SEC(3)) {
DEBUG("杞悜鐏椂闂翠笉瓒�");
// 涓嶈冻3绉掞紝涓嶅悎鏍�
reportTurnSignalError = true;
- ReportTurnSignalError(14, rtkTime);
+ AddExamFault(14, rtkTime);
}
}
}
+
+ if (((ChangeLane.gain < 0 && gain < 0) || (ChangeLane.gain > 0 && gain > 0)) && TimeGetDiff(rtkTime, &ChangeLane.time) < CHANGE_LANE_MIN_INTERVAL) {
+ DEBUG("杩炵画鍙橀亾");
+ AddExamFault(15, rtkTime);
+ }
+ ChangeLane.gain = gain;
+ ChangeLane.time = *rtkTime;
}
Lane = newLane;
@@ -1034,33 +1013,222 @@
return gain;
}
-void TestRoadGeneral(road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime)
+static void ChangeCrossingStatus(int roadIndex, int index, int status)
{
- uint32_t cts = AppTimer_GetTickCount();
- int ri = CalcRoadIndex(-1, RoadMap, car);
- bool crash = CrashRedLine(CrashLineType, 0, RoadMap, car, CarModelList);
- lane_t laneInfo;
- double redist = -1;
+ int key = roadIndex * 100 + index;
- laneInfo.road = -1;
- laneInfo.sep = -1;
- laneInfo.no = -1;
+ auto it = CrossingHint.find(key);
- if (ri >= 0) {
- GetLane(laneInfo, car->carXY[car->axial[AXIAL_FRONT]], RoadMap, ri);
+ if (it != CrossingHint.end()) {
+ CrossingHint.erase(it);
+ }
+ CrossingHint.insert(pair<int, int>(key, status));
+}
- int m = RoadMap.roads[ri].rightEdge.size();
- int n = RoadMap.roads[ri].rightEdge[m-1].points.size();
+static int GetCrossingStatus(int roadIndex, int index)
+{
+ int key = roadIndex * 100 + index;
- PointF base;
+ auto it = CrossingHint.find(key);
- base.X = 428922.2985; base.Y = 3292119.5457;
+ if (it != CrossingHint.end()) {
+ return it->second;
+ }
+ return CROSSING_NOT_HINT;
+}
- redist = CalcDistanceReference(car->carXY[car->axial[AXIAL_FRONT]], base,
- RoadMap.roads[ri].rightEdge);
+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) {
+ PointF point;
+ double distance;
+
+ point.X = road.stopLine[i].line.X1;
+ point.Y = road.stopLine[i].line.Y1;
+
+ distance = CalcDistanceReference(car->carXY[car->axial[AXIAL_FRONT]], point, road.rightEdge);
+
+ if (distance > 10 && distance < 50) {
+ // 鎻愮ず璺彛鎬庝箞璧�
+ if (GetCrossingStatus(roadIndex, i) == CROSSING_NOT_HINT) {
+ if (!road.stopLine[i].tts.empty()) {
+ DEBUG("璺彛鎻愮ず %s", road.stopLine[i].tts.c_str());
+ PlayTTS(road.stopLine[i].tts.c_str());
+ }
+ ChangeCrossingStatus(roadIndex, i, CROSSING_HAS_HINT);
+ }
+ } else if (distance > 55 && GetCrossingStatus(roadIndex, i) != CROSSING_NOT_HINT) {
+ ChangeCrossingStatus(roadIndex, i, CROSSING_NOT_HINT);
+ }
+ }
+}
+
+static map <int, int> TargetReduceRec;
+
+enum {
+ NOT_REDUCE_SPEED,
+ REDUCE_SPEED,
+ STOP_CAR,
+ OVER_SPEED
+};
+
+static void ChangeTargetReduceRec(int roadIndex, int index, int status)
+{
+ int key = roadIndex * 100 + index;
+
+ auto it = CrossingHint.find(key);
+
+ if (it != CrossingHint.end()) {
+ CrossingHint.erase(it);
+ }
+ CrossingHint.insert(pair<int, int>(key, status));
+}
+
+static int GetTargetReduceRec(int roadIndex, int index)
+{
+ int key = roadIndex * 100 + index;
+
+ auto it = CrossingHint.find(key);
+
+ if (it != CrossingHint.end()) {
+ return it->second;
+ }
+ return -1;
+}
+
+void ApproachTarget(road_exam_map &RoadMap, const car_model *car, int roadIndex, double speed, int moveDirect)
+{
+ car_sensor_value_t brk = ReadCarSensorValue(BREAK);
+
+ if (roadIndex < 0 || roadIndex >= RoadMap.roads.size())
+ return;
+
+ for (int n = 0; n < RoadMap.roads[roadIndex].stopLine.size(); ++n) {
+ PointF point;
+ double distance;
+
+ point.X = road.stopLine[i].line.X1;
+ point.Y = road.stopLine[i].line.Y1;
+
+ distance = CalcDistanceReference(car->carXY[car->axial[AXIAL_FRONT]], point, RoadMap.roads[roadIndex].rightEdge);
+
+ if (distance > 1e-3 && distance < LASTEST_BREAK_POINT) {
+ // 璁板綍鍒硅溅锛屽仠杞�
+ if (brk.value == BREAK_ACTIVE) {
+
+ }
+ } else if (distance > LASTEST_BREAK_POINT + 5) {
+
+ }
}
- DEBUG("褰撳墠閬撹矾绱㈠紩 %d, 瑙﹀彂绾㈢嚎 %d lane %d 璺濈 %f %ld", ri, crash, laneInfo.no, redist, AppTimer_GetTickCount() - cts);
+ for (int i = 0; i < RoadMap.specialAreas.size(); i++) {
+ if (RoadMap.specialAreas[i].type == GRID_AREA || RoadMap.specialAreas[i].road != RoadMap.roads[roadIndex].id)
+ continue;
+
+ if (map.specialAreas[i].area.size() == 2 && map.specialAreas[i].leftPoints.size() != 2) {
+ // 璁＄畻鐐瑰埌宸︿晶璺竟绾跨殑鍨傜偣
+ int road = 0;
+ for (road = 0; road < map.roads.size(); ++road) {
+ if (map.roads[road].id == map.specialAreas[i].road)
+ break;
+ }
+
+ PointF vPoint = CalcProjectionWithRoadEdge(map.roads[road].leftEdge, map.specialAreas[i].area[0]);
+// DEBUG("璁＄畻鍨傜偣1 (%f, %f)", vPoint.X, vPoint.Y);
+
+ map.specialAreas[i].leftPoints.push_back(vPoint);
+
+ vPoint = CalcProjectionWithRoadEdge(map.roads[road].leftEdge, map.specialAreas[i].area[1]);
+// DEBUG("璁＄畻鍨傜偣2 (%f, %f)", vPoint.X, vPoint.Y);
+ map.specialAreas[i].leftPoints.push_back(vPoint);
+ }
+
+ if (map.specialAreas[i].type == ZEBRA_CROSSING || map.specialAreas[i].type == BUS_STATION_AREA) {
+// DEBUG("鏂戦┈绾�");
+ Line startLine;
+
+ MakeLine(&startLine, &map.specialAreas[i].area[0], &map.specialAreas[i].leftPoints[0]);
+
+ // 杞﹀ご鍜屾枒椹嚎璺濈涓嶈冻30绫�
+ if (IntersectionOf(extLine, startLine) == GM_Intersection &&
+ IntersectionOfLine(car->carXY[ car->axial[AXIAL_FRONT] ], startLine) == 1 ) {
+ DEBUG("杩涘叆鍑忛�熷尯 %d", map.specialAreas[i].type);
+ if (BreakDone == BREAK_ACTIVE) {
+ map.specialAreas[i].activeBreak = true;
+ }
+ }
+
+ // 璺ㄧ嚎鍚庯紝妫�鏌ュ埞杞﹀姩浣�
+ if (CrashTheLine(startLine, car, CarModelList)) {
+ if (!map.specialAreas[i].activeBreak) {
+ // 涓嶆寜瑙勫畾鍑忛�燂紝涓嶅悎鏍�
+ DEBUG("涓嶆寜瑙勫畾鍑忛��");
+ if (map.specialAreas[i].type == ZEBRA_CROSSING) {
+ AddExamFault(48, rtkTime);
+ } else {
+ AddExamFault(50, rtkTime);
+ }
+ } else {
+ DEBUG("鎸夎瀹氬噺閫�");
+ }
+
+ }
+ } else if (map.specialAreas[i].type == SCHOOL_AREA) {
+ Polygon school;
+// DEBUG("瀛︽牎");
+ school.num = 4;
+ school.point = (PointF *) malloc(school.num * sizeof(PointF));
+
+ school.point[0] = map.specialAreas[i].area[0];
+ school.point[1] = map.specialAreas[i].area[1];
+ school.point[2] = map.specialAreas[i].leftPoints[1];
+ school.point[3] = map.specialAreas[i].leftPoints[0];
+
+ if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &school) == GM_Containment) {
+ if (ConvertMs2KMh(speed) > PASS_SCHOOL_MAX_SPEED) {
+ if (!map.specialAreas[i].overSpeed) {
+ DEBUG("閫氳繃瀛︽牎鍖哄煙瓒呴��");
+ AddExamFault(49, rtkTime);
+ map.specialAreas[i].overSpeed = true;
+ }
+ }
+ }
+
+ free(school.point);
+ }
+ }
+}
+
+
+
+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(CrashLineType, 0, RoadMap, car, CarModelList);
+// lane_t laneInfo;
+// double redist = -1;
+//
+// laneInfo.road = -1;
+// laneInfo.sep = -1;
+// laneInfo.no = -1;
+//
+// if (ri >= 0) {
+// GetLane(laneInfo, car->carXY[car->axial[AXIAL_FRONT]], RoadMap, ri);
+//
+// int m = RoadMap.roads[ri].rightEdge.size();
+// int n = RoadMap.roads[ri].rightEdge[m-1].points.size();
+//
+// PointF base;
+//
+// base.X = 428922.2985; base.Y = 3292119.5457;
+//
+// redist = CalcDistanceReference(car->carXY[car->axial[AXIAL_FRONT]], base,
+// RoadMap.roads[ri].rightEdge);
+// }
+//
+// DEBUG("褰撳墠閬撹矾绱㈠紩 %d, 瑙﹀彂绾㈢嚎 %d lane %d 璺濈 %f %ld", ri, crash, laneInfo.no, redist, AppTimer_GetTickCount() - cts);
UpdateCarSensor(rtkTime);
@@ -1225,15 +1393,27 @@
speed, moveDirect, rtkTime);
// 妫�娴嬬寮�姝よ矾娈碉紝鍏ㄨ溅闇�涓嶅湪鑼冨洿鍐�
+ int oldid = currExamMapIndex;
currExamMapIndex = CalcRoadIndex(currExamMapIndex, RoadMap, car);
+ if (currExamMapIndex >= 0) {
+ HintCrossing(currExamMapIndex, RoadMap.roads[currExamMapIndex], car, CarModelList);
+ }
+
+ if (oldid != currExamMapIndex) {
+ DEBUG("閬撹矾ID鍒囨崲 %d", currExamMapIndex);
+ }
+
+ oldid = CrashLineType;
DetectLine(currExamMapIndex, RoadMap, car, CarModelList, moveDirect, rtkTime);
+
+ if (oldid != CrashLineType) {
+ DEBUG("鍘嬬嚎绫诲瀷鍒囨崲 %d", CrashLineType);
+ }
DetectLane(RoadMap, car, currExamMapIndex, rtkTime);
// 妫�娴嬪帇绾跨姸鎬�
-
-
TestRoadStartCar(car, speed, moveDirect, rtkTime);
if (startCar != START_CAR_DONE)
@@ -1482,84 +1662,6 @@
road.arrivedTail = false;
}
}*/
-
-// 璺彛锛屾彁绀猴紝鍓�30绫宠繘鍏ワ紝 绂诲紑
-// 浜鸿閬擄紝杞︾珯锛屽墠30绫宠繘鍏ワ紝绂诲紑
-// 瀛︽牎锛岃繘鍏ワ紝绂诲紑
-
-map<int, int> CrossingStatus;
-
-static void ChangeCrossingStatus(int roadIndex, int index, int status)
-{
- int key = roadIndex * 100 + index;
-
- auto it = CrossingStatus.find(key);
-
- if (it != CrossingStatus.end()) {
- CrossingStatus.erase(it);
- }
- CrossingStatus.insert(pair<int, int>(key, status));
-}
-
-static int GetCrossingStatus(int roadIndex, int index)
-{
- int key = roadIndex * 100 + index;
-
- auto it = CrossingStatus.find(key);
-
- if (it != CrossingStatus.end()) {
- return it->second;
- }
- return -1;
-}
-
-enum {
- CROSSING_STATUS_INVALID = -1,
- CROSSING_STATUS_GUIDE,
- CROSSING_STATUS_REDUCE_SPEED,
- CROSSING_STATUS_BREAK_CAR,
- CROSSING_STATUS_STOP_CAR
-};
-
-static void CheckCrossing(int roadIndex, road_t &road, const car_model *car, LIST_CAR_MODEL &CarModelList)
-{
- for (int i = 0; i < road.stopLine.size(); ++i) {
- if (CrashTheLine(road.stopLine[i].line, car, CarModelList)) {
- if (road.stopLine[i].stopFlag) {
- if (GetCrossingStatus(roadIndex, i) != CROSSING_STATUS_STOP_CAR) {
-
- }
- } else {
- if (GetCrossingStatus(roadIndex, i) != CROSSING_STATUS_BREAK_CAR) {
-
- }
- }
- ChangeCrossingStatus(roadIndex, i, CROSSING_STATUS_INVALID);
- } else {
-
- }
- PointF point;
- double distance;
-
- point.X = road.stopLine[i].line.X1;
- point.Y = road.stopLine[i].line.Y1;
-
- distance = CalcDistanceReference(car->carXY[car->axial[AXIAL_FRONT]], point, road.rightEdge);
-
- if (distance > 10 && distance < 50) {
- if (GetCrossingStatus(roadIndex, i) == CROSSING_STATUS_INVALID) {
- if (!road.stopLine[i].tts.empty()) {
- PlayTTS(road.stopLine[i].tts.c_str());
- }
- ChangeCrossingStatus(roadIndex, i, CROSSING_STATUS_GUIDE);
- }
- } else if (distance > 0 && distance < 30) {
- if (GetCrossingStatus(roadIndex, i) != 1) {
- ChangeCrossingStatus(roadIndex, i, 1);
- }
- }
- }
-}
static trigger_line_t * EntryItem(int index, road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList)
{
--
Gitblit v1.8.0