From 59abff0d03403344619420aa0bcd9c2b28ff3522 Mon Sep 17 00:00:00 2001 From: yy1717 <fctom1215@outlook.com> Date: 星期三, 08 四月 2020 11:06:58 +0800 Subject: [PATCH] 坐标 --- lib/src/main/cpp/driver_test.cpp | 451 +++++-------------------------------------------------- 1 files changed, 47 insertions(+), 404 deletions(-) diff --git a/lib/src/main/cpp/driver_test.cpp b/lib/src/main/cpp/driver_test.cpp index b502094..22b8ff2 100644 --- a/lib/src/main/cpp/driver_test.cpp +++ b/lib/src/main/cpp/driver_test.cpp @@ -18,7 +18,7 @@ #include "driver_test.h" #include "defs.h" -#include "Geometry.h" +#include "test_common/Geometry.h" #include "common/apptimer.h" #include "jni_log.h" #include "test_items/park_edge.h" @@ -35,6 +35,7 @@ #include "mcu/mcu_if.h" #include "test_common/car_sensor.h" #include "test_items2/road_exam.h" +#include "test_items/area_exam.h" #define DEBUG(fmt, args...) LOGD("<driver_test> <%s>: " fmt, __func__, ##args) @@ -59,29 +60,16 @@ static int ExamType; static bool reportSeatbeltEject; -static int CarInArea = 0; -int errs = 0; - -vector<int> ErrorList; vector<ExamFault> ExamFaultList; static int examFaultIndex = 0; -static struct map_list { - int id; - int type; - Polygon map; - Polygon map2; -} MapList[MAP_LIST_SIZE]; +static LIST_AREA_MAP AreaMapList; static Polygon RoadMapPoints; static LIST_ROAD_MAP RoadMapList; -static int MapNum = 0; -static int CurrExamMapIndex = -1; -static int CurrEnterMapIndex = -1; -static int CurrExamStatus = EXAM_AREA_NONE; // 1 娴嬭瘯瀹屾垚 0 娴嬭瘯涓� -1 娴嬭瘯閿欒閫�鍑� static int exam_dummy_light; static car_model *CarModel = NULL; @@ -102,7 +90,6 @@ static rtk_info *RtkBuffer = NULL; static int RtkBufferNum = 0, RtkBufferIn = 0; -static void DetectEnterOrExitMap(void); static void EngineStartHold(union sigval sig); static void ExecuteExam(const struct RtkTime* rtkTime); static void ExecuteExam(double speed, int move, double azimuth, const struct RtkTime* rtkTime); @@ -111,19 +98,15 @@ 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 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); void DriverTestInit(void) { ExamStart = false; - memset(&MapList, 0, sizeof(MapList)); - MapNum = 0; + CarModel = NULL; CarModelList.clear(); + + AreaMapList.clear(); RoadMapPoints.num = 0; RoadMapPoints.point = NULL; @@ -137,8 +120,6 @@ RtkBuffer = (rtk_info *) malloc(RTK_BUFFER_SIZE * sizeof(rtk_info)); RtkBufferNum = RtkBufferIn = 0; - - CurrExamStatus = EXAM_AREA_NONE; } static void ReadDriverExamPrimerTimeout(union sigval sig) @@ -155,54 +136,54 @@ MA_ReadSensor(); } -void ClearMap(void) +void ClearAreaMap(void) { - if (ExamStart) return; - - for (int i = 0; i < MapNum; ++i) { - if (MapList[i].map.point != NULL) - free(MapList[i].map.point); - if (MapList[i].map2.point != NULL) - free(MapList[i].map2.point); - } - memset(&MapList, 0, sizeof(MapList)); - MapNum = 0; -} - -void AddMap(int id, int type, const double (*map)[2], int pointNum, const double (*map2)[2], int pointNum2) -{ - DEBUG("鍔犲叆鍦板浘淇℃伅 id %d type %d pointNum %d point2Num %d", id, type, pointNum, pointNum2); - - AppTimer_delete(ReadDriverExamPrimerTimeout); - - if (map == NULL || pointNum == 0 || ExamStart) + if (ExamStart) return; - MapList[MapNum].id = id; + for (int i = 0; i < AreaMapList.size(); ++i) { + if (AreaMapList[i].map.point != NULL) + free(AreaMapList[i].map.point); + if (AreaMapList[i].map2.point != NULL) + free(AreaMapList[i].map2.point); + } - MapList[MapNum].type = type; + AreaMapList.clear(); +} - MapList[MapNum].map.num = pointNum; +void AddAreaMap(int id, int type, const double (*map)[2], int pointNum, const double (*map2)[2], int pointNum2) +{ + if (map == NULL || pointNum == 0 || ExamStart) + return; + DEBUG("鍔犲叆鍦板浘淇℃伅 id %d type %d pointNum %d point2Num %d", id, type, pointNum, pointNum2); + + struct area_exam_map newMap; + + newMap.id = id; + newMap.type = type; + newMap.map.num = pointNum; + newMap.map2.num = 0; + newMap.map.point = NULL; + newMap.map2.point = NULL; + if (pointNum > 0) { - MapList[MapNum].map.point = (PointF *)malloc(sizeof(PointF) * pointNum); + newMap.map.point = (PointF *) malloc(pointNum * sizeof(PointF)); for (int i = 0; i < pointNum; ++i) { - MapList[MapNum].map.point[i].X = map[i][0]; - MapList[MapNum].map.point[i].Y = map[i][1]; + newMap.map.point[i].X = map[i][0]; + newMap.map.point[i].Y = map[i][1]; } } - MapList[MapNum].map2.num = pointNum2; if (pointNum2 > 0 && map2 != NULL) { - MapList[MapNum].map2.point = (PointF *)malloc(sizeof(PointF) * pointNum2); + newMap.map2.num = pointNum2; + newMap.map2.point = (PointF *) malloc(pointNum2 * sizeof(PointF)); for (int i = 0; i < pointNum2; ++i) { - MapList[MapNum].map2.point[i].X = map2[i][0]; - MapList[MapNum].map2.point[i].Y = map2[i][1]; + newMap.map2.point[i].X = map2[i][0]; + newMap.map2.point[i].Y = map2[i][1]; } } - MapNum++; - - DEBUG("AddMap num %d", MapNum); + AreaMapList.push_back(newMap); } void CleanRoadMap(void) @@ -550,14 +531,15 @@ DEBUG("缁撴潫鑰冭瘯"); TerminateRoadExam(); + TerminateAreaExam(); - CurrExamMapIndex = -1; + ExamStart = false; MA_SendExamStatus(0, 0); return; } - if (MapNum == 0 && type == TEST_TYPE_AREA) { + if (AreaMapList.size() == 0 && type == TEST_TYPE_AREA) { DEBUG("娌℃湁鍦鸿�冨湴鍥�"); err = true; MA_SendExamStatus(0, -1); @@ -590,21 +572,12 @@ if (type == TEST_TYPE_ROAD_DUMMY_LIGHT) { exam_dummy_light = 0; - -// InitRoadExam(); //////////////////// + } + if (type == TEST_TYPE_AREA) { + InitAreaExam(); } } MA_SendExamStatus(1, 0); - } -} - -void StartMapExam(int map_id, int exam) -{ - DEBUG("娴嬭瘯璇ュ満鍦� %d: %d", map_id, exam); - - if (map_id >= 0 && exam == 0) { - CurrExamMapIndex = map_id; - CurrExamStatus = EXAM_AREA_START; } } @@ -692,35 +665,8 @@ rtkTime.ss = RtkBuffer[index].ss; rtkTime.mss = RtkBuffer[index].dss; - DetectEnterOrExitMap(); - if (ExamStart) { ExecuteExam(speed, move, azimuth, &rtkTime); - } - } -} - -static void DetectEnterOrExitMap(void) -{ - if (ExamType == TEST_TYPE_AREA) { - - } - if (CurrExamMapIndex < 0) { - if (CurrEnterMapIndex < 0) { - 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); - - 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); - CurrEnterMapIndex = -1; - } } } } @@ -875,87 +821,8 @@ if (exam_dummy_light == 2) { TestRoadGeneral(RoadMapList, CarModel, CarModelList, speed, move, rtkTime); } - return; - } - - if (CurrExamMapIndex >= 0) { - int mtype = GetMapType(CurrExamMapIndex, MapList, MapNum); - - if (CurrExamStatus == EXAM_AREA_START) { - DEBUG("CurrExamMapIndex %d mtype %d", GetMapId(CurrExamMapIndex, MapList, MapNum), mtype); - - switch (mtype) { - case MAP_TYPE_PARK_BUTTOM: - DEBUG("杩涘叆鍊掕溅鍏ュ簱鍦哄湴"); - MA_SendDebugInfo("杩涘叆鍊掕溅鍏ュ簱鍦哄湴 %d", GetMapId(CurrExamMapIndex, MapList, MapNum)); - - StartParkBottom(GetMapId(CurrExamMapIndex, MapList, MapNum), move, rtkTime); - CurrExamStatus = EXAM_AREA_RUN; - break; - case MAP_TYPE_STOP_START: - DEBUG("杩涘叆涓婂潯璧锋鍦哄湴"); - MA_SendDebugInfo("杩涘叆涓婂潯璧锋鍦哄湴 %d", GetMapId(CurrExamMapIndex, MapList, MapNum)); - StartSAS(GetMapId(CurrExamMapIndex, MapList, MapNum), move, rtkTime); - CurrExamStatus = EXAM_AREA_RUN; - break; - case MAP_TYPE_PART_EDGE: - DEBUG("杩涘叆渚ф柟浣嶅仠杞﹀満鍦�"); - MA_SendDebugInfo("杩涘叆渚ф柟浣嶅仠杞﹀満鍦� %d", GetMapId(CurrExamMapIndex, MapList, MapNum)); - StartParkEdge(GetMapId(CurrExamMapIndex, MapList, MapNum), move, rtkTime); - - CurrExamStatus = EXAM_AREA_RUN; - break; - case MAP_TYPE_CURVE: - DEBUG("杩涘叆鏇茬嚎琛岄┒鍦哄湴"); - MA_SendDebugInfo("杩涘叆鏇茬嚎琛岄┒鍦哄湴 %d", GetMapId(CurrExamMapIndex, MapList, MapNum)); - StartDrivingCurve(GetMapId(CurrExamMapIndex, MapList, MapNum), move, rtkTime); - - CurrExamStatus = EXAM_AREA_RUN; - break; - case MAP_TYPE_TURN_90: - DEBUG("杩涘叆鐩磋杞集鍦哄湴"); - MA_SendDebugInfo("杩涘叆鐩磋杞集鍦哄湴 %d", GetMapId(CurrExamMapIndex, MapList, MapNum)); - StartTurnA90(GetMapId(CurrExamMapIndex, MapList, MapNum), move, azimuth, rtkTime); - CurrExamStatus = EXAM_AREA_RUN; - break; - default:break; - } - } else if (CurrExamStatus == EXAM_AREA_RUN) { - int testing = 0; - switch (mtype) { - case MAP_TYPE_PARK_BUTTOM: - testing = TestParkBottom(&MapList[CurrExamMapIndex].map, - CarModel, NULL, speed, move, rtkTime); - break; - case MAP_TYPE_STOP_START: - testing = TestSAS(&MapList[CurrExamMapIndex].map, CarModel, NULL, speed, move, rtkTime); - break; - case MAP_TYPE_PART_EDGE: - testing = TestParkEdge(&MapList[CurrExamMapIndex].map, CarModel, NULL, speed, move, rtkTime); - break; - case MAP_TYPE_CURVE: - testing = TestDrivingCurve(&MapList[CurrExamMapIndex].map, &MapList[CurrExamMapIndex].map2, CarModel, NULL, speed, move, rtkTime); - break; - case MAP_TYPE_TURN_90: - testing = TestTurnA90(&MapList[CurrExamMapIndex].map, CarModel, NULL, azimuth, speed, move, rtkTime); - break; - default: - break; - } - - if (testing > 0) { - CurrExamStatus = EXAM_AREA_RUN; - } else { - CurrExamStatus = EXAM_AREA_END; - } - } - if (CurrExamStatus != EXAM_AREA_RUN) { - // 鏌愰」缁撴潫 - //DEBUG("閫�鍑哄満鍦� %d", GetMapId(CurrExamMapIndex, MapList, MapNum)); - //MA_SendDebugInfo("閫�鍑哄満鍦� %d", GetMapId(CurrExamMapIndex, MapList, MapNum)); - CurrExamStatus = EXAM_AREA_NONE; - CurrExamMapIndex = -1; - } + } else { + TestAreaGeneral(AreaMapList, CarModel, CarModelList, speed, move, azimuth, rtkTime); } } @@ -1140,225 +1007,6 @@ return true; } -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; - - 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; - - x9 = 2*xo - mapList[i].map.point[8].X; - y9 = 2*yo - mapList[i].map.point[8].Y; - - Line triggerLine; - - triggerLine.X1 = mapList[i].map.point[0].X; - triggerLine.Y1 = mapList[i].map.point[0].Y; - triggerLine.X2 = x9; - triggerLine.Y2 = y9; - - 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; - - MakeLine(&enterLine1, &(mapList[i].map.point[0]), &(mapList[i].map.point[1])); - MakeLine(&enterLine2, &(mapList[i].map.point[6]), &(mapList[i].map.point[7])); - - if (DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], enterLine1) > 0.1 && - DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], enterLine2) > 0.1) - return i; - } - } - if (mapList[i].type == MAP_TYPE_PART_EDGE) { - // 杞﹀ご椤剁偣鍦ㄥ満鍦板唴 - if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &mapList[i].map) == GM_Containment) { - Line enterLine; - - MakeLine(&enterLine, &(mapList[i].map.point[0]), &(mapList[i].map.point[1])); - - if (DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], enterLine) > 0.1) - return i; - } - } - if (mapList[i].type == MAP_TYPE_TURN_90) { - // 杞﹀墠杞垨鍚庤疆杞ㄨ抗瓒婅繃瑙﹀彂绾� - Line triggerLine; - - MakeLine(&triggerLine, &(mapList[i].map.point[0]), &(mapList[i].map.point[1])); - - if (CrashTriggerLine(triggerLine, car, CarModelList)) - return i; - } - if (mapList[i].type == MAP_TYPE_CURVE) { - Line triggerLine; - - MakeLine(&triggerLine, &mapList[i].map2.point[0], &mapList[i].map.point[0]); - if (CrashTriggerLine(triggerLine, car, CarModelList)) - return i; - } - } - return -1; -} - -static bool ExitMap(const car_model *car, int index, const struct map_list *mapList, int mapNum) -{ - bool ret = false; - if (index < 0 || mapList == NULL || mapNum == 0) return true; - - if (mapList[index].type == MAP_TYPE_PARK_BUTTOM || - mapList[index].type == MAP_TYPE_PART_EDGE || - mapList[index].type == MAP_TYPE_TURN_90) { - // 鍏ㄨ溅閮介渶涓嶅湪鍦板浘涓� - 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]]; - } - - if (IntersectionOf(&carBody, &mapList[index].map) == GM_None) { - ret = true; - } - - free(carBody.point); - } - if (mapList[index].type == MAP_TYPE_STOP_START) { - // 鏋勯�犺櫄鎷熺殑宸︿笂瑙掔偣 - double x9, y9, xo, yo; - - bool enter = false; - - xo = (mapList[index].map.point[0].X + mapList[index].map.point[7].X) / 2; - yo = (mapList[index].map.point[0].Y + mapList[index].map.point[7].Y) / 2; - - x9 = 2*xo - mapList[index].map.point[8].X; - y9 = 2*yo - mapList[index].map.point[8].Y; - - Polygon map; - - map.num = 4; - map.point = (PointF *) malloc(map.num * sizeof(PointF)); - - map.point[0] = mapList[index].map.point[0]; - map.point[1] = mapList[index].map.point[8]; - map.point[2] = mapList[index].map.point[7]; - map.point[3].X = x9; - map.point[3].Y = y9; - - // 鍏ㄨ溅閮介渶涓嶅湪鍦板浘涓� - 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]]; - } - - if (IntersectionOf(&carBody, &map) == GM_None) { - ret = true; - } - - free(carBody.point); - free(map.point); - } - if (mapList[index].type == MAP_TYPE_CURVE) { - ret = ExitDrivingCurveArea(&mapList[index].map, &mapList[index].map2, car); - } - - return ret; -} - -static int GetMapId(int index, const struct map_list *mapList, int mapNum) -{ - if (index < 0 || mapList == NULL || mapNum == 0) - return -1; - - return mapList[index].id; -} - -static int GetMapType(int index, const struct map_list *mapList, int mapNum) -{ - if (index < 0 || mapList == NULL || mapNum == 0) - return -1; - - return mapList[index].type; -} - void AddExamFault(int wrong, const struct RtkTime *rtkTime) { struct ExamFault fault; @@ -1383,11 +1031,6 @@ MA_SendExamWrong(ExamFaultList); ExamFaultList.clear(); -} - -car_model_cache_t *GetCarModelCache(int node) -{ - return NULL; } /******************************************************************* -- Gitblit v1.8.0