From 2d0741f8e56e4f24126ea8448551e64560c45b4e Mon Sep 17 00:00:00 2001 From: yy1717 <fctom1215@outlook.com> Date: 星期三, 18 三月 2020 18:41:57 +0800 Subject: [PATCH] 通用测试 --- lib/src/main/cpp/driver_test.cpp | 22 ++++++++++++++++------ 1 files changed, 16 insertions(+), 6 deletions(-) diff --git a/lib/src/main/cpp/driver_test.cpp b/lib/src/main/cpp/driver_test.cpp index 99eac50..58c440f 100644 --- a/lib/src/main/cpp/driver_test.cpp +++ b/lib/src/main/cpp/driver_test.cpp @@ -102,8 +102,6 @@ static car_model *CarModel = NULL; -typedef list<car_model *> LIST_CAR_MODEL; - static LIST_CAR_MODEL CarModelList; // 涓�娈垫椂闂寸殑杞﹁締杞ㄨ抗闆嗗悎 static struct dummy_light_exam *DummyLightContent; @@ -126,7 +124,7 @@ 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 void ReadDriverExamPrimerTimeout(union sigval sig); -static void UpdateCarBodyCoord(double azimuth, double pitch, double roll, PointF main_ant, car_model *carModel); +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 int EnterMap(const car_model *car, const struct map_list *mapList, int mapNum); @@ -843,10 +841,20 @@ main_ant_coord.X = RtkBuffer[p1].x; main_ant_coord.Y = RtkBuffer[p1].y; - UpdateCarBodyCoord(RtkBuffer[p1].heading, RtkBuffer[p1].pitch, RtkBuffer[p1].roll, main_ant_coord, CarModel); + 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.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]; @@ -874,7 +882,8 @@ newModel->pitch = CarModel->pitch; CarModelList.push_front(newModel); - while (CarModelList.size() > 25) { + + while (CarModelList.size() > 100) { DEBUG("鍒犻櫎灏剧粨鐐�"); car_model *ptr = CarModelList.back(); @@ -1159,12 +1168,13 @@ * @param azimuth * @param coord */ -static void UpdateCarBodyCoord(double azimuth, double pitch, double roll, PointF main_ant, car_model *carModel) +static void UpdateCarBodyCoord(struct RtkTime *rtkTime, double azimuth, double pitch, double roll, PointF main_ant, car_model *carModel) { carModel->basePoint = main_ant; carModel->yaw = azimuth; carModel->pitch = pitch; + carModel->tm = *rtkTime; pitch = pitch - carModel->antPitch; -- Gitblit v1.8.0