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/driver_test.cpp | 56 ++++++++++++++++++++++++++++++++++++++------------------
1 files changed, 38 insertions(+), 18 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)
--
Gitblit v1.8.0