通用测试

yy1717

2020-03-18 2d0741f8e56e4f24126ea8448551e64560c45b4e

 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;