From 9ca8ca86beb09186ff3aecefceeddaea80b2409f Mon Sep 17 00:00:00 2001
From: yy1717 <fctom1215@outlook.com>
Date: 星期三, 26 二月 2020 17:31:00 +0800
Subject: [PATCH] 更新地图
---
lib/src/main/cpp/driver_test.cpp | 91 +++++++++++++++++++++++----------------------
1 files changed, 47 insertions(+), 44 deletions(-)
diff --git a/lib/src/main/cpp/driver_test.cpp b/lib/src/main/cpp/driver_test.cpp
index f960aa6..9a183b8 100644
--- a/lib/src/main/cpp/driver_test.cpp
+++ b/lib/src/main/cpp/driver_test.cpp
@@ -291,23 +291,23 @@
DEBUG("鍔犲叆鐐�<%d> 璺濈 %f 瑙掑害 %f", i, CarModel->carDesc[i].distance, CarModel->carDesc[i].angle);
}
- CarModel->carDesc[0].distance = 0.2465;
- CarModel->carDesc[0].angle = 0;
-
- CarModel->carDesc[1].distance = 0.2635;
- CarModel->carDesc[1].angle = 20.7;
-
- CarModel->carDesc[2].distance = 0.14;
- CarModel->carDesc[2].angle = 138.9;
-
- CarModel->carDesc[3].distance = 0.1055;
- CarModel->carDesc[3].angle = 180.0;
-
- CarModel->carDesc[4].distance = 0.14;
- CarModel->carDesc[4].angle = 221.1;
-
- CarModel->carDesc[5].distance = 0.2635;
- CarModel->carDesc[5].angle = 339.3;
+// CarModel->carDesc[0].distance = 0.2465;
+// CarModel->carDesc[0].angle = 0;
+//
+// CarModel->carDesc[1].distance = 0.2635;
+// CarModel->carDesc[1].angle = 20.7;
+//
+// CarModel->carDesc[2].distance = 0.14;
+// CarModel->carDesc[2].angle = 138.9;
+//
+// CarModel->carDesc[3].distance = 0.1055;
+// CarModel->carDesc[3].angle = 180.0;
+//
+// CarModel->carDesc[4].distance = 0.14;
+// CarModel->carDesc[4].angle = 221.1;
+//
+// CarModel->carDesc[5].distance = 0.2635;
+// CarModel->carDesc[5].angle = 339.3;
DEBUG("SetCarMeasurePoint Calc Over");
}
@@ -362,10 +362,10 @@
if (start == 0) {
DEBUG("-------------缁撴潫鑰冭瘯----------------");
- CurrExamMapIndex = -1;
- TestStart = false;
- CommTestStart(false);
- MA_SendExamStatus(0, 0);
+// CurrExamMapIndex = -1;
+// TestStart = false;
+// CommTestStart(false);
+// MA_SendExamStatus(0, 0);
return;
}
@@ -397,7 +397,7 @@
if (map_id >= 0 && exam == 0) {
CurrExamMapIndex = map_id;
- CurrExamStatus = 0;
+ CurrExamStatus = 2;
}
}
@@ -495,6 +495,9 @@
if (CurrEnterMapIndex >= 0) {
DEBUG("鍙戦�佽繘鍏ュ満鍦版姤鍛� %d", GetMapId(CurrEnterMapIndex, MapList, MapNum));
MA_EnterMap(GetMapId(CurrEnterMapIndex, MapList, MapNum), 1);
+
+ CurrExamMapIndex = CurrEnterMapIndex;
+ CurrExamStatus = 2;
}
} else {
if (ExitMap(CarModel, CurrEnterMapIndex, MapList, MapNum)) {
@@ -510,7 +513,7 @@
if (CurrExamMapIndex >= 0) {
int mtype = GetMapType(CurrExamMapIndex, MapList, MapNum);
- if (CurrExamStatus == 0) {
+ if (CurrExamStatus == 2) {
DEBUG("CurrExamMapIndex %d mtype %d", GetMapId(CurrExamMapIndex, MapList, MapNum), mtype);
switch (mtype) {
@@ -544,9 +547,7 @@
break;
default:break;
}
-
- CurrExamStatus = 1;
- } else if (CurrExamStatus == 1) {
+ } else if (CurrExamStatus == 0) {
switch (mtype) {
case MAP_TYPE_PARK_BUTTOM:
CurrExamStatus = TestParkBottom(&MapList[CurrExamMapIndex].map,
@@ -564,12 +565,12 @@
CurrExamStatus = TestTurnA90(&MapList[CurrExamMapIndex].map, CarModel, CarModelPrev, azimuth, speed, move, &rtkTime);
break;
default:
- CurrExamStatus = -1;
+ CurrExamStatus = 1;
break;
}
}
- if (CurrExamStatus < 0) {
+ if (CurrExamStatus == 1) {
// 鏌愰」缁撴潫
//DEBUG("閫�鍑哄満鍦� %d", GetMapId(CurrExamMapIndex, MapList, MapNum));
//MA_SendDebugInfo("閫�鍑哄満鍦� %d", GetMapId(CurrExamMapIndex, MapList, MapNum));
@@ -707,7 +708,7 @@
spd = speed;
mov = move;
idx = p1;
- DEBUG("tmDiff = %ld speed = %f m/Sec move = %d", tmDiff, speed, move);
+// DEBUG("tmDiff = %ld speed = %f m/Sec move = %d", tmDiff, speed, move);
return true;
/* if (!TestStart) return;
@@ -836,7 +837,7 @@
// if (mapList[i].type == MAP_TYPE_CURVE) {
//
// }
- if (mapList[i].type == MAP_TYPE_PARK_BUTTOM /*|| mapList[i].type == MAP_TYPE_PART_EDGE*/) {
+ if (mapList[i].type == MAP_TYPE_PARK_BUTTOM) {
if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &mapList[i].map) == GM_Containment) {
Line enterLine1, enterLine2;
@@ -848,6 +849,16 @@
return i;
}
}
+// if (mapList[i].type == MAP_TYPE_PART_EDGE) {
+// if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &mapList[i].map) == GM_Containment) {
+// Line enterLine;
+//
+// MakeLine(&enterLine, &(mapList[i].map.point[0]), &(mapList[i].map.point[1]));
+//
+// if (DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], enterLine) > 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;
@@ -987,21 +998,13 @@
projectAngle = 360 - projectAngle;
}
- double tx = projectDistance*sin(toRadians(azimuth));
- double ty = projectDistance*cos(toRadians(azimuth));
+// double tx = projectDistance*sin(toRadians(azimuth));
+// double ty = projectDistance*cos(toRadians(azimuth));
- carModel->carXY[i].X = tx * cos(toRadians(projectAngle)) -
- ty*sin(toRadians(projectAngle)) + main_ant.X;
- carModel->carXY[i].Y = tx * sin(toRadians(projectAngle)) +
- ty * cos(toRadians(projectAngle)) + main_ant.Y;
-
-// double tx = carModel->carDesc[i].distance*sin(toRadians(azimuth));
-// double ty = carModel->carDesc[i].distance*cos(toRadians(azimuth));
-//
-// carModel->carXY[i].X = (tx)*cos(toRadians(carModel->carDesc[i].angle)) -
-// (ty)*sin(toRadians(carModel->carDesc[i].angle)) + main_ant.X;
-// carModel->carXY[i].Y = (tx)*sin(toRadians(carModel->carDesc[i].angle)) +
-// (ty)*cos(toRadians(carModel->carDesc[i].angle)) + main_ant.Y;
+ carModel->carXY[i].X = projectDistance*sin(toRadians(azimuth)) * cos(toRadians(projectAngle)) -
+ projectDistance*cos(toRadians(azimuth)) * sin(toRadians(projectAngle)) + main_ant.X;
+ carModel->carXY[i].Y = projectDistance*sin(toRadians(azimuth)) * sin(toRadians(projectAngle)) +
+ projectDistance*cos(toRadians(azimuth)) * cos(toRadians(projectAngle)) + main_ant.Y;
}
}
--
Gitblit v1.8.0