From 2d6a9d02c77d7e08d4f18ee87d6e3d337b949f47 Mon Sep 17 00:00:00 2001
From: yy1717 <fctom1215@outlook.com>
Date: 星期四, 20 八月 2020 19:35:17 +0800
Subject: [PATCH] 坐标
---
lib/src/main/cpp/test_items2/road_exam.cpp | 308 +++++++++++++++++----------------------------------
1 files changed, 104 insertions(+), 204 deletions(-)
diff --git a/lib/src/main/cpp/test_items2/road_exam.cpp b/lib/src/main/cpp/test_items2/road_exam.cpp
index 0f0c587..3f2955a 100644
--- a/lib/src/main/cpp/test_items2/road_exam.cpp
+++ b/lib/src/main/cpp/test_items2/road_exam.cpp
@@ -16,6 +16,7 @@
#include "stop_car.h"
#include "operate_gear.h"
#include "../test_common/odo_graph.h"
+#include "car_start.h"
#include <cmath>
#include <vector>
@@ -62,13 +63,6 @@
PointF point;
} projection_t;
-typedef struct
-{
- int name;
- int value;
- struct RtkTime time;
-} car_sensor_value_t;
-
typedef struct {
int gain;
struct RtkTime time;
@@ -83,6 +77,7 @@
static const int INVALID_ROAD = -1;
+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;
@@ -107,14 +102,13 @@
static int prevMoveDirect;
static uint32_t stopTimepoint = 0;
static bool reportStopCarOnRedArea;
-static PointF stopPoint, startPoint;
+static PointF stopPoint;
static bool prevGearError = false;
static bool prevGearNSlide = false;
static bool slideLongDistance;
static bool slideNormalDistance;
static bool occurSlide;
-static bool startCarLeftTurnSignal, checkStartCarSignal;
static struct RtkTime crashGreenRunTime, crashGreenStartTime;
@@ -126,16 +120,13 @@
static int gearErrorTime;
static int gearNSlideTime;
-static int startCar, stopCar;
+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 bool checkDoor = false;
-static bool handBreakActive = false;
-static bool reportRPMOver = false;
static lane_t Lane;
static change_lane_t ChangeLane;
@@ -146,14 +137,32 @@
static map<int, int> CrossingHint;
static map<int, bool> ErrorLaneReport;
+#define ROAD_EXAM_READY_NEXT 0
+#define ROAD_EXAM_FREE_RUN 1
+#define ROAD_EXAM_ITEM_CAR_START 2
+#define ROAD_EXAM_ITEM_STRAIGHT 3
+#define ROAD_EXAM_ITEM_OP_GEAR 4
+#define ROAD_EXAM_ITEM_CHANGE_LANE 5
+#define ROAD_EXAM_ITEM_OVER_TAKE 6
+#define ROAD_EXAM_ITEM_CAR_STOP 7
+
+#define ROAD_EXAM_ITEM_NOT_EXEC 0
+#define ROAD_EXAM_ITEM_EXECING 1
+#define ROAD_EXAM_ITEM_EXECED 2
+
+
+typedef struct {
+ int name;
+ int status;
+} RoadExamItem_t;
+static map<int, int> RoadExamItem;
+static int RoadExamStatus;
+
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;
-static const double START_CAR_CHECK_DOOR_DISTANCE = 1.0;
static const uint32_t GEAR_N_SLIDE_TIMEOUT = D_SEC(5);
static const uint32_t GEAR_ERROR_TIMEOUT = D_SEC(15);
static const uint32_t STOP_CAR_TIME = D_SEC(2);
@@ -170,8 +179,7 @@
static const double MAX_SPEED = 60.0 * 1000.0 / 3600.0;
static const int SPEED_GEAR_TABLE[][2] = {{0, 20}, {5, 30}, {15, 40}, {25, 10000}, {35, 10000}};
-static int TestRoadStartCar(const car_model *car, double speed, int moveDirect, const struct RtkTime *rtkTime);
-
+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 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);
@@ -212,16 +220,9 @@
currExamMapIndex = -1;
- startCar = START_CAR_NOT_DO;
stopCar = STOP_CAR_NOT_DO;
currRoadItem = NULL;
-
- checkDoor = false;
- handBreakActive = false;
- reportRPMOver = false;
-
- checkStartCarSignal = startCarLeftTurnSignal = false;
Lane.road = Lane.sep = Lane.no = -1;
Lane.guide = 0;
@@ -234,14 +235,23 @@
ResetOdo();
ResetTarget(RoadMap);
// 鍒濆鍖栬�冮」
+ RoadExamItem.clear();
+ RoadExamItem[ROAD_EXAM_ITEM_CAR_START] = ROAD_EXAM_ITEM_NOT_EXEC;
+ RoadExamItem[ROAD_EXAM_ITEM_STRAIGHT] = ROAD_EXAM_ITEM_NOT_EXEC;
+ RoadExamItem[ROAD_EXAM_ITEM_OP_GEAR] = ROAD_EXAM_ITEM_NOT_EXEC;
+ RoadExamItem[ROAD_EXAM_ITEM_CHANGE_LANE] = ROAD_EXAM_ITEM_NOT_EXEC;
+ RoadExamItem[ROAD_EXAM_ITEM_OVER_TAKE] = ROAD_EXAM_ITEM_NOT_EXEC;
+ RoadExamItem[ROAD_EXAM_ITEM_CAR_STOP] = ROAD_EXAM_ITEM_NOT_EXEC;
+
+ RoadExamStatus = ROAD_EXAM_READY_NEXT;
}
void TerminateRoadExam(void)
{
- TerminateDummyLightExam();
- TerminateStopCarExam();
- TerminateOperateGearExam();
- TerminateDriveStraightExam();
+// TerminateDummyLightExam();
+// TerminateStopCarExam();
+// TerminateOperateGearExam();
+// TerminateDriveStraightExam();
}
/*********************************************************************
@@ -821,7 +831,7 @@
}
}
-static car_sensor_value_t ReadCarSensorValue(int name)
+car_sensor_value_t ReadCarSensorValue(int name)
{
car_sensor_value_t nv = {.name = -1};
@@ -843,78 +853,6 @@
WriteCarSensorValue(SECOND_BREAK, ReadCarStatus(SECOND_BREAK), rtkTime);
WriteCarSensorValue(GEAR, ReadCarStatus(GEAR), rtkTime);
WriteCarSensorValue(BREAK, ReadCarStatus(BREAK), rtkTime);
-}
-
-static int TestRoadStartCar(const car_model *car, double speed, int moveDirect, const struct RtkTime *rtkTime)
-{
- double moveDistance;
-
- if (startCar == START_CAR_NOT_DO) {
- startPoint = car->basePoint;
- reportRPMOver = false;
- startCar = START_CAR_DOING;
-
- PlayTTS("璇疯捣姝�");
- } else if (startCar == START_CAR_DOING) {
- moveDistance = DistanceOf(startPoint, car->basePoint);
- DEBUG("璧锋琛岄┒璺濈 %f", moveDistance);
-
- if (!startCarLeftTurnSignal && ReadCarStatus(TURN_SIGNAL_LAMP) == LEFT_TURN_LIGHT) {
- startCarLeftTurnSignal = true;
- Rtk2DriveTimer(startCarLeftTurnSignalTime, rtkTime);
- }
-
- if (!checkStartCarSignal && moveDirect == 1) {
- checkStartCarSignal = true;
- if (!startCarLeftTurnSignal) {
- 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) {
- AddExamFault(14, rtkTime);
- }
- }
-
- if (moveDistance > START_CAR_MOVE_DISTANCE) {
- if (ReadCarStatus(HAND_BREAK) == BREAK_ACTIVE) {
- DEBUG("Handbreak active move over 10m");
- // 鎵嬪埞鎷夎捣鐘舵�佷笅锛岃椹朵簡10绫充互涓婏紝涓嶅悎鏍�
- AddExamFault(25, rtkTime);
- } else if (handBreakActive) {
- // 鎵嬪埞鎷夎捣鐘舵�佷笅锛岃椹朵簡1绫充互涓婏紝鎵�10鍒�
- DEBUG("Handbreak active move over 1M");
- AddExamFault(26, rtkTime);
- }
-
- PlayTTS("瀹屾垚璧锋");
- DEBUG("############# 瀹屾垚璧锋 ############");
- startCar = START_CAR_DONE;
- } else if (moveDistance >= START_CAR_CHECK_DOOR_DISTANCE) {
- if (!checkDoor) {
- checkDoor = true;
-
- if (ReadCarStatus(DOOR) == DOOR_OPEN) {
- // 杞﹂棬鏈畬鍏ㄥ叧闂紝涓嶅悎鏍�
- DEBUG("杞﹂棬鏈叧闂�");
- AddExamFault(23, rtkTime);
- }
-
- if (ReadCarStatus(HAND_BREAK) == BREAK_ACTIVE) {
- handBreakActive = true;
- }
- }
- }
-
- if (ReadCarStatus(ENGINE_RPM) > MAX_ENGINE_RPM && !reportRPMOver) {
- // 杞�熻秴鏍囷紝涓嶅悎鏍�
- DEBUG("杞�熻秴鏍�");
- AddExamFault(29, rtkTime);
- reportRPMOver = true;
- }
- } else {
-
- }
-
- return startCar;
}
static void DetectLine(int roadIndex, road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList, int moveDirect, const struct RtkTime *rtkTime)
@@ -1209,7 +1147,7 @@
if (GetCrossingStatus(roadIndex, stopIndex) == CROSSING_NOT_HINT) {
if (!RoadMap.roads[roadIndex].stopLine[stopIndex].tts.empty()) {
DEBUG("璺彛鎻愮ず %s", RoadMap.roads[roadIndex].stopLine[stopIndex].tts.c_str());
- PlayTTS(RoadMap.roads[roadIndex].stopLine[stopIndex].tts.c_str());
+ PlayTTS(RoadMap.roads[roadIndex].stopLine[stopIndex].tts.c_str(), NULL);
}
ChangeCrossingStatus(roadIndex, stopIndex, CROSSING_HAS_HINT);
}
@@ -1273,31 +1211,7 @@
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);
+ double BigStraightRoadFree = 0, RoadCrossingFree = 0;
UpdateCarSensor(rtkTime);
@@ -1504,7 +1418,7 @@
}
if (currExamMapIndex >= 0 && Lane.guide == 0) {
- double BigStraightRoad = AnalysisRoad(RoadMap, currExamMapIndex, Lane, car);
+ BigStraightRoadFree = AnalysisRoad(RoadMap, currExamMapIndex, Lane, car);
road_end_point_t ep = NearbyRoadEndPoint(currExamMapIndex, RoadMap, car);
@@ -1517,88 +1431,17 @@
freeSepDis = ep.distance;
}
- DEBUG("鐩撮亾鍓╀綑璺濈 %f, 杞﹂亾鍓╀綑璺濈 %f", BigStraightRoad, freeSepDis);
+ RoadCrossingFree = freeSepDis;
- if (startCar == START_CAR_DONE) {
- if (itemExec[0] == 1 || itemExec[1] == 1 || itemExec[2] == 1 || itemExec[3] == 1) {
-
- DEBUG("椤圭洰鎵ц璺濈<%d %d %d %d> %f", itemExec[0], itemExec[1], itemExec[2], itemExec[3], ReadOdo() - odo);
-
- if (ReadOdo() - odo > 120) {
- odo = ReadOdo();
- if (itemExec[0] == 1) {
- itemExec[0] = 2;
- }
- if (itemExec[1] == 1) {
- itemExec[1] = 2;
- }
- if (itemExec[2] == 1) {
- itemExec[2] = 2;
- }
- if (itemExec[3] == 1) {
- itemExec[3] = 2;
- }
- }
- goto BIG_DOG;
- }
-
- if (itemExec[0] == 2 || itemExec[1] == 2 || itemExec[2] == 2 || itemExec[3] == 2) {
- DEBUG("椤圭洰浼戞伅璺濈<%d %d %d %d> %f", itemExec[0], itemExec[1], itemExec[2], itemExec[3], ReadOdo() - odo);
-
- if (ReadOdo() - odo > 100) {
- if (itemExec[0] == 2) {
- itemExec[0] = 3;
- }
- if (itemExec[1] == 2) {
- itemExec[1] = 3;
- }
- if (itemExec[2] == 2) {
- itemExec[2] = 3;
- }
- if (itemExec[3] == 2) {
- itemExec[3] = 3;
- }
- }
- goto BIG_DOG;
- }
-
-
- if (BigStraightRoad > 170 && ep.distance > 170) {
- if (itemExec[0] == 0) {
- PlayTTS("浜岀嫍鐩寸嚎琛岄┒");
- itemExec[0] = 1;
- odo = ReadOdo();
- }
- } else if (BigStraightRoad > 150 && ep.distance > 150) {
- if (itemExec[3] == 0) {
- PlayTTS("浜岀嫍鍔犲噺妗�");
- itemExec[3] = 1;
- odo = ReadOdo();
- }
- } else if (freeSepDis > 150) {
- if (itemExec[2] == 0) {
- PlayTTS("浜岀嫍鍙橀亾");
- itemExec[2] = 1;
- odo = ReadOdo();
- } else if (itemExec[1] == 0) {
- PlayTTS("浜岀嫍瓒呰溅");
- itemExec[1] = 1;
- odo = ReadOdo();
- }
- }
-
- BIG_DOG:;
- }
+ DEBUG("鐩撮亾鍓╀綑璺濈 %f, 杞﹂亾鍓╀綑璺濈 %f", BigStraightRoadFree, RoadCrossingFree);
}
// 妫�娴嬪帇绾跨姸鎬�
- TestRoadStartCar(car, speed, moveDirect, rtkTime);
// 棰濆鐨勮浆鍚戞娴�
DetectTurn(car, moveDirect, rtkTime);
- if (startCar != START_CAR_DONE)
- return;
+ ItemExam(RoadMap, car, CarModelList, speed, moveDirect, rtkTime, BigStraightRoadFree, RoadCrossingFree);
if (ReadOdo() > EXAM_RANGE && currRoadItem == NULL && AllCmp(RoadMap) && stopCar == STOP_CAR_NOT_DO) {
// 鍦ㄥ悎閫傛潯浠朵笅鍋滆溅缁撴潫鑰冭瘯
@@ -1661,6 +1504,63 @@
}
}
}*/
+}
+
+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 double freeRunDistance;
+ static double totalRunDistance;
+
+ if (RoadExamStatus == ROAD_EXAM_READY_NEXT) {
+ if (RoadExamItem[ROAD_EXAM_ITEM_CAR_START] == ROAD_EXAM_ITEM_NOT_EXEC) {
+ CarStartInit();
+ 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) {
+
+ }
+ }
+ if (RoadExamItem[ROAD_EXAM_ITEM_OP_GEAR] == ROAD_EXAM_ITEM_NOT_EXEC) {
+ if (straight > 150 && road_end > 150) {
+ StartOperateGearExam();
+ RoadExamStatus = ROAD_EXAM_ITEM_OP_GEAR;
+ }
+ }
+ if (RoadExamItem[ROAD_EXAM_ITEM_CHANGE_LANE] == ROAD_EXAM_ITEM_NOT_EXEC) {
+
+ }
+ if (RoadExamItem[ROAD_EXAM_ITEM_OVER_TAKE] == ROAD_EXAM_ITEM_NOT_EXEC) {
+
+ }
+
+ }
+ } else if (RoadExamStatus == ROAD_EXAM_FREE_RUN) {
+ if (ReadOdo() - freeRunDistance > 300) {
+ RoadExamStatus = ROAD_EXAM_READY_NEXT;
+ }
+ } else {
+ bool testing = false;
+
+ switch (RoadExamStatus) {
+ case ROAD_EXAM_ITEM_CAR_START:
+ testing = TestCarStart(car, speed, moveDirect, rtkTime);
+ break;
+ case ROAD_EXAM_ITEM_OP_GEAR:
+ testing = TestOperateGear(rtkTime);
+ break;
+ default:break;
+ }
+
+ if (!testing) {
+ RoadExamItem[RoadExamStatus] = ROAD_EXAM_ITEM_EXECED;
+
+ RoadExamStatus = ROAD_EXAM_FREE_RUN;
+ freeRunDistance = ReadOdo();
+ }
+ }
+
}
void Rtk2DriveTimer(struct drive_timer &tm, const struct RtkTime *rtkTime)
@@ -1755,7 +1655,7 @@
turnTimeCnt += TimeGetDiff(rtkTime, &prevDetectTurnTime);
int wise = isTurn((int) car->yaw, startTurnYaw, TURN_THRESHOLD);
DEBUG("杞姩瑙掑害 %d", wise);
- if (ABS(wise) > 60) {
+ if (ABS(wise) > CROSSING_TURN_THRESHOLD) {
// 纭杞集琛屼负锛屾娴嬪紑濮嬪垰杞集鏃惰浆鍚戠伅鎯呭喌
turnCnt = -1;
--
Gitblit v1.8.0