From 09b0098b3b092cb28435c8d550c2f7b30bf53e0a Mon Sep 17 00:00:00 2001
From: yy1717 <fctom1215@outlook.com>
Date: 星期三, 15 一月 2020 18:28:14 +0800
Subject: [PATCH] updatec
---
lib/src/main/cpp/driver_test.cpp | 169 +++++++++++++++++++++++++++++++++++++++++++++++++++++--
1 files changed, 161 insertions(+), 8 deletions(-)
diff --git a/lib/src/main/cpp/driver_test.cpp b/lib/src/main/cpp/driver_test.cpp
index 48c820b..9b481cc 100644
--- a/lib/src/main/cpp/driver_test.cpp
+++ b/lib/src/main/cpp/driver_test.cpp
@@ -31,6 +31,12 @@
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
@@ -52,7 +58,7 @@
};
static bool TestStart = false;
-static int TestItem = TEST_NONE;
+
static int CarInArea = 0;
int errs = 0;
@@ -68,6 +74,8 @@
} MapList[MAP_LIST_SIZE];
static int MapNum = 0;
+static int CurrExamMapId = -1;
+static int CurrExamStatus = 0; // 1 娴嬭瘯瀹屾垚 0 娴嬭瘯涓� -1 娴嬭瘯閿欒閫�鍑�
static car_model *CarModel = NULL;
static car_model *CarModelPrev = NULL;
@@ -84,6 +92,9 @@
static void UpdateCarBodyCoord(double azimuth, PointF main_ant, car_model *carModel);
static bool UpdateCarCoord(double &spd, int &mov, int &idx);
static bool FrontTireEnterArea(const Polygon *car, const Polygon *map);
+static int EnterMap(const car_model *car, 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 GetMapType(int id, const struct map_list *mapList, int mapNum);
void DriverTestInit(void)
{
@@ -127,7 +138,7 @@
void AddMap(int id, int type, const double (*map)[2], int pointNum, const double (*map2)[2], int pointNum2)
{
- DEBUG("AddMap id %d type %d pointNum %d point2Num %d", id, type, pointNum, pointNum2);
+ DEBUG("鍔犲叆鍦板浘淇℃伅 id %d type %d pointNum %d point2Num %d", id, type, pointNum, pointNum2);
AppTimer_delete(ReadDriverExamPrimerTimeout);
@@ -164,7 +175,7 @@
int *right_front_tire, int *left_rear_tire, int *right_rear_tire,
int *body, int bodyNum,double (*point)[2], int pointNum)
{
- DEBUG("SetCarMeasurePoint pointNum %d", pointNum);
+ DEBUG("鍔犲叆杞﹁締淇℃伅 pointNum %d", pointNum);
if (point == NULL || pointNum == 0 || TestStart) return;
@@ -252,6 +263,24 @@
DEBUG("鍔犲叆鐐�<%d> 璺濈 %f 瑙掑害 %f", i, CarModel->carDesc[i].distance, CarModel->carDesc[i].angle);
}
+ CarModel->carDesc[0].distance = 0.2465;
+ CarModel->carDesc[0].angle = 0;
+
+ CarModel->carDesc[1].distance = 0.2635;
+ CarModel->carDesc[1].angle = 20.7;
+
+ CarModel->carDesc[2].distance = 0.14;
+ CarModel->carDesc[2].angle = 138.9;
+
+ CarModel->carDesc[3].distance = 0.1055;
+ CarModel->carDesc[3].angle = 180.0;
+
+ CarModel->carDesc[4].distance = 0.14;
+ CarModel->carDesc[4].angle = 221.1;
+
+ CarModel->carDesc[5].distance = 0.2635;
+ CarModel->carDesc[5].angle = 339.3;
+
DEBUG("SetCarMeasurePoint Calc Over");
}
@@ -306,7 +335,7 @@
RtkBuffer[index].mm, RtkBuffer[index].ss, RtkBuffer[index].dss);
brief.qf = RtkBuffer[index].qf;
- brief.map_id = 863;
+ brief.map_id = 863;//CurrExamMapId;
brief.move = move;
brief.speed = speed * 3.6;
brief.heading = RtkBuffer[index].heading;
@@ -343,7 +372,73 @@
free(brief.point);
if (TestStart) {
-// TestParkBottom(ErrorList, 0, CarModel, CarModelPrev, speed, move);
+ if (CurrExamMapId < 0) {
+ CurrExamMapId = EnterMap(CarModel, MapList, MapNum);
+ int mtype = GetMapType(CurrExamMapId, MapList, MapNum);
+
+ switch (mtype) {
+ case MAP_TYPE_PARK_BUTTOM:
+ DEBUG("杩涘叆鍊掕溅鍏ュ簱鍦哄湴");
+ StartParkBottom();
+ CurrExamStatus = 0;
+ break;
+ case MAP_TYPE_STOP_START:
+ break;
+ case MAP_TYPE_PART_EDGE:
+ break;
+ case MAP_TYPE_CURVE:
+ break;
+ case MAP_TYPE_TURN_90:
+ break;
+ default:break;
+ }
+ }
+ }
+
+ struct RtkTime rtkTime;
+
+ rtkTime.YY = RtkBuffer[index].YY;
+ rtkTime.MM = RtkBuffer[index].MM;
+ rtkTime.DD = RtkBuffer[index].DD;
+ rtkTime.hh = RtkBuffer[index].hh;
+ rtkTime.mm = RtkBuffer[index].mm;
+ rtkTime.ss = RtkBuffer[index].ss;
+ rtkTime.mss = RtkBuffer[index].dss;
+
+ static int en = 0;
+ static int gcc = 0;
+
+ if (gcc % 20 == 0) {
+ // AddExamFault(++en, &rtkTime);
+ }
+ gcc++;
+
+ if (CurrExamMapId >= 0) {
+ if (CurrExamStatus == 0) {
+ int mtype = GetMapType(CurrExamMapId, MapList, MapNum);
+ switch (mtype) {
+ case MAP_TYPE_PARK_BUTTOM:
+ CurrExamStatus = TestParkBottom(ErrorList, &MapList[CurrExamMapId].map,
+ CarModel, CarModelPrev, speed, move, &rtkTime);
+ break;
+ case MAP_TYPE_STOP_START:
+ break;
+ case MAP_TYPE_PART_EDGE:
+ break;
+ case MAP_TYPE_CURVE:
+ break;
+ case MAP_TYPE_TURN_90:
+ break;
+ default:
+ break;
+ }
+ }
+ if (CurrExamStatus != 0) {
+ if (ExitMap(CarModel, CurrExamMapId, MapList, MapNum)) {
+ DEBUG("閫�鍑哄満鍦� %d", CurrExamMapId);
+ CurrExamMapId = -1;
+ }
+ }
}
}
}
@@ -552,19 +647,77 @@
}*/
}
-void AddExamFault(int wrong, const char *utc)
+static int EnterMap(const car_model *car, const struct map_list *mapList, int mapNum)
{
- if (!TestStart) return;
+ // 杞︾殑鏈�鍓嶇偣鏄惁杩涘叆鍦板浘
+ for (int i = 0; i < mapNum && car != NULL; ++i) {
+ if (mapList[i].type == MAP_TYPE_STOP_START) {
+ } else if (mapList[i].type == MAP_TYPE_CURVE) {
+
+ } else 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 mapList[i].id;
+ }
+ }
+ }
+ return -1;
+}
+
+static bool ExitMap(const car_model *car, int mapId, const struct map_list *mapList, int mapNum)
+{
+ // 杞︾殑鏈�鍚庣偣鏄惁杩涘叆鍦板浘
+ for (int i = 0; i < mapNum && car != NULL; ++i) {
+ if (mapList[i].id == mapId) {
+ if (mapList[i].type == MAP_TYPE_PARK_BUTTOM) {
+ if (IntersectionOf(car->carXY[ car->axial[AXIAL_REAR] ], &mapList[i].map) == GM_None) {
+ 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_REAR]], enterLine1) > 0.1 &&
+ DistanceOf(car->carXY[car->axial[AXIAL_REAR]], enterLine2) > 0.1)
+ return true;
+ }
+ }
+ return false;
+ }
+ }
+ return true;
+}
+
+static int GetMapType(int id, const struct map_list *mapList, int mapNum)
+{
+ for (int i = 0; i < mapNum && id >= 0; ++i) {
+ if (mapList[i].id == id) return mapList[i].type;
+ }
+ return -1;
+}
+
+void AddExamFault(int wrong, const struct RtkTime *rtkTime)
+{
struct ExamFault fault;
fault.sn = examFaultIndex++;
- strcpy(fault.utc, utc);
+ sprintf(fault.utc, "%04d%02d%02d%02d%02d%02d.%02d", 2000 + rtkTime->YY,
+ rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss);
fault.wrong_id = wrong;
+
+ DEBUG("鑰冭瘯鍙戠敓閿欒 %d %s", wrong, fault.utc);
ExamFaultList.push_back(fault);
MA_SendExamWrong(ExamFaultList);
+
+ ExamFaultList.clear();
}
car_model_cache_t *GetCarModelCache(int node)
--
Gitblit v1.8.0