From dc2a01d4764efd33a23afcaf4f1d7543dc35c4fa Mon Sep 17 00:00:00 2001
From: fctom1215 <fctom1215@outlook.com>
Date: 星期三, 19 二月 2020 15:32:57 +0800
Subject: [PATCH] 上坡地图修正
---
lib/src/main/cpp/driver_test.cpp | 161 ++++++++++++++++++++++++++++++++++++-----------------
1 files changed, 108 insertions(+), 53 deletions(-)
diff --git a/lib/src/main/cpp/driver_test.cpp b/lib/src/main/cpp/driver_test.cpp
index ca588b9..58f861b 100644
--- a/lib/src/main/cpp/driver_test.cpp
+++ b/lib/src/main/cpp/driver_test.cpp
@@ -26,6 +26,7 @@
#include "test_items/stop_and_start.h"
#include "master/comm_if.h"
#include "utils/xconvert.h"
+#include "test_items/comm_test.h"
#define DEBUG(fmt, args...) LOGD("<driver_test> <%s>: " fmt, __func__, ##args)
@@ -87,9 +88,14 @@
} SensorConfig[32];
static int SensorNum = 0;
+static int SensorValidLevel;
+
#define MOV_AVG_SIZE 1
#define RTK_BUFFER_SIZE 100
#define CAR_MODEL_CACHE_SIZE 10
+
+static pthread_mutex_t rtk_clock_mutex = PTHREAD_MUTEX_INITIALIZER;
+struct RtkTime rtkClock;
static rtk_info *RtkBuffer = NULL;
static int RtkBufferNum = 0, RtkBufferIn = 0;
@@ -106,14 +112,20 @@
void DriverTestInit(void)
{
+ pthread_mutex_init(&rtk_clock_mutex, NULL);
+ memset(&rtkClock, 0, sizeof(rtkClock));
+
TestStart = false;
memset(&MapList, 0, sizeof(MapList));
MapNum = 0;
CarModel = NULL;
CarModelPrev = NULL;
+ CommTestInit();
SensorNum = 0;
memset(SensorConfig, 0, sizeof(SensorConfig));
+
+ SensorValidLevel = 0;
RtkBuffer = (rtk_info *) malloc(RTK_BUFFER_SIZE * sizeof(rtk_info));
RtkBufferNum = RtkBufferIn = 0;
@@ -299,22 +311,43 @@
void SetSensorCfg(int (*sensor)[3], int sensorNum)
{
- DEBUG("SetSensorCfg sensorNum %d", sensorNum);
+ DEBUG("鍔犲叆浼犳劅鍣ㄩ厤缃� sensorNum %d", sensorNum);
+ SensorValidLevel = 0;
SensorNum = sensorNum;
for (int i = 0; i < sensorNum; ++i) {
SensorConfig[i].gpioId = sensor[i][0];
SensorConfig[i].funId = sensor[i][1];
SensorConfig[i].validLvl = sensor[i][2];
+
+ if (sensor[i][2] != 0) {
+ SensorValidLevel |= BV(i);
+ }
}
}
-void GetFuncGpio(int func, int &gpio, int &lvl)
+int GetSensorValidLevel(void)
{
+ return SensorValidLevel;
+}
+
+void GetSensorCfg(int gpio, int &func, bool &lvl)
+{
+ for (int i = 0; i < SensorNum; ++i) {
+ if (SensorConfig[i].gpioId == gpio) {
+ func = SensorConfig[i].funId;
+ lvl = SensorConfig[i].validLvl == 0 ? false : true;
+ }
+ }
+}
+
+void FindSensorCfg(int func, int &gpio, bool &lvl)
+{
+ gpio = -1;
for (int i = 0; i < SensorNum; ++i) {
if (SensorConfig[i].funId == func) {
gpio = SensorConfig[i].gpioId;
- lvl = SensorConfig[i].validLvl;
+ lvl = SensorConfig[i].validLvl == 0 ? false : true;
}
}
}
@@ -326,6 +359,7 @@
DEBUG("StartDriverExam %d", start);
if (start == 0) {
TestStart = false;
+ CommTestStart(false);
MA_SendExamStatus(0, 0);
return;
}
@@ -344,13 +378,31 @@
examFaultIndex = 0;
TestStart = true;
+ CommTestStart(true);
}
MA_SendExamStatus(1, 0);
}
}
+void GetRtkClock(struct RtkTime *s)
+{
+ pthread_mutex_lock(&rtk_clock_mutex);
+ *s = rtkClock;
+ pthread_mutex_unlock(&rtk_clock_mutex);
+}
+
void UpdateRTKInfo(const rtk_info *s)
{
+ pthread_mutex_lock(&rtk_clock_mutex);
+ rtkClock.YY = s->YY;
+ rtkClock.MM = s->MM;
+ rtkClock.DD = s->DD;
+ rtkClock.hh = s->hh;
+ rtkClock.mm = s->mm;
+ rtkClock.ss = s->ss;
+ rtkClock.mss = s->dss;
+ pthread_mutex_unlock(&rtk_clock_mutex);
+
if (s->qf == 3) {
RtkBuffer[RtkBufferIn] = *s;
RtkBufferIn = (RtkBufferIn + 1) % RTK_BUFFER_SIZE;
@@ -372,7 +424,7 @@
RtkBuffer[index].mm, RtkBuffer[index].ss, RtkBuffer[index].dss);
brief.qf = RtkBuffer[index].qf;
- brief.map_id = 866;//GetMapId(CurrExamMapIndex, MapList, MapNum);
+ brief.map_id = 863;//GetMapId(CurrExamMapIndex, MapList, MapNum);
brief.move = move;
brief.speed = speed * 3.6;
brief.heading = RtkBuffer[index].heading;
@@ -431,7 +483,7 @@
DEBUG("杩涘叆鍊掕溅鍏ュ簱鍦哄湴");
MA_SendDebugInfo("杩涘叆鍊掕溅鍏ュ簱鍦哄湴 %d", GetMapId(CurrExamMapIndex, MapList, MapNum));
- StartParkBottom();
+ StartParkBottom(move, &rtkTime);
CurrExamStatus = 0;
break;
case MAP_TYPE_STOP_START:
@@ -465,7 +517,7 @@
int mtype = GetMapType(CurrExamMapIndex, MapList, MapNum);
switch (mtype) {
case MAP_TYPE_PARK_BUTTOM:
- CurrExamStatus = TestParkBottom(ErrorList, &MapList[CurrExamMapIndex].map,
+ CurrExamStatus = TestParkBottom(&MapList[CurrExamMapIndex].map,
CarModel, CarModelPrev, speed, move, &rtkTime);
break;
case MAP_TYPE_STOP_START:
@@ -712,44 +764,46 @@
{
// 杞︾殑鏈�鍓嶇偣鏄惁杩涘叆鍦板浘
for (int i = 0; i < mapNum && car != NULL; ++i) {
- /*if (mapList[i].type == MAP_TYPE_STOP_START) {
- // 鏋勯�犺櫄鎷熺殑宸︿笂瑙掔偣
- double x9, y9, xo, yo;
-
- bool enter = false;
-
- 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;
-
- Polygon map;
-
- map.num = 4;
- map.point = (PointF *) malloc(map.num * sizeof(PointF));
-
- map.point[0] = mapList[i].map.point[0];
- map.point[1] = mapList[i].map.point[8];
- map.point[2] = mapList[i].map.point[7];
- map.point[3].X = x9;
- map.point[3].Y = y9;
-
- if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &map) == GM_Containment) {
- Line enterLine1;
-
- MakeLine(&enterLine1, &(map.point[0]), &(map.point[3]));
-
- if (DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], enterLine1) > 0.1)
- enter = true;
- }
-
- free(map.point);
-
- if (enter) return i;
- } else if (mapList[i].type == MAP_TYPE_CURVE) {
-
- } else if (mapList[i].type == MAP_TYPE_PARK_BUTTOM || mapList[i].type == MAP_TYPE_PART_EDGE) {
+// if (mapList[i].type == MAP_TYPE_STOP_START) {
+// // 鏋勯�犺櫄鎷熺殑宸︿笂瑙掔偣
+// double x9, y9, xo, yo;
+//
+// bool enter = false;
+//
+// 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;
+//
+// Polygon map;
+//
+// map.num = 4;
+// map.point = (PointF *) malloc(map.num * sizeof(PointF));
+//
+// map.point[0] = mapList[i].map.point[0];
+// map.point[1] = mapList[i].map.point[8];
+// map.point[2] = mapList[i].map.point[7];
+// map.point[3].X = x9;
+// map.point[3].Y = y9;
+//
+// if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &map) == GM_Containment) {
+// Line enterLine1;
+//
+// MakeLine(&enterLine1, &(map.point[0]), &(map.point[3]));
+//
+// if (DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], enterLine1) > 0.1)
+// enter = true;
+// }
+//
+// free(map.point);
+//
+// if (enter) return i;
+// }
+// if (mapList[i].type == MAP_TYPE_CURVE) {
+//
+// }
+ if (mapList[i].type == MAP_TYPE_PARK_BUTTOM /*|| mapList[i].type == MAP_TYPE_PART_EDGE*/) {
if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &mapList[i].map) == GM_Containment) {
Line enterLine1, enterLine2;
@@ -760,16 +814,17 @@
DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], enterLine2) > 0.1)
return i;
}
- } else */if (mapList[i].type == MAP_TYPE_TURN_90) {
- if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &mapList[i].map) == GM_Containment) {
- Line enterLine1;
-
- MakeLine(&enterLine1, &(mapList[i].map.point[0]), &(mapList[i].map.point[1]));
-
- if (DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], enterLine1) > 0.1)
- return i;
- }
}
+// if (mapList[i].type == MAP_TYPE_TURN_90) {
+// if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &mapList[i].map) == GM_Containment) {
+// Line enterLine1;
+//
+// MakeLine(&enterLine1, &(mapList[i].map.point[0]), &(mapList[i].map.point[1]));
+//
+// if (DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], enterLine1) > 0.1)
+// return i;
+// }
+// }
}
return -1;
}
--
Gitblit v1.8.0