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.h | 1
lib/src/main/cpp/driver_test.cpp | 36 ++++++------
lib/src/main/cpp/master/comm_if.cpp | 118 +++++++++++++++++++++++++++++++++------
lib/src/main/cpp/native-lib.cpp | 4
Documents/模拟驾考APP内部通讯.docx | 0
lib/src/main/cpp/test_items2/dummy_light.cpp | 3
lib/src/main/cpp/rtk_module/rtk.cpp | 2
lib/src/main/cpp/driver_test.h | 4 +
8 files changed, 126 insertions(+), 42 deletions(-)
diff --git "a/Documents/\346\250\241\346\213\237\351\251\276\350\200\203APP\345\206\205\351\203\250\351\200\232\350\256\257.docx" "b/Documents/\346\250\241\346\213\237\351\251\276\350\200\203APP\345\206\205\351\203\250\351\200\232\350\256\257.docx"
index adc7ee2..87ddcea 100644
--- "a/Documents/\346\250\241\346\213\237\351\251\276\350\200\203APP\345\206\205\351\203\250\351\200\232\350\256\257.docx"
+++ "b/Documents/\346\250\241\346\213\237\351\251\276\350\200\203APP\345\206\205\351\203\250\351\200\232\350\256\257.docx"
Binary files differ
diff --git a/lib/src/main/cpp/driver_test.cpp b/lib/src/main/cpp/driver_test.cpp
index 8856bb9..0af9a65 100644
--- a/lib/src/main/cpp/driver_test.cpp
+++ b/lib/src/main/cpp/driver_test.cpp
@@ -429,7 +429,7 @@
void SetCarMeasurePoint(double *basePoint, int *axial, int *left_front_tire,
int *right_front_tire, int *left_rear_tire, int *right_rear_tire,
- int *body, int bodyNum,double (*point)[2], int pointNum, double antPitch)
+ int *body, int bodyNum, double (*point)[2], int pointNum, double antPitch, double antHeight, double groundHeight)
{
DEBUG("鍔犲叆杞﹁締淇℃伅 pointNum %d", pointNum);
@@ -474,7 +474,9 @@
}
}
- CarModel->antPitch = -0.6;//antPitch;/////////////////////////////////////
+ CarModel->antPitch = antPitch;
+ CarModel->antHeight = antHeight;
+ CarModel->groundHeight = groundHeight;
CarModel->pointNum = pointNum;
CarModel->carDesc = (struct car_desc_ *)malloc(sizeof(struct car_desc_) * pointNum);
@@ -730,7 +732,6 @@
static void ExecuteExam(const struct RtkTime* rtkTime)
{
-// DEBUG("ExecuteExam 111111111");
{
static const char *NAME[] = {"OBD_SPEED",
"ENGINE_RPM",
@@ -754,7 +755,7 @@
"CAR_STATUS_END"};
static const char *VALUE[] = {
- "0",
+ "INACTIVE",
"HAZARD_LIGHTS",
"LEFT_TURN_LIGHT",
"RIGHT_TURN_LIGHT",
@@ -807,7 +808,6 @@
}
}
-//DEBUG("-------------- 1111111111");
if (ReadCarStatus(ENGINE_RPM) < ENGINE_MIN_ROTATE) {
if (engineRuning) {
@@ -864,13 +864,10 @@
}
}
}
-
-// DEBUG("ExecuteExam 1111111111 ----------------------");
}
static void ExecuteExam(double speed, int move, double azimuth, const struct RtkTime* rtkTime)
{
-// DEBUG("ExecuteExam 2222222");
if (move != 0) {
if (ReadCarStatus(SEATBELT) == EJECT_SEATBELT && !reportSeatbeltEject) {
DEBUG("涓嶇郴瀹夊叏甯�");
@@ -965,7 +962,6 @@
CurrExamMapIndex = -1;
}
}
-// DEBUG("ExecuteExam 2222222 -----------------------");
}
static void EngineStartHold(union sigval sig) {
@@ -1226,10 +1222,10 @@
Line carAxial;
MakeLine(&startLine, &mapList[i].map.point[0], &mapList[i].map2.point[0]);
- MakeLine(&carAxial, &car->carXY[car->axial[AXIAL_FRONT]], &car->carXY[car->axial[AXIAL_REAR]]);
+ MakeLine(&carAxial, &car->carXY[car->left_front_tire[TIRE_OUTSIDE]], &car->carXY[car->left_rear_tire[TIRE_OUTSIDE]]);
if (IntersectionOf(startLine, carAxial) == GM_Intersection) {
- if (DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], startLine) > 0.1)
+ if (DistanceOf(car->carXY[car->left_front_tire[TIRE_OUTSIDE]], startLine) > 0.1)
return i;
}
}
@@ -1360,22 +1356,23 @@
*/
static void UpdateCarBodyCoord(struct RtkTime *rtkTime, double azimuth, double pitch, double roll, PointF main_ant, car_model *carModel)
{
- carModel->basePoint = main_ant;
-
-
+ // 淇话瑙掍慨姝�
// DEBUG("淇话瑙� %f", pitch);
carModel->yaw = azimuth;
carModel->pitch = pitch;
carModel->tm = *rtkTime;
pitch = pitch - carModel->antPitch;
+// DEBUG("yaw = %f 淇淇话瑙� %f", azimuth, pitch);
-// DEBUG("xz淇话瑙� %f", pitch);
+ // 涓诲ぉ绾挎姇褰变慨姝�
+ carModel->basePoint.X = main_ant.X + fabs(carModel->antHeight - carModel->groundHeight) * sin(toRadians(pitch)) * sin(toRadians(azimuth));
+ carModel->basePoint.Y = main_ant.Y + fabs(carModel->antHeight - carModel->groundHeight) * sin(toRadians(pitch)) * cos(toRadians(azimuth));
for (int i = 0; i < carModel->pointNum; ++i) {
double qrx = carModel->carDesc[i].distance * sin(toRadians(carModel->carDesc[i].angle));
double qry =
- carModel->carDesc[i].distance * cos(toRadians(carModel->carDesc[i].angle)) /
+ carModel->carDesc[i].distance * cos(toRadians(carModel->carDesc[i].angle)) *
cos(toRadians(pitch));
double projectDistance = sqrt(pow(qrx, 2) + pow(qry, 2));
@@ -1391,11 +1388,14 @@
carModel->carXY[i].X =
projectDistance * sin(toRadians(azimuth)) * cos(toRadians(projectAngle)) -
projectDistance * cos(toRadians(azimuth)) * sin(toRadians(projectAngle)) +
- main_ant.X;
+ carModel->basePoint.X;
carModel->carXY[i].Y =
projectDistance * sin(toRadians(azimuth)) * sin(toRadians(projectAngle)) +
projectDistance * cos(toRadians(azimuth)) * cos(toRadians(projectAngle)) +
- main_ant.Y;
+ carModel->basePoint.Y;
+
+// DEBUG("<%d>. 鏍囪窛 %f 鏍囪 %f X = %f Y = %f", i, carModel->carDesc[i].distance, carModel->carDesc[i].angle,
+// carModel->carXY[i].X, carModel->carXY[i].Y);
}
}
diff --git a/lib/src/main/cpp/driver_test.h b/lib/src/main/cpp/driver_test.h
index 83bb257..684e996 100644
--- a/lib/src/main/cpp/driver_test.h
+++ b/lib/src/main/cpp/driver_test.h
@@ -52,6 +52,8 @@
int *body;
int pointNum;
double antPitch; // 鍓嶅悗澶╃嚎鍥犲畨瑁呬綅缃箣鍥烘湁pitch瑙�
+ double antHeight; // 杞﹂《澶╃嚎楂樼▼
+ double groundHeight; // 澶у湴楂樼▼
double yaw;
double pitch;
struct car_desc_ *carDesc; // 缁忕幇鍦烘祴閲忕偣璁＄畻鍚庣殑鍊�
@@ -134,7 +136,7 @@
void SetCarMeasurePoint(double *basePoint, int *axial, int *left_front_tire,
int *right_front_tire, int *left_rear_tire, int *right_rear_tire,
- int *body, int bodyNum, double (*point)[2], int pointNum, double antPitch);
+ int *body, int bodyNum, double (*point)[2], int pointNum, double antPitch, double antHeight, double groundHeight);
void StartDriverExam(int start, int type);
void StartMapExam(int map_id, int exam);
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);
}
diff --git a/lib/src/main/cpp/master/comm_if.h b/lib/src/main/cpp/master/comm_if.h
index 2f02af1..8e23f95 100644
--- a/lib/src/main/cpp/master/comm_if.h
+++ b/lib/src/main/cpp/master/comm_if.h
@@ -76,6 +76,7 @@
void MA_MainProcMsgEntry(int cmd, const char *value);
void MA_MainProcBinMsgEntry(int cmd, const uint8_t *value, int length);
+void MA_Init(void);
void MA_NdkStart(void);
void MA_ReqRtkPlatformConfig(void);
void MA_RtkPlatformConnect(int conn, const char *ip, int port);
diff --git a/lib/src/main/cpp/native-lib.cpp b/lib/src/main/cpp/native-lib.cpp
index ba7cd20..cdc854e 100644
--- a/lib/src/main/cpp/native-lib.cpp
+++ b/lib/src/main/cpp/native-lib.cpp
@@ -26,7 +26,7 @@
const int RTK_PLATFORM_PORT = 12125;
const uint8_t phone[] = {0x20,0x19,0x10,0x15,0x00,0x00,0x00,0x01};
-const char *VIRTUAL_RTK_IP = "192.168.3.52";
+const char *VIRTUAL_RTK_IP = "192.168.16.100";
const int VIRTUAL_RTK_PORT = 9001;
static pthread_mutex_t tts_mutex = PTHREAD_MUTEX_INITIALIZER;
@@ -262,7 +262,7 @@
ConfigMCU();
DriverTestInit();
ConfigRTKModule();
-
+ MA_Init();
InitPlatform(phone, RTK_PLATFORM_IP, RTK_PLATFORM_PORT);
AppTimer_add(SendBootIndicate, 500);
diff --git a/lib/src/main/cpp/rtk_module/rtk.cpp b/lib/src/main/cpp/rtk_module/rtk.cpp
index 1a32349..5fc26f8 100644
--- a/lib/src/main/cpp/rtk_module/rtk.cpp
+++ b/lib/src/main/cpp/rtk_module/rtk.cpp
@@ -200,7 +200,7 @@
}*/
if (RxBufLen > 0) {
-#if 1
+#if 0
const uint8_t *ptr = parseGPS(RxBuf, RxBuf + RxBufLen);
if(ptr != RxBuf) {
memcpy(RxBuf, ptr, RxBufLen - (ptr - RxBuf));
diff --git a/lib/src/main/cpp/test_items2/dummy_light.cpp b/lib/src/main/cpp/test_items2/dummy_light.cpp
index 81a3a02..56aaa05 100644
--- a/lib/src/main/cpp/test_items2/dummy_light.cpp
+++ b/lib/src/main/cpp/test_items2/dummy_light.cpp
@@ -59,7 +59,7 @@
void DummyLightTTSDone(int id)
{
// 绛夎闊虫挱鎶ュ畬姣曞悗璁℃椂
- if (id == examTtsSeq) {
+ if (id == examTtsSeq && testing) {
DEBUG("DummyLightTTSDone %d", id);
for (int i = 0; i < contentNum; ++i) {
if (content[i].itemStatus == TTS_DOING) {
@@ -74,6 +74,7 @@
void TerminateDummyLightExam(void)
{
+ testing = false;
AppTimer_delete(DummyLightCheckActive);
AppTimer_delete(ExamDummyLight);
}
--
Gitblit v1.8.0