From 814c006d124ef3ed344e790f1f74812fbaa11119 Mon Sep 17 00:00:00 2001
From: fctom1215 <fctom1215@outlook.com>
Date: 星期五, 21 二月 2020 15:29:53 +0800
Subject: [PATCH] GPS信息合并到RTK中，以RTK包，发送
---
lib/src/main/cpp/driver_test.cpp | 129 +++++++++++++++++++++++++++++++++++--------
1 files changed, 105 insertions(+), 24 deletions(-)
diff --git a/lib/src/main/cpp/driver_test.cpp b/lib/src/main/cpp/driver_test.cpp
index 58f861b..4c7cf51 100644
--- a/lib/src/main/cpp/driver_test.cpp
+++ b/lib/src/main/cpp/driver_test.cpp
@@ -27,6 +27,7 @@
#include "master/comm_if.h"
#include "utils/xconvert.h"
#include "test_items/comm_test.h"
+#include "mcu/mcu_if.h"
#define DEBUG(fmt, args...) LOGD("<driver_test> <%s>: " fmt, __func__, ##args)
@@ -76,6 +77,8 @@
static int MapNum = 0;
static int CurrExamMapIndex = -1;
+static int CurrEnterMapIndex = -1;
+
static int CurrExamStatus = 0; // 1 娴嬭瘯瀹屾垚 0 娴嬭瘯涓� -1 娴嬭瘯閿欒閫�鍑�
static car_model *CarModel = NULL;
@@ -102,7 +105,7 @@
static uint32_t CalcTimeDiff(const rtk_info *a, const rtk_info *b);
static void ReadDriverExamPrimerTimeout(union sigval sig);
-static void UpdateCarBodyCoord(double azimuth, PointF main_ant, car_model *carModel);
+static void UpdateCarBodyCoord(double azimuth, double pitch, double roll, PointF main_ant, car_model *carModel);
static bool UpdateCarCoord(double &spd, int &mov, int &idx);
static bool FrontTireEnterArea(const Polygon *car, const Polygon *map);
static int EnterMap(const car_model *car, const struct map_list *mapList, int mapNum);
@@ -358,6 +361,7 @@
DEBUG("StartDriverExam %d", start);
if (start == 0) {
+ CurrExamMapIndex = -1;
TestStart = false;
CommTestStart(false);
MA_SendExamStatus(0, 0);
@@ -381,6 +385,16 @@
CommTestStart(true);
}
MA_SendExamStatus(1, 0);
+ }
+}
+
+void StartMapExam(int map_id, int exam)
+{
+ DEBUG("娴嬭瘯璇ュ満鍦� %d: %d", map_id, exam);
+
+ if (map_id >= 0 && exam == 0) {
+ CurrExamMapIndex = map_id;
+ CurrExamStatus = 0;
}
}
@@ -473,9 +487,27 @@
if (TestStart) {
if (CurrExamMapIndex < 0) {
- CurrExamMapIndex = EnterMap(CarModel, MapList, MapNum);
- int mtype = GetMapType(CurrExamMapIndex, MapList, MapNum);
+ if (CurrEnterMapIndex < 0) {
+ CurrEnterMapIndex = EnterMap(CarModel, MapList, MapNum);
+ if (CurrEnterMapIndex >= 0) {
+ DEBUG("鍙戦�佽繘鍏ュ満鍦版姤鍛� %d", GetMapId(CurrEnterMapIndex, MapList, MapNum));
+ MA_EnterMap(GetMapId(CurrEnterMapIndex, MapList, MapNum), 1);
+ }
+ } else {
+ if (ExitMap(CarModel, CurrEnterMapIndex, MapList, MapNum)) {
+ DEBUG("鍙戦�佺寮�鍦哄湴鎶ュ憡 %d", GetMapId(CurrEnterMapIndex, MapList, MapNum));
+ MA_EnterMap(GetMapId(CurrEnterMapIndex, MapList, MapNum), 0);
+ CurrEnterMapIndex = -1;
+ }
+ }
+ }
+ //ExitMap(const car_model *car, int index, const struct map_list *mapList, int mapNum)
+ }
+ if (CurrExamMapIndex >= 0) {
+ int mtype = GetMapType(CurrExamMapIndex, MapList, MapNum);
+
+ if (CurrExamStatus == 0) {
DEBUG("CurrExamMapIndex %d mtype %d", GetMapId(CurrExamMapIndex, MapList, MapNum), mtype);
switch (mtype) {
@@ -509,12 +541,9 @@
break;
default:break;
}
- }
- }
- if (CurrExamMapIndex >= 0) {
- if (CurrExamStatus == 0) {
- int mtype = GetMapType(CurrExamMapIndex, MapList, MapNum);
+ CurrExamStatus = 1;
+ } else if (CurrExamStatus == 1) {
switch (mtype) {
case MAP_TYPE_PARK_BUTTOM:
CurrExamStatus = TestParkBottom(&MapList[CurrExamMapIndex].map,
@@ -532,16 +561,17 @@
CurrExamStatus = TestTurnA90(&MapList[CurrExamMapIndex].map, CarModel, CarModelPrev, azimuth, speed, move, &rtkTime);
break;
default:
+ CurrExamStatus = -1;
break;
}
}
- if (CurrExamStatus != 0) {
- // 鏌愰」缁撴潫锛�
- if (ExitMap(CarModel, CurrExamMapIndex, MapList, MapNum)) {
- DEBUG("閫�鍑哄満鍦� %d", GetMapId(CurrExamMapIndex, MapList, MapNum));
- MA_SendDebugInfo("閫�鍑哄満鍦� %d", GetMapId(CurrExamMapIndex, MapList, MapNum));
- CurrExamMapIndex = -1;
- }
+
+ if (CurrExamStatus < 0) {
+ // 鏌愰」缁撴潫
+ //DEBUG("閫�鍑哄満鍦� %d", GetMapId(CurrExamMapIndex, MapList, MapNum));
+ //MA_SendDebugInfo("閫�鍑哄満鍦� %d", GetMapId(CurrExamMapIndex, MapList, MapNum));
+
+ CurrExamMapIndex = -1;
}
}
}
@@ -605,7 +635,7 @@
CarModelPrev->carXY[i] = CarModel->carXY[i];
}
}
- UpdateCarBodyCoord(RtkBuffer[p1].heading, main_ant_coord, CarModel);
+ 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;
@@ -834,7 +864,9 @@
bool ret = false;
if (index < 0 || mapList == NULL || mapNum == 0) return true;
- if (mapList[index].type == MAP_TYPE_PARK_BUTTOM || mapList[index].type == MAP_TYPE_PART_EDGE) {
+ if (mapList[index].type == MAP_TYPE_PARK_BUTTOM ||
+ mapList[index].type == MAP_TYPE_PART_EDGE ||
+ mapList[index].type == MAP_TYPE_TURN_90) {
// 鍏ㄨ溅閮介渶涓嶅湪鍦板浘涓�
Polygon carBody;
@@ -849,6 +881,45 @@
}
free(carBody.point);
+ }
+ if (mapList[index].type == MAP_TYPE_STOP_START) {
+ // 鏋勯�犺櫄鎷熺殑宸︿笂瑙掔偣
+ double x9, y9, xo, yo;
+
+ bool enter = false;
+
+ xo = (mapList[index].map.point[0].X + mapList[index].map.point[7].X) / 2;
+ yo = (mapList[index].map.point[0].Y + mapList[index].map.point[7].Y) / 2;
+
+ x9 = 2*xo - mapList[index].map.point[8].X;
+ y9 = 2*yo - mapList[index].map.point[8].Y;
+
+ Polygon map;
+
+ map.num = 4;
+ map.point = (PointF *) malloc(map.num * sizeof(PointF));
+
+ map.point[0] = mapList[index].map.point[0];
+ map.point[1] = mapList[index].map.point[8];
+ map.point[2] = mapList[index].map.point[7];
+ map.point[3].X = x9;
+ map.point[3].Y = y9;
+
+ // 鍏ㄨ溅閮介渶涓嶅湪鍦板浘涓�
+ Polygon carBody;
+
+ 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, &map) == GM_None) {
+ ret = true;
+ }
+
+ free(carBody.point);
+ free(map.point);
}
return ret;
@@ -898,18 +969,18 @@
* @param azimuth
* @param coord
*/
-static void UpdateCarBodyCoord(double azimuth, PointF main_ant, car_model *carModel)
+static void UpdateCarBodyCoord(double azimuth, double pitch, double roll, PointF main_ant, car_model *carModel)
{
carModel->basePoint = main_ant;
for (int i = 0; i < carModel->pointNum; ++i) {
- double tx = main_ant.X + carModel->carDesc[i].distance*sin(toRadians(azimuth));
- double ty = main_ant.Y + carModel->carDesc[i].distance*cos(toRadians(azimuth));
+ double tx = carModel->carDesc[i].distance*sin(toRadians(azimuth));
+ double ty = carModel->carDesc[i].distance*cos(toRadians(azimuth));
- carModel->carXY[i].X = (tx-main_ant.X)*cos(toRadians(carModel->carDesc[i].angle)) -
- (ty-main_ant.Y)*sin(toRadians(carModel->carDesc[i].angle)) + main_ant.X;
- carModel->carXY[i].Y = (tx-main_ant.X)*sin(toRadians(carModel->carDesc[i].angle)) +
- (ty-main_ant.Y)*cos(toRadians(carModel->carDesc[i].angle)) + main_ant.Y;
+ 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;
}
}
@@ -921,4 +992,14 @@
return false;
}
+void SystemShutdown(int event, int timeout)
+{
+ // 鍏虫満
+ if (event == 1) {
+ ShutdownInd(timeout);
+ }
+ // 閲嶅惎
+ if (event == 0) {
+ }
+}
--
Gitblit v1.8.0