From 24428a1b69afcf113883e9cede1159443201a1b8 Mon Sep 17 00:00:00 2001 From: yy1717 <fctom1215@outlook.com> Date: 星期四, 02 四月 2020 18:57:07 +0800 Subject: [PATCH] 进出科目二区域调整 --- lib/src/main/cpp/test_items/driving_curve.cpp | 30 ++++ lib/src/main/cpp/driver_test.cpp | 157 ++++++++++++++++--------- lib/src/main/cpp/test_items/park_edge.h | 2 lib/src/main/cpp/test_items/park_bottom.h | 2 lib/src/main/cpp/test_items/stop_and_start.cpp | 40 +++++- lib/src/main/cpp/driver_test.h | 8 + lib/src/main/cpp/test_items/driving_curve.h | 2 lib/src/main/cpp/master/comm_if.h | 2 lib/src/main/cpp/test_items/park_bottom.cpp | 38 +++++- lib/src/main/cpp/test_items/turn_a90.cpp | 12 + lib/src/main/cpp/test_items2/drive_straight.cpp | 9 + lib/src/main/cpp/test_items/park_edge.cpp | 20 ++ lib/src/main/cpp/master/comm_if.cpp | 18 +++ lib/src/main/cpp/test_items/turn_a90.h | 2 lib/src/main/cpp/test_items/stop_and_start.h | 2 15 files changed, 256 insertions(+), 88 deletions(-) diff --git a/lib/src/main/cpp/driver_test.cpp b/lib/src/main/cpp/driver_test.cpp index 0af9a65..9f47645 100644 --- a/lib/src/main/cpp/driver_test.cpp +++ b/lib/src/main/cpp/driver_test.cpp @@ -40,12 +40,6 @@ using namespace std; -#define MAP_TYPE_PARK_BUTTOM 1 -#define MAP_TYPE_STOP_START 2 -#define MAP_TYPE_PART_EDGE 3 -#define MAP_TYPE_CURVE 4 -#define MAP_TYPE_TURN_90 5 - #define RTK_INVALID 0 #define RTK_SINGLE_POINT 1 #define RTK_DIFF 2 @@ -117,7 +111,8 @@ static void UpdateCarBodyCoord(struct RtkTime *rtkTime, double azimuth, double pitch, double roll, PointF main_ant, car_model *carModel); static bool UpdateCarCoord(double &spd, int &mov, int &idx); -static int EnterMap(const car_model *car, const struct map_list *mapList, int mapNum); +static bool CrashTriggerLine(Line triggerLine, const car_model *car, LIST_CAR_MODEL &CarModelList); +static int EnterMap(const car_model *car, LIST_CAR_MODEL &CarModelList, const struct map_list *mapList, int mapNum); static bool ExitMap(const car_model *car, int mapId, const struct map_list *mapList, int mapNum); static int GetMapId(int index, const struct map_list *mapList, int mapNum); static int GetMapType(int index, const struct map_list *mapList, int mapNum); @@ -650,7 +645,7 @@ RtkBuffer[index].mm, RtkBuffer[index].ss, RtkBuffer[index].dss); brief.qf = RtkBuffer[index].qf; - brief.map_id = GetMapId(CurrExamMapIndex, MapList, MapNum); + brief.map_id = -1;//GetMapId(CurrExamMapIndex, MapList, MapNum); brief.move = move; brief.speed = speed * 3.6; brief.heading = RtkBuffer[index].heading; @@ -712,18 +707,18 @@ } if (CurrExamMapIndex < 0) { if (CurrEnterMapIndex < 0) { - CurrEnterMapIndex = EnterMap(CarModel, MapList, MapNum); + CurrEnterMapIndex = EnterMap(CarModel, CarModelList, MapList, MapNum); if (CurrEnterMapIndex >= 0) { - DEBUG("鍙戦�佽繘鍏ュ満鍦版姤鍛� %d", GetMapId(CurrEnterMapIndex, MapList, MapNum)); - MA_EnterMap(GetMapId(CurrEnterMapIndex, MapList, MapNum), GetMapType(CurrEnterMapIndex, MapList, MapNum), 1); +// DEBUG("鍙戦�佽繘鍏ュ満鍦版姤鍛� %d", GetMapId(CurrEnterMapIndex, MapList, MapNum)); +// MA_EnterMap(GetMapId(CurrEnterMapIndex, MapList, MapNum), GetMapType(CurrEnterMapIndex, MapList, MapNum), 1); CurrExamMapIndex = CurrEnterMapIndex; CurrExamStatus = EXAM_AREA_START; } } else { if (ExitMap(CarModel, CurrEnterMapIndex, MapList, MapNum)) { - DEBUG("鍙戦�佺寮�鍦哄湴鎶ュ憡 %d", GetMapId(CurrEnterMapIndex, MapList, MapNum)); - MA_EnterMap(GetMapId(CurrEnterMapIndex, MapList, MapNum), GetMapType(CurrEnterMapIndex, MapList, MapNum), 0); +// DEBUG("鍙戦�佺寮�鍦哄湴鎶ュ憡 %d", GetMapId(CurrEnterMapIndex, MapList, MapNum)); +// MA_EnterMap(GetMapId(CurrEnterMapIndex, MapList, MapNum), GetMapType(CurrEnterMapIndex, MapList, MapNum), 0); CurrEnterMapIndex = -1; } } @@ -894,33 +889,33 @@ DEBUG("杩涘叆鍊掕溅鍏ュ簱鍦哄湴"); MA_SendDebugInfo("杩涘叆鍊掕溅鍏ュ簱鍦哄湴 %d", GetMapId(CurrExamMapIndex, MapList, MapNum)); - StartParkBottom(move, rtkTime); + StartParkBottom(CurrExamMapIndex, move, rtkTime); CurrExamStatus = EXAM_AREA_RUN; break; case MAP_TYPE_STOP_START: DEBUG("杩涘叆涓婂潯璧锋鍦哄湴"); MA_SendDebugInfo("杩涘叆涓婂潯璧锋鍦哄湴 %d", GetMapId(CurrExamMapIndex, MapList, MapNum)); - StartSAS(move, rtkTime); + StartSAS(CurrExamMapIndex, move, rtkTime); CurrExamStatus = EXAM_AREA_RUN; break; case MAP_TYPE_PART_EDGE: DEBUG("杩涘叆渚ф柟浣嶅仠杞﹀満鍦�"); MA_SendDebugInfo("杩涘叆渚ф柟浣嶅仠杞﹀満鍦� %d", GetMapId(CurrExamMapIndex, MapList, MapNum)); - StartParkEdge(move, rtkTime); + StartParkEdge(CurrExamMapIndex, move, rtkTime); CurrExamStatus = EXAM_AREA_RUN; break; case MAP_TYPE_CURVE: DEBUG("杩涘叆鏇茬嚎琛岄┒鍦哄湴"); MA_SendDebugInfo("杩涘叆鏇茬嚎琛岄┒鍦哄湴 %d", GetMapId(CurrExamMapIndex, MapList, MapNum)); - StartDrivingCurve(move, rtkTime); + StartDrivingCurve(CurrExamMapIndex, move, rtkTime); CurrExamStatus = EXAM_AREA_RUN; break; case MAP_TYPE_TURN_90: DEBUG("杩涘叆鐩磋杞集鍦哄湴"); MA_SendDebugInfo("杩涘叆鐩磋杞集鍦哄湴 %d", GetMapId(CurrExamMapIndex, MapList, MapNum)); - StartTurnA90(move, azimuth, rtkTime); + StartTurnA90(CurrExamMapIndex, move, azimuth, rtkTime); CurrExamStatus = EXAM_AREA_RUN; break; default:break; @@ -1145,15 +1140,79 @@ return true; } -static int EnterMap(const car_model *car, const struct map_list *mapList, int mapNum) +static bool CrashTriggerLine(Line triggerLine, const car_model *car, LIST_CAR_MODEL &CarModelList) { - // 杞︾殑鏈�鍓嶇偣鏄惁杩涘叆鍦板浘 + bool trigger = false; + + if (CarModelList.size() < 5) + return trigger; + + Polygon trace, trace2; + int pn = 0; + + trace2.num = trace.num = 5; + trace.point = (PointF *) malloc(sizeof(PointF) * trace.num); + trace2.point = (PointF *) malloc(sizeof(PointF) * trace2.num); + + list<car_model *>::iterator iter = CarModelList.begin(); + + car_model *c1 = *iter; + + trace.point[pn] = c1->carXY[c1->left_front_tire[TIRE_OUTSIDE]]; + trace2.point[pn++] = c1->carXY[c1->left_rear_tire[TIRE_OUTSIDE]]; + + ++iter; + + while (iter != CarModelList.end() && pn < trace.num) { + car_model *c2 = *iter; + + 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 >= D_SEC(1)) { + trace.point[pn] = c2->carXY[c2->left_front_tire[TIRE_OUTSIDE]]; + trace2.point[pn++] = c2->carXY[c2->left_rear_tire[TIRE_OUTSIDE]]; + c1 = c2; + } + ++iter; + } + + PointF p1, p2; + + p1.X = triggerLine.X1; + p1.Y = triggerLine.Y1; + p2.X = triggerLine.X2; + p2.Y = triggerLine.Y2; + + int pp = 0; + for (int p = 1; p < pn; ++p) { + Line trace_line, trace2_line; + + MakeLine(&trace_line, &trace.point[pp], &trace.point[p]); + MakeLine(&trace2_line, &trace2.point[pp], &trace2.point[p]); + + if ((IntersectionOf(trace_line, triggerLine) == GM_Intersection || IntersectionOf(trace2_line, triggerLine) == GM_Intersection) && + IntersectionOfLine(p1, p2, car->carXY[car->left_front_tire[TIRE_OUTSIDE]]) == -1 && + DistanceOf(car->carXY[car->left_front_tire[TIRE_OUTSIDE]], triggerLine) > 0.1) { + // 纰板埌瑙﹀彂绾� + DEBUG("纰版挒瑙﹀彂绾� 寮曞彂鍦板浘"); + trigger = true; + goto SEARCH_TRIGGER_LINE_END; + } + } + +SEARCH_TRIGGER_LINE_END: + free(trace.point); + + return trigger; +} + +static int EnterMap(const car_model *car, LIST_CAR_MODEL &CarModelList, const struct map_list *mapList, int mapNum) +{ for (int i = 0; i < mapNum && car != NULL; ++i) { + // 杞﹀墠杞垨鍚庤疆杞ㄨ抗瓒婅繃瑙﹀彂绾� if (mapList[i].type == MAP_TYPE_STOP_START) { // 鏋勯�犺櫄鎷熺殑宸︿笂瑙掔偣 double x9, y9, xo, yo; - - bool enter = false; xo = (mapList[i].map.point[0].X + mapList[i].map.point[7].X) / 2; yo = (mapList[i].map.point[0].Y + mapList[i].map.point[7].Y) / 2; @@ -1161,31 +1220,18 @@ x9 = 2*xo - mapList[i].map.point[8].X; y9 = 2*yo - mapList[i].map.point[8].Y; - Polygon map; + Line triggerLine; - map.num = 4; - map.point = (PointF *) malloc(map.num * sizeof(PointF)); + triggerLine.X1 = mapList[i].map.point[0].X; + triggerLine.Y1 = mapList[i].map.point[0].Y; + triggerLine.X2 = x9; + triggerLine.Y2 = y9; - map.point[0] = mapList[i].map.point[0]; - map.point[1] = mapList[i].map.point[8]; - map.point[2] = mapList[i].map.point[7]; - map.point[3].X = x9; - map.point[3].Y = y9; - - if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &map) == GM_Containment) { - Line enterLine1; - - MakeLine(&enterLine1, &(map.point[0]), &(map.point[3])); - - if (DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], enterLine1) > 0.1) - enter = true; - } - - free(map.point); - - if (enter) return i; + if (CrashTriggerLine(triggerLine, car, CarModelList)) + return i; } if (mapList[i].type == MAP_TYPE_PARK_BUTTOM) { + // 杞﹀ご椤剁偣鍦ㄥ満鍦板唴 if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &mapList[i].map) == GM_Containment) { Line enterLine1, enterLine2; @@ -1198,6 +1244,7 @@ } } if (mapList[i].type == MAP_TYPE_PART_EDGE) { + // 杞﹀ご椤剁偣鍦ㄥ満鍦板唴 if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &mapList[i].map) == GM_Containment) { Line enterLine; @@ -1208,26 +1255,20 @@ } } if (mapList[i].type == MAP_TYPE_TURN_90) { - if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &mapList[i].map) == GM_Containment) { - Line enterLine1; + // 杞﹀墠杞垨鍚庤疆杞ㄨ抗瓒婅繃瑙﹀彂绾� + Line triggerLine; - MakeLine(&enterLine1, &(mapList[i].map.point[0]), &(mapList[i].map.point[1])); + MakeLine(&triggerLine, &(mapList[i].map.point[0]), &(mapList[i].map.point[1])); - if (DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], enterLine1) > 0.1) - return i; - } + if (CrashTriggerLine(triggerLine, car, CarModelList)) + return i; } if (mapList[i].type == MAP_TYPE_CURVE) { - Line startLine; - Line carAxial; + Line triggerLine; - MakeLine(&startLine, &mapList[i].map.point[0], &mapList[i].map2.point[0]); - MakeLine(&carAxial, &car->carXY[car->left_front_tire[TIRE_OUTSIDE]], &car->carXY[car->left_rear_tire[TIRE_OUTSIDE]]); - - if (IntersectionOf(startLine, carAxial) == GM_Intersection) { - if (DistanceOf(car->carXY[car->left_front_tire[TIRE_OUTSIDE]], startLine) > 0.1) - return i; - } + MakeLine(&triggerLine, &mapList[i].map2.point[0], &mapList[i].map.point[0]); + if (CrashTriggerLine(triggerLine, car, CarModelList)) + return i; } } return -1; diff --git a/lib/src/main/cpp/driver_test.h b/lib/src/main/cpp/driver_test.h index 684e996..0cc5aa0 100644 --- a/lib/src/main/cpp/driver_test.h +++ b/lib/src/main/cpp/driver_test.h @@ -21,7 +21,13 @@ #define TIRE_OUTSIDE 0 #define TIRE_INSIDE 1 #define AXIAL_FRONT 0 -#define AXIAL_REAR 1 +#define AXIAL_REAR 1 + +#define MAP_TYPE_PARK_BUTTOM 1 +#define MAP_TYPE_STOP_START 2 +#define MAP_TYPE_PART_EDGE 3 +#define MAP_TYPE_CURVE 4 +#define MAP_TYPE_TURN_90 5 using namespace std; diff --git a/lib/src/main/cpp/master/comm_if.cpp b/lib/src/main/cpp/master/comm_if.cpp index ceaf567..90b4b1a 100644 --- a/lib/src/main/cpp/master/comm_if.cpp +++ b/lib/src/main/cpp/master/comm_if.cpp @@ -60,12 +60,15 @@ #define ID_MS_SYS_SHUTDOWN 0x8010 #define ID_SM_LIGHT_EXAM_REQ 0x0012 #define ID_MS_LIGHT_EXAM_RES 0x8012 +#define ID_SM_DISTANCE 0x0020 + #define MA_OUT_GPS_BRIEF 0x0001 #define MA_OUT_RTK_BRIEF 0x0002 #define MA_OUT_CAR_BRIEF 0x0004 #define MA_OUT_RTCM_IND 0x0008 #define MA_OUT_DBG_INFO 0x0010 + static int OnOff = 0;//0xFFFF; @@ -352,6 +355,21 @@ SendMsgToMainProcIndep(ID_SM_RTK_BRIEF, sb.GetString()); } +void MA_SendDistance(double l, double r) +{ + StringBuffer sb; + Writer<StringBuffer> writer(sb); + + writer.StartObject(); + writer.Key("left"); + writer.Double(l); + writer.Key("right"); + writer.Double(r); + writer.EndObject(); + + SendMsgToMainProcIndep(ID_SM_DISTANCE, sb.GetString()); +} + void MA_SendExamWrong(vector<ExamFault> &ExamFaultList) { StringBuffer sb; diff --git a/lib/src/main/cpp/master/comm_if.h b/lib/src/main/cpp/master/comm_if.h index 8e23f95..80d93a9 100644 --- a/lib/src/main/cpp/master/comm_if.h +++ b/lib/src/main/cpp/master/comm_if.h @@ -97,4 +97,6 @@ void MA_EnterMap(int map_id, int type, int enter); void MA_ExamLight(void); +void MA_SendDistance(double l, double r); + #endif //MYAPPLICATION2_COMM_IF_H diff --git a/lib/src/main/cpp/test_items/driving_curve.cpp b/lib/src/main/cpp/test_items/driving_curve.cpp index 7a33a68..24ad383 100644 --- a/lib/src/main/cpp/test_items/driving_curve.cpp +++ b/lib/src/main/cpp/test_items/driving_curve.cpp @@ -7,6 +7,8 @@ #include "../driver_test.h" #include "../common/apptimer.h" #include "../utils/xconvert.h" +#include "../master/comm_if.h" +#include "../native-lib.h" #include <vector> #include <cstdlib> @@ -18,6 +20,7 @@ const uint32_t STOP_CAR_TIME = D_SEC(2); static bool testing = false; +static int mapIndex = 0; static uint32_t stopTimepoint = 0; static bool reportStopCarTimeout; @@ -32,13 +35,14 @@ static bool UpdateStartLine(struct scan_window_t *zone, const Polygon *map, const Polygon *map2, const Polygon *tireRect); static bool UpdateEndLine(bool mode, struct scan_window_t *zone, const Polygon *map, const Polygon *map2, const Polygon *tireRect); -static bool CrashRedLine(const Polygon *map, const Polygon *map2, const car_model *car, struct scan_window_t *zone); +static bool CrashRedLine(const Polygon *map, const Polygon *map2, const car_model *car, struct scan_window_t *zone, int &who); -void StartDrivingCurve(int moveDirect, const struct RtkTime *rtkTime) +void StartDrivingCurve(int index, int moveDirect, const struct RtkTime *rtkTime) { DEBUG("杩涘叆鏇茬嚎琛岄┒鍦哄湴"); testing = true; + mapIndex = index; prevMoveDirect = moveDirect; if (moveDirect == 0) { @@ -48,12 +52,14 @@ crashRedLine = false; scanWindow.leftStart = scanWindow.leftEnd = scanWindow.rightStart = scanWindow.rightEnd = 0; + + MA_EnterMap(mapIndex, MAP_TYPE_CURVE, 1); } int TestDrivingCurve(const Polygon *map, const Polygon *map2, const car_model *car, const car_model *carPrev, double speed, int moveDirect, const struct RtkTime *rtkTime) { Polygon tireRect; - + int who = 0; MakePolygon(&tireRect, {car->carXY[car->left_front_tire[TIRE_OUTSIDE]], car->carXY[car->right_front_tire[TIRE_OUTSIDE]], car->carXY[car->right_rear_tire[TIRE_OUTSIDE]], @@ -71,12 +77,17 @@ DEBUG("scanWindow leftStart %d leftEnd %d rightStart %d rightEnd %d", scanWindow.leftStart, scanWindow.leftEnd, scanWindow.rightStart, scanWindow.rightEnd); - if (CrashRedLine(map, map2, car, &scanWindow)) { + if (CrashRedLine(map, map2, car, &scanWindow, who)) { if (!crashRedLine) { crashRedLine = true; // 杞﹁疆鍘嬭竟绾匡紝涓嶅悎鏍� AddExamFault(27, rtkTime); DEBUG("杞﹁疆鍘嬭竟绾�"); + if (who == 1) { + PlayTTS("鍘嬪乏鏇茬嚎"); + } else if (who == 2) { + PlayTTS("鍘嬪彸鏇茬嚎"); + } } } else { crashRedLine = false; @@ -105,6 +116,11 @@ TEST_END: CleanPolygon(&tireRect); + + if (!testing) { + MA_EnterMap(mapIndex, MAP_TYPE_CURVE, 0); + } + return testing ? 1 : 0; } @@ -276,7 +292,7 @@ } // 杞﹁疆鏄惁鍘嬭竟绾� -static bool CrashRedLine(const Polygon *map, const Polygon *map2, const car_model *car, struct scan_window_t *zone) +static bool CrashRedLine(const Polygon *map, const Polygon *map2, const car_model *car, struct scan_window_t *zone, int &who) { bool ret = false; @@ -290,9 +306,11 @@ for (int e = zone->leftStart - 1; e >= zone->leftEnd; --e) { MakeLine(&redLine, &map->point[s], &map->point[e]); if (IntersectionOf(redLine, frontTireAxial) != GM_None) { + who = 1; return true; } if (IntersectionOf(redLine, rearTireAxial) != GM_None) { + who = 1; return true; } s = e; @@ -302,9 +320,11 @@ for (int e = zone->rightStart - 1; e >= zone->rightEnd; --e) { MakeLine(&redLine, &map2->point[s], &map2->point[e]); if (IntersectionOf(redLine, frontTireAxial) != GM_None) { + who = 2; return true; } if (IntersectionOf(redLine, rearTireAxial) != GM_None) { + who = 2; return true; } s = e; diff --git a/lib/src/main/cpp/test_items/driving_curve.h b/lib/src/main/cpp/test_items/driving_curve.h index 22c7181..e9f39fc 100644 --- a/lib/src/main/cpp/test_items/driving_curve.h +++ b/lib/src/main/cpp/test_items/driving_curve.h @@ -10,7 +10,7 @@ using namespace std; -void StartDrivingCurve(int moveDirect, const struct RtkTime *rtkTime); +void StartDrivingCurve(int index, int moveDirect, const struct RtkTime *rtkTime); bool ExitDrivingCurveArea(const Polygon *map, const Polygon *map2, const car_model *car); int TestDrivingCurve(const Polygon *map, const Polygon *map2, const car_model *car, const car_model *carPrev, double speed, int moveDirect, const struct RtkTime *rtkTime); diff --git a/lib/src/main/cpp/test_items/park_bottom.cpp b/lib/src/main/cpp/test_items/park_bottom.cpp index b0a598b..3a59252 100644 --- a/lib/src/main/cpp/test_items/park_bottom.cpp +++ b/lib/src/main/cpp/test_items/park_bottom.cpp @@ -9,6 +9,7 @@ #include "../jni_log.h" #include "../driver_test.h" #include "../utils/xconvert.h" +#include "../master/comm_if.h" #include <vector> #include <cstdlib> @@ -27,7 +28,8 @@ const int PARK_TIMEOUT = D_SEC(210); -static bool testing = false; +static bool testing = false, reverseCar = false; +static int mapIndex = 0; const uint32_t STOP_CAR_TIME = D_SEC(2); static uint32_t stopTimepoint; @@ -44,13 +46,15 @@ static char CrossCtrlLine(const Polygon *map, const car_model *car, const car_model *prev_car); static bool EnterParking(const Polygon *map, const car_model *car); -static bool CrashRedLine(const Polygon *map, const car_model *car); +static bool CrashRedLine(const Polygon *map, const car_model *car, int &who); static bool ExitParkArea(const Polygon *map, const car_model *car); -void StartParkBottom(int moveDirect, const struct RtkTime *rtkTime) +void StartParkBottom(int index, int moveDirect, const struct RtkTime *rtkTime) { DEBUG("StartParkBottom"); testing = true; + reverseCar = false; + mapIndex = index; memset(carray, 0, sizeof(carray)); memset(darray, 0, sizeof(darray)); memset(parkStatus, 0, sizeof(parkStatus)); @@ -69,12 +73,13 @@ { char crossCtrlLine = 0; uint32_t tp = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10); + int who = 0; if (ExitParkArea(map, car)) { DEBUG("绂诲紑鍦哄湴"); // 绂诲紑鍦哄湴 testing = false; - if ((parkStatus[0] != 1 || parkStatus[1] != 1) && !reportParkFail) { + if ((parkStatus[0] != 1 || parkStatus[1] != 1) && !reportParkFail && reverseCar) { // 鍊掑簱涓嶅叆锛屼笉鍚堟牸 reportParkFail = true; AddExamFault(8, rtkTime); @@ -83,12 +88,18 @@ goto TEST_END; } - if (CrashRedLine(map, car)) { + + if (CrashRedLine(map, car, who)) { if (!occurCrashRedLine) { occurCrashRedLine = true; // 杞﹁韩鍑虹嚎锛屼笉鍚堟牸 AddExamFault(7, rtkTime); DEBUG("杞﹁疆鍘嬬嚎"); + if (who == 1) { + PlayTTS("鍘嬪乏搴撲綅绾�"); + } else if (who == 2) { + PlayTTS("鍘嬪彸搴撲綅绾�"); + } } } else { occurCrashRedLine = false; @@ -153,6 +164,10 @@ } } else if (moveDirect == -1) { // 鍒囨崲涓哄�掕溅 + if (!reverseCar) { + reverseCar = true; + MA_EnterMap(mapIndex, MAP_TYPE_PARK_BUTTOM, 1); + } if (darray[parkCount] == 0) { if (!crossCtrlLineSw) { // 鍊掕溅鍓嶏紝鍓嶈疆鏈┒杩囨帶鍒剁嚎 @@ -160,7 +175,7 @@ DEBUG("鍊掕溅鍓嶏紝鍓嶈疆鏈┒杩囨帶鍒剁嚎"); } darray[parkCount] = 1; - firstReverseTimepoint = tp; + firstReverseTimepoint = tp; // 棣栨鍊掕溅锛屾墠鎰忓懗鐫�姝ら」鐩紑濮� } } else { // 鍒囨崲涓哄墠杩� @@ -209,6 +224,9 @@ } TEST_END: + if (!testing && reverseCar) { + MA_EnterMap(mapIndex, MAP_TYPE_PARK_BUTTOM, 0); + } return testing ? 1 : 0; } @@ -258,7 +276,7 @@ return succ; } -static bool CrashRedLine(const Polygon *map, const car_model *car) +static bool CrashRedLine(const Polygon *map, const car_model *car, int &who) { bool ret = false; @@ -276,6 +294,12 @@ for (int i = 0; i < sizeof(red_lines) / sizeof(red_lines[0]); ++i) { MakeLine(&red_line, &(map->point[red_lines[i][0]]), &(map->point[red_lines[i][1]])); if (IntersectionOf(red_line, &car_body) != GM_None) { + if (i == 2 || i == 1) { + who = 1; + } else if (i == 4 || i == 5) { + who = 2; + } + ret = true; break; } diff --git a/lib/src/main/cpp/test_items/park_bottom.h b/lib/src/main/cpp/test_items/park_bottom.h index be36967..69cd14e 100644 --- a/lib/src/main/cpp/test_items/park_bottom.h +++ b/lib/src/main/cpp/test_items/park_bottom.h @@ -11,7 +11,7 @@ using namespace std; -void StartParkBottom(int moveStatus, const struct RtkTime *rtkTime); +void StartParkBottom(int index, int moveStatus, const struct RtkTime *rtkTime); int TestParkBottom(const Polygon *map, const car_model *car, const car_model *carPrev, double speed, int moveStatus, const struct RtkTime *rtkTime); #endif //RTKDRIVERTEST_PARK_BOTTOM_H diff --git a/lib/src/main/cpp/test_items/park_edge.cpp b/lib/src/main/cpp/test_items/park_edge.cpp index 207d9e7..921ce34 100644 --- a/lib/src/main/cpp/test_items/park_edge.cpp +++ b/lib/src/main/cpp/test_items/park_edge.cpp @@ -10,6 +10,7 @@ #include "../native-lib.h" #include "../utils/xconvert.h" #include "../test_common/car_sensor.h" +#include "../master/comm_if.h" #include <vector> #include <cstdlib> @@ -21,7 +22,10 @@ const int PARK_TIMEOUT = D_SEC(90); const uint32_t STOP_CAR_TIME = D_SEC(2); +static int mapIndex = 0; static bool reportExamTimeout; +static bool reportParkFail; + static uint32_t stopTimepoint = 0; static bool occurCrashRedLine1, occurCrashRedLine2; static int prevMoveStatus, storeMoveStatusBeforeStop; @@ -36,15 +40,16 @@ static bool ExitParkArea(const Polygon *map, const car_model *car); static bool ExitParkArea2(const Polygon *map, const car_model *car); -void StartParkEdge(int moveStatus, const struct RtkTime *rtkTime) +void StartParkEdge(int index, int moveStatus, const struct RtkTime *rtkTime) { DEBUG("杩涘叆渚ф柟鍋滆溅鍦哄湴"); testing = true; + mapIndex = index; occurCrashRedLine1 = occurCrashRedLine2 = false; // 杩欎釜绉戠洰瑙勫畾鐗规畩鐐癸紝鍙戠敓涓�娆℃墸10鍒嗭紝鑰屼笉鐩存帴娣樻卑 reportExamTimeout = false; - + reportParkFail = false; prevMoveStatus = moveStatus; parkSuccess = false; parkStatus = 0; @@ -80,9 +85,10 @@ } if (ExitParkArea(map, car) || ExitParkArea2(map, car)) { - if (!parkSuccess || !occurMoveBack) { + if (!parkSuccess && occurMoveBack && !reportParkFail) { // 鐩存帴椹剁娴嬭瘯鍖猴紝璁や负绉诲簱涓嶅叆 AddExamFault(21, rtkTime); + reportParkFail = true; DEBUG("鐩存帴椹剁娴嬭瘯鍖�,绉诲簱涓嶅叆"); } testing = false; @@ -130,9 +136,10 @@ } } - if (!parkSuccess) { + if (!parkSuccess && !reportParkFail) { // 鍋滄鍚庯紝杞﹁韩鍑虹嚎锛屼笉鍚堟牸 AddExamFault(21, rtkTime); + reportParkFail = true; DEBUG("绉诲簱涓嶅叆"); } @@ -149,6 +156,7 @@ DEBUG("寮�濮嬪�掕溅"); occurMoveBack = true; moveBackTimePoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10); + MA_EnterMap(mapIndex, MAP_TYPE_PART_EDGE, 1); } } @@ -165,7 +173,11 @@ } } } + TEST_END: + if (!testing && occurMoveBack) { + MA_EnterMap(mapIndex, MAP_TYPE_PART_EDGE, 0); + } return testing ? 1 : 0; } diff --git a/lib/src/main/cpp/test_items/park_edge.h b/lib/src/main/cpp/test_items/park_edge.h index 752e909..8916ff8 100644 --- a/lib/src/main/cpp/test_items/park_edge.h +++ b/lib/src/main/cpp/test_items/park_edge.h @@ -11,7 +11,7 @@ using namespace std; -void StartParkEdge(int moveStatus, const struct RtkTime *rtkTime); +void StartParkEdge(int index, int moveStatus, const struct RtkTime *rtkTime); int TestParkEdge(const Polygon *map, const car_model *car, const car_model *carPrev, double speed, int moveStatus, const struct RtkTime *rtkTime); #endif //RTKDRIVERTEST_PARK_EDGE_H diff --git a/lib/src/main/cpp/test_items/stop_and_start.cpp b/lib/src/main/cpp/test_items/stop_and_start.cpp index 91a9b0b..b57ceec 100644 --- a/lib/src/main/cpp/test_items/stop_and_start.cpp +++ b/lib/src/main/cpp/test_items/stop_and_start.cpp @@ -12,6 +12,7 @@ #include "../common/apptimer.h" #include "../utils/xconvert.h" #include "../test_common/car_sensor.h" +#include "../master/comm_if.h" #define DEBUG(fmt, args...) LOGD("<stop_and_start> <%s>: " fmt, __func__, ##args) @@ -31,6 +32,7 @@ static PointF stopPoint; +static int mapIndex = 0; static int prevMoveDirect; static uint32_t stopTimepoint = 0; static bool stopCar = false; @@ -46,11 +48,11 @@ static double DistanceOfTire2Edge(const Polygon *map, const car_model *car); static bool ExitTestArea(const Polygon *map, const car_model *car); -void StartSAS(int moveDirect, const struct RtkTime *rtkTime) +void StartSAS(int index, int moveDirect, const struct RtkTime *rtkTime) { DEBUG("杩涘叆鍧¤捣椤圭洰"); testing = true; - + mapIndex = index; prevMoveDirect = moveDirect; if (moveDirect == 0) { @@ -63,6 +65,8 @@ slideNormalDistance = false; reportSlideFault = false; reportStartTimeout = false; + + MA_EnterMap(mapIndex, MAP_TYPE_STOP_START, 1); } int TestSAS(const Polygon *map, const car_model *car, const car_model *carPrev, double speed, int moveDirect, const struct RtkTime *rtkTime) @@ -81,6 +85,12 @@ if (ExitTestArea(map, car)) { // 椹剁娴嬭瘯鍖� testing = false; + } + + { + double dis2 = DistanceOfTire2Edge(map, car); + + MA_SendDistance(-1, dis2); } if (prevMoveDirect != moveDirect) { @@ -172,6 +182,10 @@ } } + if (!testing) { + MA_EnterMap(mapIndex, MAP_TYPE_STOP_START, 0); + } + return testing ? 1 : 0; } @@ -245,10 +259,22 @@ // 鏁翠釜杞﹁締閮借椹剁璇ユ祴璇曞尯鍩� static bool ExitTestArea(const Polygon *map, const car_model *car) { - // 鍦�8->7绾跨殑鍙充晶 - for (int i = 0; i < car->bodyNum; ++i) { - if (IntersectionOfLine(map->point[8], map->point[7], car->carXY[car->body[i]]) != -1) - return false; + // 鍏ㄨ溅閮介渶涓嶅湪鍦板浘涓� + bool ret = false; + + Polygon carBody; + + carBody.num = car->bodyNum; + carBody.point = (PointF *)malloc(carBody.num * sizeof(PointF)); + for (int i = 0; i < carBody.num; ++i) { + carBody.point[i] = car->carXY[car->body[i]]; } - return true; + + if (IntersectionOf(&carBody, map) == GM_None) { + ret = true; + } + + free(carBody.point); + + return ret; } diff --git a/lib/src/main/cpp/test_items/stop_and_start.h b/lib/src/main/cpp/test_items/stop_and_start.h index b3f69cb..894a7e3 100644 --- a/lib/src/main/cpp/test_items/stop_and_start.h +++ b/lib/src/main/cpp/test_items/stop_and_start.h @@ -9,7 +9,7 @@ using namespace std; -void StartSAS(int moveDirect, const struct RtkTime *rtkTime); +void StartSAS(int index, int moveDirect, const struct RtkTime *rtkTime); int TestSAS(const Polygon *map, const car_model *car, const car_model *carPrev, double speed, int moveStatus, const struct RtkTime *rtkTime); #endif //RTKDRIVERTEST_STOP_AND_START_H diff --git a/lib/src/main/cpp/test_items/turn_a90.cpp b/lib/src/main/cpp/test_items/turn_a90.cpp index 6ee18df..4b3ba68 100644 --- a/lib/src/main/cpp/test_items/turn_a90.cpp +++ b/lib/src/main/cpp/test_items/turn_a90.cpp @@ -10,6 +10,7 @@ #include "../utils/xconvert.h" #include "../defs.h" #include "../test_common/car_sensor.h" +#include "../master/comm_if.h" #include <vector> #include <cstdlib> @@ -21,6 +22,7 @@ const uint32_t STOP_CAR_TIME = D_SEC(2); static bool testing; +static int mapIndex; static int enterAreaHeading; static bool turnLeftFinished; @@ -33,7 +35,7 @@ static bool CrashRedLine(const Polygon *map, const car_model *car); static bool ExitTestArea(const Polygon *map, const car_model *car); -void StartTurnA90(int moveDirect, double heading, const struct RtkTime *rtkTime) +void StartTurnA90(int index, int moveDirect, double heading, const struct RtkTime *rtkTime) { DEBUG("杩涘叆鐩磋杞集鍦哄湴"); testing = true; @@ -45,6 +47,9 @@ reportStopCarTimeout = false; crashRedLine = false; turnLeftFinished = false; + mapIndex = index; + + MA_EnterMap(mapIndex, MAP_TYPE_TURN_90, 1); } int TestTurnA90(const Polygon *map, const car_model *car, const car_model *carPrev, double heading, double speed, int moveDirect, const struct RtkTime *rtkTime) @@ -118,6 +123,11 @@ if (turnLeftFinished) { } + + if (!testing) { + MA_EnterMap(mapIndex, MAP_TYPE_TURN_90, 0); + } + TEST_END: return testing? 1:0; } diff --git a/lib/src/main/cpp/test_items/turn_a90.h b/lib/src/main/cpp/test_items/turn_a90.h index f6e88b2..857285d 100644 --- a/lib/src/main/cpp/test_items/turn_a90.h +++ b/lib/src/main/cpp/test_items/turn_a90.h @@ -10,7 +10,7 @@ using namespace std; -void StartTurnA90(int moveDirect, double heading, const struct RtkTime *rtkTime); +void StartTurnA90(int index, int moveDirect, double heading, const struct RtkTime *rtkTime); int TestTurnA90(const Polygon *map, const car_model *car, const car_model *carPrev, double heading, double speed, int moveDirect, const struct RtkTime *rtkTime); #endif //RTKDRIVERTEST_TURN_A90_H diff --git a/lib/src/main/cpp/test_items2/drive_straight.cpp b/lib/src/main/cpp/test_items2/drive_straight.cpp index 16a8832..8db23e0 100644 --- a/lib/src/main/cpp/test_items2/drive_straight.cpp +++ b/lib/src/main/cpp/test_items2/drive_straight.cpp @@ -7,6 +7,7 @@ #include "../native-lib.h" #include "../jni_log.h" #include "road_exam.h" +#include "../master/comm_if.h" #include <cmath> #define DEBUG(fmt, args...) LOGD("<drive_straight> <%s>: " fmt, __func__, ##args) @@ -43,6 +44,14 @@ MakeLine(&road_edge, &RoadMapList[index].roadEdgeLine[0].point[0], &RoadMapList[index].roadEdgeLine[0].point[1]); + { + double l1 = DistanceOf(car->carXY[car->right_front_tire[TIRE_OUTSIDE]], road_edge); + + double l2 = DistanceOf(car->carXY[car->right_rear_tire[TIRE_OUTSIDE]], road_edge); + + MA_SendDistance(-1, (l1+l2)/2.0); + } + if (!crossStartLine) { PointF p1, p2; -- Gitblit v1.8.0