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