From eb645b6bf08ec50459c737821e63ee20dea4486d Mon Sep 17 00:00:00 2001
From: yy1717 <fctom1215@outlook.com>
Date: 星期三, 22 一月 2020 13:24:07 +0800
Subject: [PATCH] updatecrrDOC1
---
lib/src/main/cpp/test_items/park_bottom.cpp | 21 ++++++----
lib/src/main/cpp/driver_test.cpp | 56 +++++++++++++++++++---------
lib/src/main/cpp/rtk_platform/platform.cpp | 27 +++++++++++++
3 files changed, 78 insertions(+), 26 deletions(-)
diff --git a/lib/src/main/cpp/driver_test.cpp b/lib/src/main/cpp/driver_test.cpp
index d2d2d44..e109bb3 100644
--- a/lib/src/main/cpp/driver_test.cpp
+++ b/lib/src/main/cpp/driver_test.cpp
@@ -296,6 +296,11 @@
bool err = false;
DEBUG("StartDriverExam %d", start);
+ if (start == 0) {
+ TestStart = false;
+ MA_SendExamStatus(0, 0);
+ return;
+ }
if (MapNum == 0) {
err = true;
@@ -318,7 +323,7 @@
void UpdateRTKInfo(const rtk_info *s)
{
- if (s->qf > 0) {
+ if (s->qf == 3) {
RtkBuffer[RtkBufferIn] = *s;
RtkBufferIn = (RtkBufferIn + 1) % RTK_BUFFER_SIZE;
if (RtkBufferNum < RTK_BUFFER_SIZE)
@@ -434,6 +439,7 @@
}
}
if (CurrExamStatus != 0) {
+ // 鏌愰」缁撴潫锛�
if (ExitMap(CarModel, CurrExamMapIndex, MapList, MapNum)) {
DEBUG("閫�鍑哄満鍦� %d", GetMapId(CurrExamMapIndex, MapList, MapNum));
MA_SendDebugInfo("閫�鍑哄満鍦� %d", GetMapId(CurrExamMapIndex, MapList, MapNum));
@@ -458,18 +464,28 @@
static bool UpdateCarCoord(double &spd, int &mov, int &idx)
{
+ long tmDiff;
+
if (CarModel == NULL)
return false;
- if (RtkBufferNum < 3)
+ if (RtkBufferNum < 2)
return false;
- int p1 = ((RtkBufferIn-1)+RTK_BUFFER_SIZE)%RTK_BUFFER_SIZE;
- int p2 = ((RtkBufferIn-2)+RTK_BUFFER_SIZE)%RTK_BUFFER_SIZE;
- int p3 = ((RtkBufferIn-3)+RTK_BUFFER_SIZE)%RTK_BUFFER_SIZE;
+ int p1 = ((RtkBufferIn-1)+RTK_BUFFER_SIZE)%RTK_BUFFER_SIZE; // 鏈�杩戦噰鏍峰��
+ int p2 = ((RtkBufferIn-2)+RTK_BUFFER_SIZE)%RTK_BUFFER_SIZE; // 鎵惧埌1绉掑墠鐨勯噰鏍峰��
+ int pn = 0;
+
+ for (pn = 1; pn < RtkBufferNum; ++pn) {
+ p2 = ((RtkBufferIn-1-pn)+RTK_BUFFER_SIZE)%RTK_BUFFER_SIZE;
+ if ((tmDiff = CalcTimeDiff(&RtkBuffer[p1], &RtkBuffer[p2])) >= D_SEC(1)) break;
+ }
+
+ if (pn == RtkBufferNum)
+ return false;
// 濡傛灉涓�瀹氱殑鏃堕棿閮芥病鏈夋湁鏁堝畾浣嶏紝鍒犻櫎涔嬪墠鐨勫��
- uint32_t tmDiff = CalcTimeDiff(&RtkBuffer[p1], &RtkBuffer[p2]);
+ /*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);
@@ -478,7 +494,7 @@
RtkBuffer[0] = RtkBuffer[p1];
RtkBufferIn = RtkBufferNum = 1;
return false;
- }
+ }*/
// 璁＄畻杞﹁締杞粨鐐�
PointF main_ant_coord;
@@ -562,7 +578,7 @@
spd = speed;
mov = move;
idx = p1;
- DEBUG("speed = %f m/Sec move = %d", speed, move);
+ DEBUG("tmDiff = %ld speed = %f m/Sec move = %d", tmDiff, speed, move);
return true;
/* if (!TestStart) return;
@@ -674,23 +690,27 @@
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) {
- if (IntersectionOf(car->carXY[car->axial[AXIAL_REAR]], &mapList[index].map) == GM_None) {
- Line enterLine1, enterLine2;
+ // 鍏ㄨ溅閮介渶涓嶅湪鍦板浘涓�
+ Polygon carBody;
- 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;
+ 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);
}
- return true;
+ return ret;
}
static int GetMapId(int index, const struct map_list *mapList, int mapNum)
diff --git a/lib/src/main/cpp/rtk_platform/platform.cpp b/lib/src/main/cpp/rtk_platform/platform.cpp
index 8fd338c..833dd63 100644
--- a/lib/src/main/cpp/rtk_platform/platform.cpp
+++ b/lib/src/main/cpp/rtk_platform/platform.cpp
@@ -53,6 +53,13 @@
uint32_t downloadRtk : 1;
} platformStatus;
+static struct event_queue_t {
+ uint32_t event;
+ int length;
+ void *data;
+ struct event_queue_t *next;
+} *eventQueue;
+
struct platformSocket exceptSocket, currSocket;
static uint32_t eventMask;
static void *eventData[32];
@@ -82,6 +89,24 @@
static void TriggerHeartbeat(union sigval sig);
static void RequestRtkNoResp(union sigval sig);
+void AddEvnet(uint32_t event, const uint8_t *data, int length)
+{
+ struct event_queue_t *p = eventQueue;
+
+ while (p != NULL) {
+ struct event_queue_t *nw = (struct event_queue_t *)malloc(sizeof(struct event_queue_t));
+
+ nw->event = event;
+ nw->next = NULL;
+ nw->data = NULL;
+ nw->length = 0;
+ if (data != NULL && length > 0) {
+ nw->data = malloc(length);
+ nw->length = length;
+ }
+ }
+}
+
void InitPlatform(const uint8_t *phone, const char *domain_name, int port)
{
DEBUG("InitPlatform");
@@ -89,6 +114,8 @@
pthread_mutex_init(&platform_tx_mutex, NULL);
pthread_mutex_init(&events_mutex, NULL);
+ eventQueue = NULL;
+
eventMask = 0;
memset(eventData, 0, sizeof(eventData));
memset(eventDataLength, 0, sizeof(eventDataLength));
diff --git a/lib/src/main/cpp/test_items/park_bottom.cpp b/lib/src/main/cpp/test_items/park_bottom.cpp
index 9c4d4ba..2b87406 100644
--- a/lib/src/main/cpp/test_items/park_bottom.cpp
+++ b/lib/src/main/cpp/test_items/park_bottom.cpp
@@ -83,6 +83,7 @@
// 鏄惁瓒呮椂
if (parkTimeout) {
// 涓嶅悎鏍硷細鍔ㄤ綔瓒呮椂
+ DEBUG("涓嶅悎鏍硷細鍔ㄤ綔瓒呮椂");
err.push_back(10);
AddExamFault(10, rtkTime);
status = -1;
@@ -90,6 +91,7 @@
// 鏄惁鍘嬬嚎
if (CrashRedLine(map, car)) {
// 涓嶅悎鏍硷細杞﹁韩鍑虹嚎
+ DEBUG("涓嶅悎鏍硷細杞﹁韩鍑虹嚎");
err.push_back(7);
AddExamFault(7, rtkTime);
status = -1;
@@ -110,6 +112,7 @@
if (run_status > 0) {
if (stopCar2S && currTarget != FIRST_TOUCH_CTRL_LINE) {
// 鎵�5鍒嗭細涓�斿仠杞﹁秴杩�2绉�
+ DEBUG("鎵�5鍒嗭細涓�斿仠杞﹁秴杩�2绉�");
err.push_back(11);
AddExamFault(11, rtkTime);
}
@@ -127,6 +130,7 @@
DEBUG("鏉ラ亾鍦虹帺鐨�");
} else {
// 涓嶅悎鏍硷細鏈寜瑙勫畾绾胯矾琛岄┒锛堢洿鎺ヨ窇鍑烘祴璇曞尯浜嗭級
+ DEBUG("涓嶅悎鏍硷細鏈寜瑙勫畾绾胯矾琛岄┒锛堢洿鎺ヨ窇鍑烘祴璇曞尯浜嗭級");
err.push_back(6);
AddExamFault(6, rtkTime);
status = -1;
@@ -166,6 +170,7 @@
status = 1;
} else if (the_ctrl_line_crossed != 0) {
// 涓嶅悎鏍硷細鏈寜瑙勫畾绾胯矾琛岄┒锛堟湭鍥炲埌璧峰鐐癸級
+ DEBUG("涓嶅悎鏍硷細鏈寜瑙勫畾绾胯矾琛岄┒锛堟湭鍥炲埌璧峰鐐癸級");
err.push_back(6);
AddExamFault(6, rtkTime);
status = -1;
@@ -187,6 +192,7 @@
if (first_ctrl_line_id > 0 && first_ctrl_line_id == the_ctrl_line_crossed) {
// 涓嶅悎鏍硷細鏈寜瑙勫畾绾胯矾琛岄┒锛堣瘯鍥惧仛2娆″悓鏂瑰悜鐨勫�掑簱锛�
+ DEBUG("涓嶅悎鏍硷細鏈寜瑙勫畾绾胯矾琛岄┒锛堣瘯鍥惧仛2娆″悓鏂瑰悜鐨勫�掑簱锛�");
err.push_back(6);
AddExamFault(6, rtkTime);
status = -1;
@@ -208,6 +214,7 @@
DEBUG("绗簩娆″�掑簱");
} else if (currTarget != THIRD_TOUCH_CTRL_LINE) {
// 涓嶅悎鏍硷細鍊掕溅鍓嶏紝2鍓嶈疆娌￠┒杩囨帶鍒剁嚎
+ DEBUG("涓嶅悎鏍硷細鍊掕溅鍓嶏紝2鍓嶈疆娌￠┒杩囨帶鍒剁嚎");
err.push_back(9);
AddExamFault(9, rtkTime);
status = -1;
@@ -227,6 +234,7 @@
if (run_status < 0) {
if (stopCar2S) {
// 鎵�5鍒嗭細涓�斿仠杞�
+ DEBUG("鎵�5鍒嗭細涓�斿仠杞�");
err.push_back(11);
AddExamFault(11, rtkTime);
}
@@ -245,6 +253,7 @@
if (carStopEvent) {
if (!carParkSuccess) {
// 涓嶅悎鏍硷細鍊掑簱涓嶅叆
+ DEBUG("涓嶅悎鏍硷細鍊掑簱涓嶅叆");
err.push_back(8);
AddExamFault(8, rtkTime);
status = -1;
@@ -301,14 +310,12 @@
p2.Y = (prev_car->carXY[car->left_front_tire[TIRE_OUTSIDE]].Y + prev_car->carXY[car->left_front_tire[TIRE_INSIDE]].Y) / 2;
MakeLine(&track1, &p1, &p2);
if (IntersectionOf(track1, leftCtrlLine) == GM_Intersection &&
- IntersectionOfLine(map->point[0], map->point[1], p1) == -1
- /*IntersectionOf(p1, map) == GM_None*/) {
+ IntersectionOfLine(map->point[0], map->point[1], p1) == -1) {
leftTireCrossLeftLine = true;
DEBUG("宸﹁疆璺ㄨ秺宸︽帶鍒剁嚎");
}
if (IntersectionOf(track1, rightCtrlLine) == GM_Intersection &&
- IntersectionOfLine(map->point[6], map->point[7], p1) == -1
- /*IntersectionOf(p1, map) == GM_None*/) {
+ IntersectionOfLine(map->point[6], map->point[7], p1) == -1) {
leftTireCrossRightLine = true;
DEBUG("宸﹁疆璺ㄨ秺鍙虫帶鍒剁嚎");
}
@@ -319,14 +326,12 @@
p2.Y = (prev_car->carXY[car->right_front_tire[TIRE_OUTSIDE]].Y + prev_car->carXY[car->right_front_tire[TIRE_INSIDE]].Y) / 2;
MakeLine(&track1, &p1, &p2);
if (IntersectionOf(track1, leftCtrlLine) == GM_Intersection &&
- IntersectionOfLine(map->point[0], map->point[1], p1) == -1
- /*IntersectionOf(p1, map) == GM_None*/) {
+ IntersectionOfLine(map->point[0], map->point[1], p1) == -1) {
rightTireCrossLeftLine = true;
DEBUG("鍙宠疆璺ㄨ秺宸︽帶鍒剁嚎");
}
if (IntersectionOf(track1, rightCtrlLine) == GM_Intersection &&
- IntersectionOfLine(map->point[6], map->point[7], p1) == -1
- /*IntersectionOf(p1, map) == GM_None*/) {
+ IntersectionOfLine(map->point[6], map->point[7], p1) == -1) {
rightTireCrossRightLine = true;
DEBUG("鍙宠疆璺ㄨ秺鍙虫帶鍒剁嚎");
}
--
Gitblit v1.8.0