From a3c194e003b0cfb272e153ff11c510e02c5f72cd Mon Sep 17 00:00:00 2001
From: yy1717 <fctom1215@outlook.com>
Date: 星期一, 25 五月 2020 18:32:11 +0800
Subject: [PATCH] 坐标
---
lib/src/main/cpp/test_items2/road_exam.cpp | 102 +++++++++++++++++++++++++++++++++++++++++++++------
1 files changed, 90 insertions(+), 12 deletions(-)
diff --git a/lib/src/main/cpp/test_items2/road_exam.cpp b/lib/src/main/cpp/test_items2/road_exam.cpp
index d132808..f7b028d 100644
--- a/lib/src/main/cpp/test_items2/road_exam.cpp
+++ b/lib/src/main/cpp/test_items2/road_exam.cpp
@@ -34,6 +34,12 @@
START_CAR_DONE
};
+enum {
+ STOP_CAR_NOT_DO,
+ STOP_CAR_DOING,
+ STOP_CAR_DONE
+};
+
static const int INVALID_ROAD = -1;
static const int TURN_THRESHOLD = 1;
@@ -43,6 +49,7 @@
const double CHANGE_LANE_RANGE = 100.0;
const double OVERTAKE_RANGE = 150.0;
const double OVERTAKE_HOLD_RANGE = 30.0; // 鍦ㄨ秴杞﹂亾琛岄┒鐨勪竴娈佃窛绂�
+const double EXAM_RANGE = 3000.0; // 鑷冲皯椹鹃┒璺濈
static const double LASTEST_BREAK_POINT = 30.0;
static const double NEXT_ROAD_TIP = 100.0; // 鍒拌揪璺彛鍓嶆彁绀轰笅涓�涓�庝箞璧�
@@ -81,7 +88,7 @@
static int gearErrorTime;
static int gearNSlideTime;
-static int startCar;
+static int startCar, stopCar;
static int currExamMapIndex;
static trigger_line_t *currRoadItem;
static int nextRoadId;
@@ -104,6 +111,10 @@
int type; // 瀹炵嚎锛岃櫄绾�
} CurrentLane;
static bool laneChanging;
+static double odoGraph;
+static struct drive_timer odoTimer;
+static double odoPrevSpeed;
+static int odoCnt;
static const int MAX_ENGINE_RPM = 2500;
static const double START_CAR_MOVE_DISTANCE = 10.0;
@@ -140,6 +151,7 @@
static void ArrivedRoadEnd(road_t &road, const car_model *car, LIST_CAR_MODEL &CarModelList);
static trigger_line_t * EntryItem(int index, road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList);
static void ClearAll(road_exam_map &map);
+static bool AllCmp(road_exam_map &map);
static void CheckBreakActive(road_exam_map &map, const car_model *car, LIST_CAR_MODEL &CarModelList,
double speed, int moveDirect, const struct RtkTime *rtkTime);
@@ -172,6 +184,8 @@
currExamMapIndex = FIND_POSITION;
startCar = START_CAR_NOT_DO;
+ stopCar = STOP_CAR_NOT_DO;
+
currRoadItem = NULL;
checkDoor = false;
@@ -187,6 +201,11 @@
nextRoadId = -1;
ClearAll(RoadMap);
+
+ odoGraph = 0.0;
+ odoCnt = 0;
+
+ // 鍒濆鍖栬�冮」
}
void TerminateRoadExam(void)
@@ -194,6 +213,7 @@
TerminateDummyLightExam();
TerminateStopCarExam();
TerminateOperateGearExam();
+ TerminateDriveStraightExam();
AppTimer_delete(TurnSignalError13ColdTimer);
AppTimer_delete(TurnSignalError14ColdTimer);
@@ -306,6 +326,25 @@
void TestRoadGeneral(road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime)
{
+ // 琛岄┒璺濈锛屼笉鍖呭惈鍊掕溅
+ if (odoCnt == 0 && moveDirect == 1) {
+ odoPrevSpeed = speed;
+ odoCnt = 1;
+ Rtk2DriveTimer(odoTimer, rtkTime);
+ } else if (odoCnt == 1) {
+ uint32_t tm = TimeGetDiff(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10,
+ odoTimer.hour, odoTimer.min, odoTimer.sec, odoTimer.msec*10);
+ if (tm >= D_SEC(1)) {
+ odoGraph += ((double)tm)*(odoPrevSpeed + speed)/2.0/1000.0;
+ if (moveDirect == 1) {
+ Rtk2DriveTimer(odoTimer, rtkTime);
+ odoPrevSpeed = speed;
+ } else {
+ odoCnt = 0;
+ }
+ }
+ }
+
// 瓒呴�熸娴�
if (moveDirect != 0 && speed > MAX_SPEED) {
if (!occurOverSpeed) {
@@ -636,7 +675,9 @@
crashRedLineNow = true;
}
+ // 妫�鏌ュ綋鍓嶈溅閬�
struct car_on_lane lane;
+
if (UpdateLane(lane, RoadMap.roads[currExamMapIndex], car)) {
if (lane.road == CurrentLane.road && lane.separate == CurrentLane.separate &&
@@ -737,7 +778,7 @@
}
}
- if (lane.road != CurrentLane.road || lane.separate != CurrentLane.separate) {
+ if (lane.road != CurrentLane.road || lane.separate != CurrentLane.separate || lane.direct != CurrentLane.direct) {
// 璺垨娈靛彉鏇达紝鎾ら攢鍙橀亾璺熻釜
DEBUG("============== 璺垨娈靛彉鏇� CURR %d, %d dir %d NEW %d, %d, dir %d", CurrentLane.road,
CurrentLane.separate, CurrentLane.direct,
@@ -818,7 +859,17 @@
if (startCar != START_CAR_DONE)
return;
- // 妫�娴嬬敱瑙﹀彂绾挎帶鍒剁殑椤圭洰
+ if (odoGraph > EXAM_RANGE && currRoadItem == NULL && AllCmp(RoadMap) && stopCar == STOP_CAR_NOT_DO) {
+ // 鍦ㄥ悎閫傛潯浠朵笅鍋滆溅缁撴潫鑰冭瘯
+ StartStopCarExam("");
+ stopCar = STOP_CAR_DOING;
+ } else if (stopCar == STOP_CAR_DOING) {
+ if (ExecuteStopCarExam(RoadMap.roads[currExamMapIndex], car, CarModelList, speed, moveDirect, rtkTime) < 0)
+ stopCar = STOP_CAR_DONE;
+ }
+
+
+ // 鎵ц鏌愪釜椤圭洰
if (currRoadItem != NULL) {
if (currRoadItem->active == ROAD_ITEM_CHANGE_LANE) {
if (DistanceOf(car->basePoint, roadItemStartPoint) > CHANGE_LANE_RANGE) {
@@ -837,11 +888,20 @@
currRoadItem = NULL;
}
} else if (currRoadItem->active == ROAD_ITEM_OPERATE_GEAR) {
- ExecuteOperateGearExam(rtkTime);
+ if (ExecuteOperateGearExam(rtkTime) < 0) {
+ currRoadItem = NULL;
+ }
+ } else if (currRoadItem->active == ROAD_ITEM_STRAIGHT) {
+ if (ExecuteDriveStraightExam(RoadMap.roads[currExamMapIndex], car, currRoadItem, rtkTime) < 0) {
+ currRoadItem = NULL;
+ }
}
- } else if (currExamMapIndex >= 0) {
- currRoadItem = EntryItem(currExamMapIndex, RoadMap, car, CarModelList);
- if (currRoadItem != NULL) {
+ } // 妫�娴嬬敱瑙﹀彂绾挎帶鍒剁殑椤圭洰
+ else if (currExamMapIndex >= 0) {
+ trigger_line_t *new_item = EntryItem(currExamMapIndex, RoadMap, car, CarModelList);
+
+ if (new_item != NULL && !new_item->cmp) {
+ currRoadItem = new_item;
if (!currRoadItem->tts.empty())
PlayTTS(currRoadItem->tts.c_str());
@@ -853,6 +913,8 @@
overtake = false;
} else if (currRoadItem->active == ROAD_ITEM_OPERATE_GEAR) {
StartOperateGearExam(rtkTime);
+ } else if (currRoadItem->active == ROAD_ITEM_STRAIGHT) {
+ StartDriveStraightExam(currRoadItem->tts);
}
}
}
@@ -903,7 +965,7 @@
MakeLine(&rightExtLine, &car->carXY[car->axial[AXIAL_FRONT]], &vp);
goto RIGHT_EXT_CMP;
} else {
- DEBUG("鍙充晶涓嶅瀭鐐� %d p1(%f,%f) p2(%f,%f) (%f,%f)", j, p1.X, p1.Y, p2.X, p2.Y, vp.X, vp.Y);
+// DEBUG("鍙充晶涓嶅瀭鐐� %d p1(%f,%f) p2(%f,%f) (%f,%f)", j, p1.X, p1.Y, p2.X, p2.Y, vp.X, vp.Y);
}
p1 = p2;
}
@@ -946,7 +1008,6 @@
orthogonalInSegment = true;
intersection = true;
-// DEBUG("鍒嗛亾绾� %d 宸︽浜�", j);
break;
} else if (IntersectionOf(rightExtLine, sep) == GM_Intersection) {
vector<int> stor(4);
@@ -960,7 +1021,6 @@
orthogonalInSegment = true;
intersection = true;
-// DEBUG("鍒嗛亾绾� %d 鍙虫浜�", j);
break;
}
p1 = p2;
@@ -983,12 +1043,15 @@
lane.road = road.id;
lane.separate = i;
lane.lane = itx->first;
+
+// DEBUG("left_char %d second %d", left_char, itx->second[1]);
+
if ((left_char == LINE_SOLID || left_char == LINE_HALF_SOLID_RIGHT) &&
(itx->second[1] == LINE_SOLID || itx->second[1] == LINE_HALF_SOLID_LEFT)) // 杞﹂亾宸﹀彸鍧囨槸瀹炵嚎
lane.direct = itx->second[2];
else
lane.direct = 0;
-// DEBUG("璺� %d 娈� %d 杞﹂亾 %d", lane.road, lane.separate, lane.lane);
+// DEBUG("璺� %d 娈� %d 杞﹂亾 %d 闄愬畾 %d", lane.road, lane.separate, lane.lane, lane.direct);
break;
} else {
right_direct = itx->second[3];
@@ -1000,12 +1063,15 @@
lane.road = road.id;
lane.separate = i;
lane.lane = orthogonal.size();
+
+// DEBUG("left_char %d right_direct %d", left_char, right_direct);
+
// 鏈�鍚庤溅閬撶殑鍙充晶闄愬畾
if (left_char == LINE_SOLID || left_char == LINE_HALF_SOLID_RIGHT)
lane.direct = right_direct;
else
lane.direct = 0;
-// DEBUG("璺� %d 娈� %d 杞﹂亾 %d", lane.road, lane.separate, lane.lane);
+// DEBUG("璺� %d 娈� %d 杞﹂亾 %d 闄愬畾 %d", lane.road, lane.separate, lane.lane, lane.direct);
}
out = lane;
return true;
@@ -1835,6 +1901,15 @@
return NULL;
}
+static bool AllCmp(road_exam_map &map)
+{
+ for (int i = 0; i < map.triggerLines.size(); ++i) {
+ if (!map.triggerLines[i].cmp)
+ return false;
+ }
+ return true;
+}
+
/************************************************************************
* 寮�濮嬫柊鐨勮�冭瘯鍚庯紝娓呴櫎鍦板浘鎵�鏈夌殑鍒硅溅銆佸仠杞﹁褰�
* @param map
@@ -1849,6 +1924,9 @@
for (int i = 0; i < map.specialAreas.size(); i++) {
map.specialAreas[i].overSpeed = map.specialAreas[i].activeBreak = false;
}
+ for (int i = 0; i < map.triggerLines.size(); ++i) {
+ map.triggerLines[i].cmp = false;
+ }
}
static void CheckBreakActive(road_exam_map &map, const car_model *car, LIST_CAR_MODEL &CarModelList,
--
Gitblit v1.8.0