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 +++++++++++++++--------------
lib/src/main/cpp/master/comm_if.cpp | 4
lib/src/main/cpp/native-lib.cpp | 2
lib/src/main/cpp/rtk_module/rtk.cpp | 21 +++---
lib/src/main/cpp/rtk_module/virtual_rtk.cpp | 4 +
5 files changed, 64 insertions(+), 58 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;
}
}
diff --git a/lib/src/main/cpp/master/comm_if.cpp b/lib/src/main/cpp/master/comm_if.cpp
index c4bc105..3c227c9 100644
--- a/lib/src/main/cpp/master/comm_if.cpp
+++ b/lib/src/main/cpp/master/comm_if.cpp
@@ -50,8 +50,8 @@
#define ID_SM_RTCM_IND 0x000F
#define ID_SM_DEBUG_INFO 0x0010
#define ID_MS_FILE 0x8100
-#define ID_MS_READ_CARD 0x800F
-#define ID_SM_PUT_CARD 0x000F
+#define ID_MS_READ_CARD 0x8011
+#define ID_SM_PUT_CARD 0x0011
#define ID_MS_SYS_SHUTDOWN 0x8010
#define MA_OUT_GPS_BRIEF 0x0001
diff --git a/lib/src/main/cpp/native-lib.cpp b/lib/src/main/cpp/native-lib.cpp
index 0601d0b..9fb8def 100644
--- a/lib/src/main/cpp/native-lib.cpp
+++ b/lib/src/main/cpp/native-lib.cpp
@@ -245,7 +245,7 @@
InitPlatform(phone, RTK_PLATFORM_IP, RTK_PLATFORM_PORT);
AppTimer_add(SendBootIndicate, 500);
- InitVirtualDevice("192.168.3.52", 9001);
+ InitVirtualDevice("192.168.86.101", 9001);
}
static void SendBootIndicate(union sigval sig) {
diff --git a/lib/src/main/cpp/rtk_module/rtk.cpp b/lib/src/main/cpp/rtk_module/rtk.cpp
index fce0093..bb5e332 100644
--- a/lib/src/main/cpp/rtk_module/rtk.cpp
+++ b/lib/src/main/cpp/rtk_module/rtk.cpp
@@ -202,17 +202,18 @@
}*/
if (RxBufLen > 0) {
-// const uint8_t *ptr = parseGPS(RxBuf, RxBuf + RxBufLen);
-// if(ptr != RxBuf) {
-// memcpy(RxBuf, ptr, RxBufLen - (ptr - RxBuf));
-// RxBufLen -= ptr - RxBuf;
-// } else if(RxBufLen == PARSE_BUFF_SIZE) { //濉弧浜嗭紝涓旀病鏈変竴涓猏r锛岄兘鎶涘純
-// DEBUG("Parse GPS error");
-// RxBufLen = 0;
-// }
-
-
+#if 0
+ const uint8_t *ptr = parseGPS(RxBuf, RxBuf + RxBufLen);
+ if(ptr != RxBuf) {
+ memcpy(RxBuf, ptr, RxBufLen - (ptr - RxBuf));
+ RxBufLen -= ptr - RxBuf;
+ } else if(RxBufLen == PARSE_BUFF_SIZE) { //濉弧浜嗭紝涓旀病鏈変竴涓猏r锛岄兘鎶涘純
+ DEBUG("Parse GPS error");
+ RxBufLen = 0;
+ }
+#else
RxBufLen = 0; //PC妯℃嫙鐢ㄦ椂
+#endif
}
}
if (res == 0) {
diff --git a/lib/src/main/cpp/rtk_module/virtual_rtk.cpp b/lib/src/main/cpp/rtk_module/virtual_rtk.cpp
index 903255b..498b0c6 100644
--- a/lib/src/main/cpp/rtk_module/virtual_rtk.cpp
+++ b/lib/src/main/cpp/rtk_module/virtual_rtk.cpp
@@ -24,7 +24,7 @@
void InitVirtualDevice(const char *domain_name, int port)
{
- DEBUG("InitVirtualDevice");
+ DEBUG("InitVirtualDevice %s: %d", domain_name, port);
struct vSocket *ptr = (struct vSocket *)malloc(sizeof(struct vSocket));
@@ -57,6 +57,8 @@
if (fd > 0) {
DEBUG("铏氭嫙骞冲彴杩炴帴鎴愬姛");
+ } else {
+ DEBUG("铏氭嫙骞冲彴杩炴帴澶辫触");
}
while (fd > 0) {
--
Gitblit v1.8.0