From 79f2500657c85c84aceed46ec00f9a703eae395b Mon Sep 17 00:00:00 2001
From: yy1717 <fctom1215@outlook.com>
Date: 星期二, 31 三月 2020 14:55:22 +0800
Subject: [PATCH] 坡道坐标修正、发往主进程的消息线程独立
---
lib/src/main/cpp/master/comm_if.cpp | 118 +++++++++++++++++++++++++++++++++++++++++++++++++---------
1 files changed, 99 insertions(+), 19 deletions(-)
diff --git a/lib/src/main/cpp/master/comm_if.cpp b/lib/src/main/cpp/master/comm_if.cpp
index 6d4a2eb..ceaf567 100644
--- a/lib/src/main/cpp/master/comm_if.cpp
+++ b/lib/src/main/cpp/master/comm_if.cpp
@@ -20,6 +20,8 @@
#include <string>
#include <iostream>
#include <vector>
+#include <list>
+#include <semaphore.h>
#define DEBUG(fmt, args...) LOGD("<comm_if> <%s>: " fmt, __func__, ##args)
@@ -67,14 +69,80 @@
static int OnOff = 0;//0xFFFF;
+struct msg_2_main_t {
+ int cmd;
+ string value;
+};
+
+static list<struct msg_2_main_t> MessageBuffer;
+
+static sem_t sem_msg_income;
+static pthread_mutex_t msg_mutex = PTHREAD_MUTEX_INITIALIZER;
+
+static void SendMsgToMainProcIndep(int cmd, const char *value);
+static void *SendMsgToMainProcThread(void *p);
+
+static void SendMsgToMainProcIndep(int cmd, const char *value)
+{
+ struct msg_2_main_t msg;
+
+ msg.cmd = cmd;
+
+ if (value != NULL) {
+ msg.value = value;
+ } else {
+ msg.value.clear();
+ }
+
+ pthread_mutex_lock(&msg_mutex);
+ MessageBuffer.push_front(msg);
+ pthread_mutex_unlock(&msg_mutex);
+
+ sem_post(&sem_msg_income);
+}
+
+static void *SendMsgToMainProcThread(void *p) {
+ while (true) {
+ sem_wait(&sem_msg_income);
+
+ if (MessageBuffer.size() > 0) {
+ struct msg_2_main_t msg;
+
+ pthread_mutex_lock(&msg_mutex);
+ msg = MessageBuffer.back();
+ MessageBuffer.pop_back();
+ pthread_mutex_unlock(&msg_mutex);
+
+ if (msg.value.length() > 0)
+ SendMsgToMainProc(msg.cmd, msg.value.c_str());
+ else
+ SendMsgToMainProc(msg.cmd, NULL);
+ }
+ }
+}
+
+void MA_Init(void)
+{
+ sem_init(&sem_msg_income, 0, 0);
+ MessageBuffer.clear();
+
+ pthread_mutex_init(&msg_mutex, NULL);
+
+ pthread_t pid;
+ pthread_attr_t attr;
+ pthread_attr_init(&attr);
+ pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_DETACHED);
+ pthread_create(&pid, &attr, SendMsgToMainProcThread, NULL);
+}
+
void MA_NdkStart(void)
{
- SendMsgToMainProc(ID_SM_NDK_START, NULL);
+ SendMsgToMainProcIndep(ID_SM_NDK_START, NULL);
}
void MA_ReqRtkPlatformConfig(void)
{
- SendMsgToMainProc(ID_SM_REQ_RTK_PLAT_CFG, NULL);
+ SendMsgToMainProcIndep(ID_SM_REQ_RTK_PLAT_CFG, NULL);
}
void MA_RtkPlatformConnect(int conn, const char *ip, int port)
@@ -92,7 +160,7 @@
writer.EndObject();
- SendMsgToMainProc(ID_SM_RTK_PLAT_CONN, sb.GetString());
+ SendMsgToMainProcIndep(ID_SM_RTK_PLAT_CONN, sb.GetString());
}
void MA_RtkPlatformRegister(int reg, const uint8_t *pwd, int length)
@@ -114,7 +182,7 @@
writer.EndObject();
- SendMsgToMainProc(ID_SM_RTK_PLAT_REG, sb.GetString());
+ SendMsgToMainProcIndep(ID_SM_RTK_PLAT_REG, sb.GetString());
}
void MA_RtkPlatformLogin(int login)
@@ -129,17 +197,17 @@
writer.EndObject();
- SendMsgToMainProc(ID_SM_RTK_PLAT_LOGIN, sb.GetString());
+ SendMsgToMainProcIndep(ID_SM_RTK_PLAT_LOGIN, sb.GetString());
}
void MA_ReadMap(void)
{
- SendMsgToMainProc(ID_SM_READ_MAP, NULL);
+ SendMsgToMainProcIndep(ID_SM_READ_MAP, NULL);
}
void MA_ReadCar(void)
{
- SendMsgToMainProc(ID_SM_READ_CAR, NULL);
+ SendMsgToMainProcIndep(ID_SM_READ_CAR, NULL);
}
void MA_ReadSensor(void)
@@ -161,7 +229,7 @@
writer.EndObject();
- SendMsgToMainProc(ID_SM_EXAM_STATUS, sb.GetString());
+ SendMsgToMainProcIndep(ID_SM_EXAM_STATUS, sb.GetString());
}
void MA_SendCardBrief(const struct cardBrief *brief)
@@ -176,7 +244,7 @@
writer.String(brief->card);
writer.EndObject();
- SendMsgToMainProc(ID_SM_PUT_CARD, sb.GetString());
+ SendMsgToMainProcIndep(ID_SM_PUT_CARD, sb.GetString());
}
void MA_SendMcuBrief(const struct mcuBrief *brief)
@@ -201,7 +269,7 @@
writer.EndObject();
- SendMsgToMainProc(ID_SM_MCU_BRIEF, sb.GetString());
+ SendMsgToMainProcIndep(ID_SM_MCU_BRIEF, sb.GetString());
}
void MA_SendGpsBrief(const struct gpsBrief *brief)
@@ -231,7 +299,7 @@
writer.EndObject();
- SendMsgToMainProc(ID_SM_GPS_BRIEF, sb.GetString());
+ SendMsgToMainProcIndep(ID_SM_GPS_BRIEF, sb.GetString());
}
void MA_SendRtkBrief(const struct rtkBrief *brief)
@@ -281,7 +349,7 @@
writer.EndObject();
- SendMsgToMainProc(ID_SM_RTK_BRIEF, sb.GetString());
+ SendMsgToMainProcIndep(ID_SM_RTK_BRIEF, sb.GetString());
}
void MA_SendExamWrong(vector<ExamFault> &ExamFaultList)
@@ -306,7 +374,7 @@
writer.EndArray();
- SendMsgToMainProc(ID_SM_EXAM_BRIEF, sb.GetString());
+ SendMsgToMainProcIndep(ID_SM_EXAM_BRIEF, sb.GetString());
}
void MA_SendCarPosition(const struct carBrief *brief)
@@ -383,7 +451,7 @@
writer.EndArray();
writer.EndObject();
- SendMsgToMainProc(ID_SM_CAR, sb.GetString());
+ SendMsgToMainProcIndep(ID_SM_CAR, sb.GetString());
}
void MA_SendRtcmInd(int length)
@@ -398,7 +466,7 @@
writer.Int(length);
writer.EndObject();
- SendMsgToMainProc(ID_SM_RTCM_IND, sb.GetString());
+ SendMsgToMainProcIndep(ID_SM_RTCM_IND, sb.GetString());
}
void MA_SendDebugInfo(const char *str, ...)
@@ -422,7 +490,7 @@
writer.String(buffer);
writer.EndObject();
- SendMsgToMainProc(ID_SM_DEBUG_INFO, sb.GetString());
+ SendMsgToMainProcIndep(ID_SM_DEBUG_INFO, sb.GetString());
}
void MA_MainProcMsgEntry(int cmd, const char *value)
@@ -744,6 +812,8 @@
int *body = NULL;
int pointNum = 0;
double antPitch = 0;
+ double antHeight = 0;
+ double groundHeight = 0;
double (*point)[2] = NULL;
@@ -842,8 +912,18 @@
antPitch = s.GetDouble();
}
+ if (doc.HasMember("ant_height")) {
+ const Value& s = doc["ant_height"];
+ antHeight = s.GetDouble();
+ }
+
+ if (doc.HasMember("ground_height")) {
+ const Value& s = doc["ground_height"];
+ groundHeight = s.GetDouble();
+ }
+
SetCarMeasurePoint(basePoint, axial, left_front_tire, right_front_tire,
- left_rear_tire, right_rear_tire, body, bodyNum, point, pointNum, antPitch);
+ left_rear_tire, right_rear_tire, body, bodyNum, point, pointNum, antPitch, antHeight, groundHeight);
if (body != NULL) delete []body;
if (point != NULL) delete []point;
@@ -1011,10 +1091,10 @@
writer.Int(enter);
writer.EndObject();
- SendMsgToMainProc(ID_SM_ENTER_MAP, sb.GetString());
+ SendMsgToMainProcIndep(ID_SM_ENTER_MAP, sb.GetString());
}
void MA_ExamLight(void)
{
- SendMsgToMainProc(ID_SM_LIGHT_EXAM_REQ, NULL);
+ SendMsgToMainProcIndep(ID_SM_LIGHT_EXAM_REQ, NULL);
}
--
Gitblit v1.8.0