From 682b17ff66dff23e03c6a57de276ea0c3e670c0e Mon Sep 17 00:00:00 2001 From: yy1717 <fctom1215@outlook.com> Date: 星期一, 23 三月 2020 18:44:47 +0800 Subject: [PATCH] 起步和直线 --- lib/src/main/cpp/test_items2/road_exam.cpp | 127 ++++++++++++++++++++++++++++++++++++------ 1 files changed, 109 insertions(+), 18 deletions(-) diff --git a/lib/src/main/cpp/test_items2/road_exam.cpp b/lib/src/main/cpp/test_items2/road_exam.cpp index ae82561..b158dd5 100644 --- a/lib/src/main/cpp/test_items2/road_exam.cpp +++ b/lib/src/main/cpp/test_items2/road_exam.cpp @@ -12,6 +12,7 @@ #include "../native-lib.h" #include "through_something.h" #include "../master/comm_if.h" +#include "drive_straight.h" #include <vector> #include <list> @@ -23,6 +24,12 @@ using namespace std; #define TURN_CHECK_CNT 4 + +enum { + START_CAR_NOT_DO, + START_CAR_DOING, + START_CAR_DONE +}; static const int TURN_THRESHOLD = 5; static const int TURN_CHECK_INTERVAL = 500; @@ -40,7 +47,7 @@ static int prevMoveDirect; static uint32_t stopTimepoint = 0; static bool reportStopCarOnRedArea; -static PointF stopPoint; +static PointF stopPoint, startPoint; static bool prevGearError = false; static bool prevGearNSlide = false; @@ -61,8 +68,12 @@ static int gearErrorTime; static int gearNSlideTime; +static int startCar; static int currExamMapIndex; +static const int MAX_ENGINE_RPM = 2500; +static const double START_CAR_MOVE_DISTANCE = 0.5;//10.0; +static const double START_CAR_CHECK_DOOR_DISTANCE = 0.1;//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); @@ -73,8 +84,9 @@ static const double MAX_SPEED = 40.0 * 1000.0 / 3600.0; static const int SPEED_GEAR_TABLE[][2] = {{0, 20}, {5, 30}, {15, 40}, {25, 10000}, {35, 10000}}; +static void TestRoadStartCar(const car_model *car, double speed, int moveDirect, const struct RtkTime *rtkTime); static void Rtk2DriveTimer(struct drive_timer &tm, const struct RtkTime *rtkTime); -static char isTurn(int currYaw, int prevYaw); +static char isTurn(int currYaw, int prevYaw, int &ang); static char CheckCarTurn(LIST_CAR_MODEL &CarModelList); static bool CrashRedLine(LIST_ROAD_MAP &RoadMapList, const car_model *car); static bool CrashRedArea(LIST_ROAD_MAP &RoadMapList, const car_model *car); @@ -107,10 +119,69 @@ gearNSlideTime = 0; currExamMapIndex = -1; + + startCar = START_CAR_NOT_DO; +} + +static void TestRoadStartCar(const car_model *car, double speed, int moveDirect, const struct RtkTime *rtkTime) +{ + double moveDistance; + static bool checkDoor = false; + static bool handBreakActive = false; + static bool reportRPMOver = false; + + 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); + 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); + } + 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 { + + } } void TestRoadGeneral(LIST_ROAD_MAP &RoadMapList, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime) { + // 璧锋妫�娴� + TestRoadStartCar(car, speed, moveDirect, rtkTime); + // 瓒呴�熸娴� if (moveDirect != 0 && speed > MAX_SPEED) { if (!occurOverSpeed) { @@ -266,7 +337,7 @@ // 妫�鏌ユ槸鍚︽寔缁浆鍚� char turnDirect = CheckCarTurn(CarModelList); if (turnDirect == 'L') { - PlayTTS("宸�1", 5692); +// PlayTTS("宸�1"); if (currTurnSignalStatus != LEFT_TURN_LIGHT) { if (!reportTurnSignalError) { DEBUG("娌℃墦宸﹁浆鐏�"); @@ -284,7 +355,7 @@ } } } else if (turnDirect == 'R') { - PlayTTS("鍙�1", 5692); +// PlayTTS("鍙�1"); if (currTurnSignalStatus != RIGHT_TURN_LIGHT) { if (!reportTurnSignalError) { DEBUG("娌℃墦鍙宠浆鐏�"); @@ -386,7 +457,7 @@ // 妫�鏌ュ彉閬撳墠锛屾槸鍚︽彁鍓嶈浆鍚戠伅 if (inter == 1) { - PlayTTS("宸�2", 5698); +// PlayTTS("宸�2"); // 鍚戝乏渚у彉閬� DEBUG("鍚戝乏渚у彉閬�"); if (turnSignalStatusWhenCrashGreenLine != LEFT_TURN_LIGHT) { @@ -395,7 +466,7 @@ AddExamFault(13, rtkTime); } } else { - PlayTTS("鍙�2", 5698); +// PlayTTS("鍙�2"); // 鍚戝彸渚у彉閬� DEBUG("鍚戝彸渚у彉閬�"); if (turnSignalStatusWhenCrashGreenLine != RIGHT_TURN_LIGHT) { @@ -410,20 +481,32 @@ checkCrashGreenTimeout = 0; } - // 瑙﹀彂绾挎娴� - if (currExamMapIndex == -1) { + // 瀹屾垚璧锋鍚庯紝瑙﹀彂绾挎娴� + if (currExamMapIndex == -1 && startCar == START_CAR_DONE) { currExamMapIndex = CrashTriggerLine(RoadMapList, car, CarModelList); if (currExamMapIndex != -1) { DEBUG("纰版挒瑙﹀彂绾�"); MA_EnterMap(RoadMapList[currExamMapIndex].id, RoadMapList[currExamMapIndex].type, 1); - StartThroughExam(currExamMapIndex, RoadMapList); + + if (RoadMapList[currExamMapIndex].type >= THROUGH_INTERSECTION_MAP && + RoadMapList[currExamMapIndex].type <= TURN_AROUND_MAP) { + StartThroughExam(currExamMapIndex, RoadMapList); + } else if (RoadMapList[currExamMapIndex].type == DRIVE_STRAIGHT_MAP) { + StartDriveStraightExam(currExamMapIndex, RoadMapList); + } } - } else { + } else if (startCar == START_CAR_DONE) { int prevIdx = currExamMapIndex; - currExamMapIndex = ExecuteThroughExam(currExamMapIndex, RoadMapList, car, - CarModelList, speed, moveDirect, rtkTime); + if (currExamMapIndex >= THROUGH_INTERSECTION_MAP && currExamMapIndex <= TURN_AROUND_MAP) { + currExamMapIndex = ExecuteThroughExam(currExamMapIndex, RoadMapList, car, + CarModelList, speed, moveDirect, rtkTime); + } + else if (currExamMapIndex == DRIVE_STRAIGHT_MAP) { + currExamMapIndex = ExecuteDriveStraightExam(currExamMapIndex, RoadMapList, car, + CarModelList, speed, moveDirect, rtkTime); + } if (currExamMapIndex == -1) { MA_EnterMap(RoadMapList[prevIdx].id, RoadMapList[prevIdx].type, 1); @@ -521,10 +604,9 @@ tm.msec = rtkTime->mss; } -static char isTurn(int currYaw, int prevYaw) +static char isTurn(int currYaw, int prevYaw, int &ang) { // DEBUG("currYaw %d prevYaw %d", currYaw, prevYaw); - int deltaAng = 0; if (ABS(currYaw - prevYaw) > 180) { @@ -532,6 +614,8 @@ } else { deltaAng = ABS(currYaw - prevYaw); } + + ang = deltaAng; // DEBUG("瑙掑害宸�� %d", deltaAng); @@ -545,12 +629,12 @@ } } - return 0; + return 'N'; } static char CheckCarTurn(LIST_CAR_MODEL &CarModelList) { - // 鏈�杩�3绉掑唴锛屾瘡绉掔殑瑙掑害宸ぇ浜�10搴︼紝涓旀柟鍚戠浉鍚岋紝杩炵画3绉掞紝璁や负杞悜 + // 鏈�杩�2绉掑唴锛屾瘡0.5绉掔殑瑙掑害宸ぇ浜�5搴︼紝涓旀柟鍚戠浉鍚岋紝杩炵画4娆★紱鎴栫獊鐜拌秴30搴︾殑杞悜锛涜涓鸿浆鍚戙�� if (CarModelList.size() < 1) return false; @@ -571,10 +655,15 @@ uint32_t tdiff = TimeGetDiff(c1->tm.hh, c1->tm.mm, c1->tm.ss, c1->tm.mss * 10, c2->tm.hh, c2->tm.mm, c2->tm.ss, c2->tm.mss*10); if (tdiff >= TURN_CHECK_INTERVAL) { - turn[checkCnt] = isTurn((int)c1->yaw, (int)c2->yaw); + int ang = 0; + turn[checkCnt] = isTurn((int)c1->yaw, (int)c2->yaw, ang); // DEBUG("%c 瑙掑害姣旇緝 %02d:%02d:%02d.%03d %02d:%02d:%02d.%03d", turn[checkCnt], c1->tm.hh, c1->tm.mm, c1->tm.ss, c1->tm.mss * 10, c2->tm.hh, c2->tm.mm, c2->tm.ss, c2->tm.mss*10); - if (turn[checkCnt] == 0) { + + if (turn[checkCnt] == 'N') { break; + } else if (ang >= 30) { + DEBUG("宸﹀彸杞‘璁� %c", turn[checkCnt]); + return turn[checkCnt]; } c1 = c2; @@ -790,3 +879,5 @@ } return -1; } + + -- Gitblit v1.8.0