From babb05583f59eb5e2c853168490f54fc7261e84a Mon Sep 17 00:00:00 2001
From: lizhanwei <Dana_Lee1016@126.com>
Date: 星期六, 28 三月 2020 14:44:00 +0800
Subject: [PATCH] uuuu
---
lib/src/main/cpp/driver_test.cpp | 26 ++++++++++++++++++++++----
1 files changed, 22 insertions(+), 4 deletions(-)
diff --git a/lib/src/main/cpp/driver_test.cpp b/lib/src/main/cpp/driver_test.cpp
index ca85d04..8856bb9 100644
--- a/lib/src/main/cpp/driver_test.cpp
+++ b/lib/src/main/cpp/driver_test.cpp
@@ -474,7 +474,7 @@
}
}
- CarModel->antPitch = antPitch;
+ CarModel->antPitch = -0.6;//antPitch;/////////////////////////////////////
CarModel->pointNum = pointNum;
CarModel->carDesc = (struct car_desc_ *)malloc(sizeof(struct car_desc_) * pointNum);
@@ -730,6 +730,7 @@
static void ExecuteExam(const struct RtkTime* rtkTime)
{
+// DEBUG("ExecuteExam 111111111");
{
static const char *NAME[] = {"OBD_SPEED",
"ENGINE_RPM",
@@ -783,12 +784,21 @@
int cs_temp[CAR_STATUS_END];
for (int i = 0; i < CAR_STATUS_END; ++i) {
+// DEBUG("璇诲彇......");
cs_temp[i] = ReadCarStatus(i);
// DEBUG("璇诲彇 %d <---- %d", i, cs_temp[i]);
}
- for (int i = 0; i < CAR_STATUS_END; ++i) {
+ for (int i = 0; i < 2; ++i) {
+ if (cs_temp[i] != cs[i]) {
+ DEBUG("杞﹁締鐘舵�� %s = %d", NAME[i], cs_temp[i]);
+
+ cs[i] = cs_temp[i];
+ }
+ }
+
+ for (int i = 2; i < CAR_STATUS_END; ++i) {
if (cs_temp[i] != cs[i]) {
DEBUG("杞﹁締鐘舵�� %s = %s", NAME[i], VALUE[ cs_temp[i] ]);
@@ -797,7 +807,7 @@
}
}
-
+//DEBUG("-------------- 1111111111");
if (ReadCarStatus(ENGINE_RPM) < ENGINE_MIN_ROTATE) {
if (engineRuning) {
@@ -854,10 +864,13 @@
}
}
}
+
+// 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("涓嶇郴瀹夊叏甯�");
@@ -952,6 +965,7 @@
CurrExamMapIndex = -1;
}
}
+// DEBUG("ExecuteExam 2222222 -----------------------");
}
static void EngineStartHold(union sigval sig) {
@@ -1348,16 +1362,20 @@
{
carModel->basePoint = main_ant;
+
+// DEBUG("淇话瑙� %f", pitch);
carModel->yaw = azimuth;
carModel->pitch = pitch;
carModel->tm = *rtkTime;
pitch = pitch - carModel->antPitch;
+// DEBUG("xz淇话瑙� %f", pitch);
+
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));
--
Gitblit v1.8.0