From 4053e385bb8dc70ad8c8d12eeaec9c10ee87fcb9 Mon Sep 17 00:00:00 2001
From: yy1717 <fctom1215@outlook.com>
Date: 星期一, 16 三月 2020 18:06:07 +0800
Subject: [PATCH] 更新地图
---
lib/src/main/cpp/driver_test.cpp | 281 ++++++++++++++++++++++++++++------------------
lib/src/main/cpp/master/comm_if.cpp | 66 +++++++++++
Documents/模拟驾考APP内部通讯.docx | 0
lib/src/main/cpp/rtk_module/rtk.cpp | 2
lib/src/main/cpp/driver_test.h | 4
5 files changed, 241 insertions(+), 112 deletions(-)
diff --git "a/Documents/\346\250\241\346\213\237\351\251\276\350\200\203APP\345\206\205\351\203\250\351\200\232\350\256\257.docx" "b/Documents/\346\250\241\346\213\237\351\251\276\350\200\203APP\345\206\205\351\203\250\351\200\232\350\256\257.docx"
index 40407a1..adc7ee2 100644
--- "a/Documents/\346\250\241\346\213\237\351\251\276\350\200\203APP\345\206\205\351\203\250\351\200\232\350\256\257.docx"
+++ "b/Documents/\346\250\241\346\213\237\351\251\276\350\200\203APP\345\206\205\351\203\250\351\200\232\350\256\257.docx"
Binary files differ
diff --git a/lib/src/main/cpp/driver_test.cpp b/lib/src/main/cpp/driver_test.cpp
index 8d94ecf..f3000a4 100644
--- a/lib/src/main/cpp/driver_test.cpp
+++ b/lib/src/main/cpp/driver_test.cpp
@@ -11,6 +11,10 @@
#include <pthread.h>
#include <cstring>
#include <vector>
+#include <list>
+#include <numeric>
+#include <algorithm>
+
#include "driver_test.h"
#include "defs.h"
#include "Geometry.h"
@@ -84,6 +88,43 @@
Polygon map2;
} MapList[MAP_LIST_SIZE];
+struct trigger_line {
+ int mapId;
+ Line tirgger;
+};
+
+struct road_exam_parent_map {
+ int id;
+ int type;
+ Polygon map;
+ int redLineNum;
+ Polygon *redLine;
+ int greenLineNum;
+ Polygon *greenLine;
+ int redAreaNum;
+ Polygon *redArea;
+ int triggerLineNum;
+ struct trigger_line *triggerLine;
+};
+
+static struct road_exam_parent_map * RoadParentMap;
+
+struct road_exam_son_map {
+ int id;
+ int type;
+ char tts[512];
+ Line triggerLine;
+ Line controlLine;
+ Line targetLine;
+ int stop_flag;
+
+ int redLineNum;
+ Polygon *redLine;
+};
+
+typedef list<struct road_exam_son_map *> LIST_ROAD_SON_MAP;
+static LIST_ROAD_SON_MAP RoadSonMapList;
+
static int MapNum = 0;
static int CurrExamMapIndex = -1;
static int CurrEnterMapIndex = -1;
@@ -92,7 +133,10 @@
static int exam_dummy_light;
static car_model *CarModel = NULL;
-static car_model *CarModelPrev = NULL;
+
+typedef list<car_model *> LIST_CAR_MODEL;
+
+static LIST_CAR_MODEL CarModelList;
static struct dummy_light_exam *DummyLightContent;
static int DummyLightContentSize;
@@ -128,7 +172,10 @@
memset(&MapList, 0, sizeof(MapList));
MapNum = 0;
CarModel = NULL;
- CarModelPrev = NULL;
+ CarModelList.clear();
+
+ RoadParentMap = NULL;
+ RoadSonMapList.clear();
CarSensorInit();
@@ -205,6 +252,8 @@
DEBUG("AddMap num %d", MapNum);
}
+
+
void SetCarMeasurePoint(double *basePoint, int *axial, int *left_front_tire,
int *right_front_tire, int *left_rear_tire, int *right_rear_tire,
int *body, int bodyNum,double (*point)[2], int pointNum, double antPitch)
@@ -223,24 +272,6 @@
free(CarModel);
CarModel = NULL;
}
-
- if (CarModelPrev != NULL) {
- if (CarModelPrev->body != NULL)
- free(CarModelPrev->body);
- if (CarModelPrev->carDesc != NULL)
- free(CarModelPrev->carDesc);
- if (CarModelPrev->carXY != NULL)
- free(CarModelPrev->carXY);
- free(CarModelPrev);
- CarModelPrev = NULL;
- }
-
- // 浠呭垎閰峜arXY, body鍜宑arDesc鍧囨病鏈�
- CarModelPrev = (car_model *)malloc(sizeof(car_model));
- CarModelPrev->body = NULL;
- CarModelPrev->carDesc = NULL;
- CarModelPrev->pointNum = 0;
- CarModelPrev->carXY = (PointF *) malloc(sizeof(PointF) * pointNum);
CarModel = (car_model *)malloc(sizeof(car_model));
CarModel->basePoint.X = basePoint[0];
@@ -623,19 +654,19 @@
switch (mtype) {
case MAP_TYPE_PARK_BUTTOM:
testing = TestParkBottom(&MapList[CurrExamMapIndex].map,
- CarModel, CarModelPrev, speed, move, rtkTime);
+ CarModel, NULL, speed, move, rtkTime);
break;
case MAP_TYPE_STOP_START:
- testing = TestSAS(&MapList[CurrExamMapIndex].map, CarModel, CarModelPrev, speed, move, rtkTime);
+ testing = TestSAS(&MapList[CurrExamMapIndex].map, CarModel, NULL, speed, move, rtkTime);
break;
case MAP_TYPE_PART_EDGE:
- testing = TestParkEdge(&MapList[CurrExamMapIndex].map, CarModel, CarModelPrev, speed, move, rtkTime);
+ testing = TestParkEdge(&MapList[CurrExamMapIndex].map, CarModel, NULL, speed, move, rtkTime);
break;
case MAP_TYPE_CURVE:
- testing = TestDrivingCurve(&MapList[CurrExamMapIndex].map, &MapList[CurrExamMapIndex].map2, CarModel, CarModelPrev, speed, move, rtkTime);
+ testing = TestDrivingCurve(&MapList[CurrExamMapIndex].map, &MapList[CurrExamMapIndex].map2, CarModel, NULL, speed, move, rtkTime);
break;
case MAP_TYPE_TURN_90:
- testing = TestTurnA90(&MapList[CurrExamMapIndex].map, CarModel, CarModelPrev, azimuth, speed, move, rtkTime);
+ testing = TestTurnA90(&MapList[CurrExamMapIndex].map, CarModel, NULL, azimuth, speed, move, rtkTime);
break;
default:
break;
@@ -715,21 +746,51 @@
main_ant_coord.X = RtkBuffer[p1].x;
main_ant_coord.Y = RtkBuffer[p1].y;
- // 淇濆瓨涓婁竴涓溅韬疆寤撶偣
- if (CarModelPrev->pointNum > 0) {
- CarModelPrev->pointNum = CarModel->pointNum;
- CarModelPrev->basePoint = CarModel->basePoint;
- for (int i = 0; i < CarModel->pointNum; ++i) {
- CarModelPrev->carXY[i] = CarModel->carXY[i];
- }
- }
UpdateCarBodyCoord(RtkBuffer[p1].heading, RtkBuffer[p1].pitch, RtkBuffer[p1].roll, main_ant_coord, CarModel);
- if (CarModelPrev->pointNum == 0) {
- CarModelPrev->pointNum = CarModel->pointNum;
- CarModelPrev->basePoint = CarModel->basePoint;
- for (int i = 0; i < CarModel->pointNum; ++i) {
- CarModelPrev->carXY[i] = CarModel->carXY[i];
- }
+
+ car_model *newModel = (car_model *)malloc(sizeof(car_model));
+
+ newModel->basePoint = CarModel->basePoint;
+ newModel->axial[0] = CarModel->axial[0];
+ newModel->axial[1] = CarModel->axial[1];
+ newModel->left_front_tire[0] = CarModel->left_front_tire[0];
+ newModel->left_front_tire[1] = CarModel->left_front_tire[1];
+ newModel->right_front_tire[0] = CarModel->right_front_tire[0];
+ newModel->right_front_tire[1] = CarModel->right_front_tire[1];
+ newModel->left_rear_tire[0] = CarModel->left_rear_tire[0];
+ newModel->left_rear_tire[1] = CarModel->left_rear_tire[1];
+ newModel->right_rear_tire[0] = CarModel->right_rear_tire[0];
+ newModel->right_rear_tire[1] = CarModel->right_rear_tire[1];
+ newModel->bodyNum = CarModel->bodyNum;
+ newModel->body = (int *) malloc(sizeof(int) * newModel->bodyNum);
+ for (int i = 0; i < newModel->bodyNum; ++i) {
+ newModel->body[i] = CarModel->body[i];
+ }
+ newModel->pointNum = CarModel->pointNum;
+ newModel->carXY = (PointF *) malloc(sizeof(PointF) * newModel->pointNum);
+ for (int i = 0; i < newModel->pointNum; ++i) {
+ newModel->carXY[i] = CarModel->carXY[i];
+ }
+ newModel->carDesc = NULL;
+ newModel->antPitch = CarModel->antPitch;
+ newModel->yaw = CarModel->yaw;
+ newModel->pitch = CarModel->pitch;
+
+ CarModelList.push_front(newModel);
+ while (CarModelList.size() > 25) {
+ DEBUG("鍒犻櫎灏剧粨鐐�");
+ car_model *ptr = CarModelList.back();
+
+ if (ptr->body != NULL)
+ free(ptr->body);
+ if (ptr->carXY != NULL)
+ free(ptr->carXY);
+ if (ptr->carDesc != NULL)
+ free(ptr->carDesc);
+
+ free(ptr);
+
+ CarModelList.pop_back();
}
// 璁＄畻閫熷害(绫�/绉�)銆佸墠杩涘悗閫�
@@ -801,77 +862,74 @@
{
// 杞︾殑鏈�鍓嶇偣鏄惁杩涘叆鍦板浘
for (int i = 0; i < mapNum && car != NULL; ++i) {
-// if (mapList[i].type == MAP_TYPE_STOP_START) {
-// // 鏋勯�犺櫄鎷熺殑宸︿笂瑙掔偣
-// double x9, y9, xo, yo;
-//
-// 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;
-// }
-// if (mapList[i].type == MAP_TYPE_CURVE) {
-//
-// }
-// if (mapList[i].type == MAP_TYPE_PARK_BUTTOM) {
-// if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &mapList[i].map) == GM_Containment) {
-// Line enterLine1, enterLine2;
-//
-// MakeLine(&enterLine1, &(mapList[i].map.point[0]), &(mapList[i].map.point[1]));
-// MakeLine(&enterLine2, &(mapList[i].map.point[6]), &(mapList[i].map.point[7]));
-//
-// if (DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], enterLine1) > 0.1 &&
-// DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], enterLine2) > 0.1)
-// 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;
-//
-// 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;
-// }
-// }
+ if (mapList[i].type == MAP_TYPE_STOP_START) {
+ // 鏋勯�犺櫄鎷熺殑宸︿笂瑙掔偣
+ double x9, y9, xo, yo;
+
+ 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;
+ }
+ if (mapList[i].type == MAP_TYPE_PARK_BUTTOM) {
+ if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &mapList[i].map) == GM_Containment) {
+ Line enterLine1, enterLine2;
+
+ MakeLine(&enterLine1, &(mapList[i].map.point[0]), &(mapList[i].map.point[1]));
+ MakeLine(&enterLine2, &(mapList[i].map.point[6]), &(mapList[i].map.point[7]));
+
+ if (DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], enterLine1) > 0.1 &&
+ DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], enterLine2) > 0.1)
+ 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;
+
+ 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;
+ }
+ }
if (mapList[i].type == MAP_TYPE_CURVE) {
Line startLine;
Line carAxial;
@@ -1008,6 +1066,9 @@
{
carModel->basePoint = main_ant;
+ carModel->yaw = azimuth;
+ carModel->pitch = pitch;
+
pitch = pitch - carModel->antPitch;
for (int i = 0; i < carModel->pointNum; ++i) {
diff --git a/lib/src/main/cpp/driver_test.h b/lib/src/main/cpp/driver_test.h
index 0fe5431..24c2444 100644
--- a/lib/src/main/cpp/driver_test.h
+++ b/lib/src/main/cpp/driver_test.h
@@ -38,7 +38,9 @@
int bodyNum;
int *body;
int pointNum;
- double antPitch;
+ double antPitch; // 鍓嶅悗澶╃嚎鍥犲畨瑁呬綅缃箣鍥烘湁pitch瑙�
+ double yaw;
+ double pitch;
struct car_desc_ *carDesc; // 缁忕幇鍦烘祴閲忕偣璁＄畻鍚庣殑鍊�
PointF *carXY; // 璁＄畻鍚庣殑杞﹁韩鍚勭偣鍧愭爣
} car_model;
diff --git a/lib/src/main/cpp/master/comm_if.cpp b/lib/src/main/cpp/master/comm_if.cpp
index b1ea6af..9c668ca 100644
--- a/lib/src/main/cpp/master/comm_if.cpp
+++ b/lib/src/main/cpp/master/comm_if.cpp
@@ -35,6 +35,8 @@
#define ID_SM_RTK_PLAT_LOGIN 0x0005
#define ID_SM_READ_MAP 0x0006
#define ID_MS_MAP 0x8006
+#define ID_MS_ROAD_MAP 0x8013
+
#define ID_SM_READ_CAR 0x0007
#define ID_MS_CAR 0x8007
#define ID_SM_MCU_BRIEF 0x0008
@@ -483,6 +485,70 @@
ConfigPlatform(&cfg);
break;
}
+ case ID_MS_ROAD_MAP: {
+ Document doc;
+ doc.Parse(value);
+
+ if (!doc.HasParseError()) {
+ const Value &a = doc.GetArray();
+ if (a.IsArray()) {
+ for (Value::ConstValueIterator itr = a.Begin(); itr != a.End(); ++itr) {
+ // a Map
+ int id, type, pointNum = 0;
+ double (*map)[2] = NULL;
+ int *line = NULL;
+
+ if (itr->IsObject()) {
+ if (itr->HasMember("id")) {
+ const Value &s = (*itr)["id"];
+ id = s.GetInt();
+ }
+
+ if (itr->HasMember("item")) {
+ const Value &s = (*itr)["item"];
+ type = s.GetInt();
+ }
+
+ if (itr->HasMember("map")) {
+ const Value &s = (*itr)["map"];
+ int i = 0, j = 0;
+
+ pointNum = s.Size() / 2;
+ map = (double (*)[2]) new double[pointNum][2];
+
+ for (Value::ConstValueIterator itr2 = s.Begin();
+ itr2 != s.End(); ++itr2) {
+ map[i][j] = (*itr2).GetDouble();
+ if (++j == 2) {
+ j = 0;
+ i++;
+ }
+ }
+ }
+
+ AddMap(id, type, map, pointNum, redLineNum, redLine, greenLine, redArea, triggerLine);
+
+ if (itr->HasMember("red_line")) {
+ const Value &s = (*itr)["red_line"];
+ for (Value::ConstValueIterator itr2 = s.Begin();
+ itr2 != s.End(); ++itr2) {
+ // 鍙互杩炴帴涓轰竴鏉＄嚎鐨勭偣
+ const Value &s2 = (*itr2)["line"];
+
+ lineNum = s2.Size() / 2;
+
+ for (Value::ConstValueIterator itr3 = s2.Begin();
+ itr3 != s2.End(); ++itr3) {
+ line[x++] = (*itr3).GetInt();
+ }
+ }
+ }
+ }
+ }
+ }
+ }
+ break;
+ }
case ID_MS_MAP: {
Document doc;
doc.Parse(value);
diff --git a/lib/src/main/cpp/rtk_module/rtk.cpp b/lib/src/main/cpp/rtk_module/rtk.cpp
index 1a32349..5fc26f8 100644
--- a/lib/src/main/cpp/rtk_module/rtk.cpp
+++ b/lib/src/main/cpp/rtk_module/rtk.cpp
@@ -200,7 +200,7 @@
}*/
if (RxBufLen > 0) {
-#if 1
+#if 0
const uint8_t *ptr = parseGPS(RxBuf, RxBuf + RxBufLen);
if(ptr != RxBuf) {
memcpy(RxBuf, ptr, RxBufLen - (ptr - RxBuf));
--
Gitblit v1.8.0