From c64da51ae481dfc2a25e5a6e39bb94f22d65428a Mon Sep 17 00:00:00 2001
From: yy1717 <fctom1215@outlook.com>
Date: 星期五, 10 一月 2020 13:33:09 +0800
Subject: [PATCH] s
---
lib/src/main/cpp/driver_test.cpp | 90 ++++++++++++++++++++++++++++++++++++++++++---
1 files changed, 84 insertions(+), 6 deletions(-)
diff --git a/lib/src/main/cpp/driver_test.cpp b/lib/src/main/cpp/driver_test.cpp
index 55464ad..945f6bb 100644
--- a/lib/src/main/cpp/driver_test.cpp
+++ b/lib/src/main/cpp/driver_test.cpp
@@ -86,6 +86,19 @@
struct car_desc_ *CarDesc = NULL;
+typedef struct {
+ PointF basePoint;
+ int axial[2];
+ int left_front_tire[2];
+ int right_front_tire[2];
+ int left_rear_tire[2];
+ int right_rear_tire[2];
+ int pointNum;
+ struct car_desc_ *carDesc;
+} car_model;
+
+static car_model *CarModel = NULL;
+
struct car_coord_ *CarCoord = NULL;
static double currSpeed = 0, currAzimuth = 0;
static PointF currCoord;
@@ -122,6 +135,7 @@
{
memset(&MapList, 0, sizeof(MapList));
MapNum = 0;
+ CarModel = NULL;
carModelDescFile = (carModelDesc_t *)malloc(sizeof(carModelDesc_t));
@@ -259,22 +273,20 @@
ErrorList.clear();
TextSpeak("寮�濮嬫祴璇�");
-
- AppTimer_add(ReadDriverExamPrimerTimeout, D_SEC(2));
- ReadDriverExamPrimer();
}
static void ReadDriverExamPrimerTimeout(union sigval sig)
{
AppTimer_delete(ReadDriverExamPrimerTimeout);
+ AppTimer_add(ReadDriverExamPrimerTimeout, D_SEC(2));
ReadDriverExamPrimer();
}
void ReadDriverExamPrimer(void)
{
-// MA_ReadMap();
-// MA_ReadCar();
-// MA_ReadSensor();
+ MA_ReadMap();
+ MA_ReadCar();
+ MA_ReadSensor();
}
void ClearMap(void)
@@ -327,6 +339,56 @@
double (*point)[2], int pointNum)
{
DEBUG("SetCarMeasurePoint pointNum %d", pointNum);
+
+ if (point == NULL || pointNum == 0) return;
+
+ if (CarModel != NULL) {
+ CarModel->carDesc != NULL;
+ free(CarModel->carDesc);
+ free(CarModel);
+ CarModel = NULL;
+ }
+
+ CarModel = (car_model *)malloc(sizeof(CarModel));
+ CarModel->basePoint.X = basePoint[0];
+ CarModel->basePoint.Y = basePoint[1];
+ CarModel->axial[0] = axial[0];
+ CarModel->axial[1] = axial[1];
+ CarModel->left_front_tire[0] = left_front_tire[0];
+ CarModel->left_front_tire[1] = left_front_tire[1];
+ CarModel->right_front_tire[0] = right_front_tire[0];
+ CarModel->right_front_tire[1] = right_front_tire[1];
+ CarModel->left_rear_tire[0] = left_rear_tire[0];
+ CarModel->left_rear_tire[1] = left_rear_tire[1];
+ CarModel->right_rear_tire[0] = right_rear_tire[0];
+ CarModel->right_rear_tire[1] = right_rear_tire[1];
+ CarModel->pointNum = pointNum;
+
+ CarModel->carDesc = (struct car_desc_ *)malloc(sizeof(struct car_desc_) * pointNum);
+
+ // 娴嬮噺鍧愭爣杞崲涓鸿窛绂�-瑙掑害褰㈠紡
+ double C02 = (point[0][0]-basePoint[0])*(point[0][0]-basePoint[0]) +
+ (point[0][1]-basePoint[1])*(point[0][1]-basePoint[1]);
+ double C0 = sqrt(C02);
+
+ CarModel->carDesc[0].distance = sqrt(C02);
+ CarModel->carDesc[0].angle = 0.0;
+
+ for (int i = 1; i < pointNum; ++i) {
+ double dis2 = (point[i][0]-basePoint[0])*(point[i][0]-basePoint[0]) +
+ (point[i][1]-basePoint[1])*(point[i][1]-basePoint[1]);
+ double dis = sqrt(dis2);
+
+ CarModel->carDesc[i].distance = dis;
+
+ CarModel->carDesc[i].angle = 180 * acos((dis2 + C02 - ((point[i][0]-point[0][0])*(point[i][0]-point[0][0]) +
+ (point[i][1]-point[0][1])*(point[i][1]-point[0][1])))/(2*C0*dis)) / M_PI;
+
+ if (i > axial[1])
+ CarModel->carDesc[i].angle = 360.0 - CarModel->carDesc[i].angle;
+ }
+
+ DEBUG("SetCarMeasurePoint Calc Over");
}
void SetSensorCfg(int (*sensor)[2], int sensorNum)
@@ -334,6 +396,22 @@
DEBUG("SetSensorCfg sensorNum %d", sensorNum);
}
+void StartDriverExam(int start)
+{
+ bool err = false;
+
+ if (MapNum == 0) {
+ err = true;
+ MA_SendExamStatus(0, -1);
+ }
+ if (CarModel == NULL) {
+ err = true;
+ MA_SendExamStatus(0, -2);
+ }
+ if (!err)
+ MA_SendExamStatus(1, 0);
+}
+
void UpdateRTKInfo(struct rtk_info *s)
{
struct tm test_tm;
--
Gitblit v1.8.0