From eced3d013f06b623a49cb9deaba42218c4e37bb8 Mon Sep 17 00:00:00 2001
From: yy1717 <fctom1215@outlook.com>
Date: 星期三, 15 一月 2020 10:56:05 +0800
Subject: [PATCH] update
---
lib/src/main/cpp/driver_test.cpp | 146 +++++++++++++++++++++++++++++++++++-------------
1 files changed, 105 insertions(+), 41 deletions(-)
diff --git a/lib/src/main/cpp/driver_test.cpp b/lib/src/main/cpp/driver_test.cpp
index 28b5bfc..48c820b 100644
--- a/lib/src/main/cpp/driver_test.cpp
+++ b/lib/src/main/cpp/driver_test.cpp
@@ -57,10 +57,8 @@
int errs = 0;
vector<int> ErrorList;
-
-static Polygon theParkEdgeMap; // 渚т綅鍋滆溅鍦板浘
-static Polygon theTurn90Map;
-static Polygon theSSMap;
+vector<ExamFault> ExamFaultList;
+static int examFaultIndex = 0;
static struct map_list {
int id;
@@ -71,35 +69,8 @@
static int MapNum = 0;
-
-#define CAR_COORD_STORE_SIZE 10
-
-struct car_coord_ {
- uint32_t uptime;
- double azimuth;
- PointF coord;
-};
-
-struct car_desc_ {
- double distance; // 璺濈涓诲ぉ绾跨殑璺濈
- double angle; // 浠庝腑杞寸嚎閫嗘椂閽堝舰鎴愮殑瑙掑害
-};
-
-struct car_desc_ *CarDesc = NULL;
-
-typedef struct {
- PointF basePoint;
- int axial[2];
- int left_front_tire[2];
- int right_front_tire[2];
- int left_rear_tire[2];
- int right_rear_tire[2];
- int pointNum;
- struct car_desc_ *carDesc;
- PointF *carXY;
-} car_model;
-
static car_model *CarModel = NULL;
+static car_model *CarModelPrev = NULL;
#define MOV_AVG_SIZE 1
#define RTK_BUFFER_SIZE 100
@@ -110,15 +81,17 @@
static uint32_t CalcTimeDiff(const rtk_info *a, const rtk_info *b);
static void ReadDriverExamPrimerTimeout(union sigval sig);
-static void UpdateCarBodyCoord(double azimuth, PointF main_ant, const car_model *carModel);
+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);
void DriverTestInit(void)
{
+ TestStart = false;
memset(&MapList, 0, sizeof(MapList));
MapNum = 0;
CarModel = NULL;
+ CarModelPrev = NULL;
RtkBuffer = (rtk_info *) malloc(RTK_BUFFER_SIZE * sizeof(rtk_info));
RtkBufferNum = RtkBufferIn = 0;
@@ -140,6 +113,8 @@
void ClearMap(void)
{
+ if (TestStart) return;
+
for (int i = 0; i < MapNum; ++i) {
if (MapList[i].map.point != NULL)
free(MapList[i].map.point);
@@ -156,7 +131,7 @@
AppTimer_delete(ReadDriverExamPrimerTimeout);
- if (map == NULL || pointNum == 0)
+ if (map == NULL || pointNum == 0 || TestStart)
return;
MapList[MapNum].id = id;
@@ -187,13 +162,15 @@
void SetCarMeasurePoint(double *basePoint, int *axial, int *left_front_tire,
int *right_front_tire, int *left_rear_tire, int *right_rear_tire,
- double (*point)[2], int pointNum)
+ int *body, int bodyNum,double (*point)[2], int pointNum)
{
DEBUG("SetCarMeasurePoint pointNum %d", pointNum);
- if (point == NULL || pointNum == 0) return;
+ if (point == NULL || pointNum == 0 || TestStart) return;
if (CarModel != NULL) {
+ if (CarModel->body != NULL)
+ free(CarModel->body);
if (CarModel->carDesc != NULL)
free(CarModel->carDesc);
if (CarModel->carXY != NULL)
@@ -201,6 +178,23 @@
free(CarModel);
CarModel = NULL;
}
+
+ if (CarModelPrev != NULL) {
+ if (CarModelPrev->body != NULL)
+ free(CarModelPrev->body);
+ if (CarModelPrev->carDesc != NULL)
+ free(CarModelPrev->carDesc);
+ if (CarModelPrev->carXY != NULL)
+ free(CarModelPrev->carXY);
+ free(CarModelPrev);
+ CarModelPrev = NULL;
+ }
+
+ CarModelPrev = (car_model *)malloc(sizeof(car_model));
+ CarModelPrev->body = NULL;
+ CarModelPrev->carDesc = NULL;
+ CarModelPrev->pointNum = 0;
+ CarModelPrev->carXY = (PointF *) malloc(sizeof(PointF) * pointNum);
CarModel = (car_model *)malloc(sizeof(car_model));
CarModel->basePoint.X = basePoint[0];
@@ -215,8 +209,22 @@
CarModel->left_rear_tire[1] = left_rear_tire[1];
CarModel->right_rear_tire[0] = right_rear_tire[0];
CarModel->right_rear_tire[1] = right_rear_tire[1];
- CarModel->pointNum = pointNum;
+ CarModel->bodyNum = bodyNum;
+ if (bodyNum == 0 || body == NULL) {
+ CarModel->bodyNum = 6;
+ CarModel->body = (int *) malloc(sizeof(int) * 6);
+ for (int i = 0; i < 6; ++i) {
+ CarModel->body[i] = i;
+ }
+ } else {
+ CarModel->body = (int *) malloc(sizeof(int) * CarModel->bodyNum);
+ for (int i = 0; i < CarModel->bodyNum; ++i) {
+ CarModel->body[i] = body[i];
+ }
+ }
+
+ CarModel->pointNum = pointNum;
CarModel->carDesc = (struct car_desc_ *)malloc(sizeof(struct car_desc_) * pointNum);
CarModel->carXY = (PointF *) malloc(sizeof(PointF) * pointNum);
@@ -264,8 +272,15 @@
err = true;
MA_SendExamStatus(0, -2);
}
- if (!err)
+ if (!err) {
+ if (!TestStart) {
+ ExamFaultList.clear();
+ examFaultIndex = 0;
+
+ TestStart = true;
+ }
MA_SendExamStatus(1, 0);
+ }
}
void UpdateRTKInfo(const rtk_info *s)
@@ -275,6 +290,8 @@
RtkBufferIn = (RtkBufferIn + 1) % RTK_BUFFER_SIZE;
if (RtkBufferNum < RTK_BUFFER_SIZE)
RtkBufferNum++;
+ } else {
+ return;
}
double speed;
@@ -289,9 +306,9 @@
RtkBuffer[index].mm, RtkBuffer[index].ss, RtkBuffer[index].dss);
brief.qf = RtkBuffer[index].qf;
- brief.map_id = -1;
+ brief.map_id = 863;
brief.move = move;
- brief.speed = speed;
+ brief.speed = speed * 3.6;
brief.heading = RtkBuffer[index].heading;
brief.main_ant[0] = RtkBuffer[index].x;
brief.main_ant[1] = RtkBuffer[index].y;
@@ -307,6 +324,12 @@
brief.right_rear_tire[0] = CarModel->right_rear_tire[0];
brief.right_rear_tire[1] = CarModel->right_rear_tire[1];
+ brief.bodyNum = CarModel->bodyNum;
+ brief.body = (int *) malloc(sizeof(int) * CarModel->bodyNum);
+ for (int i = 0; i < CarModel->bodyNum; ++i) {
+ brief.body[i] = CarModel->body[i];
+ }
+
brief.pointNum = CarModel->pointNum;
brief.point = (double *) malloc(CarModel->pointNum * 2 * sizeof(double));
for (int i = 0, j = 0; i < CarModel->pointNum; ++i) {
@@ -315,6 +338,13 @@
}
MA_SendCarPosition(&brief);
+
+ free(brief.body);
+ free(brief.point);
+
+ if (TestStart) {
+// TestParkBottom(ErrorList, 0, CarModel, CarModelPrev, speed, move);
+ }
}
}
@@ -345,6 +375,8 @@
// 濡傛灉涓�瀹氱殑鏃堕棿閮芥病鏈夋湁鏁堝畾浣嶏紝鍒犻櫎涔嬪墠鐨勫��
uint32_t tmDiff = CalcTimeDiff(&RtkBuffer[p1], &RtkBuffer[p2]);
+ DEBUG("tmDiff = %ld, p1 = %d p2 = %d dss = %d dss2 = %d", tmDiff, p1, p2, RtkBuffer[p1].dss, RtkBuffer[p2].dss);
+
if (tmDiff > D_SEC(5)) {
if (p1 != 0)
RtkBuffer[0] = RtkBuffer[p1];
@@ -357,7 +389,22 @@
main_ant_coord.X = RtkBuffer[p1].x;
main_ant_coord.Y = RtkBuffer[p1].y;
+ // 淇濆瓨涓婁竴涓溅韬疆寤撶偣
+ if (CarModelPrev->pointNum > 0) {
+ CarModelPrev->pointNum = CarModel->pointNum;
+ CarModelPrev->basePoint = CarModel->basePoint;
+ for (int i = 0; i < CarModel->pointNum; ++i) {
+ CarModelPrev->carXY[i] = CarModel->carXY[i];
+ }
+ }
UpdateCarBodyCoord(RtkBuffer[p1].heading, main_ant_coord, CarModel);
+ if (CarModelPrev->pointNum == 0) {
+ CarModelPrev->pointNum = CarModel->pointNum;
+ CarModelPrev->basePoint = CarModel->basePoint;
+ for (int i = 0; i < CarModel->pointNum; ++i) {
+ CarModelPrev->carXY[i] = CarModel->carXY[i];
+ }
+ }
// 璁＄畻閫熷害(绫�/绉�)銆佸墠杩涘悗閫�
double speed = sqrt(pow(RtkBuffer[p1].x - RtkBuffer[p2].x, 2) + pow(RtkBuffer[p1].y - RtkBuffer[p2].y, 2)) * 1000 /
@@ -505,6 +552,21 @@
}*/
}
+void AddExamFault(int wrong, const char *utc)
+{
+ if (!TestStart) return;
+
+ struct ExamFault fault;
+
+ fault.sn = examFaultIndex++;
+ strcpy(fault.utc, utc);
+ fault.wrong_id = wrong;
+
+ ExamFaultList.push_back(fault);
+
+ MA_SendExamWrong(ExamFaultList);
+}
+
car_model_cache_t *GetCarModelCache(int node)
{
return NULL;
@@ -515,8 +577,10 @@
* @param azimuth
* @param coord
*/
-static void UpdateCarBodyCoord(double azimuth, PointF main_ant, const car_model *carModel)
+static void UpdateCarBodyCoord(double azimuth, PointF main_ant, car_model *carModel)
{
+ carModel->basePoint = main_ant;
+
for (int i = 0; i < carModel->pointNum; ++i) {
double tx = main_ant.X + carModel->carDesc[i].distance*sin(toRadians(azimuth));
double ty = main_ant.Y + carModel->carDesc[i].distance*cos(toRadians(azimuth));
--
Gitblit v1.8.0