From 4bd08f0355b6b2cf3c027202d5ad301b4e182953 Mon Sep 17 00:00:00 2001
From: yy1717 <fctom1215@outlook.com>
Date: 星期五, 31 三月 2023 17:16:22 +0800
Subject: [PATCH] 科目二修改
---
lib/src/main/cpp/driver_test.cpp | 1043 ++++++++++++++++-----------------------------------------
1 files changed, 297 insertions(+), 746 deletions(-)
diff --git a/lib/src/main/cpp/driver_test.cpp b/lib/src/main/cpp/driver_test.cpp
index 365bbd8..1563f56 100644
--- a/lib/src/main/cpp/driver_test.cpp
+++ b/lib/src/main/cpp/driver_test.cpp
@@ -35,10 +35,11 @@
#include "test_common/car_sensor.h"
#include "mcu/mcu_if.h"
#include "test_common/car_sensor.h"
-#include "test_items2/road_exam.h"
#include "test_items/area_exam.h"
-#include "test_items2/prepare.h"
#include "test_common/odo_graph.h"
+#include "map.h"
+#include "common/semaphore.h"
+#include "common/string_util.h"
#define DEBUG(fmt, args...) LOGD("<driver_test> <%s>: " fmt, __func__, ##args)
@@ -133,15 +134,18 @@
vector<ExamFault> ExamFaultList;
static int examFaultIndex = 0;
-static LIST_AREA_MAP AreaMapList;
+//static LIST_AREA_MAP AreaMapList;
+static area_map_t AreaMap;
+
+ilovers::semaphore sem(0);
static road_exam_map RoadMap;
static int exam_dummy_light;
-static car_model *CarModel = NULL;
+static car_model_t CarModel;
-static LIST_CAR_MODEL CarModelList; // 涓�娈垫椂闂寸殑杞﹁締杞ㄨ抗闆嗗悎
+//static LIST_CAR_MODEL CarModelList; // 涓�娈垫椂闂寸殑杞﹁締杞ㄨ抗闆嗗悎
static struct dummy_light_exam *DummyLightContent;
static int DummyLightContentSize;
@@ -155,35 +159,32 @@
#define RTK_BUFFER_SIZE 100
#define CAR_MODEL_CACHE_SIZE 10
-static rtk_info *RtkBuffer = NULL;
-static int RtkBufferNum = 0, RtkBufferIn = 0;
+static std::list<rtk_info_t> RtkInfoList;
-static std::mutex clock_mutex;
-static struct RtkTime rtkClock;
+// 瀛樺叆鍓嶅悗2缁勫缓妯℃暟鎹�
+static modeling_t realtimeBodyModeling[2];
+static motion_t realtimeMotionStatus;
+
+static prime_t prime;
static void SetExamParamDefault(void);
static void EngineStartHold(apptimer_var_t val);
-static void ExecuteExam(const struct RtkTime* rtkTime);
-static void ExecuteExam(double speed, int move, double azimuth, const struct RtkTime* rtkTime);
-static uint32_t CalcTimeDiff(const rtk_info *a, const rtk_info *b);
+static motion_t CalcMotionState(std::list<rtk_info_t> &s);
+static void ExecuteExam(prime_t &prime);
+
static void ReadDriverExamPrimerTimeout(apptimer_var_t val);
-static void UpdateCarBodyCoord(struct RtkTime *rtkTime, double azimuth, double pitch, double roll, PointF main_ant, car_model *carModel);
-static bool UpdateCarCoord(double &spd, int &mov, int &idx);
-
-static void PrintObdInfo(struct RtkTime *rtkTime, double speed);
-
-
-static void ClockGener(apptimer_var_t val);
+static void CalcBodyModeling(modeling_t &car, car_model_t &carModel, rtk_info_t &rtk);
+static void work_thread(void);
+static void UploadModeling(motion_t &motion, modeling_t &modeling);
void DriverTestInit(void)
{
ExamStart = false;
SetExamParamDefault();
- CarModel = NULL;
- CarModelList.clear();
+// CarModelList.clear();
- AreaMapList.clear();
+// AreaMapList.clear();
RoadMap.roads.clear();
RoadMap.specialAreas.clear();
@@ -195,10 +196,7 @@
DummyLightContentSize = 0;
DummyLightContent = NULL;
- RtkBuffer = (rtk_info *) malloc(RTK_BUFFER_SIZE * sizeof(rtk_info));
- RtkBufferNum = RtkBufferIn = 0;
-
-// AppTimer_add(ClockGener, 200); // App鑷繁浜х敓瀹氭椂鑺傛媿
+ std::thread(work_thread).detach();
}
static void SetExamParamDefault(void)
@@ -313,14 +311,21 @@
if (ExamStart)
return;
- for (int i = 0; i < AreaMapList.size(); ++i) {
- if (AreaMapList[i].map.point != NULL)
- free(AreaMapList[i].map.point);
- if (AreaMapList[i].map2.point != NULL)
- free(AreaMapList[i].map2.point);
- }
+// for (int i = 0; i < AreaMapList.size(); ++i) {
+// if (AreaMapList[i].map.point != NULL)
+// free(AreaMapList[i].map.point);
+// if (AreaMapList[i].map2.point != NULL)
+// free(AreaMapList[i].map2.point);
+// }
+//
+// LIST_AREA_MAP().swap(AreaMapList);
- LIST_AREA_MAP().swap(AreaMapList);
+
+ vector<curve_map_t>().swap(AreaMap.curve_map);
+ vector<park_button_map_t>().swap(AreaMap.park_button_map);
+ vector<park_edge_map_t>().swap(AreaMap.park_edge_map);
+ vector<turn_a90_map_t>().swap(AreaMap.turn_a90_map);
+ vector<uphill_map_t>().swap(AreaMap.uphill_map);
}
void AddAreaMap(int id, int type, const double (*map)[2], int pointNum, const double (*map2)[2], int pointNum2)
@@ -329,33 +334,48 @@
return;
DEBUG("鍔犲叆鍦板浘淇℃伅 id %d type %d pointNum %d point2Num %d", id, type, pointNum, pointNum2);
- struct area_exam_map newMap;
+// struct area_exam_map newMap;
+//
+// newMap.id = id;
+// newMap.type = type;
+// newMap.map.num = pointNum;
+// newMap.map2.num = 0;
+// newMap.map.point = NULL;
+// newMap.map2.point = NULL;
+//
+// if (pointNum > 0) {
+// newMap.map.point = (PointF *) malloc(pointNum * sizeof(PointF));
+// for (int i = 0; i < pointNum; ++i) {
+// newMap.map.point[i].X = map[i][0];
+// newMap.map.point[i].Y = map[i][1];
+// }
+// }
+//
+// if (pointNum2 > 0 && map2 != NULL) {
+// newMap.map2.num = pointNum2;
+// newMap.map2.point = (PointF *) malloc(pointNum2 * sizeof(PointF));
+// for (int i = 0; i < pointNum2; ++i) {
+// newMap.map2.point[i].X = map2[i][0];
+// newMap.map2.point[i].Y = map2[i][1];
+// }
+// }
+//
+// AreaMapList.push_back(newMap);
+}
- newMap.id = id;
- newMap.type = type;
- newMap.map.num = pointNum;
- newMap.map2.num = 0;
- newMap.map.point = NULL;
- newMap.map2.point = NULL;
+void AddCurveMap(curve_map_t &map)
+{
+ AreaMap.curve_map.push_back(map);
+}
- if (pointNum > 0) {
- newMap.map.point = (PointF *) malloc(pointNum * sizeof(PointF));
- for (int i = 0; i < pointNum; ++i) {
- newMap.map.point[i].X = map[i][0];
- newMap.map.point[i].Y = map[i][1];
- }
- }
+void AddParkButtonMap(park_button_map_t &map)
+{
+ AreaMap.park_button_map.push_back(map);
+}
- if (pointNum2 > 0 && map2 != NULL) {
- newMap.map2.num = pointNum2;
- newMap.map2.point = (PointF *) malloc(pointNum2 * sizeof(PointF));
- for (int i = 0; i < pointNum2; ++i) {
- newMap.map2.point[i].X = map2[i][0];
- newMap.map2.point[i].Y = map2[i][1];
- }
- }
-
- AreaMapList.push_back(newMap);
+void AddParkEdgeMap(park_edge_map_t &map)
+{
+ AreaMap.park_edge_map.push_back(map);
}
void CleanRoadMap(void)
@@ -426,75 +446,59 @@
if (point == NULL || pointNum == 0 || ExamStart) return;
- if (CarModel != NULL) {
- if (CarModel->body != NULL)
- free(CarModel->body);
- if (CarModel->carDesc != NULL)
- free(CarModel->carDesc);
- if (CarModel->carXY != NULL)
- free(CarModel->carXY);
- free(CarModel);
- CarModel = NULL;
- }
+ vector<int>().swap(CarModel.body);
+ vector<car_desc_t>().swap(CarModel.carDesc);
- CarModel = (car_model *)malloc(sizeof(car_model));
- 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.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->bodyNum = bodyNum;
if (bodyNum == 0 || body == NULL) {
- CarModel->bodyNum = 6;
- CarModel->body = (int *) malloc(sizeof(int) * 6);
for (int i = 0; i < 6; ++i) {
- CarModel->body[i] = i;
+ CarModel.body.push_back(i);
}
} else {
- CarModel->body = (int *) malloc(sizeof(int) * CarModel->bodyNum);
- for (int i = 0; i < CarModel->bodyNum; ++i) {
- CarModel->body[i] = body[i];
+ for (int i = 0; i < bodyNum; ++i) {
+ CarModel.body.push_back(body[i]);
}
}
- CarModel->antPitch = antPitch;
- CarModel->antHeight = antHeight;
- CarModel->groundHeight = groundHeight;
+ CarModel.antPitch = antPitch;
+ CarModel.antHeight = antHeight;
+ CarModel.groundHeight = groundHeight;
- CarModel->pointNum = pointNum;
- CarModel->carDesc = (struct car_desc_ *)malloc(sizeof(struct car_desc_) * pointNum);
- CarModel->carXY = (PointF *) malloc(sizeof(PointF) * pointNum);
-
+ CarModel.carDesc.resize(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;
+ 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].distance = dis;
- CarModel->carDesc[i].angle = 180 * acos((dis2 + C02 - ((point[i][0]-point[0][0])*(point[i][0]-point[0][0]) +
+ 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;
+ CarModel.carDesc[i].angle = 360.0 - CarModel.carDesc[i].angle;
- DEBUG("鍔犲叆鐐�<%d> 璺濈 %f 瑙掑害 %f", i, CarModel->carDesc[i].distance, CarModel->carDesc[i].angle);
+ DEBUG("鍔犲叆鐐�<%d> 璺濈 %f 瑙掑害 %f", i, CarModel.carDesc[i].distance, CarModel.carDesc[i].angle);
}
// CarModel->carDesc[0].distance = 0.2465;
@@ -518,81 +522,6 @@
DEBUG("SetCarMeasurePoint Calc Over");
}
-/***********************************************
- * TIME1 - TIME2: millisecond
- * @param rtkTime1
- * @param rtkTime2
- * @return
- */
-uint32_t TimeGetDiff(const struct RtkTime *rtkTime1, const struct RtkTime *rtkTime2)
-{
- char tm1[64], tm2[64];
-
- sprintf(tm1, "%02d%02d%02d%02d%02d%02d%03d", rtkTime1->YY, rtkTime1->MM, rtkTime1->DD, rtkTime1->hh, rtkTime1->mm, rtkTime1->ss, rtkTime1->mss*10);
- sprintf(tm2, "%02d%02d%02d%02d%02d%02d%03d", rtkTime2->YY, rtkTime2->MM, rtkTime2->DD, rtkTime2->hh, rtkTime2->mm, rtkTime2->ss, rtkTime2->mss*10);
-
- if (strcmp(tm1, tm2) < 0) {
- return (uint32_t)(-1);
- }
-
- if (rtkTime1->YY == rtkTime2->YY && rtkTime1->MM == rtkTime2->MM && rtkTime1->DD == rtkTime2->DD) {
- return TimeGetDiff(rtkTime1->hh, rtkTime1->mm, rtkTime1->ss, rtkTime1->mss*10,
- rtkTime2->hh, rtkTime2->mm, rtkTime2->ss, rtkTime2->mss*10);
- } else {
- return (TimeMakeComposite(2000 + rtkTime1->YY, rtkTime1->MM, rtkTime1->DD, rtkTime1->hh, rtkTime1->mm, rtkTime1->ss) -
- TimeMakeComposite(2000 + rtkTime2->YY, rtkTime2->MM, rtkTime2->DD, rtkTime2->hh, rtkTime2->mm, rtkTime2->ss)) * 1000
- + (1000 + rtkTime1->mss*10 - rtkTime2->mss*10) % 1000;
- }
-}
-
-void SetDummyLightExam(int n, struct dummy_light_exam *cfg)
-{
- DEBUG("鑾峰彇妯℃嫙璺�冪伅鍏夋祴璇曢」鐩� N = %d %d", n, ExamStart);
-
- static const int CONV_TABLE[] = {(FLASH_BEAM_LAMP<<8)+OFF_LIGHT,
- (TURN_SIGNAL_LAMP<<8)+OFF_LIGHT,
- (TURN_SIGNAL_LAMP<<8)+OFF_LIGHT,
- (TURN_SIGNAL_LAMP<<8)+OFF_LIGHT,
- (FOG_LAMP<<8)+OFF_LIGHT,
- (CLEARANCE_LAMP<<8)+OFF_LIGHT,
- (MAIN_BEAM_LAMP<<8)+OFF_LIGHT,
- (DIPPED_BEAM_LAMP<<8)+OFF_LIGHT,
- 0,
- (DIPPED_BEAM_LAMP<<8)+DIPPED_BEAM_LIGHT,
- (MAIN_BEAM_LAMP<<8)+MAIN_BEAM_LIGHT,
- (CLEARANCE_LAMP<<8)+CLEARANCE_LIGHT,
- (FOG_LAMP<<8)+FOG_LIGHT,
- (TURN_SIGNAL_LAMP<<8)+LEFT_TURN_LIGHT,
- (TURN_SIGNAL_LAMP<<8)+RIGHT_TURN_LIGHT,
- (TURN_SIGNAL_LAMP<<8)+HAZARD_LIGHTS,
- (FLASH_BEAM_LAMP<<8)+FLASH_BEAM_LIGHT};
-
- const int *cov = CONV_TABLE + 8;
-
- if (ExamStart) return;
-
- if (DummyLightContent != NULL) {
- delete []DummyLightContent;
- DummyLightContent = NULL;
- DummyLightContentSize = 0;
- }
-
- DummyLightContent = new struct dummy_light_exam[n];
- DummyLightContentSize = n;
-
- for (int i = 0; i < n; i++) {
- DummyLightContent[i].item = cfg[i].item;
- DummyLightContent[i].tts = cfg[i].tts;
- DummyLightContent[i].wrongCode = cfg[i].wrongCode;
- // Sensor Name<<8 + Sensor Status
- for (int j = 0; j < cfg[i].process.size(); ++j) {
- DummyLightContent[i].process.push_back(cov[cfg[i].process[j]]);
- }
- for (int j = 0; j < cfg[i].solution.size(); ++j) {
- DummyLightContent[i].solution.push_back(cov[cfg[i].solution[j]]);
- }
- }
-}
void StartDriverExam(int start, int type)
{
@@ -602,9 +531,7 @@
if (start == 0) {
DEBUG("缁撴潫鑰冭瘯");
- TerminateRoadExam();
TerminateAreaExam();
-
ExamStart = false;
MA_SendExamStatus(0, 0);
@@ -613,12 +540,12 @@
// type = TEST_TYPE_ROAD_CALIBRATE;
- if (AreaMapList.size() == 0 && type == TEST_TYPE_AREA) {
+ /*if (AreaMapList.size() == 0 && type == TEST_TYPE_AREA) {
DEBUG("娌℃湁鍦鸿�冨湴鍥�");
err = true;
MA_SendExamStatus(0, -1);
- }
- if (CarModel == NULL) {
+ }*/
+ if (CarModel.carDesc.size() == 0) {
DEBUG("娌℃湁杞︽ā");
err = true;
MA_SendExamStatus(0, -2);
@@ -656,311 +583,197 @@
}
if (type == TEST_TYPE_ROAD_TRUE_LIGHT) {
RoadMap.calibrate = 0;
- InitRoadExam(RoadMap);
}
if (type == TEST_TYPE_AREA) {
InitAreaExam();
}
if (type == TEST_TYPE_ROAD_CALIBRATE) {
RoadMap.calibrate = 1;
- InitRoadExam(RoadMap);
}
}
MA_SendExamStatus(1, 0);
}
}
-static void ClockGener(apptimer_var_t val)
+/***************************************
+ * 瑙﹀彂鑰冭瘯璇勫垽
+ */
+static void work_thread(void)
{
- rtk_info rtk;
+ while (true) {
+ sem.wait();
- rtk.qf = 3;
- rtk.heading = 0;
- rtk.pitch = 0;
- rtk.roll = 0;
- rtk.x = 0;
- rtk.y = 0;
-
- struct timeval tv;
- struct timezone tz;
-
- gettimeofday(&tv, &tz);
-
- struct tm *pTime = localtime(&tv.tv_sec);
-
- rtk.YY = (pTime->tm_year + 1900) % 100;
- rtk.MM = pTime->tm_mon + 1;
- rtk.DD = pTime->tm_mday;
- rtk.hh = pTime->tm_hour;
- rtk.mm = pTime->tm_min;
- rtk.ss = pTime->tm_sec;
- rtk.dss = tv.tv_usec / 1000;
-
-// DEBUG("妯℃嫙鏃堕棿 锛�%d-%d-%d %d:%d:%d.%d", rtk.YY, 1+pTime->tm_mon, pTime->tm_mday, pTime->tm_hour, pTime->tm_min, pTime->tm_sec, rtk.dss);
-
- UpdateRTKInfo(&rtk);
-
- AppTimer_add(ClockGener, 200);
+ // 璁＄畻褰撳墠杞﹂�燂紝鍓嶈繘鍚庨��鎴栧仠姝�
+ realtimeMotionStatus = CalcMotionState(RtkInfoList);
+ // 绱姞閲岀▼
+ UpdataOdo(realtimeMotionStatus);
+ // 璁＄畻褰撳墠杞﹁締鐐瑰潗鏍囧��
+ prime.prev_modeling_index = prime.curr_modeling_index;
+ prime.curr_modeling_index = (prime.curr_modeling_index+1) % (sizeof (realtimeBodyModeling) / sizeof (realtimeBodyModeling[0]));
+ CalcBodyModeling(realtimeBodyModeling[prime.curr_modeling_index], CarModel, RtkInfoList.front());
+ // 鍚慤I涓婃姤杞﹁締鐐瑰潗鏍�
+ UploadModeling(realtimeMotionStatus, realtimeBodyModeling[prime.curr_modeling_index]);
+ // 瑙﹀彂鑰冭瘯椤圭洰
+ if (ExamStart) {
+ ExecuteExam(prime);
+ }
+ }
}
-void UpdateRTKInfo(const rtk_info *s)
+static motion_t CalcMotionState(std::list<rtk_info_t> &s)
{
- std::unique_lock<std::mutex> lk(clock_mutex);
- rtkClock.YY = s->YY;
- rtkClock.MM = s->MM;
- rtkClock.DD = s->DD;
- rtkClock.hh = s->hh;
- rtkClock.mm = s->mm;
- rtkClock.ss = s->ss;
- rtkClock.mss = s->dss;
- lk.unlock();
-
- if (ExamStart) {
- ExecuteExam(&rtkClock); // 鎵ц鏃犻渶杞﹁締瀹氫綅鐨勯」鐩�
- }
-
- if (s->qf == 3) {
- RtkBuffer[RtkBufferIn] = *s;
- RtkBufferIn = (RtkBufferIn + 1) % RTK_BUFFER_SIZE;
- if (RtkBufferNum < RTK_BUFFER_SIZE)
- RtkBufferNum++;
- } else {
- return;
- }
+ motion_t motion;
double speed;
- int move;
- int index;
+ move_status_t move;
- if (UpdateCarCoord(speed, move, index)) {
- struct carBrief brief;
+ if (s.size() < 2) {
+ return motion;
+ }
- sprintf(brief.utc, "%04d%02d%02d%02d%02d%02d.%02d", 2000 + RtkBuffer[index].YY,
- RtkBuffer[index].MM, RtkBuffer[index].DD, RtkBuffer[index].hh,
- RtkBuffer[index].mm, RtkBuffer[index].ss, RtkBuffer[index].dss);
+ auto curr = s.begin();
- brief.qf = RtkBuffer[index].qf;
- brief.map_id = -1;//GetMapId(CurrExamMapIndex, MapList, MapNum);
- brief.move = move;
- brief.speed = speed * 3.6;
- brief.heading = RtkBuffer[index].heading;
- brief.main_ant[0] = RtkBuffer[index].x;
- brief.main_ant[1] = RtkBuffer[index].y;
+ motion.timestamp = curr->utc_time;
- brief.axial[0] = CarModel->axial[0];
- brief.axial[1] = CarModel->axial[1];
- brief.left_front_tire[0] = CarModel->left_front_tire[0];
- brief.left_front_tire[1] = CarModel->left_front_tire[1];
- brief.right_front_tire[0] = CarModel->right_front_tire[0];
- brief.right_front_tire[1] = CarModel->right_front_tire[1];
- brief.left_rear_tire[0] = CarModel->left_rear_tire[0];
- brief.left_rear_tire[1] = CarModel->left_rear_tire[1];
- brief.right_rear_tire[0] = CarModel->right_rear_tire[0];
- brief.right_rear_tire[1] = CarModel->right_rear_tire[1];
+ // 鏌ユ壘1绉掑墠鐨勭偣,濡傛灉鎵句笉鍒板垯瑙嗕负鍋滆溅
+ auto prev = s.begin();
+ std::advance(prev, 1);
- brief.bodyNum = CarModel->bodyNum;
- brief.body = (int *) malloc(sizeof(int) * CarModel->bodyNum);
- for (int i = 0; i < CarModel->bodyNum; ++i) {
- brief.body[i] = CarModel->body[i];
+ for (; prev != s.end(); ++prev) {
+ if (prev->utc_time - curr->utc_time > 3000) {
+ return motion;
+ }
+ if (prev->utc_time - curr->utc_time >= 1000) {
+ break;
+ }
+ }
+
+ if (prev == s.end()) {
+ return motion;
+ }
+
+ // 璁＄畻閫熷害(绫�/绉�)銆佸墠杩涘悗閫�
+ speed = sqrt(pow(curr->x - prev->x, 2) + pow(curr->y - prev->y, 2)) * 1000 /
+ static_cast<double>(curr->utc_time - prev->utc_time);
+
+ double deg = 0.0;
+
+ if (speed < 0.05) {
+ // 鍋滆溅
+ move = STOP;
+ } else {
+ // 鍒ゆ柇鍓嶈繘杩樻槸鍚庨��
+ if (fabs(curr->y - prev->y) <= GLB_EPSILON) {
+ if (curr->x > prev->x) {
+ deg = 90;
+ } else {
+ deg = 270;
+ }
+ } else if (fabs(curr->x - prev->x) <= GLB_EPSILON) {
+ if (curr->y > prev->y) {
+ deg = 0;
+ } else {
+ deg = 180;
+ }
+ } else {
+ deg = toDegree(atan(fabs(curr->x - prev->x) /
+ fabs(curr->y - prev->y)));
+
+ if (curr->x > prev->x &&
+ curr->y > prev->y) {
+
+ } else if (curr->x < prev->x &&
+ curr->y > prev->y) {
+ deg = 360 - deg;
+ } else if (curr->x < prev->x &&
+ curr->y < prev->y) {
+ deg = 180 + deg;
+ } else if (curr->x > prev->x &&
+ curr->y < prev->y) {
+ deg = 180 - deg;
+ }
}
- brief.pointNum = CarModel->pointNum;
- brief.point = (double *) malloc(CarModel->pointNum * 2 * sizeof(double));
- for (int i = 0, j = 0; i < CarModel->pointNum; ++i) {
- brief.point[j++] = round(CarModel->carXY[i].X, 4);
- brief.point[j++] = round(CarModel->carXY[i].Y, 4);
+ deg = fabs(curr->heading - deg);
+ if (deg > 180) {
+ deg = 360 - deg;
+ }
+ if (deg < 90) {
+ // 鍓嶈繘
+ move = FORWARD;
+ } else {
+ // 鍚庨��
+ move = BACKWARD;
+ }
+ }
+
+ motion.speed = speed;
+ motion.move = move;
+
+ return motion;
+}
+
+void UpdateRTKInfo(const rtk_info_t *s)
+{
+ RtkInfoList.push_front(*s);
+
+ while (RtkInfoList.size() > 100) {
+ RtkInfoList.pop_back();
+ }
+ sem.signal();
+}
+
+static void UploadModeling(motion_t &motion, modeling_t &modeling)
+{
+ struct carBrief brief;
+
+ struct TimeStructure ts;
+
+ TimeBreakdown(modeling.utc_time / 1000, &ts);
+
+ sprintf(brief.utc, "%04d%02d%02d%02d%02d%02d.%02d", ts.Year, ts.Month, ts.Day,
+ ts.Hour, ts.Minute, ts.Second, (modeling.utc_time % 1000) / 10);
+
+ brief.qf = 3;
+ brief.map_id = -1;//GetMapId(CurrExamMapIndex, MapList, MapNum);
+ brief.move = motion.move;
+ brief.speed = ConvertMs2KMh(motion.speed);
+ brief.heading = modeling.yaw;
+ brief.main_ant[0] = modeling.base_point.X;
+ brief.main_ant[1] = modeling.base_point.Y;
+
+ brief.axial[0] = CarModel.axial[0];
+ brief.axial[1] = CarModel.axial[1];
+ brief.left_front_tire[0] = CarModel.left_front_tire[0];
+ brief.left_front_tire[1] = CarModel.left_front_tire[1];
+ brief.right_front_tire[0] = CarModel.right_front_tire[0];
+ brief.right_front_tire[1] = CarModel.right_front_tire[1];
+ brief.left_rear_tire[0] = CarModel.left_rear_tire[0];
+ brief.left_rear_tire[1] = CarModel.left_rear_tire[1];
+ brief.right_rear_tire[0] = CarModel.right_rear_tire[0];
+ brief.right_rear_tire[1] = CarModel.right_rear_tire[1];
+
+ brief.body.assign(CarModel.body.begin(), CarModel.body.end());
+
+ for (auto po: modeling.points) {
+ brief.point.push_back({round(po.X, 4), round(po.Y, 4)});
}
MA_SendCarPosition(&brief);
-
- free(brief.body);
- free(brief.point);
-
- struct RtkTime rtkTime;
- double azimuth = RtkBuffer[index].heading;
-
- rtkTime.YY = RtkBuffer[index].YY;
- rtkTime.MM = RtkBuffer[index].MM;
- rtkTime.DD = RtkBuffer[index].DD;
- rtkTime.hh = RtkBuffer[index].hh;
- rtkTime.mm = RtkBuffer[index].mm;
- rtkTime.ss = RtkBuffer[index].ss;
- rtkTime.mss = RtkBuffer[index].dss;
-
- if (ExamStart) {
- ExecuteExam(speed, move, azimuth, &rtkTime);
- }
-
-// PrintObdInfo(&rtkTime, speed);
- }
}
-static void PrintObdInfo(struct RtkTime *rtkTime, double speed) {
- static struct RtkTime cTime = *rtkTime;
-
- if (TimeGetDiff(rtkTime, &cTime) >= D_SEC(3)) {
- cTime = *rtkTime;
- DEBUG("GEAR %d RPM %d OBD_SPEED %f SPEED %f",
- ReadCarStatus(GEAR) - GEAR_N,
- ReadCarStatus(ENGINE_RPM),
- ((double)ReadCarStatus(OBD_SPEED)) / 10.0,
- speed * 3.6);
- }
-}
-
-static void ExecuteExam(const struct RtkTime* rtkTime)
-{
- {
- static const char *NAME[] = {"OBD_SPEED",
- "ENGINE_RPM",
- "鎸′綅",
- "杞悜鐏�",
- "杩戝厜鐏�",
- "闆剧伅",
- "绀哄粨鐏�",
- "闂伅鎻愮ず",
- "杩滃厜鐏�",
- "瀹夊叏甯�",
- "鍚姩寮曟搸",
- "鍒硅溅",
- "鎵嬪埞",
- "鍓埞杞�",
- "杞﹂棬",
- "缁曡溅涓�",
- "缁曡溅浜�",
- "缁曡溅涓�",
- "缁曡溅鍥�",
- "CAR_STATUS_END"};
-
- static const char *VALUE[] = {
- "鍏抽棴",
- "鍛婅鐏�",
- "宸﹁浆淇″彿",
- "鍙宠浆淇″彿",
- "绀哄粨鐏寒",
- "杩戝厜鐏寒",
- "杩滃厜鐏寒",
- "杩滆繎鍒囨崲",
- "闆剧伅浜�",
- "鎻掑叆",
- "鍦ㄥ惎鍔ㄤ綅",
- "绌烘。",
- "涓�妗�",
- "浜屾。",
- "涓夋。",
- "鍥涙。",
- "浜旀。",
- "鍊掓尅",
- "韪╀笅",
- "闂ㄥ叧闂�",
- "缁曡溅鍙戠敓"
- };
-
-
-
- static int cs[CAR_STATUS_END] = {0};
-
- 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 < 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] ]);
-
- cs[i] = cs_temp[i];
-
-// char buff[128];
-// sprintf(buff, "%s锛�%s", NAME[i], VALUE[ cs_temp[i] ]);
-// PlayTTS(buff, NULL);
- }
- }
-
- }
-
- if (ReadCarStatus(ENGINE_RPM) < ENGINE_MIN_ROTATE) {
- if (engineRuning) {
- engineRuning = false;
- if (ExamType == TEST_TYPE_AREA) {
- // 鐔勭伀1娆★紝鎵�10鍒�
- AddExamFault(10210, rtkTime);
- } else {
- AddExamFault(30208, rtkTime);
- }
- }
- } else {
- engineRuning = true;
- }
-
- if (ReadCarStatus(ENGINE_START) == ENGINE_START_ACTIVE) {
- if (!engineStart) {
- DEBUG("妫�娴嬪埌鐐圭伀");
- engineStart = true;
- if (ReadCarStatus(GEAR) != GEAR_N) {
- DEBUG("涓嶅湪绌烘尅鐐圭伀");
- // 涓嶆槸绌烘尅鐐圭伀锛屼笉鍚堟牸
- if (ExamType == TEST_TYPE_AREA)
- AddExamFault(10105, rtkTime);
- else
- AddExamFault(30105, rtkTime);
- }
- AppTimer_delete(EngineStartHold);
- AppTimer_add(EngineStartHold, examParam.hold_start_key_limit_time);
- }
- } else if (engineStart) {
- DEBUG("妫�娴嬪埌鍏抽棴鐐圭伀");
- engineStart = false;
- AppTimer_delete(EngineStartHold);
- }
-
- if (ExamType == TEST_TYPE_ROAD_DUMMY_LIGHT) {
- if (exam_dummy_light == 0) {
-// StartPrepare();
-//// StartDummyLightExam(DummyLightContent, DummyLightContentSize, rtkTime);
-// exam_dummy_light = 1;
-// DEBUG("寮�濮嬩笂杞﹀噯澶�");
-
- exam_dummy_light = 2; // 棰戦棴涓婅溅鍑嗗
- } else if (exam_dummy_light == 2) {
- DEBUG("寮�濮嬬伅鍏夎�冭瘯");
- StartDummyLightExam(DummyLightContent, DummyLightContentSize, rtkTime);
- exam_dummy_light = 3;
- } else if (exam_dummy_light == 3) {
- if (!ExecuteDummyLightExam(rtkTime)) {
- exam_dummy_light = 4;
- // 姹囨姤鐏厜鑰冭瘯缁撴潫
- DEBUG("鐏厜鑰冭瘯缁撴潫");
- InitRoadExam(RoadMap);
- }
- }
- }
-}
-
-static void ExecuteExam(double speed, int move, double azimuth, const struct RtkTime* rtkTime)
+static void ExecuteExam(prime_t &prime)
{
static bool rec = false;
static bool handBreakActive = false, handBreakActive2 = false;
static double startCarMoveDistance;
- static int prevMove = 0;
+ static move_status_t prevMove = STOP;
- if (move != 0) {
+ if (prime.pMotion->move != STOP) {
if (ReadCarStatus(SEATBELT) == EJECT_SEATBELT && !reportSeatbeltEject) {
DEBUG("涓嶇郴瀹夊叏甯�");
reportSeatbeltEject = true;
- AddExamFault(ExamType == TEST_TYPE_AREA? 10101: 30101, rtkTime);
+ AddExamFault(ExamType == TEST_TYPE_AREA? 10101: 30101);
}
if (rec) {
if (!handBreakActive2 && ReadOdo() - startCarMoveDistance >= examParam.start_car_limit_distance) {
@@ -970,11 +783,11 @@
if (ReadCarStatus(HAND_BREAK) == BREAK_ACTIVE) {
DEBUG("Handbreak active move over 10m");
// 鎵嬪埞鎷夎捣鐘舵�佷笅锛岃椹朵簡10绫充互涓婏紝涓嶅悎鏍�
- AddExamFault(40205, rtkTime);
+ AddExamFault(40205);
} else if (handBreakActive) {
// 鎵嬪埞鎷夎捣鐘舵�佷笅锛岃椹朵簡1绫充互涓婏紝鎵�10鍒�
DEBUG("Handbreak active move over 1M");
- AddExamFault(40206, rtkTime);
+ AddExamFault(40206);
}
}
} else if (!handBreakActive && ReadOdo() - startCarMoveDistance >= examParam.open_door_drive_allow_distance && ReadCarStatus(HAND_BREAK) == BREAK_ACTIVE) {
@@ -982,254 +795,34 @@
if (ExamType == TEST_TYPE_AREA) {
DEBUG("Handbreak active move over 1M");
- AddExamFault(10107, rtkTime);
+ AddExamFault(10107);
}
}
}
- } else if (!rec || prevMove != 0) { // 璁板綍鍋滆溅鐐�
+ } else if (!rec || prevMove != STOP) { // 璁板綍鍋滆溅鐐�
rec = true;
handBreakActive = handBreakActive2 = false;
startCarMoveDistance = ReadOdo();
}
- prevMove = move;
+ prevMove = prime.pMotion->move;
- if (ExamType != TEST_TYPE_AREA) {
- if (exam_dummy_light == 4 || ExamType == TEST_TYPE_ROAD_TRUE_LIGHT || ExamType == TEST_TYPE_ROAD_CALIBRATE) {
- TestRoadGeneral(RoadMap, CarModel, CarModelList, speed, move, rtkTime);
- }
- } else {
- TestAreaGeneral(AreaMapList, CarModel, CarModelList, speed, move, azimuth, rtkTime);
- }
+ AreaExam(prime);
}
static void EngineStartHold(apptimer_var_t val) {
DEBUG("鐐圭伀瓒呮椂");
if (ReadCarStatus(ENGINE_START) == ENGINE_START_ACTIVE) {
- struct RtkTime rtkTime;
-
- std::unique_lock<std::mutex> lk(clock_mutex);
- rtkTime = rtkClock;
- lk.unlock();
-
// 涓嶅強鏃舵澗寮�鍚姩寮�鍏筹紝鎵�10鍒�
if (ExamType == TEST_TYPE_AREA) {
- AddExamFault(10201, &rtkTime);
+ AddExamFault(10201);
} else if (ExamType != TEST_TYPE_ROAD_CALIBRATE) {
- AddExamFault(40207, &rtkTime);
+ AddExamFault(40207);
}
}
}
-/*************************************************
- * 2娆￠噰鏍风浉宸殑鏃堕棿, a 鏈�杩戠殑锛宐 鍏堝墠鐨�
- * @param a
- * @param b
- * @return ms
- */
-static uint32_t CalcTimeDiff(const rtk_info *a, const rtk_info *b)
-{
- return TimeGetDiff(a->hh, a->mm, a->ss, a->dss*10, b->hh, b->mm, b->ss, b->dss*10);
-}
-
-static bool UpdateCarCoord(double &spd, int &mov, int &idx)
-{
- long tmDiff;
-
- if (CarModel == NULL)
- return false;
-
- if (RtkBufferNum < 2)
- return false;
-
- int p1 = ((RtkBufferIn-1)+RTK_BUFFER_SIZE)%RTK_BUFFER_SIZE; // 鏈�杩戦噰鏍峰��
- int p2 = ((RtkBufferIn-2)+RTK_BUFFER_SIZE)%RTK_BUFFER_SIZE; // 鎵惧埌1绉掑墠鐨勯噰鏍峰��
- int pn = 0;
-
- for (pn = 1; pn < RtkBufferNum; ++pn) {
- p2 = ((RtkBufferIn-1-pn)+RTK_BUFFER_SIZE)%RTK_BUFFER_SIZE;
- if ((tmDiff = CalcTimeDiff(&RtkBuffer[p1], &RtkBuffer[p2])) >= D_SEC(1)) break;
- }
-
- if (pn == RtkBufferNum)
- return false;
-
- // 濡傛灉涓�瀹氱殑鏃堕棿閮芥病鏈夋湁鏁堝畾浣嶏紝鍒犻櫎涔嬪墠鐨勫��
- /*tmDiff = CalcTimeDiff(&RtkBuffer[p1], &RtkBuffer[p2]);
-
- DEBUG("tmDiff = %ld, p1 = %d p2 = %d dss = %d dss2 = %d", tmDiff, p1, p2, RtkBuffer[p1].dss, RtkBuffer[p2].dss);
-
- if (tmDiff > D_SEC(5)) {
- if (p1 != 0)
- RtkBuffer[0] = RtkBuffer[p1];
- RtkBufferIn = RtkBufferNum = 1;
- return false;
- }*/
-
- // 璁＄畻杞﹁締杞粨鐐�
- PointF main_ant_coord;
- main_ant_coord.X = RtkBuffer[p1].x;
- main_ant_coord.Y = RtkBuffer[p1].y;
-
- struct RtkTime tm;
-
- tm.YY = RtkBuffer[p1].YY;
- tm.MM = RtkBuffer[p1].MM;
- tm.DD = RtkBuffer[p1].DD;
- tm.hh = RtkBuffer[p1].hh;
- tm.mm = RtkBuffer[p1].mm;
- tm.ss = RtkBuffer[p1].ss;
- tm.mss = RtkBuffer[p1].dss;
-
- UpdateCarBodyCoord(&tm, RtkBuffer[p1].heading, RtkBuffer[p1].pitch, RtkBuffer[p1].roll, main_ant_coord, CarModel);
-
- car_model *newModel = (car_model *)malloc(sizeof(car_model));
-
- newModel->tm = CarModel->tm;
- newModel->basePoint = CarModel->basePoint;
- newModel->axial[0] = CarModel->axial[0];
- newModel->axial[1] = CarModel->axial[1];
- newModel->left_front_tire[0] = CarModel->left_front_tire[0];
- newModel->left_front_tire[1] = CarModel->left_front_tire[1];
- newModel->right_front_tire[0] = CarModel->right_front_tire[0];
- newModel->right_front_tire[1] = CarModel->right_front_tire[1];
- newModel->left_rear_tire[0] = CarModel->left_rear_tire[0];
- newModel->left_rear_tire[1] = CarModel->left_rear_tire[1];
- newModel->right_rear_tire[0] = CarModel->right_rear_tire[0];
- newModel->right_rear_tire[1] = CarModel->right_rear_tire[1];
- newModel->bodyNum = CarModel->bodyNum;
- newModel->body = (int *) malloc(sizeof(int) * newModel->bodyNum);
- for (int i = 0; i < newModel->bodyNum; ++i) {
- newModel->body[i] = CarModel->body[i];
- }
- newModel->pointNum = CarModel->pointNum;
- newModel->carXY = (PointF *) malloc(sizeof(PointF) * newModel->pointNum);
- for (int i = 0; i < newModel->pointNum; ++i) {
- newModel->carXY[i] = CarModel->carXY[i];
- }
- newModel->carDesc = NULL;
- newModel->antPitch = CarModel->antPitch;
- newModel->yaw = CarModel->yaw;
- newModel->pitch = CarModel->pitch;
-
- CarModelList.push_front(newModel);
-
- while (CarModelList.size() > 100) {
- car_model *ptr = CarModelList.back();
-
- if (ptr->body != NULL)
- free(ptr->body);
- if (ptr->carXY != NULL)
- free(ptr->carXY);
- if (ptr->carDesc != NULL)
- free(ptr->carDesc);
-
- free(ptr);
-
- CarModelList.pop_back();
- }
-
- // 璁＄畻閫熷害(绫�/绉�)銆佸墠杩涘悗閫�
- double speed = sqrt(pow(RtkBuffer[p1].x - RtkBuffer[p2].x, 2) + pow(RtkBuffer[p1].y - RtkBuffer[p2].y, 2)) * 1000 /
- (double)(tmDiff);
-
-// DEBUG("浣嶇Щ %f 鏃堕棿 %ld 閫熷害 %f", sqrt(pow(RtkBuffer[p1].x - RtkBuffer[p2].x, 2) + pow(RtkBuffer[p1].y - RtkBuffer[p2].y, 2)), tmDiff, speed);
-// DEBUG("%d %d %f, %f - %d %d %f, %f", RtkBuffer[p1].ss, RtkBuffer[p1].dss, RtkBuffer[p1].x, RtkBuffer[p1].y, RtkBuffer[p2].ss, RtkBuffer[p2].dss, RtkBuffer[p2].x, RtkBuffer[p2].y);
-
- int move = 0;
- double deg = 0.0;
-
- if (speed < 0.05) {
- // 鍋滆溅
- move = 0;
- } else {
- // 鍒ゆ柇鍓嶈繘杩樻槸鍚庨��
- if (fabs(RtkBuffer[p1].y - RtkBuffer[p2].y) <= GLB_EPSILON) {
- if (RtkBuffer[p1].x > RtkBuffer[p2].x) {
- deg = 90;
- } else {
- deg = 270;
- }
- } else if (fabs(RtkBuffer[p1].x - RtkBuffer[p2].x) <= GLB_EPSILON) {
- if (RtkBuffer[p1].y > RtkBuffer[p2].y) {
- deg = 0;
- } else {
- deg = 180;
- }
- } else {
- deg = atan(fabs(RtkBuffer[p1].x - RtkBuffer[p2].x) /
- fabs(RtkBuffer[p1].y - RtkBuffer[p2].y));
-
- deg = toDegree(deg);
-
- if (RtkBuffer[p1].x > RtkBuffer[p2].x &&
- RtkBuffer[p1].y > RtkBuffer[p2].y) {
-
- } else if (RtkBuffer[p1].x < RtkBuffer[p2].x &&
- RtkBuffer[p1].y > RtkBuffer[p2].y) {
- deg = 360 - deg;
- } else if (RtkBuffer[p1].x < RtkBuffer[p2].x &&
- RtkBuffer[p1].y < RtkBuffer[p2].y) {
- deg = 180 + deg;
- } else if (RtkBuffer[p1].x > RtkBuffer[p2].x &&
- RtkBuffer[p1].y < RtkBuffer[p2].y) {
- deg = 180 - deg;
- }
- }
-
- deg = fabs(RtkBuffer[p1].heading - deg);
- if (deg > 180) {
- deg = 360 - deg;
- }
- if (deg < 90) {
- // 鍓嶈繘
- move = 1;
- } else {
- // 鍚庨��
- move = -1;
- }
- }
-
- spd = speed;
- mov = move;
- idx = p1;
-// DEBUG("tmDiff = %ld speed = %f m/Sec move = %d", tmDiff, speed, move);
-
- return true;
-}
-
-static int currRoad = -1, currCrossing = -1;
-
-void RoadChange(int road, int status)
-{
- struct roadStatusBrief brief;
-
- brief.road_id = road;
- brief.status = status;
-
- MA_SendRoadStatus(&brief);
-
- currRoad = (status == 1? road : -1);
-
- DEBUG("鎶ュ憡闀垮畼 杩涘嚭璺 road %d status %d", road, status);
-}
-
-void CrossingChange(int road, int crossing, int status)
-{
- struct crossingStatusBrief brief;
-
- brief.road_id = road;
- brief.crossing_index = crossing;
- brief.status = status;
-
- MA_SendCrossingStatus(&brief);
-
- currCrossing = (status == 1? crossing : -1);
-
- DEBUG("鎶ュ憡闀垮畼 杩涘嚭璺彛 road %d crossing %d status %d", road, crossing, status);
-}
-
-void AddExamFault(int wrong, const struct RtkTime *rtkTime)
+void AddExamFault(int wrong)
{
struct ExamFault fault;
@@ -1237,8 +830,8 @@
return;
fault.sn = examFaultIndex++;
- sprintf(fault.utc, "%04d%02d%02d%02d%02d%02d.%02d", 2000 + rtkTime->YY,
- rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss);
+
+ strcpy(fault.utc, StringUtil::FormatUTCTime(AppTimer_GetGmtTickCount()).c_str());
// if (ExamType != TEST_TYPE_AREA) {
// wrong += 1000;
@@ -1253,35 +846,6 @@
MA_SendExamWrong(ExamFaultList);
ExamFaultList.clear();
-}
-
-void MasterInqRoadStatus(void)
-{
- struct roadStatusBrief brief;
- struct crossingStatusBrief brief2;
-
- brief2.road_id = brief.road_id = currRoad;
-
- if (currRoad >= 0) {
- brief.status = 1;
-
- if (currCrossing >= 0) {
- brief2.crossing_index = currCrossing;
- brief2.status = 1;
- } else {
- brief2.crossing_index = -1;
- brief2.status = 0;
- }
- }
- else {
- brief.status = 0;
- brief2.crossing_index = -1;
- brief2.status = 0;
- }
-
- MA_SendRoadStatus(&brief);
-
- MA_SendCrossingStatus(&brief2);
}
/***************************************************************
@@ -1300,46 +864,47 @@
* @param azimuth
* @param coord
*/
-static void UpdateCarBodyCoord(struct RtkTime *rtkTime, double azimuth, double pitch, double roll, PointF main_ant, car_model *carModel)
+static void CalcBodyModeling(modeling_t &car, car_model_t &carModel, rtk_info_t &rtk)
{
- // 淇话瑙掍慨姝�
-// DEBUG("淇话瑙� %f", pitch);
- carModel->yaw = azimuth;
- carModel->pitch = pitch;
- carModel->tm = *rtkTime;
-
- pitch = pitch - carModel->antPitch;
+ car.utc_time = rtk.utc_time;
+ car.yaw = rtk.heading;
+ car.pitch = rtk.pitch;
+ car.roll = rtk.roll;
+// 淇话瑙掍慨姝�
+ double pitch = rtk.pitch - carModel.antPitch;
+ double azimuth = rtk.heading;
// DEBUG("yaw = %f 淇淇话瑙� %f", azimuth, 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));
+ car.base_point.X = rtk.x + fabs(carModel.antHeight - carModel.groundHeight) * sin(toRadians(pitch)) * sin(toRadians(azimuth));
+ car.base_point.Y = rtk.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)) *
+ // 棣栨璁＄畻
+ if (car.points.size() != carModel.carDesc.size()) {
+ car.points.resize(carModel.carDesc.size());
+ }
+
+ for (int i = 0; i < carModel.carDesc.size(); ++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)) *
cos(toRadians(pitch));
double projectDistance = sqrt(pow(qrx, 2) + pow(qry, 2));
double projectAngle = toDegree(acos(qry / projectDistance));
- if (carModel->carDesc[i].angle > 180) {
+ if (carModel.carDesc[i].angle > 180) {
projectAngle = 360 - projectAngle;
}
// double tx = projectDistance*sin(toRadians(azimuth));
// double ty = projectDistance*cos(toRadians(azimuth));
- carModel->carXY[i].X =
- projectDistance * sin(toRadians(azimuth)) * cos(toRadians(projectAngle)) -
- projectDistance * cos(toRadians(azimuth)) * sin(toRadians(projectAngle)) +
- carModel->basePoint.X;
- carModel->carXY[i].Y =
- projectDistance * sin(toRadians(azimuth)) * sin(toRadians(projectAngle)) +
- projectDistance * cos(toRadians(azimuth)) * cos(toRadians(projectAngle)) +
- carModel->basePoint.Y;
-
+ car.points[i].X = projectDistance * sin(toRadians(azimuth)) * cos(toRadians(projectAngle)) -
+ projectDistance * cos(toRadians(azimuth)) * sin(toRadians(projectAngle)) +
+ car.base_point.X;
+ car.points[i].Y = projectDistance * sin(toRadians(azimuth)) * sin(toRadians(projectAngle)) +
+ projectDistance * cos(toRadians(azimuth)) * cos(toRadians(projectAngle)) +
+ car.base_point.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);
}
@@ -1359,21 +924,7 @@
void SensorXChanged(uint16_t id, int value)
{
- handlePrepare(id, value);
- handleLigthExam(id, value);
+// handlePrepare(id, value);
+// handleLigthExam(id, value);
}
-void PrepareOver(int res)
-{
- DEBUG("涓婅溅鍑嗗缁撴潫 %d", res);
-
- if (res != 0) {
- struct RtkTime rtkTime;
-
- std::unique_lock<std::mutex> lk(clock_mutex);
- rtkTime = rtkClock;
- lk.unlock();
- AddExamFault(40101, &rtkTime);
- }
- exam_dummy_light = 2;
-}
--
Gitblit v1.8.0