From 7c66884c800b0b48c2b3aac2ac29ff0b4529b95e Mon Sep 17 00:00:00 2001
From: yy1717 <fctom1215@outlook.com>
Date: 星期二, 21 一月 2020 15:00:12 +0800
Subject: [PATCH] updatecrrDOC1
---
lib/src/main/cpp/driver_test.cpp | 82 ++++++++++++++++++++++------------------
1 files changed, 45 insertions(+), 37 deletions(-)
diff --git a/lib/src/main/cpp/driver_test.cpp b/lib/src/main/cpp/driver_test.cpp
index 41b802d..d2d2d44 100644
--- a/lib/src/main/cpp/driver_test.cpp
+++ b/lib/src/main/cpp/driver_test.cpp
@@ -74,7 +74,7 @@
} MapList[MAP_LIST_SIZE];
static int MapNum = 0;
-static int CurrExamMapId = -1;
+static int CurrExamMapIndex = -1;
static int CurrExamStatus = 0; // 1 娴嬭瘯瀹屾垚 0 娴嬭瘯涓� -1 娴嬭瘯閿欒閫�鍑�
static car_model *CarModel = NULL;
@@ -94,7 +94,8 @@
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);
+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)
{
@@ -201,6 +202,7 @@
CarModelPrev = NULL;
}
+ // 浠呭垎閰峜arXY, body鍜宑arDesc鍧囨病鏈�
CarModelPrev = (car_model *)malloc(sizeof(car_model));
CarModelPrev->body = NULL;
CarModelPrev->carDesc = NULL;
@@ -337,7 +339,7 @@
RtkBuffer[index].mm, RtkBuffer[index].ss, RtkBuffer[index].dss);
brief.qf = RtkBuffer[index].qf;
- brief.map_id = 863;//CurrExamMapId;
+ brief.map_id = 863;//GetMapId(CurrExamMapIndex, MapList, MapNum);
brief.move = move;
brief.speed = speed * 3.6;
brief.heading = RtkBuffer[index].heading;
@@ -374,16 +376,16 @@
free(brief.point);
if (TestStart) {
- if (CurrExamMapId < 0) {
- CurrExamMapId = EnterMap(CarModel, MapList, MapNum);
- int mtype = GetMapType(CurrExamMapId, MapList, MapNum);
+ if (CurrExamMapIndex < 0) {
+ CurrExamMapIndex = EnterMap(CarModel, MapList, MapNum);
+ int mtype = GetMapType(CurrExamMapIndex, MapList, MapNum);
- DEBUG("CurrExamMapId %d mtype %d", CurrExamMapId, mtype);
+ DEBUG("CurrExamMapIndex %d mtype %d", GetMapId(CurrExamMapIndex, MapList, MapNum), mtype);
switch (mtype) {
case MAP_TYPE_PARK_BUTTOM:
DEBUG("杩涘叆鍊掕溅鍏ュ簱鍦哄湴");
- MA_SendDebugInfo("杩涘叆鍊掕溅鍏ュ簱鍦哄湴 %d", CurrExamMapId);
+ MA_SendDebugInfo("杩涘叆鍊掕溅鍏ュ簱鍦哄湴 %d", GetMapId(CurrExamMapIndex, MapList, MapNum));
StartParkBottom();
CurrExamStatus = 0;
@@ -411,12 +413,12 @@
rtkTime.ss = RtkBuffer[index].ss;
rtkTime.mss = RtkBuffer[index].dss;
- if (CurrExamMapId >= 0) {
+ if (CurrExamMapIndex >= 0) {
if (CurrExamStatus == 0) {
- int mtype = GetMapType(CurrExamMapId, MapList, MapNum);
+ int mtype = GetMapType(CurrExamMapIndex, MapList, MapNum);
switch (mtype) {
case MAP_TYPE_PARK_BUTTOM:
- CurrExamStatus = TestParkBottom(ErrorList, &MapList[CurrExamMapId].map,
+ CurrExamStatus = TestParkBottom(ErrorList, &MapList[CurrExamMapIndex].map,
CarModel, CarModelPrev, speed, move, &rtkTime);
break;
case MAP_TYPE_STOP_START:
@@ -432,10 +434,10 @@
}
}
if (CurrExamStatus != 0) {
- if (ExitMap(CarModel, CurrExamMapId, MapList, MapNum)) {
- DEBUG("閫�鍑哄満鍦� %d", CurrExamMapId);
- MA_SendDebugInfo("閫�鍑哄満鍦� %d", CurrExamMapId);
- CurrExamMapId = -1;
+ if (ExitMap(CarModel, CurrExamMapIndex, MapList, MapNum)) {
+ DEBUG("閫�鍑哄満鍦� %d", GetMapId(CurrExamMapIndex, MapList, MapNum));
+ MA_SendDebugInfo("閫�鍑哄満鍦� %d", GetMapId(CurrExamMapIndex, MapList, MapNum));
+ CurrExamMapIndex = -1;
}
}
}
@@ -450,8 +452,8 @@
*/
static uint32_t CalcTimeDiff(const rtk_info *a, const rtk_info *b)
{
- return ABS(TimeMakeComposite(2000 + a->YY, a->MM, a->DD, a->hh, a->mm, a->ss) * 1000 + a->dss*10 -
- (TimeMakeComposite(2000 + b->YY, b->MM, b->DD, b->hh, b->mm, b->ss) * 1000 + b->dss*10));
+ return ABS((long)(TimeMakeComposite(2000 + a->YY, a->MM, a->DD, a->hh, a->mm, a->ss) * 1000 + a->dss*10) -
+ (long)(TimeMakeComposite(2000 + b->YY, b->MM, b->DD, b->hh, b->mm, b->ss) * 1000 + b->dss*10));
}
static bool UpdateCarCoord(double &spd, int &mov, int &idx)
@@ -663,42 +665,48 @@
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 i;
}
}
}
return -1;
}
-static bool ExitMap(const car_model *car, int mapId, const struct map_list *mapList, int mapNum)
+static bool ExitMap(const car_model *car, int index, 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;
+ if (index < 0 || mapList == NULL || mapNum == 0) return true;
- 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 (mapList[index].type == MAP_TYPE_PARK_BUTTOM) {
+ if (IntersectionOf(car->carXY[car->axial[AXIAL_REAR]], &mapList[index].map) == GM_None) {
+ Line enterLine1, enterLine2;
- 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;
+ MakeLine(&enterLine1, &(mapList[index].map.point[0]), &(mapList[index].map.point[1]));
+ MakeLine(&enterLine2, &(mapList[index].map.point[6]), &(mapList[index].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 true;
}
-static int GetMapType(int id, const struct map_list *mapList, int mapNum)
+static int GetMapId(int index, 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;
+ 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)
--
Gitblit v1.8.0