From 74bb2be0e23e9f2290ff8ecfb6506acf8a070339 Mon Sep 17 00:00:00 2001
From: yy1717 <fctom1215@outlook.com>
Date: 星期四, 09 一月 2020 16:46:59 +0800
Subject: [PATCH] 实现rtk平台基础配置
---
lib/src/main/cpp/driver_test.cpp | 89 ++++++++++++++++++++++++++++++++++++++++++++
1 files changed, 89 insertions(+), 0 deletions(-)
diff --git a/lib/src/main/cpp/driver_test.cpp b/lib/src/main/cpp/driver_test.cpp
index 2f3d191..55464ad 100644
--- a/lib/src/main/cpp/driver_test.cpp
+++ b/lib/src/main/cpp/driver_test.cpp
@@ -24,6 +24,7 @@
#include "test_items/turn_a90.h"
#include "test_items/driving_curve.h"
#include "test_items/stop_and_start.h"
+#include "master/comm_if.h"
#define DEBUG(fmt, args...) LOGD("<driver_test> <%s>: " fmt, __func__, ##args)
@@ -36,6 +37,7 @@
#define RTK_FLOAT 4
#define CAR_MODEL_POINT_NUM 32
+#define MAP_LIST_SIZE 32
enum {
TEST_NONE,
@@ -58,6 +60,16 @@
static Polygon theParkEdgeMap; // 渚т綅鍋滆溅鍦板浘
static Polygon theTurn90Map;
static Polygon theSSMap;
+
+static struct map_list {
+ int id;
+ int type;
+ Polygon map;
+ Polygon map2;
+} MapList[MAP_LIST_SIZE];
+
+static int MapNum = 0;
+
#define CAR_COORD_STORE_SIZE 10
@@ -101,12 +113,16 @@
static car_model_cache_t carModelCache[CAR_MODEL_CACHE_SIZE];
static int carModelCacheIn, carModelCacheNum;
+static void ReadDriverExamPrimerTimeout(union sigval sig);
static void UpdateCarBodyCoord(double azimuth, PointF coord);
static void UpdateCarBodyCoord(double azimuth, PointF coord, car_model_cache_t *carModel, int &carModelIn, int &carModelNum);
static bool FrontTireEnterArea(const Polygon *car, const Polygon *map);
void DriverTestInit(void)
{
+ memset(&MapList, 0, sizeof(MapList));
+ MapNum = 0;
+
carModelDescFile = (carModelDesc_t *)malloc(sizeof(carModelDesc_t));
carModelDescFile->body_num = 6;
@@ -243,6 +259,79 @@
ErrorList.clear();
TextSpeak("寮�濮嬫祴璇�");
+
+ AppTimer_add(ReadDriverExamPrimerTimeout, D_SEC(2));
+ ReadDriverExamPrimer();
+}
+
+static void ReadDriverExamPrimerTimeout(union sigval sig)
+{
+ AppTimer_delete(ReadDriverExamPrimerTimeout);
+ ReadDriverExamPrimer();
+}
+
+void ReadDriverExamPrimer(void)
+{
+// MA_ReadMap();
+// MA_ReadCar();
+// MA_ReadSensor();
+}
+
+void ClearMap(void)
+{
+ for (int i = 0; i < MapNum; ++i) {
+ if (MapList[i].map.point != NULL)
+ free(MapList[i].map.point);
+ if (MapList[i].map2.point != NULL)
+ free(MapList[i].map2.point);
+ }
+ memset(&MapList, 0, sizeof(MapList));
+ MapNum = 0;
+}
+
+void AddMap(int id, int type, const double (*map)[2], int pointNum, const double (*map2)[2], int pointNum2)
+{
+ DEBUG("AddMap id %d type %d pointNum %d point2Num %d", id, type, pointNum, pointNum2);
+
+ AppTimer_delete(ReadDriverExamPrimerTimeout);
+
+ if (map == NULL || pointNum == 0)
+ return;
+
+ MapList[MapNum].id = id;
+ MapList[MapNum].type = type;
+
+ MapList[MapNum].map.num = pointNum;
+ if (pointNum > 0) {
+ MapList[MapNum].map.point = (PointF *)malloc(sizeof(PointF) * pointNum);
+ for (int i = 0; i < pointNum; ++i) {
+ MapList[MapNum].map.point[i].X = map[i][0];
+ MapList[MapNum].map.point[i].Y = map[i][0];
+ }
+ }
+
+ MapList[MapNum].map2.num = pointNum2;
+ if (pointNum2 > 0 && map2 != NULL) {
+ MapList[MapNum].map2.point = (PointF *)malloc(sizeof(PointF) * pointNum2);
+ for (int i = 0; i < pointNum2; ++i) {
+ MapList[MapNum].map2.point[i].X = map2[i][0];
+ MapList[MapNum].map2.point[i].Y = map2[i][0];
+ }
+ }
+
+ MapNum++;
+}
+
+void SetCarMeasurePoint(double *basePoint, int *axial, int *left_front_tire,
+ int *right_front_tire, int *left_rear_tire, int *right_rear_tire,
+ double (*point)[2], int pointNum)
+{
+ DEBUG("SetCarMeasurePoint pointNum %d", pointNum);
+}
+
+void SetSensorCfg(int (*sensor)[2], int sensorNum)
+{
+ DEBUG("SetSensorCfg sensorNum %d", sensorNum);
}
void UpdateRTKInfo(struct rtk_info *s)
--
Gitblit v1.8.0