From 7b6dd5603277ac8fddba735c456f99d4c07e2e9b Mon Sep 17 00:00:00 2001
From: fctom1215 <fctom1215@outlook.com>
Date: 星期三, 12 二月 2020 17:43:36 +0800
Subject: [PATCH] 修改了上坡起步。
---
lib/src/main/cpp/driver_test.cpp | 61 ++++++++++++++++++++++++++++--
1 files changed, 56 insertions(+), 5 deletions(-)
diff --git a/lib/src/main/cpp/driver_test.cpp b/lib/src/main/cpp/driver_test.cpp
index 3608fbf..64c63c9 100644
--- a/lib/src/main/cpp/driver_test.cpp
+++ b/lib/src/main/cpp/driver_test.cpp
@@ -148,8 +148,6 @@
MapList[MapNum].id = id;
- if (type == 1) type = 3; //////////////////////////////////////
-
MapList[MapNum].type = type;
MapList[MapNum].map.num = pointNum;
@@ -347,7 +345,7 @@
RtkBuffer[index].mm, RtkBuffer[index].ss, RtkBuffer[index].dss);
brief.qf = RtkBuffer[index].qf;
- brief.map_id = 863;//GetMapId(CurrExamMapIndex, MapList, MapNum);
+ brief.map_id = 865;//GetMapId(CurrExamMapIndex, MapList, MapNum);
brief.move = move;
brief.speed = speed * 3.6;
brief.heading = RtkBuffer[index].heading;
@@ -384,6 +382,7 @@
free(brief.point);
struct RtkTime rtkTime;
+ double azimuth = RtkBuffer[index].heading;
rtkTime.YY = RtkBuffer[index].YY;
rtkTime.MM = RtkBuffer[index].MM;
@@ -409,6 +408,10 @@
CurrExamStatus = 0;
break;
case MAP_TYPE_STOP_START:
+ DEBUG("杩涘叆涓婂潯璧锋鍦哄湴");
+ MA_SendDebugInfo("杩涘叆涓婂潯璧锋鍦哄湴 %d", GetMapId(CurrExamMapIndex, MapList, MapNum));
+ StartSAS(move, &rtkTime);
+ CurrExamStatus = 0;
break;
case MAP_TYPE_PART_EDGE:
DEBUG("杩涘叆渚ф柟浣嶅仠杞﹀満鍦�");
@@ -420,6 +423,10 @@
case MAP_TYPE_CURVE:
break;
case MAP_TYPE_TURN_90:
+ DEBUG("杩涘叆鐩磋杞集鍦哄湴");
+ MA_SendDebugInfo("杩涘叆鐩磋杞集鍦哄湴 %d", GetMapId(CurrExamMapIndex, MapList, MapNum));
+ StartTurnA90(move, azimuth, &rtkTime);
+ CurrExamStatus = 0;
break;
default:break;
}
@@ -435,6 +442,7 @@
CarModel, CarModelPrev, speed, move, &rtkTime);
break;
case MAP_TYPE_STOP_START:
+ CurrExamStatus = TestSAS(&MapList[CurrExamMapIndex].map, CarModel, CarModelPrev, speed, move, &rtkTime);
break;
case MAP_TYPE_PART_EDGE:
CurrExamStatus = TestParkEdge(&MapList[CurrExamMapIndex].map, CarModel, CarModelPrev, speed, move, &rtkTime);
@@ -442,6 +450,7 @@
case MAP_TYPE_CURVE:
break;
case MAP_TYPE_TURN_90:
+ CurrExamStatus = TestTurnA90(&MapList[CurrExamMapIndex].map, CarModel, CarModelPrev, azimuth, speed, move, &rtkTime);
break;
default:
break;
@@ -678,8 +687,41 @@
// 杞︾殑鏈�鍓嶇偣鏄惁杩涘叆鍦板浘
for (int i = 0; i < mapNum && car != NULL; ++i) {
if (mapList[i].type == MAP_TYPE_STOP_START) {
+ // 鏋勯�犺櫄鎷熺殑宸︿笂瑙掔偣
+ double x9, y9, xo, yo;
- } else if (mapList[i].type == MAP_TYPE_CURVE) {
+ 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 (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &mapList[i].map) == GM_Containment) {
@@ -692,7 +734,16 @@
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;
+ }
+ }*/
}
return -1;
}
--
Gitblit v1.8.0