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.cpp | 211 ++++++++++++++++++++++++++++++++++++++++++++++++++++
1 files changed, 211 insertions(+), 0 deletions(-)
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;
}
}
--
Gitblit v1.8.0