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