科目二修改

yy1717

2023-03-31 4bd08f0355b6b2cf3c027202d5ad301b4e182953

 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());
        // 向UI上报车辆点坐标
        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 最近的，b 先前的
 * @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;
}