From 8a09b209f1c546a2fa15329e8f69b4a4f89557c9 Mon Sep 17 00:00:00 2001
From: yy1717 <fctom1215@outlook.com>
Date: 星期五, 21 八月 2020 18:59:19 +0800
Subject: [PATCH] 自适应触发和个子项修改。
---
lib/src/main/cpp/test_items2/road_exam.cpp | 215 ++++++++++++++++++-----------------------------------
1 files changed, 72 insertions(+), 143 deletions(-)
diff --git a/lib/src/main/cpp/test_items2/road_exam.cpp b/lib/src/main/cpp/test_items2/road_exam.cpp
index 3f2955a..e4e8c9a 100644
--- a/lib/src/main/cpp/test_items2/road_exam.cpp
+++ b/lib/src/main/cpp/test_items2/road_exam.cpp
@@ -17,6 +17,8 @@
#include "operate_gear.h"
#include "../test_common/odo_graph.h"
#include "car_start.h"
+#include "overtake.h"
+#include "change_lane.h"
#include <cmath>
#include <vector>
@@ -30,19 +32,6 @@
using namespace std;
-#define TURN_CHECK_CNT 6
-
-enum {
- START_CAR_NOT_DO,
- START_CAR_DOING,
- START_CAR_DONE
-};
-
-enum {
- STOP_CAR_NOT_DO,
- STOP_CAR_DOING,
- STOP_CAR_DONE
-};
enum {
CROSSING_NOT_HINT,
@@ -79,25 +68,15 @@
static const int CROSSING_TURN_THRESHOLD = 45;
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;
-const double CHANGE_LANE_RANGE = 100.0;
-const double OVERTAKE_RANGE = 150.0;
-const int OVERTAKE_HOLD_TIME = D_SEC(3); // 鍦ㄨ秴杞﹂亾琛岄┒鐨勪竴娈垫椂闂�
+
const double EXAM_RANGE = 3000.0; // 鑷冲皯椹鹃┒璺濈
-static const double NEXT_ROAD_TIP = 100.0; // 鍒拌揪璺彛鍓嶆彁绀轰笅涓�涓�庝箞璧�
-
-
-static bool occurCrashRedLine;
-static bool occurCrashGreenLine;
static bool occurOverSpeed;
static bool occurSecondBreak;
-static char carIntersectionOfGreenLine;
-static int currTurnSignalStatus;
-static int turnSignalStatusWhenCrashGreenLine;
static int prevMoveDirect;
static uint32_t stopTimepoint = 0;
@@ -114,19 +93,11 @@
static struct drive_timer gearErrorTimePoint;
static struct drive_timer gearNSlideTimePoint;
-static struct drive_timer startCarLeftTurnSignalTime;
-static struct drive_timer overTakeCmpTime;
static int gearErrorTime;
static int gearNSlideTime;
-static int stopCar;
static int currExamMapIndex;
-static trigger_line_t *currRoadItem;
-static PointF roadItemStartPoint;
-static struct drive_timer roadItemStartTime;
-static bool overtake = false;
-static bool checkTurn = false;
static lane_t Lane;
static change_lane_t ChangeLane;
@@ -170,16 +141,12 @@
static const uint32_t CRASH_DOTTED_LINE_TIMEOUT = D_SEC(10);
static const uint32_t TURN_SIGNAL_LAMP_ADVANCE = D_SEC(3);
-static const int CRL_NONE = 0;
-static const int CRL_SEP_DOTTED = 1;
-static const int CRL_SEP_SOLID = 2;
-static const int CRL_EDGE_DOTTED = 3;
-static const int CRL_EDGE_SOLID = 4;
-static const double MAX_SPEED = 60.0 * 1000.0 / 3600.0;
+static const double MAX_SPEED = 60.0 * 1000.0 / 3600.0; // 瓒呴�熺‘璁�
+static const double DEC_MAX_SPEED = 55.0 * 1000.0 / 3600.0; // 瓒呴�熷彇娑�
static const int SPEED_GEAR_TABLE[][2] = {{0, 20}, {5, 30}, {15, 40}, {25, 10000}, {35, 10000}};
-static void ItemExam(road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime, double straight, double road_end);
+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 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);
@@ -187,8 +154,6 @@
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 bool AllCmp(road_exam_map &map);
static int CalcRoadIndex(int currRoadIndex, road_exam_map &RoadMap, const car_model *car);
@@ -198,15 +163,10 @@
{
DEBUG("Start road_exam");
- occurCrashRedLine = false;
- occurCrashGreenLine = false;
occurOverSpeed = false;
occurSecondBreak = false;
- carIntersectionOfGreenLine = 0;
-
- currTurnSignalStatus = OFF_LIGHT;
prevMoveDirect = 0;
reportStopCarOnRedArea = false;
@@ -220,14 +180,9 @@
currExamMapIndex = -1;
- stopCar = STOP_CAR_NOT_DO;
-
- currRoadItem = NULL;
-
Lane.road = Lane.sep = Lane.no = -1;
Lane.guide = 0;
- checkTurn = false;
CrashLineType = -1;
turnCnt = -1;
@@ -934,7 +889,7 @@
DEBUG("鍙樿皟鏈墦鐏�!!");
// 娌℃墦鐏紝涓嶅悎鏍�
AddExamFault(13, rtkTime);
- } else if (TimeGetDiff(&crashGreenStartTime, &lamp.time) >= D_SEC(3)) {
+ } else if (TimeGetDiff(&crashGreenStartTime, &lamp.time) >= TURN_SIGNAL_LAMP_ADVANCE) {
DEBUG("杞悜鐏椂闂翠笉瓒�");
// 涓嶈冻3绉掞紝涓嶅悎鏍�
AddExamFault(14, rtkTime);
@@ -944,7 +899,7 @@
DEBUG("鍙樿皟鏈墦鐏�!!");
// 娌℃墦鐏紝涓嶅悎鏍�
AddExamFault(13, rtkTime);
- } else if (TimeGetDiff(&crashGreenStartTime, &lamp.time) >= D_SEC(3)) {
+ } else if (TimeGetDiff(&crashGreenStartTime, &lamp.time) >= TURN_SIGNAL_LAMP_ADVANCE) {
DEBUG("杞悜鐏椂闂翠笉瓒�");
// 涓嶈冻3绉掞紝涓嶅悎鏍�
AddExamFault(14, rtkTime);
@@ -1206,9 +1161,6 @@
return guide;
}
-static uint8_t itemExec[4] = {0};
-static double odo;
-
void TestRoadGeneral(road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime)
{
double BigStraightRoadFree = 0, RoadCrossingFree = 0;
@@ -1218,14 +1170,14 @@
UpdataOdo(speed, moveDirect, rtkTime);
// 瓒呴�熸娴�
- if (moveDirect != 0 && speed > MAX_SPEED) {
+ if (speed > MAX_SPEED) {
if (!occurOverSpeed) {
occurOverSpeed = true;
// 瓒呴�燂紝涓嶅悎鏍�
- DEBUG("瓒呴�� %f", speed);
+ DEBUG("瓒呴�� %f", ConvertMs2KMh(speed));
AddExamFault(10, rtkTime);
}
- } else {
+ } else if (speed < DEC_MAX_SPEED ) {
occurOverSpeed = false;
}
@@ -1433,7 +1385,7 @@
RoadCrossingFree = freeSepDis;
- DEBUG("鐩撮亾鍓╀綑璺濈 %f, 杞﹂亾鍓╀綑璺濈 %f", BigStraightRoadFree, RoadCrossingFree);
+// DEBUG("绱琛岄┒璺濈 %f, 鐩撮亾鍓╀綑璺濈 %f, 杞﹂亾鍓╀綑璺濈 %f", ReadOdo(), BigStraightRoadFree, RoadCrossingFree);
}
// 妫�娴嬪帇绾跨姸鎬�
@@ -1441,75 +1393,14 @@
// 棰濆鐨勮浆鍚戞娴�
DetectTurn(car, moveDirect, rtkTime);
- ItemExam(RoadMap, car, CarModelList, speed, moveDirect, rtkTime, BigStraightRoadFree, RoadCrossingFree);
-
- if (ReadOdo() > 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) {
- DEBUG("鍙橀亾璺濈瓒呮爣");
- AddExamFault(3, rtkTime);
- currRoadItem = NULL;
- }
- } else if (currRoadItem->active == ROAD_ITEM_OVERTAKE) {
- if (!overtake && DistanceOf(car->basePoint, roadItemStartPoint) > OVERTAKE_RANGE) {
- DEBUG("瓒呰溅璺濈瓒呮爣");
- AddExamFault(3, rtkTime);
- currRoadItem = NULL;
- } else if (overtake && TimeGetDiff(rtkTime->hh, rtkTime->mm, rtkTime->ss,
- rtkTime->mss * 10,
- overTakeCmpTime.hour, overTakeCmpTime.min,
- overTakeCmpTime.sec, overTakeCmpTime.msec * 10) > OVERTAKE_HOLD_TIME) {
- DEBUG("鍥炲師杞﹂亾");
- PlayTTS("璇疯繑鍥炲師杞﹂亾");
- currRoadItem = NULL;
- }
- } else if (currRoadItem->active == ROAD_ITEM_OPERATE_GEAR) {
- 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) {
- 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());
-
- // 鍒濆鏃堕棿鍜岃窛绂婚檺鍒�
- roadItemStartPoint = car->basePoint;
- Rtk2DriveTimer(roadItemStartTime, rtkTime);
-
- if (currRoadItem->active == ROAD_ITEM_OVERTAKE) {
- overtake = false;
- } else if (currRoadItem->active == ROAD_ITEM_OPERATE_GEAR) {
- StartOperateGearExam(rtkTime);
- } else if (currRoadItem->active == ROAD_ITEM_STRAIGHT) {
- StartDriveStraightExam(currRoadItem->tts);
- }
- }
- }*/
+ ItemExam(RoadMap, currExamMapIndex, car, CarModelList, speed, moveDirect, rtkTime, BigStraightRoadFree, RoadCrossingFree);
}
-static void ItemExam(road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime, double straight, double road_end)
+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 double freeRunDistance;
static double totalRunDistance;
+ static double freeRunExceptDistance = 50.0;
if (RoadExamStatus == ROAD_EXAM_READY_NEXT) {
if (RoadExamItem[ROAD_EXAM_ITEM_CAR_START] == ROAD_EXAM_ITEM_NOT_EXEC) {
@@ -1517,28 +1408,59 @@
totalRunDistance = ReadOdo();
RoadExamStatus = ROAD_EXAM_ITEM_CAR_START;
} else {
- if (RoadExamItem[ROAD_EXAM_ITEM_STRAIGHT] == ROAD_EXAM_ITEM_NOT_EXEC) {
- if (straight > 170 && road_end > 170) {
+ bool not_complete = false;
+ 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 (RoadExamItem[ROAD_EXAM_ITEM_OP_GEAR] == ROAD_EXAM_ITEM_NOT_EXEC) {
- if (straight > 150 && road_end > 150) {
+ not_complete = true;
+ 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 (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 (!not_complete) {
+ if (road_end > 200 && ReadOdo() > EXAM_RANGE) {
+ RoadExamStatus = ROAD_EXAM_ITEM_CAR_STOP;
+ StartStopCarExam();
+ return;
+ }
+ }
}
} else if (RoadExamStatus == ROAD_EXAM_FREE_RUN) {
- if (ReadOdo() - freeRunDistance > 300) {
+ if (ReadOdo() - freeRunDistance > freeRunExceptDistance) {
RoadExamStatus = ROAD_EXAM_READY_NEXT;
+
}
} else {
bool testing = false;
@@ -1550,17 +1472,33 @@
case ROAD_EXAM_ITEM_OP_GEAR:
testing = TestOperateGear(rtkTime);
break;
+ case ROAD_EXAM_ITEM_CHANGE_LANE:
+ testing = TestChangeLane(Lane.no, rtkTime);
+ break;
+ case ROAD_EXAM_ITEM_OVER_TAKE:
+ testing = TestOvertake(Lane.no, rtkTime);
+ break;
+ case ROAD_EXAM_ITEM_STRAIGHT:
+ testing = TestDriveStraight(car, rtkTime);
+ break;
+ case ROAD_EXAM_ITEM_CAR_STOP:
+ testing = TestStopCar(RoadMap, roadIndex, car, moveDirect, rtkTime);
+ break;
default:break;
}
if (!testing) {
RoadExamItem[RoadExamStatus] = ROAD_EXAM_ITEM_EXECED;
+ if (RoadExamStatus == ROAD_EXAM_ITEM_CAR_START) {
+ freeRunExceptDistance = 60.0;
+ } else {
+ freeRunExceptDistance = 200.0;
+ }
RoadExamStatus = ROAD_EXAM_FREE_RUN;
freeRunDistance = ReadOdo();
}
}
-
}
void Rtk2DriveTimer(struct drive_timer &tm, const struct RtkTime *rtkTime)
@@ -1666,7 +1604,7 @@
DEBUG("鍙樿皟鏈墦鐏�!!");
// 娌℃墦鐏紝涓嶅悎鏍�
AddExamFault(13, rtkTime);
- } else if (TimeGetDiff(&beginTurnTime, &lamp.time) >= D_SEC(3)) {
+ } else if (TimeGetDiff(&beginTurnTime, &lamp.time) >= TURN_SIGNAL_LAMP_ADVANCE) {
DEBUG("杞悜鐏椂闂翠笉瓒�");
// 涓嶈冻3绉掞紝涓嶅悎鏍�
AddExamFault(14, rtkTime);
@@ -1676,7 +1614,7 @@
DEBUG("鍙樿皟鏈墦鐏�!!");
// 娌℃墦鐏紝涓嶅悎鏍�
AddExamFault(13, rtkTime);
- } else if (TimeGetDiff(&beginTurnTime, &lamp.time) >= D_SEC(3)) {
+ } else if (TimeGetDiff(&beginTurnTime, &lamp.time) >= TURN_SIGNAL_LAMP_ADVANCE) {
DEBUG("杞悜鐏椂闂翠笉瓒�");
// 涓嶈冻3绉掞紝涓嶅悎鏍�
AddExamFault(14, rtkTime);
@@ -1790,15 +1728,6 @@
}
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;
}
static double AnalysisRoad(road_exam_map &RoadMap, int roadIndex, lane_t lane, const car_model *car)
--
Gitblit v1.8.0