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/master/comm_if.h | 1
lib/src/main/cpp/driver_test.cpp | 89 ++++++++++++++
lib/src/main/cpp/master/comm_if.cpp | 211 +++++++++++++++++++++++++++++++++++
lib/src/main/cpp/driver_test.h | 8 +
lib/src/main/cpp/rtk_platform/platform.cpp | 5
5 files changed, 311 insertions(+), 3 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)
diff --git a/lib/src/main/cpp/driver_test.h b/lib/src/main/cpp/driver_test.h
index 6979f04..896814b 100644
--- a/lib/src/main/cpp/driver_test.h
+++ b/lib/src/main/cpp/driver_test.h
@@ -45,6 +45,14 @@
} car_model_cache_t;
void DriverTestInit(void);
+void ReadDriverExamPrimer(void);
+void ClearMap(void);
+void AddMap(int id, int type, const double (*map)[2], int pointNum, const double (*map2)[2], int pointNum2);
+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);
+void SetSensorCfg(int (*sensor)[2], int sensorNum);
+
void UpdateRTKInfo(struct rtk_info *s);
void UpdateCarCoord(void);
car_model_cache_t *GetCarModelCache(int node);
diff --git a/lib/src/main/cpp/master/comm_if.cpp b/lib/src/main/cpp/master/comm_if.cpp
index 836fc49..5301d23 100644
--- a/lib/src/main/cpp/master/comm_if.cpp
+++ b/lib/src/main/cpp/master/comm_if.cpp
@@ -10,6 +10,7 @@
#include "../rapidjson/stringbuffer.h"
#include "../rtk_platform/platform.h"
#include "../utils/xconvert.h"
+#include "../driver_test.h"
#include <string>
#include <iostream>
@@ -26,6 +27,13 @@
#define ID_MS_MAP 0x8006
#define ID_SM_READ_CAR 0x0007
#define ID_MS_CAR 0x8007
+#define ID_SM_READ_SENSOR_CFG 0x0008
+#define ID_MS_SENSOR_CFG 0x8008
+#define ID_MS_START_EXAM 0x8009
+#define ID_SM_EXAM_STATUS 0x0009
+#define ID_SM_GPS_BRIEF 0x000A
+#define ID_SM_RTK_BRIEF 0x000B
+#define ID_SM_EXAM_BRIEF 0x000B
void MA_NdkStart(void)
{
@@ -102,6 +110,11 @@
SendMsgToMainProc(ID_SM_READ_CAR, NULL);
}
+void MA_ReadSensor(void)
+{
+ SendMsgToMainProc(ID_SM_READ_SENSOR_CFG, NULL);
+}
+
void MA_MainProcMsgEntry(int cmd, const char *value)
{
switch (cmd) {
@@ -160,5 +173,203 @@
ConfigPlatform(&cfg);
break;
}
+ case ID_MS_MAP: {
+ Document doc;
+ doc.Parse(value);
+ if (!doc.HasParseError()) {
+ ClearMap();
+
+ 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, point2Num = 0;
+ double (*map)[2] = NULL, (*map2)[2] = 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("point")) {
+ const Value &s = (*itr)["point"];
+ int map_index = 0;
+
+ for (Value::ConstValueIterator itr2 = s.Begin();
+ itr2 != s.End(); ++itr2, ++map_index) {
+ // 鏇茬嚎椹鹃┒鏈�2缁�
+ const Value &s2 = (*itr2)["x-y"];
+
+ if (map_index == 0) {
+ int i = 0, j = 0;
+ pointNum = s2.Size()/2;
+ map = new double[pointNum][2];
+ for (Value::ConstValueIterator itr3 = s2.Begin();
+ itr3 != s2.End(); ++itr3) {
+ map[i][j] = (*itr3).GetDouble();
+ if (++j == 2) {
+ j = 0;
+ i++;
+ }
+ }
+ } else if (map_index == 1) {
+ int i = 0, j = 0;
+ point2Num = s2.Size()/2;
+ map2 = new double[s2.Size()][2];
+ for (Value::ConstValueIterator itr3 = s2.Begin();
+ itr3 != s2.End(); ++itr3) {
+ map2[i][j] = (*itr3).GetDouble();
+ if (++j == 2) {
+ j = 0;
+ i++;
+ }
+ }
+ }
+ }
+ }
+
+ AddMap(id, type, map, pointNum, map2, point2Num);
+ if (map) delete []map;
+ if (map2) delete []map2;
+ }
+ }
+ }
+ }
+ break;
+ }
+ case ID_MS_CAR: {
+ Document doc;
+
+ doc.Parse(value);
+
+ if (!doc.HasParseError()) {
+ double basePoint[2];
+ int axial[2], left_front_tire[2], right_front_tire[2], left_rear_tire[2], right_rear_tire[2];
+ int pointNum = 0;
+ double (*point)[2] = NULL;
+
+ if (doc.HasMember("main_ant")) {
+ const Value& s = doc["main_ant"];
+ if (s.IsArray()) {
+ int i = 0;
+ for(Value::ConstValueIterator itr = s.Begin(); itr != s.End(); ++itr) {
+ if (i < 2) basePoint[i++] = itr->GetDouble();
+ }
+ }
+ }
+
+ if (doc.HasMember("axial")) {
+ const Value& s = doc["axial"];
+ if (s.IsArray()) {
+ int i = 0;
+ for(Value::ConstValueIterator itr = s.Begin(); itr != s.End(); ++itr) {
+ if (i < 2) axial[i++] = itr->GetInt();
+ }
+ }
+ }
+
+ if (doc.HasMember("left_front_tire")) {
+ const Value& s = doc["left_front_tire"];
+ if (s.IsArray()) {
+ int i = 0;
+ for(Value::ConstValueIterator itr = s.Begin(); itr != s.End(); ++itr) {
+ if (i < 2) left_front_tire[i++] = itr->GetInt();
+ }
+ }
+ }
+
+ if (doc.HasMember("right_front_tire")) {
+ const Value& s = doc["right_front_tire"];
+ if (s.IsArray()) {
+ int i = 0;
+ for(Value::ConstValueIterator itr = s.Begin(); itr != s.End(); ++itr) {
+ if (i < 2) right_front_tire[i++] = itr->GetInt();
+ }
+ }
+ }
+
+ if (doc.HasMember("left_rear_tire")) {
+ const Value& s = doc["left_rear_tire"];
+ if (s.IsArray()) {
+ int i = 0;
+ for(Value::ConstValueIterator itr = s.Begin(); itr != s.End(); ++itr) {
+ if (i < 2) left_rear_tire[i++] = itr->GetInt();
+ }
+ }
+ }
+
+ if (doc.HasMember("right_rear_tire")) {
+ const Value& s = doc["right_rear_tire"];
+ if (s.IsArray()) {
+ int i = 0;
+ for(Value::ConstValueIterator itr = s.Begin(); itr != s.End(); ++itr) {
+ if (i < 2) right_rear_tire[i++] = itr->GetInt();
+ }
+ }
+ }
+
+ if (doc.HasMember("point")) {
+ const Value& s = doc["point"];
+ if (s.IsArray()) {
+ int i = 0, j = 0;
+ pointNum = s.Size()/2;
+ point = new double[pointNum][2];
+
+ for(Value::ConstValueIterator itr = s.Begin(); itr != s.End(); ++itr) {
+ point[i][j] = itr->GetDouble();
+ if (++j == 2) {
+ j = 0;
+ i++;
+ }
+ }
+ }
+ }
+
+ SetCarMeasurePoint(basePoint, axial, left_front_tire, right_front_tire,
+ left_rear_tire, right_rear_tire, point, pointNum);
+
+ if (point != NULL) delete []point;
+ }
+ break;
+ }
+ case ID_MS_SENSOR_CFG: {
+ Document doc;
+ doc.Parse(value);
+ if (!doc.HasParseError()) {
+ const Value &a = doc.GetArray();
+
+ if (a.IsArray() && a.Size() > 0) {
+ int n = a.Size();
+ int i = 0;
+ int (*sensor)[2] = new int[n][2];
+
+ for (Value::ConstValueIterator itr = a.Begin(); itr != a.End(); ++itr) {
+ // a gpio mapping
+ if (itr->HasMember("gpio_num")) {
+ const Value &s = (*itr)["gpio_num"];
+ sensor[i][0] = s.GetInt();
+ }
+ if (itr->HasMember("func_id")) {
+ const Value &s = (*itr)["func_id"];
+ sensor[i][1] = s.GetInt();
+ }
+ ++i;
+ }
+ SetSensorCfg(sensor, n);
+ delete []sensor;
+ }
+ }
+ break;
+ }
+ case ID_MS_START_EXAM: {
+
+ break;
+ }
+ default:break;
}
}
diff --git a/lib/src/main/cpp/master/comm_if.h b/lib/src/main/cpp/master/comm_if.h
index 8286f14..48633c7 100644
--- a/lib/src/main/cpp/master/comm_if.h
+++ b/lib/src/main/cpp/master/comm_if.h
@@ -16,5 +16,6 @@
void MA_RtkPlatformLogin(int login);
void MA_ReadMap(void);
void MA_ReadCar(void);
+void MA_ReadSensor(void);
#endif //MYAPPLICATION2_COMM_IF_H
diff --git a/lib/src/main/cpp/rtk_platform/platform.cpp b/lib/src/main/cpp/rtk_platform/platform.cpp
index cd9540d..f3d5bb9 100644
--- a/lib/src/main/cpp/rtk_platform/platform.cpp
+++ b/lib/src/main/cpp/rtk_platform/platform.cpp
@@ -19,6 +19,7 @@
#include "../master/comm_if.h"
#include "../utils/xconvert.h"
#include "../utils/num.h"
+#include "../driver_test.h"
#define PARSE_BUFF_SIZE 4096
@@ -109,9 +110,6 @@
AppTimer_add(ReqRtkPlatformConfigTimeout, D_SEC(2));
MA_ReqRtkPlatformConfig();
-
- MA_ReadMap();
- MA_ReadCar();
}
static void ReqRtkPlatformConfigTimeout(union sigval sig)
@@ -125,6 +123,7 @@
void ConfigPlatform(const rtk_platform_cfg_t *p)
{
DEBUG("ConfigPlatform");
+
AppTimer_delete(ReqRtkPlatformConfigTimeout);
strcpy(exceptSocket.domain_name, p->domain_name);
--
Gitblit v1.8.0