科目二修改

yy1717

2023-03-31 4bd08f0355b6b2cf3c027202d5ad301b4e182953

科目二修改

 lib/src/main/cpp/CMakeLists.txt

@@ -48,19 +48,6 @@
        test_items/turn_a90.cpp
        test_items/area_exam.cpp

        test_items2/prepare.cpp
        test_items2/common_check.cpp
        test_items2/dummy_light.cpp
        test_items2/road_exam.cpp
        test_items2/through_something.cpp
        test_items2/drive_straight.cpp
        test_items2/stop_car.cpp
        test_items2/operate_gear.cpp
        test_items2/smart_item.cpp
        test_items2/car_start.cpp
        test_items2/overtake.cpp
        test_items2/change_lane.cpp

        rtk_module/rtk.cpp
        rtk_module/virtual_rtk.cpp
        master/comm_if.cpp

 lib/src/main/cpp/common/apptimer.cpp

@@ -203,14 +203,14 @@
    return as.count();
}

uint32_t AppTimer_GetGmtTickCount(void)
uint64_t AppTimer_GetGmtTickCount(void)
{
    std::chrono::seconds as = std::chrono::duration_cast<std::chrono::seconds>(std::chrono::system_clock::now().time_since_epoch());

    return as.count();
}

std::string FormatTime(const char *fmt)
std::string FormatTime(const char *fmt = "%Y-%m-%d %H:%M:%S")
{
    auto now = std::chrono::system_clock::now();
    auto timet = std::chrono::system_clock::to_time_t(now);

 lib/src/main/cpp/common/apptimer.h

@@ -58,7 +58,7 @@

uint32_t AppTimer_GetTickCount(void);

uint32_t AppTimer_GetGmtTickCount(void);
uint64_t AppTimer_GetGmtTickCount(void);

std::string FormatTime(const char *fmt);


 lib/src/main/cpp/common/observer.h

New file
@@ -0,0 +1,64 @@
//
// Created by YY on 2023/3/30.
//

#ifndef MYAPPLICATION3_OBSERVER_H
#define MYAPPLICATION3_OBSERVER_H

#include <iostream>
#include <string>
#include <functional>
#include <map>

namespace ilovers {
    class NonCopyable {
    protected:
        NonCopyable() = default;

        ~NonCopyable() = default;

        NonCopyable(const NonCopyable &) = delete;

        NonCopyable &operator=(const NonCopyable &) = delete;
    };

    template<typename Func>
    class Observer : NonCopyable {
    public:
        Observer() {}

        ~Observer() {}

        int Connect(Func &&f) {
            return Assgin(f);
        }

        int Connect(const Func &f) {
            return Assgin(f);
        }

        void Disconnect(int key) {
            m_connections.erase(key);
        }

        template<typename... Args>
        void Notify(Args &&... args) {
            for (auto &it: m_connections) {
                it.second(std::forward<Args>(args)...);
            }
        }

    private:
        template<typename F>
        int Assgin(F &&f) {
            int k = m_observerId++;
            m_connections.emplace(k, std::forward<F>(f));
            return k;
        }

        int m_observerId = 0;
        std::map<int, Func> m_connections;
    };
}

#endif //MYAPPLICATION3_OBSERVER_H

 lib/src/main/cpp/common/semaphore.h

New file
@@ -0,0 +1,39 @@
//
// Created by YY on 2022/12/30.
//

#ifndef MYAPPLICATION3_SEMAPHORE_H
#define MYAPPLICATION3_SEMAPHORE_H

#include <mutex>
#include <condition_variable>

namespace ilovers{
    class semaphore {
    public:
        semaphore(int value=1): count{value}, wakeups{0} {}

        void wait(void) {
            std::unique_lock<std::mutex> lock{mutex};
            if (--count<0) { // count is not enough ?
                condition.wait(lock, [&]()->bool{ return wakeups>0;}); // suspend and wait ...
                --wakeups;  // ok, me wakeup !
            }
        }
        void signal(void) {
            std::lock_guard<std::mutex> lock{mutex};
            if(++count<=0) { // have some thread suspended ?
                ++wakeups;
                condition.notify_one(); // notify one !
            }
        }

    private:
        int count;
        int wakeups;
        std::mutex mutex;
        std::condition_variable condition;
    };
};

#endif //MYAPPLICATION3_SEMAPHORE_H

 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;
}

 lib/src/main/cpp/driver_test.h

@@ -5,13 +5,12 @@
#ifndef RTKDRIVERTEST_DRIVER_TEST_H
#define RTKDRIVERTEST_DRIVER_TEST_H

#include "test_common/Geometry.h"
#include "rtk_module/rtk.h"
#include "test_items2/dummy_light.h"

#include <vector>
#include <string>
#include <list>
#include "test_common/Geometry.h"
#include "rtk_module/rtk.h"
#include "map.h"

#define EXAM_AREA_NONE          0
#define EXAM_AREA_START         1
@@ -23,68 +22,78 @@
#define AXIAL_FRONT     0
#define AXIAL_REAR      1

#define MAP_TYPE_PARK_BUTTOM        1
#define MAP_TYPE_STOP_START         2
#define MAP_TYPE_PART_EDGE          3
#define MAP_TYPE_CURVE              4
#define MAP_TYPE_TURN_90            5

using namespace std;

struct car_desc_ {
    double distance;            // 距离主天线的距离
    double angle;               // 从中轴线逆时针形成的角度
};

struct RtkTime {
    int YY;             // 2位数
    int MM;
    int DD;
    int hh;
    int mm;
    int ss;
    int mss;
};
typedef enum {
    MAP_TYPE_NONE,
    MAP_TYPE_PARK_BUTTOM,
    MAP_TYPE_UPHILL,
    MAP_TYPE_PARK_EDGE,
    MAP_TYPE_CURVE,
    MAP_TYPE_TURN_90
} map_type_t;

typedef struct {
    struct RtkTime tm;
    double distance;            // 距离主天线的距离
    double angle;               // 从中轴线逆时针形成的角度
} car_desc_t;

typedef struct {
//    struct RtkTime tm;
    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 bodyNum;
    int *body;
    int pointNum;
//    int bodyNum;
    std::vector<int> body;
//    int pointNum;
    double antPitch;                // 前后天线因安装位置之固有pitch角
    double antHeight;               // 车顶天线高程
    double groundHeight;            // 大地高程
    double yaw;
    double pitch;
    struct car_desc_ *carDesc;      // 经现场测量点计算后的值
    PointF *carXY;                  // 计算后的车身各点坐标
} car_model;

// 总数
// 中轴线前点,id 0，中轴线后点id
// 左包围， 右包围
// 左前轮，外内
typedef struct {
    int body_num;
    int body[64];
    int front_left_tire[2];
    int front_right_tire[2];
    int rear_left_tire[2];
    int rear_right_tire[2];
} carModelDesc_t;
    std::vector<car_desc_t> carDesc;
//    struct car_desc_ *carDesc;      // 经现场测量点计算后的值
//    PointF *carXY;                  // 计算后的车身各点坐标
} car_model_t;

typedef struct {
    uint32_t uptime;
    int point_num;
    PointF *points;
    carModelDesc_t *desc;
} car_model_cache_t;
    int64_t utc_time = 0;
    double yaw;
    double pitch;
    double roll;
    PointF base_point;
    std::vector<PointF> points;
} modeling_t;

typedef enum {
    BACKWARD = -1,
    STOP = 0,
    FORWARD = 1
} move_status_t;

typedef struct {
    int64_t timestamp = 0;
    double speed = 0;
    move_status_t move = STOP;
} motion_t;

typedef struct {
    map_type_t type;
    int map_idx;        // 对应科目地图集合中的序号
} map_t;

typedef struct {
    bool examing = false;
    int curr_modeling_index = -1;
    int prev_modeling_index = -1;
    area_map_t *pMap;
    car_model_t *pModel;
    modeling_t *pModeling;
    motion_t *pMotion;
    map_t curr_exam_map;
    map_t arriving_map;
} prime_t;

struct ExamFault {
    int sn;
@@ -236,7 +245,7 @@
    double road_slide_red_distance;
    int road_total_distance;
    int road_max_speed;                     // Km per hour
    vector<vector<int>> gear_speed_table;
    std::vector<std::vector<int>> gear_speed_table;
    int gear_n_allow_time;                  // Sec
    int same_gear_min_time;                 // Sec, x秒内，不允许N->X->N->X置同一挡位
    int gear_speed_error_cumulative_time;   // Sec
@@ -248,69 +257,67 @@
    int start_car_limit_distance;
    double open_door_drive_allow_distance;

    string prepare_tts;
    string touch_leftfront_tts;
    string touch_leftrear_tts;
    string touch_rightfront_tts;
    string touch_rightrear_tts;
    string start_engine_tts;
    std::string prepare_tts;
    std::string touch_leftfront_tts;
    std::string touch_leftrear_tts;
    std::string touch_rightfront_tts;
    std::string touch_rightrear_tts;
    std::string start_engine_tts;

    string start_car_begin_tts;
    string start_car_end_tts;
    std::string start_car_begin_tts;
    std::string start_car_end_tts;

    int change_lane_limit_distance;
    string change_lane_begin_tts;
    string change_lane_end_tts;
    std::string change_lane_begin_tts;
    std::string change_lane_end_tts;

    int shift_limit_distance;
    int shift_hold_time;                    // Sec
    string shift_begin_tts;
    string shift_end_tts;
    string shift_up_tts;
    string shift_down_tts;
    std::string shift_begin_tts;
    std::string shift_end_tts;
    std::string shift_up_tts;
    std::string shift_down_tts;

    string straight_begin_tts;
    string straight_end_tts;
    std::string straight_begin_tts;
    std::string straight_end_tts;
    int straight_limit_distance;
    double straight_max_offset;

    int overtake_limit_distance;
    string overtake_begin_tts;
    string overtake_end_tts;
    std::string overtake_begin_tts;
    std::string overtake_end_tts;

    int stop_car_limit_distance;
    int stop_car_open_door_allow_time;
    double stop_car_edge_red_distance;
    double stop_car_edge_yellow_distance;
    string stop_car_begin_tts;
    string stop_car_end_tts;
    std::string stop_car_begin_tts;
    std::string stop_car_end_tts;
    double crossing_stop_valid_distance;
    int cross_school_max_speed;
    int crossing_break_valid_distance;

    string crossing_go_straight_tts;
    string crossing_turn_left_tts;
    string crossing_turn_right_tts;
    string crossing_turn_back_tts;
    string crossing_turn_unknown_tts;
    std::string crossing_go_straight_tts;
    std::string crossing_turn_left_tts;
    std::string crossing_turn_right_tts;
    std::string crossing_turn_back_tts;
    std::string crossing_turn_unknown_tts;
} exam_param_t;

extern exam_param_t examParam;

typedef vector<struct area_exam_map> LIST_AREA_MAP;

typedef list<car_model *> LIST_CAR_MODEL;

void DriverTestInit(void);
void ReadDriverExamPrimer(void);

void ClearAreaMap(void);
void AddAreaMap(int id, int type, const double (*map)[2], int pointNum, const double (*map2)[2], int pointNum2);

void AddCurveMap(curve_map_t &map);
void AddParkButtonMap(park_button_map_t &map);
void AddParkEdgeMap(park_edge_map_t &map);

void CleanRoadMap(void);
void SetRoadMap(road_exam_map &map, vector<scheme_t> &scheme);
void SetRoadExamScheme(vector<scheme_t> &scheme);
void SetRoadMap(road_exam_map &map, std::vector<scheme_t> &scheme);
void SetRoadExamScheme(std::vector<scheme_t> &scheme);

void SetCarMeasurePoint(double *basePoint, int *axial, int *left_front_tire,
                        int *right_front_tire, int *left_rear_tire, int *right_rear_tire,
@@ -318,16 +325,14 @@

void StartDriverExam(int start, int type);

void UpdateRTKInfo(const rtk_info *s);
void AddExamFault(int wrong, const struct RtkTime *rtkTime);
void UpdateRTKInfo(const rtk_info_t *s);
void AddExamFault(int wrong);
void RoadChange(int road, int status);
void CrossingChange(int road, int crossing, int status);

void SystemShutdown(int event, int timeout);

void SetDummyLightExam(int n, struct dummy_light_exam *cfg);

uint32_t TimeGetDiff(const struct RtkTime *rtkTime1, const struct RtkTime *rtkTime2);

void MasterInqRoadStatus(void);


 lib/src/main/cpp/map.h

New file
@@ -0,0 +1,66 @@
//
// Created by YY on 2022/12/28.
//

#ifndef MYAPPLICATION3_MAP_H
#define MYAPPLICATION3_MAP_H

#include <string>
#include <vector>
#include "test_common/Geometry.h"

typedef struct {
    int id;
    std::string name;
    double line_width;
    std::vector<PointF> map;
} park_button_map_t;

typedef struct {
    int id;
    std::string name;
    double line_width;
    std::vector<PointF> map;
} park_edge_map_t;

typedef struct {
    int id;
    std::string name;
    double line_width;
    std::vector<PointF> map;
} turn_a90_map_t;

typedef struct {
    int id;
    std::string name;
    double line_width;
    std::vector<PointF> map;
} uphill_map_t;

typedef struct {
    int id;
    std::string name;
    double line_width;
    PointF left_start_point;
    PointF right_start_point;
    PointF left_end_point;
    PointF right_end_point;
    PointF front_half_big_circle_centre;
    double front_half_big_circle_radius;
    PointF front_half_small_circle_centre;
    double front_half_small_circle_radius;
    PointF back_half_big_circle_centre;
    double back_half_big_circle_radius;
    PointF back_half_small_circle_centre;
    double back_half_small_circle_radius;
} curve_map_t;

typedef struct {
    std::vector<curve_map_t> curve_map;
    std::vector<park_button_map_t> park_button_map;
    std::vector<park_edge_map_t> park_edge_map;
    std::vector<turn_a90_map_t> turn_a90_map;
    std::vector<uphill_map_t> uphill_map;
} area_map_t;

#endif //MYAPPLICATION3_MAP_H

 lib/src/main/cpp/master/comm_if.cpp

@@ -16,6 +16,7 @@
#include "../jni_log.h"
#include "../mcu/mcu_if.h"
#include "../test_common/car_sensor.h"
#include "../map.h"

#include <string>
#include <iostream>
@@ -146,6 +147,31 @@
                DEBUG("发送失败");
                usleep(500000);
                sem_post(&sem_msg_income);
            }
        }
    }
}

static void LoadPoint(PointF &p, const rapidjson::Value &v)
{
    if (v.IsArray() && v.Size() >= 2) {
        p.X = v[0].GetDouble();
        p.Y = v[1].GetDouble();
    }
}

static void LoadPoints(vector<PointF> &ps, const rapidjson::Value &v)
{
    if (v.IsArray()) {
        PointF point;
        for (int i = 0; i < v.Size();) {
            if (v.Size() - i >= 2) {
                point.X = v[i].GetDouble();
                point.Y = v[i+1].GetDouble();
                ps.push_back(point);
                i += 2;
            } else {
                break;
            }
        }
    }
@@ -420,7 +446,7 @@
    writer.Double(brief->coord_y);
    writer.Key("coord_y_dir");
    writer.String(b);
    writer.Key("heading");
    writer.Key("yaw");
    writer.Double(brief->heading);
    writer.Key("pitch");
    writer.Double(brief->pitch);
@@ -556,7 +582,7 @@
    writer.Int(brief->move);
    writer.Key("speed");
    writer.Double(brief->speed);
    writer.Key("heading");
    writer.Key("yaw");
    writer.Double(brief->heading);

    writer.Key("main_ant");
@@ -597,16 +623,16 @@

    writer.Key("body");
    writer.StartArray();
    for (int i = 0; i < brief->bodyNum; ++i) {
    for (int i = 0; i < brief->body.size(); ++i) {
        writer.Int(brief->body[i]);
    }
    writer.EndArray();

    writer.Key("point");
    writer.StartArray();
    for (int i = 0; i < brief->pointNum; ++i) {
        writer.Double(brief->point[i*2]);
        writer.Double(brief->point[i*2+1]);
    for (int i = 0; i < brief->point.size(); ++i) {
        writer.Double(brief->point[i][0]);
        writer.Double(brief->point[i][1]);
    }
    writer.EndArray();
    writer.EndObject();
@@ -1673,63 +1699,136 @@
                    if (a.IsArray()) {
                        for (Value::ConstValueIterator itr = a.Begin(); itr != a.End(); ++itr) {
                            // a Map
                            int id, type, pointNum = 0, point2Num = 0;
                            double (*map)[2] = NULL, (*map2)[2] = NULL;
                            string name;
                            int id = -1, type = -1;
                            //int id, type, pointNum = 0, point2Num = 0;
                            //double (*map)[2] = NULL, (*map2)[2] = NULL;

                            if (itr->IsObject()) {
                                if (itr->HasMember("id")) {
                                    const Value &s = (*itr)["id"];
                                    id = s.GetInt();
                                }
                                if (itr->HasMember("name")) {
                                    const Value &s = (*itr)["id"];
                                    name = s.GetString();
                                }
                                if (itr->HasMember("item")) {
                                    const Value &s = (*itr)["item"];
                                    type = s.GetInt();
                                }
                                if (itr->HasMember("point")) {
                                    const Value &s = (*itr)["point"];
                                    int map_index = 0;

                                    for (Value::ConstValueIterator itr2 = s.Begin();
                                         itr2 != s.End(); ++itr2, ++map_index) {
                                        // 曲线驾驶有2组
                                        const Value &s2 = (*itr2)["x-y"];

                                        if (map_index == 0) {
                                            int i = 0, j = 0;
                                            pointNum = s2.Size() / 2;
                                            map = (double (*)[2]) new double[pointNum][2];
//                                        map = (double (*)[2]) malloc(pointNum * 2 * sizeof(double));

                                            for (Value::ConstValueIterator itr3 = s2.Begin();
                                                 itr3 != s2.End(); ++itr3) {
                                                map[i][j] = (*itr3).GetDouble();
                                                if (++j == 2) {
                                                    j = 0;
                                                    i++;
                                                }
                                    switch (type) {
                                        case MAP_TYPE_CURVE: {
                                            curve_map_t map;
                                            map.id = id;
                                            if (itr->HasMember("left_start_point")) {
                                                const Value &s2 = (*itr)["left_start_point"];
                                                LoadPoint(map.left_start_point, s2);
                                            }
                                        } else if (map_index == 1) {
                                            int i = 0, j = 0;
                                            point2Num = s2.Size() / 2;
                                            map2 = (double (*)[2]) new double[s2.Size()][2];
//                                        map2 = (double (*)[2]) malloc(point2Num * 2 * sizeof(double));

                                            for (Value::ConstValueIterator itr3 = s2.Begin();
                                                 itr3 != s2.End(); ++itr3) {
                                                map2[i][j] = (*itr3).GetDouble();
                                                if (++j == 2) {
                                                    j = 0;
                                                    i++;
                                                }
                                            if (itr->HasMember("right_start_point")) {
                                                const Value &s2 = (*itr)["right_start_point"];
                                                LoadPoint(map.right_start_point, s2);
                                            }
                                            if (itr->HasMember("left_end_point")) {
                                                const Value &s2 = (*itr)["left_end_point"];
                                                LoadPoint(map.left_end_point, s2);
                                            }
                                            if (itr->HasMember("right_end_point")) {
                                                const Value &s2 = (*itr)["right_end_point"];
                                                LoadPoint(map.right_end_point, s2);
                                            }
                                            AddCurveMap(map);
                                            break;
                                        }
                                        case MAP_TYPE_PARK_BUTTOM: {
                                            park_button_map_t map;

                                            map.id = id;
                                            if (itr->HasMember("point") && (*itr)["point"].IsArray()) {
                                                const Value &arr = (*itr)["point"];
                                                LoadPoints(map.map, arr);
                                            }
                                            AddParkButtonMap(map);
                                            break;
                                        }
                                        case MAP_TYPE_PARK_EDGE: {
                                            park_edge_map_t map;
                                            map.id = id;
                                            if (itr->HasMember("point") && (*itr)["point"].IsArray()) {
                                                const Value &arr = (*itr)["point"];
                                                LoadPoints(map.map, arr);
                                            }
                                            AddParkEdgeMap(map);
                                            break;
                                        }
                                        case MAP_TYPE_UPHILL: {
                                            uphill_map_t map;
                                            map.id = id;
                                            if (itr->HasMember("point") && (*itr)["point"].IsArray()) {
                                                const Value &arr = (*itr)["point"];
                                                LoadPoints(map.map, arr);
                                            }
                                            break;
                                        }
                                        case MAP_TYPE_TURN_90: {
                                            turn_a90_map_t map;
                                            map.id = id;
                                            if (itr->HasMember("point") && (*itr)["point"].IsArray()) {
                                                const Value &arr = (*itr)["point"];
                                                LoadPoints(map.map, arr);
                                            }
                                            break;
                                        }
                                        default:
                                            break;
                                    }
                                }

                                AddAreaMap(id, type, map, pointNum, map2, point2Num);
//                                if (itr->HasMember("point")) {
//                                    const Value &s = (*itr)["point"];
//                                    int map_index = 0;
//
//                                    for (Value::ConstValueIterator itr2 = s.Begin();
//                                         itr2 != s.End(); ++itr2, ++map_index) {
//                                        // 曲线驾驶有2组
//                                        const Value &s2 = (*itr2)["x-y"];
//
//                                        if (map_index == 0) {
//                                            int i = 0, j = 0;
//                                            pointNum = s2.Size() / 2;
//                                            map = (double (*)[2]) new double[pointNum][2];
////                                        map = (double (*)[2]) malloc(pointNum * 2 * sizeof(double));
//
//                                            for (Value::ConstValueIterator itr3 = s2.Begin();
//                                                 itr3 != s2.End(); ++itr3) {
//                                                map[i][j] = (*itr3).GetDouble();
//                                                if (++j == 2) {
//                                                    j = 0;
//                                                    i++;
//                                                }
//                                            }
//                                        } else if (map_index == 1) {
//                                            int i = 0, j = 0;
//                                            point2Num = s2.Size() / 2;
//                                            map2 = (double (*)[2]) new double[s2.Size()][2];
////                                        map2 = (double (*)[2]) malloc(point2Num * 2 * sizeof(double));
//
//                                            for (Value::ConstValueIterator itr3 = s2.Begin();
//                                                 itr3 != s2.End(); ++itr3) {
//                                                map2[i][j] = (*itr3).GetDouble();
//                                                if (++j == 2) {
//                                                    j = 0;
//                                                    i++;
//                                                }
//                                            }
//                                        }
//                                    }
//                                }

                                if (map) delete[]map;
                                if (map2) delete[]map2;
//                                AddAreaMap(id, type, map, pointNum, map2, point2Num);
//
//                                if (map) delete[]map;
//                                if (map2) delete[]map2;
                            }
                        }
                    }
@@ -1968,7 +2067,7 @@
            Document doc;
            doc.Parse(value);
            if (!doc.HasParseError()) {
                DEBUG("解析灯光配置...");
                /*DEBUG("解析灯光配置...");
                if (doc.HasMember("exam")) {
                    const Value& s = doc["exam"];
                    s.GetInt();
@@ -2022,12 +2121,11 @@

                        delete []content;
                    }
                }
                }*/
            }
            break;
        }
        case ID_MS_INQ_ROAD_CROSSING: {
            MasterInqRoadStatus();
            break;
        }
        case ID_MS_BLUETOOTH_NAME: {

 lib/src/main/cpp/master/comm_if.h

@@ -6,6 +6,7 @@
#define MYAPPLICATION2_COMM_IF_H

#include <cstdint>
#include <vector>
#include "../driver_test.h"

struct mcuBrief {
@@ -106,10 +107,8 @@
    int right_front_tire[2];
    int left_rear_tire[2];
    int right_rear_tire[2];
    int *body;
    int bodyNum;
    double *point;
    int pointNum;
    std::vector<int> body;
    std::vector<std::vector<double>> point;
};

struct roadStatusBrief {

 lib/src/main/cpp/mcu/ada.cpp

@@ -23,7 +23,6 @@

static void AdaCommandEntry(uint16_t id, const uint8_t *data, int length)
{
    DEBUG("McuCommandEntry 0x%02X", id);
    switch (id) {
        case ID_ADA_INFO_RESP: {
            DEBUG("ID_ADA_INFO_RESP");
@@ -120,7 +119,22 @@
void InitAda(void)
{
    // TODO
    std::thread(AdaRun).detach();
    static struct serial_config serialConfig;
    strcpy(serialConfig.name, "/dev/ttyCH341USB4");
    serialConfig.baud = 115200;
    serialConfig.data_bit = 8;
    serialConfig.verify_bit = 'N';
    serialConfig.stop_bit = 1;
    serialConfig.flow_ctrl = 0;

    std::thread([&] {
        while (true) {
            std::thread t(UartThread1, &serialConfig);
            t.join();
            std::this_thread::sleep_for(std::chrono::seconds(3));
        }
    }).detach();

}

static void SendDfuFile(int fileLen, int sentLen, int blockLen, const uint8_t *data) {

 lib/src/main/cpp/mcu/ahp.cpp

@@ -36,7 +36,7 @@
        }

        case ID_GNSS_DATA: {
//            DEBUG("ID_GNSS_DATA %d", length);
            DEBUG("ID_GNSS_DATA %d", length);
            memcpy(RxBuf + RxBufLen, data, length);
            RxBufLen += length;

@@ -113,8 +113,9 @@
    pCls = nullptr;
}

static void AhpRun(void)
void InitAhp(void)
{
    // TODO
    static struct serial_config serialConfig;

    strcpy(serialConfig.name, "/dev/ttyCH341USB3");
@@ -124,17 +125,13 @@
    serialConfig.stop_bit = 1;
    serialConfig.flow_ctrl = 0;

    while (true) {
        std::thread t(UartThread, &serialConfig);
        t.join();
        std::this_thread::sleep_for(std::chrono::seconds(3));
    }
}

void InitAhp(void)
{
    // TODO
    std::thread(AhpRun).detach();
    std::thread([&] {
        while (true) {
            std::thread t(UartThread, &serialConfig);
            t.join();
            std::this_thread::sleep_for(std::chrono::seconds(3));
        }
    }).detach();
}

static void SendDfuFile(int fileLen, int sentLen, int blockLen, const uint8_t *data) {

 lib/src/main/cpp/native-lib.cpp

@@ -419,7 +419,7 @@
static void startNative(JNIEnv *env, jobject thiz, jboolean ayDevice) {
    // TODO: implement startNative()
    // 保存全局JVM以便在子线程中使用
    DEBUG("启动Native");
    DEBUG("启动Native 逻辑cpu数量 = %d", std::thread::hardware_concurrency());
    // 不能直接赋值(g_obj = ojb)
    sg_obj = env->NewGlobalRef(thiz);


 lib/src/main/cpp/rtk_module/rtk.cpp

@@ -21,6 +21,7 @@
#include "virtual_rtk.h"
#include "../mcu/mcu_if.h"
#include "../mcu/ahp.h"
#include "../utils/xconvert.h"

#define DEBUG(fmt, args...)     LOGD("<rtk> <%s>: " fmt, __func__, ##args)

@@ -40,9 +41,25 @@
static gpsStatus_t gpsStatus;
static char rtkModel[32] = {0};

static rtk_info CurrRTKInfo;
static bool needSetPjk = false;
static int lostCnt;

// 组合PJK和TRA的消息内容
static struct {
    int year;
    int month;
    int day;
    int hour;
    int min;
    int sec;
    int millisec;
    int qf;
    double heading;
    double pitch;
    double roll;
    double x;
    double y;
} xy_temp;

static void CheckPjkParam(void);
static void CheckPjkParamTimeout(apptimer_var_t val);
@@ -312,28 +329,31 @@
    int ss = str2int(s->nmea_value[0].data + 4, 2);
    int dss = str2int(s->nmea_value[0].data + 7, 2);

    CurrRTKInfo.MM = str2int(s->nmea_value[1].data, 2);
    CurrRTKInfo.DD = str2int(s->nmea_value[1].data + 2, 2);
    CurrRTKInfo.YY = str2int(s->nmea_value[1].data + 4, 2);
    xy_temp.month = str2int(s->nmea_value[1].data, 2);
    xy_temp.day = str2int(s->nmea_value[1].data + 2, 2);
    xy_temp.year = str2int(s->nmea_value[1].data + 4, 2);

    CurrRTKInfo.qf = str2int(s->nmea_value[6].data, s->nmea_value[6].length);
    xy_temp.qf = str2int(s->nmea_value[6].data, s->nmea_value[6].length);

    // NOTE: RTK模块是以南北向为X轴，西东向为Y轴，我们交换下，以符合一般逻辑
    str2float(&CurrRTKInfo.y, s->nmea_value[2].data, s->nmea_value[2].length);
    str2float(&CurrRTKInfo.x, s->nmea_value[4].data, s->nmea_value[4].length);
    str2float(&xy_temp.y, s->nmea_value[2].data, s->nmea_value[2].length);
    str2float(&xy_temp.x, s->nmea_value[4].data, s->nmea_value[4].length);

    if (CurrRTKInfo.hh == hh && CurrRTKInfo.mm == mm && CurrRTKInfo.ss == ss && CurrRTKInfo.dss == dss) {
        PlatformStatusChanged(RTK_UPDATE_EVT, (uint8_t *)&CurrRTKInfo, sizeof(CurrRTKInfo));
//        UpdateRTKInfo(&CurrRTKInfo);
//        up_num++;
        /*if ((up_num % 5) == 0)*/ {
//            NewMgrEvent(DRIVER_UPDATE_EVT);
        }
    if (xy_temp.hour == hh && xy_temp.min == mm && xy_temp.sec == ss && xy_temp.millisec == dss) {
        rtk_info_t rtk;
        rtk.qf = xy_temp.qf;
        rtk.utc_time = static_cast<int64_t>(TimeMakeComposite(2000 + xy_temp.year, xy_temp.month, xy_temp.day, xy_temp.hour, xy_temp.min, xy_temp.sec))*static_cast<int64_t>(1000) + static_cast<int64_t>(dss*10);
        rtk.x = xy_temp.x;
        rtk.y = xy_temp.y;
        rtk.heading = xy_temp.heading;
        rtk.pitch = xy_temp.pitch;
        rtk.roll = xy_temp.roll;
        PlatformStatusChanged(RTK_UPDATE_EVT, (uint8_t *)&rtk, sizeof(rtk));
    }
    CurrRTKInfo.hh = hh;
    CurrRTKInfo.mm = mm;
    CurrRTKInfo.ss = ss;
    CurrRTKInfo.dss = dss;
    xy_temp.hour = hh;
    xy_temp.min = mm;
    xy_temp.sec = ss;
    xy_temp.millisec = dss;
}

void handleGPTRA(const struct nmea *s) {
@@ -344,22 +364,25 @@
    int ss = str2int(s->nmea_value[0].data + 4, 2);
    int dss = str2int(s->nmea_value[0].data + 7, 2);

    str2float(&CurrRTKInfo.heading, s->nmea_value[1].data, s->nmea_value[1].length);
    str2float(&CurrRTKInfo.pitch, s->nmea_value[2].data, s->nmea_value[2].length);
    str2float(&CurrRTKInfo.roll, s->nmea_value[3].data, s->nmea_value[3].length);
    str2float(&xy_temp.heading, s->nmea_value[1].data, s->nmea_value[1].length);
    str2float(&xy_temp.pitch, s->nmea_value[2].data, s->nmea_value[2].length);
    str2float(&xy_temp.roll, s->nmea_value[3].data, s->nmea_value[3].length);

    if (CurrRTKInfo.hh == hh && CurrRTKInfo.mm == mm && CurrRTKInfo.ss == ss && CurrRTKInfo.dss == dss) {
        PlatformStatusChanged(RTK_UPDATE_EVT, (uint8_t *)&CurrRTKInfo, sizeof(CurrRTKInfo));
//        UpdateRTKInfo(&CurrRTKInfo);
//        up_num++;
        /*if ((up_num % 5) == 0)*/ {
//            NewMgrEvent(DRIVER_UPDATE_EVT);
        }
    if (xy_temp.hour == hh && xy_temp.min == mm && xy_temp.sec == ss && xy_temp.millisec == dss) {
        rtk_info_t rtk;
        rtk.qf = xy_temp.qf;
        rtk.utc_time = static_cast<int64_t>(TimeMakeComposite(2000 + xy_temp.year, xy_temp.month, xy_temp.day, xy_temp.hour, xy_temp.min, xy_temp.sec))*static_cast<int64_t>(1000) + static_cast<int64_t>(dss*10);
        rtk.x = xy_temp.x;
        rtk.y = xy_temp.y;
        rtk.heading = xy_temp.heading;
        rtk.pitch = xy_temp.pitch;
        rtk.roll = xy_temp.roll;
        PlatformStatusChanged(RTK_UPDATE_EVT, (uint8_t *)&rtk, sizeof(rtk));
    }
    CurrRTKInfo.hh = hh;
    CurrRTKInfo.mm = mm;
    CurrRTKInfo.ss = ss;
    CurrRTKInfo.dss = dss;
    xy_temp.hour = hh;
    xy_temp.min = mm;
    xy_temp.sec = ss;
    xy_temp.millisec = dss;
}

static void CheckPjkParam(void)

 lib/src/main/cpp/rtk_module/rtk.h

@@ -43,13 +43,15 @@
}gpsStatus_t;

typedef struct {
    int YY;
    int MM;
    int DD;
    int hh;
    int mm;
    int ss;
    int dss;
//    int YY;
//    int MM;
//    int DD;
//    int hh;
//    int mm;
//    int ss;
//    int dss;

    int64_t utc_time;    // 单位毫秒
    int qf;
    double heading;
    double pitch;
@@ -61,8 +63,8 @@
        std::stringstream sst;

        sst<<"RTK:"<<"qf="<<qf
           <<" "<<2000+YY<<intw(MM, 2)<<intw(DD, 2)<<intw(hh,2)<<intw(mm, 2)<<intw(ss, 2)<<"."<<intw(dss, 2)
           <<" heading="<<round(heading, 3)
//           <<" "<<2000+YY<<intw(MM, 2)<<intw(DD, 2)<<intw(hh,2)<<intw(mm, 2)<<intw(ss, 2)<<"."<<intw(dss, 2)
           <<" yaw="<<round(heading, 3)
           <<" pitch="<<round(pitch, 3)
           <<" roll="<<round(roll, 3)
           <<" x="<<round(x, 3)
@@ -70,7 +72,7 @@

        return sst.str();
    }
}rtk_info;
}rtk_info_t;

void RtkCommModeSel(int mode);
void ConfigRTKModule(bool ayDevice);

 lib/src/main/cpp/rtk_platform/platform.cpp

@@ -23,9 +23,6 @@
#include "../utils/num.h"
#include "../driver_test.h"
#include "../test_common/car_sensor.h"
#include "../test_items2/stop_car.h"
#include "../test_items2/operate_gear.h"
#include "../test_items2/drive_straight.h"
#include "../rtk_module/virtual_rtk.h"

#define PARSE_BUFF_SIZE         4096
@@ -402,7 +399,7 @@
        case GPS_UPDATE_EVT: {
            const gpsStatus_t *gps = (gpsStatus_t *) data;

            DEBUG("GPS: %s", const_cast<gpsStatus_t *>(gps)->toString().c_str());
//            DEBUG("GPS: %s", const_cast<gpsStatus_t *>(gps)->toString().c_str());

            gbf.qf = gps->gps_status;
            gbf.latitude = gps->latitude;
@@ -432,9 +429,9 @@
            uint32_t ost = AppTimer_GetTickCount();
            uint32_t ost1 = ost;

            const rtk_info *rtk = (rtk_info *) data;
            const rtk_info_t *rtk = (rtk_info_t *) data;

            DEBUG("RTK: %s", const_cast<rtk_info *>(rtk)->toString().c_str());
//            DEBUG("RTK: %s", const_cast<rtk_info_t *>(rtk)->toString().c_str());

            rbf.qf = rtk->qf;
            rbf.coord_x = rtk->y;
@@ -446,8 +443,11 @@
            rbf.coord_y_dir = 'E';
            rbf.rtcm_length = rtcmLength;

            sprintf(rbf.utc, "%04d%02d%02d%02d%02d%02d.%02d", 2000 + rtk->YY, rtk->MM, rtk->DD,
                    rtk->hh, rtk->mm, rtk->ss, rtk->dss);
            struct TimeStructure ts;

            TimeBreakdown(rtk->utc_time / 1000, &ts);
            sprintf(rbf.utc, "%04d%02d%02d%02d%02d%02d.%02d", ts.Year, ts.Month, ts.Day,
                    ts.Hour, ts.Minute, ts.Second, (rtk->utc_time % 1000) / 10);
            if (!strcmp(rbf.utc, gbf.utc)) {
                rbf.sat_num = gbf.sat_num;
                rbf.latitude = gbf.latitude;
@@ -461,7 +461,8 @@

            ost1 = AppTimer_GetTickCount() - ost1;

            UpdateRTKInfo(rtk);
            if (rtk->qf == 3)
                UpdateRTKInfo(rtk);

            ost = AppTimer_GetTickCount() - ost;


 lib/src/main/cpp/test_common/Geometry.cpp

@@ -78,7 +78,7 @@
    polygon->num = 0;
}

Relation IntersectionOf(const Polygon *polygon1, const Polygon *polygon2)
relation_t IntersectionOf(const Polygon *polygon1, const Polygon *polygon2)
{
    bool inside = false, outside = false, tangent = false;

@@ -87,7 +87,7 @@
    }

    for (int idx = 0; idx < polygon1->num; ++idx) {
        Relation relation = IntersectionOf(polygon1->point[idx], polygon2);
        relation_t relation = IntersectionOf(polygon1->point[idx], polygon2);

        if (relation == GM_Containment) {
            inside = true;
@@ -112,7 +112,7 @@
    return GM_None;
}

Relation IntersectionOf(Line line, const Polygon *polygon)
relation_t IntersectionOf(Line line, const Polygon *polygon)
{
    if (polygon->num == 0) {
        return GM_None;
@@ -135,7 +135,7 @@
//        LOGD("line1(%d %d - %d %d) line2(%d %d - %d %d)", line.X1, line.Y1, line.X2, line.Y2,
//             line2.X1, line2.Y1, line2.X2, line2.Y2);

        Relation relation = IntersectionOf(line, line2);
        relation_t relation = IntersectionOf(line, line2);

//        LOGD("relation = %d", relation);

@@ -155,7 +155,7 @@
    return tangent ? GM_Tangent : IntersectionOf(point2, polygon);
}

Relation IntersectionOf(PointF point, const Polygon *polygon)
relation_t IntersectionOf(PointF point, const Polygon *polygon)
{
    switch (polygon->num)
    {
@@ -222,7 +222,7 @@
    return (counter % 2 == 1) ? GM_Containment : GM_None;
}

Relation IntersectionOf(PointF point, Line line)
relation_t IntersectionOf(PointF point, Line line)
{
    double bottomY = fmin(line.Y1, line.Y2);
    double topY = fmax(line.Y1, line.Y2);
@@ -247,7 +247,7 @@
    }
}

Relation IntersectionOf(Line line1, Line line2)
relation_t IntersectionOf(Line line1, Line line2)
{
    //  Fail if either line segment is zero-length.
    if ((isEqual(line1.X1, line1.X2) && isEqual(line1.Y1, line1.Y2)) || (isEqual(line2.X1, line2.X2) && isEqual(line2.Y1, line2.Y2)))
@@ -313,7 +313,7 @@
}

/*********************************************************
 * p2----------->p1 线端和Y轴的夹角
 * p1----------->p2 线端和Y轴的夹角
 * @param p1
 * @param p2
 * @return yaw
@@ -322,40 +322,102 @@
{
    double deg = 0.0;

    if (fabs(p1.Y - p2.Y) <= GLB_EPSILON) {
        if (p1.X > p2.X) {
    if (fabs(p2.Y - p1.Y) <= GLB_EPSILON) {
        if (p2.X > p1.X) {
            deg = 90;
        } else {
            deg = 270;
        }
    } else if (fabs(p1.X - p2.X) <= GLB_EPSILON) {
        if (p1.Y > p2.Y) {
    } else if (fabs(p2.X - p1.X) <= GLB_EPSILON) {
        if (p2.Y > p1.Y) {
            deg = 0;
        } else {
            deg = 180;
        }
    } else {
        deg = atan(fabs(p1.X - p2.X) /
                   fabs(p1.Y - p2.Y));
        deg = atan(fabs(p2.X - p1.X) /
                   fabs(p2.Y - p1.Y));

        deg = toDegree(deg);

        if (p1.X > p2.X &&
            p1.Y > p2.Y) {
        if (p2.X > p1.X &&
            p2.Y > p1.Y) {

        } else if (p1.X < p2.X &&
                   p1.Y > p2.Y) {
        } else if (p2.X < p1.X &&
                   p2.Y > p1.Y) {
            deg = 360 - deg;
        } else if (p1.X < p2.X &&
                   p1.Y < p2.Y) {
        } else if (p2.X < p1.X &&
                   p2.Y < p1.Y) {
            deg = 180 + deg;
        } else if (p1.X > p2.X &&
                   p1.Y < p2.Y) {
        } else if (p2.X > p1.X &&
                   p2.Y < p1.Y) {
            deg = 180 - deg;
        }
    }

    return deg;
}

double YawOf(Line &line)
{
    PointF p1 = {
            .X = line.X1,
            .Y = line.Y1
    };

    PointF p2 = {
            .X = line.X2,
            .Y = line.Y2
    };

    return YawOf(p1, p2);
}

/**************************************************
 * p1----->p3 与 p1----->p2 两射线的逆时针夹角
 * 左侧小于180，右侧大于180
 * @param p1
 * @param p2
 * @param p3
 * @return
 */
double AngleOf(PointF p1, PointF p2, PointF p3)
{
    int rel = IntersectionOfLine(p1, p2, p3);

    switch (rel) {
        case RELATION_LEFT:
        case RELATION_RIGHT: {
            double a = DistanceOf(p2, p3);
            double b = DistanceOf(p1, p2);
            double c = DistanceOf(p1, p3);

            double deg = toDegree(acos((pow(b, 2) + pow(c, 2) - pow(a, 2)) / (2 * b * c)));

            return (rel == RELATION_LEFT) ? deg : (360-deg);
        }
        case RELATION_BACK:
            return 180;
        default:
            return 0;
    };

    return 0;
}

/************************************************
 * dest相对于base的逆时针夹角
 * @param base
 * @param dest
 * @return
 */
double AngleOfTowLine(Line base, Line dest)
{
    PointF p1 = {.X = base.X1, .Y = base.Y1};
    PointF p2 = {.X = base.X2, .Y = base.Y2};
    PointF p3 = {.X = dest.X2 + (base.X1 - dest.X1), .Y = dest.Y2 + (base.Y1 - dest.Y1)};

    return AngleOf(p1, p2, p3);
}

double DeltaYaw(double yaw1, double yaw2)
@@ -449,27 +511,27 @@
 * @param p3
 * @return 0 - straight, 1 - left, -1 - right, 2 - front, -2 - back
 */
int IntersectionOfLine(PointF p1, PointF p2, PointF p3)
relational_position_t IntersectionOfLine(PointF p1, PointF p2, PointF p3)
{
    double lr = (p1.X-p3.X)*(p2.Y-p3.Y) - (p1.Y-p3.Y)*(p2.X-p3.X);

    if (fabs(lr) <= EPSILON2) {
        double fb = (p2.X-p1.X)*(p3.X-p1.X) + (p2.Y-p1.Y)*(p3.Y-p1.Y);
        if (fabs(fb) <= EPSILON2)
            return 0;
            return REL_POS_ON;
        else if (fb > 0)
            return 2;
            return REL_POS_FRONT;
        else
            return -2;
            return REL_POS_REAR;
    } else if (lr > 0) {
        return 1;
        return REL_POS_LEFT;
    } else {
        return -1;
        return REL_POS_RIGHT;
    }
}

// 0 - straight, 1 - left, -1 - right, 2 - front, -2 - back
int IntersectionOfLine(PointF p, Line line)
relational_position_t IntersectionOfLine(PointF p, Line line)
{
    PointF p1, p2;

@@ -688,4 +750,30 @@
    }

    return deg;
}
}

bool Calc3PointCircle(PointF p1, PointF p2, PointF p3, Circle &circle)
{
    double a = p1.X - p2.X;
    double b = p1.Y - p2.Y;
    double c = p1.X - p3.X;
    double d = p1.Y - p3.Y;
    double e = ((pow(p1.X,2) - pow(p2.X,2)) - (pow(p2.Y,2) - pow(p1.Y,2))) / 2;
    double f = ((pow(p1.X,2) - pow(p3.X,2)) - (pow(p3.Y,2) - pow(p1.Y,2))) / 2;

    // 三点不能共线
    if (isEqual2(a*d, b*c)) {
        circle.centre.X = circle.centre.Y = circle.radius = 0;
        return false;
    }

    double x0 = (d*e - b*f)/(a*d - b*c);
    double y0 = (a*f - c*e)/(a*d - b*c);
    double r = sqrt(pow(x0-p1.X, 2) + pow(y0-p1.Y, 2));

    circle.centre.X = x0;
    circle.centre.Y = y0;
    circle.radius = r;

    return true;
}

 lib/src/main/cpp/test_common/Geometry.h

@@ -9,13 +9,13 @@
#include <initializer_list>
#include <vector>

enum Relation
typedef enum
{
    GM_None,
    GM_Tangent,
    GM_Intersection,
    GM_Containment
};
} relation_t;

typedef struct PointF_ {
    double X;
@@ -34,6 +34,13 @@
    PointF *point;
} Polygon;

typedef struct Circle_ {
    PointF centre;
    double radius;
} Circle;

#define MAKE_LINE(a, b, c)    { (a).X1=b.X; (a).Y1=b.Y; (a).X2=c.X; (a).Y2=c.Y; }

 double toRadians(double degree);
 double toDegree(double radians);
 bool isEqual(double a, double b);
@@ -44,14 +51,17 @@
void CleanPolygon(Polygon *polygon);
void MakeHidePoint(PointF *point, const PointF *bp, const Line *bl);

Relation IntersectionOf(const Polygon *polygon1, const Polygon *polygon2);
Relation IntersectionOf(Line line, const Polygon *polygon);
Relation IntersectionOf(PointF point, const Polygon *polygon);
Relation IntersectionOf(PointF point, Line line);
Relation IntersectionOf(Line line1, Line line2);
relation_t IntersectionOf(const Polygon *polygon1, const Polygon *polygon2);
relation_t IntersectionOf(Line line, const Polygon *polygon);
relation_t IntersectionOf(PointF point, const Polygon *polygon);
relation_t IntersectionOf(PointF point, Line line);
relation_t IntersectionOf(Line line1, Line line2);
double DistanceOf(PointF point1, PointF point2);
double DistanceOf(PointF point, Line line);
double YawOf(PointF p1, PointF p2);
double YawOf(Line &line);
double AngleOf(PointF p1, PointF p2, PointF p3);
double AngleOfTowLine(Line base, Line dest);
double DeltaYaw(double yaw1, double yaw2);
double CalculateAngle(Line base, Line dest);
PointF rotatePoint(PointF oldPoint, PointF centre, double degree);
@@ -59,8 +69,17 @@
bool InsidePolygon(const Polygon *t1, const Polygon *t2);
bool PartInsidePolygon(const Polygon *t1, const Polygon *t2);
bool OutsidePolygon(const Polygon *t1, const Polygon *t2);
int IntersectionOfLine(PointF p1, PointF p2, PointF p3);
int IntersectionOfLine(PointF p, Line line);

typedef enum {
    REL_POS_ON,
    REL_POS_LEFT,
    REL_POS_RIGHT,
    REL_POS_FRONT,
    REL_POS_REAR
} relational_position_t;

relational_position_t IntersectionOfLine(PointF p1, PointF p2, PointF p3);
relational_position_t IntersectionOfLine(PointF p, Line line);
PointF GetVerticalPoint(PointF p1, PointF p2, PointF p3);
bool VerticalPointOnLine(PointF point, Line line);
bool VerticalPointOnLine(PointF point, Line line, PointF &vp);
@@ -68,5 +87,6 @@
PointF PointExtend(PointF ori, double length, double yaw);
bool IsSamePoint(PointF p1, PointF p2);
double AvgYaw(std::vector<double> &angles);
bool Calc3PointCircle(PointF p1, PointF p2, PointF p3, Circle &circle);

#endif //GUI_GEOMETRY_H

 lib/src/main/cpp/test_common/odo_graph.cpp

@@ -3,18 +3,18 @@
//

#include "odo_graph.h"
#include "../test_items2/road_exam.h"
#include "../common/apptimer.h"
#include "../utils/xconvert.h"
#include "../driver_test.h"

static double odoGraph;
static struct RtkTime odoTimer;
static double odoPrevSpeed;
static int odoCnt;
static int64_t prevTimestamp;
static double prevSpeed;
static bool isstop;

void ResetOdo(void)
{
    odoCnt = 0;
    isstop = true;
    odoGraph = 0;
}

@@ -23,22 +23,27 @@
    return odoGraph;
}

void UpdataOdo(double speed, int moveDirect, const struct RtkTime *rtkTime) {
void UpdataOdo(motion_t &motion) {
    // 行驶距离，含倒车
    if (odoCnt == 0 && moveDirect != 0) {
        odoPrevSpeed = speed;
        odoCnt = 1;
        odoTimer = *rtkTime;
    } else if (odoCnt == 1) {
        if (moveDirect != 0) {
            uint32_t tm = TimeGetDiff(rtkTime, &odoTimer);
            if (tm >= D_SEC(1)) {
                odoGraph += ((double) tm) * (odoPrevSpeed + speed) / 2.0 / 1000.0;
                odoTimer = *rtkTime;
                odoPrevSpeed = speed;
    if (isstop && motion.move != STOP) {
        prevSpeed = motion.speed;
        isstop = false;
        prevTimestamp = motion.timestamp;
    } else if (!isstop) {
        if (motion.move != STOP) {
            uint32_t elapsed = motion.timestamp - prevTimestamp;
            if (elapsed >= D_SEC(5)) {      // 中途长时间未定位，重新开始测量
                prevSpeed = motion.speed;
                prevTimestamp = motion.timestamp;
                return;
            }
            if (elapsed >= D_SEC(1)) {
                odoGraph += ((double) elapsed) * (prevSpeed + motion.speed) / 2.0 / 1000.0;
                prevTimestamp = motion.timestamp;
                prevSpeed = motion.speed;
            }
        } else {
            odoCnt = 0;
            isstop = true;
        }
    }
}

 lib/src/main/cpp/test_common/odo_graph.h

@@ -5,8 +5,10 @@
#ifndef MYAPPLICATION2_ODO_GRAPH_H
#define MYAPPLICATION2_ODO_GRAPH_H

#include "../driver_test.h"

void ResetOdo(void);
double ReadOdo(void);
void UpdataOdo(double speed, int moveDirect, const struct RtkTime *rtkTime);
void UpdataOdo(motion_t &motion);

#endif //MYAPPLICATION2_ODO_GRAPH_H

 lib/src/main/cpp/test_items/area_exam.cpp

@@ -16,17 +16,208 @@
#include "../common/apptimer.h"
#include "../test_common/odo_graph.h"
#include "../test_common/Geometry.h"
#include "../common/observer.h"

#define DEBUG(fmt, args...)     LOGD("<area_exam> <%s>: " fmt, __func__, ##args)

static int CurrExamStatus = EXAM_AREA_NONE;      // 1 测试完成 0 测试中 -1 测试错误退出
static int CurrExamMapIndex = -1;
static int CurrEnterMapIndex = -1;
ilovers::Observer<std::function<void(move_status_t)>> CarMoveEvent;

static void DetectEnterOrExitMap(int moveDirect, const car_model *CarModel, LIST_CAR_MODEL &CarModelList, LIST_AREA_MAP &mapList);
static int EnterMap(int moveDirect, const car_model *car, LIST_CAR_MODEL &CarModelList, LIST_AREA_MAP &mapList);
static bool ExitMap(const car_model *car, int index, LIST_AREA_MAP &mapList);
static bool CrashTriggerLine(Line triggerLine, const car_model *car, LIST_CAR_MODEL &CarModelList);
static void ExecuteExam(int index, LIST_AREA_MAP &AreaMapList, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int move, double azimuth, const struct RtkTime* rtkTime);
static bool ProximityArea(Line &base_line, Line &line);
static bool CrossingStartLine(Line &trace, Line &start_line);
static void ProximityReminders(prime_t &prime);
static void DetectCarMove(prime_t &prime);

void AreaExam(prime_t &prime)
{
    switch (prime.curr_exam_map.type) {
        case MAP_TYPE_PARK_BUTTOM:
            break;
        case MAP_TYPE_PARK_EDGE:
            break;
        case MAP_TYPE_TURN_90:
            break;
        case MAP_TYPE_UPHILL:
            break;
        case MAP_TYPE_CURVE:
            break;
        default:
            if (EnterMap()) {

            }
            break;
    }
    DetectCarMove(prime);
}

static void DetectCarMove(prime_t &prime)
{
    static move_status_t prevMove = STOP;
    if (prime.pMotion->move != prevMove) {
        // Notify
        CarMoveEvent.Notify(prime.pMotion->move);
        prevMove = prime.pMotion->move;
    }
}

void RegisterCarMoveObserver(void (*ptr)(move_status_t))
{
    CarMoveEvent.Connect(ptr);
}

void UnregisterCarMoveObserver(int handle)
{
    CarMoveEvent.Disconnect(handle);
}

void EnterMap(prime_t &prime)
{
    if (prime.curr_exam_map.type != 0) {
        return;
    }

    if (prime.prev_modeling_index == -1 || prime.curr_modeling_index == -1) {
        return;
    }

    PointF &lp1 = prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_front_tire[TIRE_OUTSIDE]];
    PointF &lp2 = prime.pModeling[prime.prev_modeling_index].points[prime.pModel->left_front_tire[TIRE_OUTSIDE]];
    PointF &rp1 = prime.pModeling[prime.curr_modeling_index].points[prime.pModel->right_front_tire[TIRE_OUTSIDE]];
    PointF &rp2 = prime.pModeling[prime.prev_modeling_index].points[prime.pModel->right_front_tire[TIRE_OUTSIDE]];

    Line left_trace, right_trace;

    MAKE_LINE(left_trace, lp1, lp2);
    MAKE_LINE(right_trace, rp1, rp2);

    Line start_line;
    for (int i = 0; i < prime.pMap->park_button_map.size(); ++i) {
        MAKE_LINE(start_line, prime.pMap->park_button_map[i].map[1], prime.pMap->park_button_map[i].map[0]);
        if (CrossingStartLine(left_trace, start_line)) {
            prime.curr_exam_map.type = MAP_TYPE_PARK_BUTTOM;
            prime.curr_exam_map.map_idx = i;
            StartParkBottom(prime);
            return;
        }
        MAKE_LINE(start_line, prime.pMap->park_button_map[i].map[7], prime.pMap->park_button_map[i].map[6]);
        if (CrossingStartLine(left_trace, start_line)) {
            prime.curr_exam_map.type = MAP_TYPE_PARK_BUTTOM;
            prime.curr_exam_map.map_idx = i;
            StartParkBottom(prime);
            return;
        }
    }

    for (int i = 0; i < prime.pMap->park_edge_map.size(); ++i) {
        MAKE_LINE(start_line, prime.pMap->park_edge_map[i].map[1], prime.pMap->park_edge_map[i].map[0]);
        if (CrossingStartLine(left_trace, start_line)) {
            prime.curr_exam_map.type = MAP_TYPE_PARK_EDGE;
            prime.curr_exam_map.map_idx = i;
            return;
        }
    }

    for (int i = 0; i < prime.pMap->uphill_map.size(); ++i) {
        PointF vPoint = Calc3Point(prime.pMap->uphill_map[i].map[8], prime.pMap->uphill_map[i].map[0], DistanceOf(prime.pMap->uphill_map[i].map[8], prime.pMap->uphill_map[i].map[7]), 'R');
        MAKE_LINE(start_line, prime.pMap->uphill_map[i].map[0], vPoint);
        if (CrossingStartLine(left_trace, start_line)) {
            prime.curr_exam_map.type = MAP_TYPE_UPHILL;
            prime.curr_exam_map.map_idx = i;
            return;
        }
    }
    for (int i = 0; i < prime.pMap->curve_map.size(); ++i) {
        MAKE_LINE(start_line, prime.pMap->curve_map[i].right_start_point, prime.pMap->curve_map[i].left_start_point);
        if (CrossingStartLine(left_trace, start_line)) {
            prime.curr_exam_map.type = MAP_TYPE_CURVE;
            prime.curr_exam_map.map_idx = i;
            return;
        }
    }
    for (int i = 0; i < prime.pMap->turn_a90_map.size(); ++i) {
        MAKE_LINE(start_line, prime.pMap->turn_a90_map[i].map[0], prime.pMap->turn_a90_map[i].map[1]);
        if (CrossingStartLine(left_trace, start_line)) {
            prime.curr_exam_map.type = MAP_TYPE_TURN_90;
            prime.curr_exam_map.map_idx = i;
            return;
        }
    }
}

// 车轮驶过线，且车头位于右侧
static bool CrossingStartLine(Line &trace, Line &start_line)
{
    PointF head = {.X = trace.X1, .Y = trace.Y1};

    if (IntersectionOf(trace, start_line) == GM_Intersection
        && IntersectionOfLine(head, start_line) == RELATION_RIGHT) {
        return true;
    }
    return false;
}

void FarawayMap(prime_t &prime)
{
    if (prime.arriving_map.type != 0) {
        PointF &car_head = prime.pModeling[prime.curr_modeling_index].points[prime.pModel->axial[AXIAL_FRONT]];
        PointF car_head_trend = PointExtend(car_head, 7, prime.pModeling->yaw);
        Line car_head_line;
        MAKE_LINE(car_head_line, car_head, car_head_trend);


    }
}

static void ProximityReminders(prime_t &prime)
{
    if (prime.curr_exam_map.type != MAP_TYPE_NONE && prime.arriving_map.type != MAP_TYPE_NONE) {
        return;
    }

    PointF &car_head = prime.pModeling[prime.curr_modeling_index].points[prime.pModel->axial[AXIAL_FRONT]];
    PointF car_head_trend = PointExtend(car_head, 6, prime.pModeling->yaw);
    Line car_head_line;
    MAKE_LINE(car_head_line, car_head, car_head_trend);

    Line start_line;
    for (int i = 0; i < prime.pMap->park_button_map.size(); ++i) {  // 左右2条控制线都可作为入口
        MAKE_LINE(start_line, prime.pMap->park_button_map[i].map[1], prime.pMap->park_button_map[i].map[0]);

        MAKE_LINE(start_line, prime.pMap->park_button_map[i].map[7], prime.pMap->park_button_map[i].map[6]);

    }
    for (int i = 0; i < prime.pMap->park_edge_map.size(); ++i) {
        MAKE_LINE(start_line, prime.pMap->park_edge_map[i].map[1], prime.pMap->park_edge_map[i].map[0]);
    }

    for (int i = 0; i < prime.pMap->uphill_map.size(); ++i) {
        PointF vPoint = Calc3Point(prime.pMap->uphill_map[i].map[8], prime.pMap->uphill_map[i].map[0], DistanceOf(prime.pMap->uphill_map[i].map[8], prime.pMap->uphill_map[i].map[7]), 'R');
        MAKE_LINE(start_line, prime.pMap->uphill_map[i].map[0], vPoint);
    }
    for (int i = 0; i < prime.pMap->curve_map.size(); ++i) {
        MAKE_LINE(start_line, prime.pMap->curve_map[i].right_start_point, prime.pMap->curve_map[i].left_start_point);
    }
    for (int i = 0; i < prime.pMap->turn_a90_map.size(); ++i) {
        MAKE_LINE(start_line, prime.pMap->turn_a90_map[i].map[0], prime.pMap->turn_a90_map[i].map[1]);
    }
}

// 判断是否接近场地的起始线
// 车头趋势线是否和入口线相交
// 和入口线的夹角
static bool ProximityArea(Line &base_line, Line &line)
{
    PointF head = {.X = line.X1, .Y = line.Y1};

    if (IntersectionOf(base_line, line) == GM_Intersection
        && DistanceOf(head, base_line) > 1) {
        double angle = AngleOfTowLine(base_line, line);
        if (angle >= 240 && angle <= 300) {
            return true;
        }
    }

    return false;
}

void TerminateAreaExam(void)
{
@@ -37,479 +228,4 @@
{
    CurrExamMapIndex = -1;
    ResetOdo();
}

void TestAreaGeneral(LIST_AREA_MAP &AreaMapList, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, double azimuth, const struct RtkTime *rtkTime)
{
    UpdataOdo(speed, moveDirect, rtkTime);

    DetectEnterOrExitMap(moveDirect, car, CarModelList, AreaMapList);

    ExecuteExam(CurrExamMapIndex, AreaMapList, car, CarModelList, speed, moveDirect, azimuth, rtkTime);
}

void DistanceOfTire2X(std::vector<double> &array, const car_model *car, std::vector<Line> line_set)
{
    double ld = 100, rd = 100;

    array.clear();

    for (auto line = line_set.begin(); line != line_set.end(); line++) {
        double ld_t = 100, rd_t = 100;
        double d;
        if (VerticalPointOnLine(car->carXY[car->left_front_tire[TIRE_OUTSIDE]], *line)) {
            d = DistanceOf(car->carXY[car->left_front_tire[TIRE_OUTSIDE]], *line);
            if (d < ld_t) {
                ld_t = d;
            }
        }
        if (VerticalPointOnLine(car->carXY[car->left_rear_tire[TIRE_OUTSIDE]], *line)) {
            d = DistanceOf(car->carXY[car->left_rear_tire[TIRE_OUTSIDE]], *line);
            if (d < ld_t) {
                ld_t = d;
            }
        }

        if (VerticalPointOnLine(car->carXY[car->right_front_tire[TIRE_OUTSIDE]], *line)) {
            d = DistanceOf(car->carXY[car->right_front_tire[TIRE_OUTSIDE]], *line);
            if (d < rd_t) {
                rd_t = d;
            }
        }
        if (VerticalPointOnLine(car->carXY[car->right_rear_tire[TIRE_OUTSIDE]], *line)) {
            d = DistanceOf(car->carXY[car->right_rear_tire[TIRE_OUTSIDE]], *line);
            if (d < rd_t) {
                rd_t = d;
            }
        }

        if (isEqual2(ld_t, rd_t)) {
            if (!isEqual(ld_t, 100)) {
                if (rd_t < rd)
                    rd = rd_t;
                if (ld_t < ld)
                    ld = ld_t;
            }
        } else if (ld_t > rd_t) {
            if (rd_t < rd)
                rd = rd_t;
        } else {
            if (ld_t < ld)
                ld = ld_t;
        }
    }

    array.push_back(ld);
    array.push_back(rd);
}

static void DetectEnterOrExitMap(int moveDirect, const car_model *car, LIST_CAR_MODEL &CarModelList, LIST_AREA_MAP &mapList)
{
    if (CurrExamMapIndex < 0) {
        if (CurrEnterMapIndex < 0) {
            CurrEnterMapIndex = EnterMap(moveDirect, car, CarModelList, mapList);
            if (CurrEnterMapIndex >= 0) {
                DEBUG("进入某个子项目 idx = %d", CurrEnterMapIndex);
                CurrExamMapIndex = CurrEnterMapIndex;
                CurrExamStatus = EXAM_AREA_START;
            }
        } else {
            if (ExitMap(car, CurrEnterMapIndex, mapList)) {
                DEBUG("离开某个子项目 idx = %d", CurrEnterMapIndex);
                CurrEnterMapIndex = -1;
            }
        }
    }
}

static void ExecuteExam(int index, LIST_AREA_MAP &AreaMapList, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int move, double azimuth, const struct RtkTime* rtkTime)
{
    if (index >= 0) {
        if (CurrExamStatus == EXAM_AREA_START) {
            DEBUG("CurrExamMapIndex %d mtype %d", AreaMapList[index].id, AreaMapList[index].type);

            switch (AreaMapList[index].type) {
                case MAP_TYPE_PARK_BUTTOM:
                    DEBUG("进入倒车入库场地 %d", AreaMapList[index].id);

                    StartParkBottom(AreaMapList[index].id, move, rtkTime);
                    CurrExamStatus = EXAM_AREA_RUN;
                    break;
                case MAP_TYPE_STOP_START:
                    DEBUG("进入上坡起步场地 %d", AreaMapList[index].id);

                    StartSAS(AreaMapList[index].id, &AreaMapList[index].map, car, move, rtkTime);
                    CurrExamStatus = EXAM_AREA_RUN;
                    break;
                case MAP_TYPE_PART_EDGE:
                    DEBUG("进入侧方位停车场地 %d",  AreaMapList[index].id);

                    StartParkEdge(AreaMapList[index].id, move, rtkTime);
                    CurrExamStatus = EXAM_AREA_RUN;
                    break;
                case MAP_TYPE_CURVE:
                    DEBUG("进入曲线行驶场地 %d", AreaMapList[index].id);

                    StartDrivingCurve(AreaMapList[index].id, move, rtkTime);
                    CurrExamStatus = EXAM_AREA_RUN;
                    break;
                case MAP_TYPE_TURN_90:
                    DEBUG("进入直角转弯场地 %d", AreaMapList[index].id);

                    StartTurnA90(AreaMapList[index].id, move, azimuth, rtkTime);
                    CurrExamStatus = EXAM_AREA_RUN;
                    break;
                default:break;
            }
        } else if (CurrExamStatus == EXAM_AREA_RUN) {
            int testing = 0;
            switch (AreaMapList[index].type) {
                case MAP_TYPE_PARK_BUTTOM:
                    testing = TestParkBottom(&AreaMapList[index].map,
                                             car, NULL, speed, move, rtkTime);
                    break;
                case MAP_TYPE_STOP_START:
                    testing = TestSAS(&AreaMapList[index].map, car, NULL, speed, move, rtkTime);
                    break;
                case MAP_TYPE_PART_EDGE:
                    testing = TestParkEdge(&AreaMapList[index].map, car, NULL, speed, move, rtkTime);
                    break;
                case MAP_TYPE_CURVE:
                    testing = TestDrivingCurve(&AreaMapList[index].map, &AreaMapList[index].map2, car, NULL, speed, move, rtkTime);
                    break;
                case MAP_TYPE_TURN_90:
                    testing = TestTurnA90(&AreaMapList[index].map, car, NULL, azimuth, speed, move, rtkTime);
                    break;
                default:
                    break;
            }

            if (testing > 0) {
                CurrExamStatus = EXAM_AREA_RUN;
            } else {
                CurrExamStatus = EXAM_AREA_END;
            }
        }
        if (CurrExamStatus != EXAM_AREA_RUN) {
            // 某项结束
            CurrExamStatus = EXAM_AREA_NONE;
            CurrExamMapIndex = -1;
            DEBUG("结束当前项目");
        }
    }
}

static int EnterMap(int moveDirect, const car_model *car, LIST_CAR_MODEL &CarModelList, LIST_AREA_MAP &mapList)
{
    vector<int> score(mapList.size(), 0);       // 场地重合时，车头尾都在内的优先

    for (int i = 0; i < mapList.size() && car != NULL; ++i) {
        // 车前轮或后轮轨迹越过触发线
        if (mapList[i].type == MAP_TYPE_STOP_START) {
            // 构造虚拟的左上角点
            double x9, y9, xo, yo;

            xo = (mapList[i].map.point[0].X + mapList[i].map.point[7].X) / 2;
            yo = (mapList[i].map.point[0].Y + mapList[i].map.point[7].Y) / 2;

            x9 = 2*xo - mapList[i].map.point[8].X;
            y9 = 2*yo - mapList[i].map.point[8].Y;

            Line triggerLine;

            triggerLine.X1 = mapList[i].map.point[0].X;
            triggerLine.Y1 = mapList[i].map.point[0].Y;
            triggerLine.X2 = x9;
            triggerLine.Y2 = y9;

            if (moveDirect > 0 && CrashTriggerLine(triggerLine, car, CarModelList))
                return i;
        }
        if (mapList[i].type == MAP_TYPE_PARK_BUTTOM) {
            Line triggerLine;

            MakeLine(&triggerLine, &(mapList[i].map.point[1]), &(mapList[i].map.point[0]));

            if (moveDirect > 0 && CrashTriggerLine(triggerLine, car, CarModelList))
                return i;

            MakeLine(&triggerLine, &(mapList[i].map.point[7]), &(mapList[i].map.point[6]));

            if (moveDirect > 0 && CrashTriggerLine(triggerLine, car, CarModelList))
                return i;

            // 全车在场地内
            if (moveDirect == 0) {
                Polygon carBody;
                bool ret = false;
                carBody.num = car->bodyNum;
                carBody.point = (PointF *) malloc(carBody.num * sizeof(PointF));
                for (int i = 0; i < carBody.num; ++i) {
                    carBody.point[i] = car->carXY[car->body[i]];
                }

                if (IntersectionOf(&carBody, &mapList[i].map) == GM_Containment) {
                    ret = true;
                }

                free(carBody.point);

                if (ret)
                    return i;
            }
//            // 车头顶点在场地内
//            if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &mapList[i].map) == GM_Containment) {
//                Line enterLine1, enterLine2;
//
//                MakeLine(&enterLine1, &(mapList[i].map.point[0]), &(mapList[i].map.point[1]));
//                MakeLine(&enterLine2, &(mapList[i].map.point[6]), &(mapList[i].map.point[7]));
//
//                if (DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], enterLine1) > 0.1 &&
//                    DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], enterLine2) > 0.1)
//                    score[i]++;
//            }
//            // 车尾顶点在场地内
//            if (IntersectionOf(car->carXY[ car->axial[AXIAL_REAR] ], &mapList[i].map) == GM_Containment) {
//                Line enterLine1, enterLine2;
//
//                MakeLine(&enterLine1, &(mapList[i].map.point[0]), &(mapList[i].map.point[1]));
//                MakeLine(&enterLine2, &(mapList[i].map.point[6]), &(mapList[i].map.point[7]));
//
//                if (DistanceOf(car->carXY[car->axial[AXIAL_REAR]], enterLine1) > 0.1 &&
//                    DistanceOf(car->carXY[car->axial[AXIAL_REAR]], enterLine2) > 0.1)
//                    score[i]++;
//            }
        }
        if (mapList[i].type == MAP_TYPE_PART_EDGE) {
            Line triggerLine;

            MakeLine(&triggerLine, &(mapList[i].map.point[1]), &(mapList[i].map.point[0]));

            if (moveDirect > 0 && CrashTriggerLine(triggerLine, car, CarModelList))
                return i;

            // 全车在场地内
            if (moveDirect == 0) {
                Polygon carBody;
                bool ret = false;
                carBody.num = car->bodyNum;
                carBody.point = (PointF *) malloc(carBody.num * sizeof(PointF));
                for (int i = 0; i < carBody.num; ++i) {
                    carBody.point[i] = car->carXY[car->body[i]];
                }

                if (IntersectionOf(&carBody, &mapList[i].map) == GM_Containment) {
                    ret = true;
                }

                free(carBody.point);

                if (ret)
                    return i;
            }
//            // 车头顶点在场地内
//            if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &mapList[i].map) == GM_Containment) {
//                Line enterLine;
//
//                MakeLine(&enterLine, &(mapList[i].map.point[0]), &(mapList[i].map.point[1]));
//
//                if (DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], enterLine) > 0.1)
//                    score[i]++;
//            }
//            // 车尾顶点在场地内
//            if (IntersectionOf(car->carXY[ car->axial[AXIAL_REAR] ], &mapList[i].map) == GM_Containment) {
//                Line enterLine;
//
//                MakeLine(&enterLine, &(mapList[i].map.point[0]), &(mapList[i].map.point[1]));
//
//                if (DistanceOf(car->carXY[car->axial[AXIAL_REAR]], enterLine) > 0.1)
//                    score[i]++;
//            }
        }
        if (mapList[i].type == MAP_TYPE_TURN_90) {
            // 车前轮或后轮轨迹越过触发线
            Line triggerLine;

            MakeLine(&triggerLine, &(mapList[i].map.point[0]), &(mapList[i].map.point[1]));

            if (moveDirect > 0 && CrashTriggerLine(triggerLine, car, CarModelList))
                return i;
        }
        if (mapList[i].type == MAP_TYPE_CURVE) {
            // 车前轮或后轮轨迹越过触发线
            Line triggerLine;

            MakeLine(&triggerLine, &mapList[i].map2.point[0], &mapList[i].map.point[0]);
            if (moveDirect > 0 && CrashTriggerLine(triggerLine, car, CarModelList))
                return i;
        }
    }

    for (int i = 0; i < score.size(); ++i) {
        if (score[i] == 2) {
            return i;
        }
    }

    for (int i = 0; i < score.size(); ++i) {
        if (score[i] == 1) {
            return i;
        }
    }

    return -1;
}

static bool ExitMap(const car_model *car, int index, LIST_AREA_MAP &mapList)
{
    bool ret = false;
    if (index < 0 || index >= mapList.size()) return true;

    if (mapList[index].type == MAP_TYPE_PARK_BUTTOM ||
        mapList[index].type == MAP_TYPE_PART_EDGE ||
        mapList[index].type == MAP_TYPE_TURN_90) {
        // 全车都需不在地图中
        Polygon carBody;

        carBody.num = car->bodyNum;
        carBody.point = (PointF *)malloc(carBody.num * sizeof(PointF));
        for (int i = 0; i < carBody.num; ++i) {
            carBody.point[i] = car->carXY[car->body[i]];
        }

        if (IntersectionOf(&carBody, &mapList[index].map) == GM_None) {
            ret = true;
        }

        free(carBody.point);
    }
    if (mapList[index].type == MAP_TYPE_STOP_START) {
        // 构造虚拟的左上角点
        double x10, y10, x9, y9, xo, yo;

        bool enter = false;

        xo = (mapList[index].map.point[0].X + mapList[index].map.point[7].X) / 2;
        yo = (mapList[index].map.point[0].Y + mapList[index].map.point[7].Y) / 2;

        x9 = 2*xo - mapList[index].map.point[8].X;
        y9 = 2*yo - mapList[index].map.point[8].Y;

        if (mapList[index].map.num > 9) {
            // 构造虚拟的右上角点
            xo = (mapList[index].map.point[9].X + mapList[index].map.point[7].X) / 2;
            yo = (mapList[index].map.point[9].Y + mapList[index].map.point[7].Y) / 2;
            x10 = 2*xo - mapList[index].map.point[8].X;
            y10 = 2*yo - mapList[index].map.point[8].Y;
        }

        Polygon map;

        map.num = 4;
        map.point = (PointF *) malloc(map.num * sizeof(PointF));

        if (mapList[index].map.num <= 9) {
            map.point[0] = mapList[index].map.point[0];
            map.point[1] = mapList[index].map.point[8];
            map.point[2] = mapList[index].map.point[7];
            map.point[3].X = x9;
            map.point[3].Y = y9;
        } else {
            map.point[0] = mapList[index].map.point[0];
            map.point[1] = mapList[index].map.point[9];
            map.point[2].X = x10;
            map.point[2].Y = y10;
            map.point[3].X = x9;
            map.point[3].Y = y9;
        }
        // 全车都需不在地图中
        Polygon carBody;

        carBody.num = car->bodyNum;
        carBody.point = (PointF *)malloc(carBody.num * sizeof(PointF));
        for (int i = 0; i < carBody.num; ++i) {
            carBody.point[i] = car->carXY[car->body[i]];
        }

        if (IntersectionOf(&carBody, &map) == GM_None) {
            ret = true;
        }

        free(carBody.point);
        free(map.point);
    }
    if (mapList[index].type == MAP_TYPE_CURVE) {
        ret = ExitDrivingCurveArea(&mapList[index].map, &mapList[index].map2, car);
    }

    return ret;
}

static bool CrashTriggerLine(Line triggerLine, const car_model *car, LIST_CAR_MODEL &CarModelList)
{
    bool trigger = false;

    if (CarModelList.size() < 5)
        return trigger;

    Polygon trace, trace2;
    int pn = 0;

    trace2.num = trace.num = 5;
    trace.point = (PointF *) malloc(sizeof(PointF) * trace.num);
    trace2.point = (PointF *) malloc(sizeof(PointF) * trace2.num);

    list<car_model *>::iterator iter = CarModelList.begin();

    car_model *c1 = *iter;

    trace.point[pn] = centerOfTwoPoint(c1->carXY[c1->left_front_tire[TIRE_OUTSIDE]], c1->carXY[c1->right_front_tire[TIRE_OUTSIDE]]);
    trace2.point[pn++] = centerOfTwoPoint(c1->carXY[c1->left_rear_tire[TIRE_OUTSIDE]], c1->carXY[c1->right_rear_tire[TIRE_OUTSIDE]]);

    ++iter;

    while (iter != CarModelList.end() && pn < trace.num) {
        car_model *c2 = *iter;

        uint32_t tdiff = TimeGetDiff(c1->tm.hh, c1->tm.mm, c1->tm.ss, c1->tm.mss * 10, c2->tm.hh, c2->tm.mm, c2->tm.ss, c2->tm.mss*10);

        if (tdiff >= D_SEC(1)) {
            trace.point[pn] = centerOfTwoPoint(c2->carXY[c2->left_front_tire[TIRE_OUTSIDE]], c2->carXY[c2->right_front_tire[TIRE_OUTSIDE]]);
            trace2.point[pn++] = centerOfTwoPoint(c2->carXY[c2->left_rear_tire[TIRE_OUTSIDE]], c2->carXY[c2->right_rear_tire[TIRE_OUTSIDE]]);
            c1 = c2;
        }
        ++iter;
    }

    PointF p1, p2;

    p1.X = triggerLine.X1;
    p1.Y = triggerLine.Y1;
    p2.X = triggerLine.X2;
    p2.Y = triggerLine.Y2;

    int pp = 0;
    for (int p = 1; p < pn; ++p) {
        Line trace_line, trace2_line;

        MakeLine(&trace_line, &trace.point[pp], &trace.point[p]);
        MakeLine(&trace2_line, &trace2.point[pp], &trace2.point[p]);

        if ((IntersectionOf(trace_line, triggerLine) == GM_Intersection &&
            IntersectionOfLine(p1, p2, centerOfTwoPoint(car->carXY[car->left_front_tire[TIRE_OUTSIDE]], car->carXY[car->right_front_tire[TIRE_OUTSIDE]])) == -1 &&
            DistanceOf(centerOfTwoPoint(car->carXY[car->left_front_tire[TIRE_OUTSIDE]], car->carXY[car->right_front_tire[TIRE_OUTSIDE]]), triggerLine) > 0.1) ||
                (IntersectionOf(trace2_line, triggerLine) == GM_Intersection &&
                        IntersectionOfLine(p1, p2, centerOfTwoPoint(car->carXY[car->left_rear_tire[TIRE_OUTSIDE]], car->carXY[car->right_rear_tire[TIRE_OUTSIDE]])) == -1 &&
                        DistanceOf(centerOfTwoPoint(car->carXY[car->left_rear_tire[TIRE_OUTSIDE]], car->carXY[car->right_rear_tire[TIRE_OUTSIDE]]), triggerLine) > 0.1) ) {
            // 碰到触发线
            DEBUG("碰撞触发线 引发地图");
            trigger = true;
            goto SEARCH_TRIGGER_LINE_END;
        }
    }

    SEARCH_TRIGGER_LINE_END:
    free(trace.point);

    return trigger;
}

 lib/src/main/cpp/test_items/area_exam.h

@@ -8,9 +8,12 @@
#include <vector>
#include "../driver_test.h"


void InitAreaExam(void);
void TerminateAreaExam(void);
void TestAreaGeneral(LIST_AREA_MAP &AreaMapList, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, double azimuth, const struct RtkTime *rtkTime);
void DistanceOfTire2X(std::vector<double> &array, const car_model *car, std::vector<Line> line_set);
void AreaExam(prime_t &prime);

void TestAreaGeneral(area_map_t &map, const car_model_t *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, double azimuth, const struct RtkTime *rtkTime);
void DistanceOfTire2X(std::vector<double> &array, const car_model_t *car, std::vector<Line> line_set);

#endif //MYAPPLICATION2_AREA_EXAM_H

 lib/src/main/cpp/test_items/driving_curve.cpp

@@ -34,7 +34,7 @@

static bool UpdateStartLine(struct scan_window_t *zone, const Polygon *map, const Polygon *map2, const Polygon *tireRect);
static bool UpdateEndLine(bool mode, struct scan_window_t *zone, const Polygon *map, const Polygon *map2, const Polygon *tireRect);
static bool CrashRedLine(const Polygon *map, const Polygon *map2, const car_model *car, struct scan_window_t *zone, int &who);
static bool CrashRedLine(const Polygon *map, const Polygon *map2, const car_model_t *car, struct scan_window_t *zone, int &who);

void StartDrivingCurve(int index, int moveDirect, const struct RtkTime *rtkTime)
{
@@ -55,7 +55,54 @@
    MA_EnterMap(mapIndex, MAP_TYPE_CURVE, 1);
}

int TestDrivingCurve(const Polygon *map, const Polygon *map2, const car_model *car, const car_model *carPrev, double speed, int moveDirect, const struct RtkTime *rtkTime)
// 曲线场地关键点
// 入口左右两点，出口左右两点，前半部大小圆圆心和直径，后半部大小圆圆心和直径，实际测量大小圆圆心偏差在20cm内
int TestCurve(const curve_map_t *map, const car_model_t *car, double speed, int moveDirect, const struct RtkTime *rtkTime)
{
    // 判断是否在范围内，如果4个车轮都不在曲线内，视为车辆离开场地
    vector<PointF> pxs = {car->carXY[car->left_front_tire[TIRE_OUTSIDE]],
                          car->carXY[car->left_rear_tire[TIRE_OUTSIDE]],
                          car->carXY[car->right_front_tire[TIRE_OUTSIDE]],
                          car->carXY[car->right_rear_tire[TIRE_OUTSIDE]]};

    int collide = 0;

    for (auto px: pxs) {
        if (IntersectionOfLine(map->front_half_small_circle_centre, map->back_half_small_circle_centre, px) == RELATION_RIGHT) {
            // 位于前半部,实际场地左右边线起点、终点和圆心基本不会共线，可能差异近一米，所以要分开计算
            // 判断的点超过左右起点，即在线的左侧
            if (AngleOf(map->front_half_small_circle_centre, map->left_start_point, px) < 180) {
                if (DistanceOf(map->front_half_small_circle_centre, px) < map->front_half_small_circle_radius) {
                    collide = 1;
                    continue;
                }
            }
            if (AngleOf(map->front_half_big_circle_centre, map->right_start_point, px) < 180) {
                if (DistanceOf(map->front_half_big_circle_centre, px) > map->front_half_big_circle_radius) {
                    collide = 2;
                    continue;
                }
            }
        } else {
            // 判断的点不超过左右终点，即在线的左侧
            if (AngleOf(map->back_half_small_circle_centre, map->right_end_point, px) < 180) {
                if (DistanceOf(map->back_half_small_circle_centre, px) < map->back_half_small_circle_radius) {
                    collide = 2;
                    continue;
                }
            }
            if (AngleOf(map->back_half_big_circle_centre, map->left_end_point, px) < 180) {
                if (DistanceOf(map->back_half_big_circle_centre, px) > map->back_half_big_circle_radius) {
                    collide = 1;
                    continue;
                }
            }
        }
    }

}

int TestDrivingCurve(const Polygon *map, const Polygon *map2, const car_model_t *car, const car_model_t *carPrev, double speed, int moveDirect, const struct RtkTime *rtkTime)
{
    Polygon tireRect;
    int who = 0;
@@ -279,7 +326,7 @@
    return crash;
}

bool ExitDrivingCurveArea(const Polygon *map, const Polygon *map2, const car_model *car)
bool ExitDrivingCurveArea(const Polygon *map, const Polygon *map2, const car_model_t *car)
{
// 全车都需不在地图中
    bool ret = false;
@@ -316,7 +363,7 @@
}

// 车轮是否压边线
static bool CrashRedLine(const Polygon *map, const Polygon *map2, const car_model *car, struct scan_window_t *zone, int &who)
static bool CrashRedLine(const Polygon *map, const Polygon *map2, const car_model_t *car, struct scan_window_t *zone, int &who)
{
    bool ret = false;


 lib/src/main/cpp/test_items/driving_curve.h

@@ -11,8 +11,8 @@
using namespace std;

void StartDrivingCurve(int index, int moveDirect, const struct RtkTime *rtkTime);
bool ExitDrivingCurveArea(const Polygon *map, const Polygon *map2, const car_model *car);
bool ExitDrivingCurveArea(const Polygon *map, const Polygon *map2, const car_model_t *car);

int TestDrivingCurve(const Polygon *map, const Polygon *map2, const car_model *car, const car_model *carPrev, double speed, int moveDirect, const struct RtkTime *rtkTime);
int TestDrivingCurve(const Polygon *map, const Polygon *map2, const car_model_t *car, const car_model_t *carPrev, double speed, int moveDirect, const struct RtkTime *rtkTime);

#endif //RTKDRIVERTEST_DRIVING_CURVE_H

 lib/src/main/cpp/test_items/park_bottom.cpp

@@ -1,6 +1,17 @@
//
// Created by YY on 2019/10/23.
//
//
// 0 ____________________________________7
//
//
// 1 _____________2     5_________________6
//              8|      |9
//               |      |
//               |      |
//               |      |
//              3--------4
//

#include "park_bottom.h"
#include "../common/apptimer.h"
@@ -35,19 +46,22 @@
    TEST_FINISH
};

static int testStatus;
typedef enum {
    NONE_CTRL_LINE,
    LEFT_CTRL_LINE,
    RIGHT_CTRL_LINE
} ctrl_line_t;

static bool reverseCar = false;
static int mapIndex = 0;

const uint32_t CHECK_PARK_DELAY = 400;

static uint32_t stopTimepoint;
static int prevMoveDirect, storeMoveDirectBeforeStop;
static int prevMoveDirect;
static bool occurCrashRedLine;
static bool checkPartStatus;

static uint32_t firstReverseTimepoint;
static bool reportExamTimeout, reportParkFail;
static bool crossCtrlLineSw;

static int parkCount;
static char carray[3];
static int darray[3];
@@ -55,49 +69,44 @@

static int gearAtStop;
static int currGear;
static char prevCrossedCtrlLine;
static ctrl_line_t prevCrossedCtrlLine;
static double odo;
static int exitAreaCfm;
static char CrossCtrlLine(const Polygon *map, const car_model *car, const car_model *prev_car);
static bool EnterParking(const Polygon *map, const car_model *car);
static bool CrashRedLine(const Polygon *map, const car_model *car, int &who);
static bool ExitParkArea(const Polygon *map, const car_model *car);
static bool AllTireExitParkArea(const Polygon *map, const car_model *car);

void StartParkBottom(int index, int moveDirect, const struct RtkTime *rtkTime)
static ctrl_line_t CrossCtrlLine(prime_t &prime);
static bool EnterParking(prime_t &prime);
static bool CrashRedLine(prime_t &prime);


void StartParkBottom(prime_t &prime)
{
    DEBUG("StartParkBottom");
    testStatus = TESTING;

    reverseCar = false;
    mapIndex = index;
    memset(carray, 0, sizeof(carray));
    memset(darray, 0, sizeof(darray));
    memset(parkStatus, 0, sizeof(parkStatus));
    prevMoveDirect = moveDirect;
    prevMoveDirect = prime.pMotion->move;

    checkPartStatus = false;
    firstReverseTimepoint = 0;
    reportExamTimeout = false;

    parkCount = 0;
    crossCtrlLineSw = false;
    reportParkFail = false;

    occurCrashRedLine = false;

    currGear = ReadCarStatus(GEAR);
    prevCrossedCtrlLine = 0;
    prevCrossedCtrlLine = NONE_CTRL_LINE;
    stopTimepoint = 0;
    exitAreaCfm = 0;

    odo = ReadOdo();

    PlayTTS("您已进入倒车入库区域", NULL);
}

int TestParkBottom(const Polygon *map, const car_model *car, const car_model *carPrev, double speed, int moveDirect, const struct RtkTime *rtkTime)
void TestParkBottom(prime_t &prime)
{
    char crossCtrlLine = 0;
    uint32_t tp = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
    int who = 0;
    ctrl_line_t crossCtrlLine = NONE_CTRL_LINE;
    uint32_t tp = AppTimer_GetTickCount();

    vector<double> dtox;
    vector<Line> line_set;
    Line distance_line;
@@ -128,32 +137,32 @@
        odo = ReadOdo();
    }

    if (testStatus == TESTING && gear_change) {
    if (gear_change) {
        if (currGear == GEAR_R) {
            // 挂倒挡,检测是否过控制线
            DEBUG("开始挂倒挡");
            if (!reverseCar) {
                DEBUG("开始首轮入库");
                reverseCar = true;
                MA_EnterMap(mapIndex, MAP_TYPE_PARK_BUTTOM, 1);
                MA_EnterMap(prime.pMap->park_button_map[prime.curr_exam_map.map_idx].id, MAP_TYPE_PARK_BUTTOM, 1);
                firstReverseTimepoint = tp;             // 开始210秒计时
            }
            crossCtrlLine = CrossCtrlLine(map, car, carPrev);
            crossCtrlLine = CrossCtrlLine(prime);

            if (parkCount >= 2) {
                DEBUG("开始次轮入库");
                parkCount = 0;
                MA_EnterMap(mapIndex, MAP_TYPE_PARK_BUTTOM, 1);
                MA_EnterMap(prime.pMap->park_button_map[prime.curr_exam_map.map_idx].id, MAP_TYPE_PARK_BUTTOM, 1);
                firstReverseTimepoint = tp;             // 开始210秒计时
            }

            if (crossCtrlLine == 0) {
            if (crossCtrlLine == NONE_CTRL_LINE) {
                // 倒车前，前轮未驶过控制线
                AddExamFault(20104, rtkTime);
                AddExamFault(20104);
                DEBUG("倒车前，前轮未驶过控制线");
            } else if (crossCtrlLine == prevCrossedCtrlLine) {
                // 重复跨越同一控制线，不按规定线路，顺序形式，不合格
                AddExamFault(20101, rtkTime);
                AddExamFault(20101);
                DEBUG("不按规定线路，顺序形式， 同 %c 侧", prevCrossedCtrlLine);
            } else {
                prevCrossedCtrlLine = crossCtrlLine;
@@ -167,78 +176,45 @@

            DEBUG("库位检查次数 = %d", parkCount);

            if (EnterParking(map, car)) {
            if (EnterParking(prime)) {
                DEBUG("倒库成功");
            } else {
                AddExamFault(20103, rtkTime);
                AddExamFault(20103);
                DEBUG("倒库不入");
            }
        }
    }
    
    if (testStatus == TESTING && parkCount < 2 && AllTireExitParkArea(map, car)) {
        if (++exitAreaCfm >= 4) {       // 避免信号漂移造成的误判
            testStatus = TEST_FAIL;
            AddExamFault(10103, rtkTime);

    if (ExitParkArea(prime)) {
        // 离开场地
        DEBUG("离开场地");
        if (parkCount < 2) {
            AddExamFault(10103);
            DEBUG("直接驶离测试区,不按考试员指令驾驶");
        }
    } else {
        exitAreaCfm = 0;
    }
    
    if (ExitParkArea(map, car)) {
        DEBUG("离开场地");
        // 离开场地
        testStatus = TEST_FINISH;
        /*if ((parkStatus[0] != 1 || parkStatus[1] != 1) && !reportParkFail && reverseCar) {
            // 倒库不入，不合格
            reportParkFail = true;
            AddExamFault(20103, rtkTime);
            DEBUG("倒库不入");
        }*/
        prime.curr_exam_map.type = 0;
        goto TEST_END;
    }

    // 距离检测
    MakeLine(&distance_line, &map->point[0], &map->point[7]);
    line_set.push_back(distance_line);
    MakeLine(&distance_line, &map->point[1], &map->point[2]);
    line_set.push_back(distance_line);
    MakeLine(&distance_line, &map->point[2], &map->point[3]);
    line_set.push_back(distance_line);
    MakeLine(&distance_line, &map->point[3], &map->point[4]);
    line_set.push_back(distance_line);
    MakeLine(&distance_line, &map->point[4], &map->point[5]);
    line_set.push_back(distance_line);
    MakeLine(&distance_line, &map->point[5], &map->point[6]);
    line_set.push_back(distance_line);
    DistanceOfTire2X(dtox, car, line_set);
    MA_SendDistance(dtox[0], dtox[1]);

    if (CrashRedLine(map, car, who)) {
        if (!occurCrashRedLine /*&& reverseCar*/) {
    if (BodyCollidingLine(prime) >= 0) {
        if (!occurCrashRedLine) {
            occurCrashRedLine = true;
            // 车身出线，不合格
            AddExamFault(10116, rtkTime);
            DEBUG("车轮压线");
            /*if (who == 1) {
                PlayTTS("压左库位线", NULL);
            } else if (who == 2) {
                PlayTTS("压右库位线", NULL);
            }*/
            AddExamFault(10116);
            DEBUG("车身出线");
        }
    } else {
        occurCrashRedLine = false;
    }

    if (moveDirect != prevMoveDirect) {
        if (moveDirect == 0) {
    if (prime.pMotion->move != prevMoveDirect) {
        if (prime.pMotion->move == STOP) {
            stopTimepoint = tp;
            gearAtStop = (currGear == GEAR_R ? 1 : 0);
            DEBUG("停车了 %d %d %d %d %d %d %d", rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss);
            DEBUG("停车了");
            DEBUG("停车时挡位 = %d", gearAtStop);
        } else if (prevMoveDirect == 0 && stopTimepoint > 0) {
            DEBUG("继续行驶 %d %d %d %d %d %d %d", rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss);
        } else if (prevMoveDirect == STOP && stopTimepoint > 0) {
            DEBUG("继续行驶");

            DEBUG("停车时间 %ld", tp - stopTimepoint);
            DEBUG("再次移动时挡位 = %d", currGear == GEAR_R ? 1 : 0);
@@ -246,176 +222,122 @@
            if (tp - stopTimepoint >= CorrectPauseCriteria(examParam.park_bottom_pause_criteria)
                && gearAtStop == (currGear == GEAR_R ? 1 : 0)) {
                // 停车超2秒，每次扣5分
                AddExamFault(20106, rtkTime);
                AddExamFault(20106);
                DEBUG("中途停车");
            }
        }

        prevMoveDirect = moveDirect;
        prevMoveDirect = prime.pMotion->move;
    }

/*    crossCtrlLine = CrossCtrlLine(map, car, carPrev);
    if (crossCtrlLine > 0 && !crossCtrlLineSw) {
        crossCtrlLineSw = true;
        if (parkCount == 0) {
            carray[0] = crossCtrlLine;
        } else if (parkCount == 1) {
            if (carray[0] == crossCtrlLine) {
                // 不按规定线路，顺序形式，不合格
                AddExamFault(20101, rtkTime);
                DEBUG("不按规定线路，顺序形式");
            }
            carray[1] = crossCtrlLine;
        } else if (parkCount == 2) {
            if (carray[0] != crossCtrlLine) {
                // 不按规定线路，顺序形式，不合格
                AddExamFault(20101, rtkTime);
                DEBUG("不按规定线路，顺序形式");
            } else {
                // 离开测试区，停止计时
                DEBUG("离开测试区，停止计时");
                testStatus = false;
                goto TEST_END;
            }
            carray[2] = crossCtrlLine;
        }
    }

    if (testStatus && darray[0] > 0 && tp - firstReverseTimepoint >= examParam.park_bottom_limit_time) {
        // 完成超时，不合格
        if (!reportExamTimeout) {
            reportExamTimeout = true;
            AddExamFault(20105, rtkTime);
            DEBUG("项目超时");
        }
    }

    if (moveDirect != prevMoveDirect) {
        if (moveDirect == 0) {
            stopTimepoint = tp;
            storeMoveDirectBeforeStop = prevMoveDirect;
            if (prevMoveDirect == -1) {
                checkPartStatus = true;         // 每次倒车停止，触发入库检查
            }

            DEBUG("停车了 %d %d %d %d %d %d %d", rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss);
        } else {
            DEBUG("继续行驶 %d %d %d %d %d %d %d", rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss);

            DEBUG("停车时间 %ld", tp - stopTimepoint);

            if (moveDirect == storeMoveDirectBeforeStop) {
                // 同方向再启动，继续判断是否停车超时
                if (tp - stopTimepoint >= CorrectPauseCriteria(examParam.park_bottom_pause_criteria) && reverseCar) {
                    // 停车超2秒，每次扣5分
                    AddExamFault(20106, rtkTime);
                    DEBUG("中途停车");
                }
            } else if (moveDirect == -1) {
                // 切换为倒车
                if (!reverseCar) {
                    reverseCar = true;
                    MA_EnterMap(mapIndex, MAP_TYPE_PARK_BUTTOM, 1);
                }
                if (darray[parkCount] == 0) {
                    if (!crossCtrlLineSw) {
                        // 倒车前，前轮未驶过控制线
                        AddExamFault(20104, rtkTime);
                        DEBUG("倒车前，前轮未驶过控制线");
                    }
                    darray[parkCount] = 1;
                    firstReverseTimepoint = tp;         // 首次倒车，才意味着此项目开始
                }
            } else {
                // 切换为前进
                DEBUG("切换为前进");

                if (tp - stopTimepoint >= CHECK_PARK_DELAY) {
                    if (crossCtrlLineSw) {
                        if (parkStatus[parkCount] != 1) {
                            // 倒库不入，不合格
                            reportParkFail = true;
                            AddExamFault(20103, rtkTime);
                            DEBUG("倒库不入");
                        }
                    }

                    crossCtrlLineSw = false;

                    if (parkCount < 2)
                        parkCount++;
                }
            }
        }
        prevMoveDirect = moveDirect;
    } else if (moveDirect == -1) {
        if (darray[parkCount] == 0) {
            // 切换为倒车
            if (!crossCtrlLineSw) {
                // 倒车前，前轮未驶过控制线
                AddExamFault(20104, rtkTime);
                DEBUG("倒车前，前轮未驶过控制线");
            }
            darray[parkCount] = 1;
            firstReverseTimepoint = tp;
        }
    } else if (moveDirect == 0 && crossCtrlLineSw) {
        if (tp - stopTimepoint >= CHECK_PARK_DELAY && checkPartStatus) {
            if (EnterParking(map, car)) {
                parkStatus[parkCount] = 1;
            }
            checkPartStatus = false;
        }
    }*/

TEST_END:
    if (testStatus == TEST_FINISH) {
        DEBUG("倒库结束");
        MA_EnterMap(mapIndex, MAP_TYPE_PARK_BUTTOM, 0);
        return 0;
    DEBUG("倒库结束");
    MA_EnterMap(prime.pMap->park_button_map[prime.curr_exam_map.map_idx].id, MAP_TYPE_PARK_BUTTOM, 0);
}

static int BodyCollidingLine(prime_t &prime)
{
    vector<Line> lines;

    Polygon car_body;

    car_body.num = prime.pModel->body.size();
    car_body.point = new PointF[car_body.num];

    for (int i = 0; i < car_body.num; ++i) {
        car_body.point[i] = prime.pModeling[prime.curr_modeling_index].points[prime.pModel->body[i]];
    }
    return 1;

    Line line;
    MAKE_LINE(line, prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[0],
              prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[7]);
    lines.push_back(line);
    MAKE_LINE(line, prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[1],
              prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[2]);
    lines.push_back(line);
    MAKE_LINE(line, prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[2],
              prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[3]);
    lines.push_back(line);
    MAKE_LINE(line, prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[3],
              prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[4]);
    lines.push_back(line);
    MAKE_LINE(line, prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[4],
              prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[5]);
    lines.push_back(line);
    MAKE_LINE(line, prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[5],
              prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[6]);
    lines.push_back(line);

    int idx = 0;

    for (auto line: lines) {
        if (IntersectionOf(line, &car_body) != GM_None) {
            break;
        }
        idx++;
    }
    delete []car_body.point;
    return idx < lines.size()? idx : -1;
}

// 检测2前轮是否正向越过左右控制线
static char CrossCtrlLine(const Polygon *map, const car_model *car, const car_model *prev_car)
static ctrl_line_t CrossCtrlLine(prime_t &prime)
{
    // 过右控制线
    if ((IntersectionOfLine(map->point[6], map->point[7], car->carXY[car->left_front_tire[TIRE_OUTSIDE]]) == -1) &&
        (IntersectionOfLine(map->point[6], map->point[7], car->carXY[car->right_front_tire[TIRE_OUTSIDE]]) == -1) &&
        (IntersectionOfLine(map->point[6], map->point[7], car->carXY[car->axial[AXIAL_REAR]]) == 1)) {
        return 'R';
    Line left_trace, right_trace;
    Line left_ctrl, right_ctrl;

    MAKE_LINE(left_trace, prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_front_tire[TIRE_OUTSIDE]],
              prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_rear_tire[TIRE_OUTSIDE]]);
    MAKE_LINE(right_trace, prime.pModeling[prime.curr_modeling_index].points[prime.pModel->right_front_tire[TIRE_OUTSIDE]],
              prime.pModeling[prime.curr_modeling_index].points[prime.pModel->right_rear_tire[TIRE_OUTSIDE]]);

    MAKE_LINE(left_ctrl, prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[1], prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[0]);
    MAKE_LINE(right_ctrl, prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[6], prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[7]);


    if (IntersectionOf(left_trace, left_ctrl) == GM_Intersection &&
            IntersectionOf(right_trace, left_ctrl) == GM_Intersection &&
            IntersectionOfLine(prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_front_tire[TIRE_OUTSIDE]], left_ctrl) == RELATION_LEFT) {
        return LEFT_CTRL_LINE;
    }

    // 过左控制线
    if ((IntersectionOfLine(map->point[1], map->point[0], car->carXY[car->left_front_tire[TIRE_OUTSIDE]]) == 1) &&
        (IntersectionOfLine(map->point[1], map->point[0], car->carXY[car->right_front_tire[TIRE_OUTSIDE]]) == 1) &&
        (IntersectionOfLine(map->point[1], map->point[0], car->carXY[car->axial[AXIAL_REAR]]) == -1)) {
        return 'L';
    if (IntersectionOf(left_trace, right_ctrl) == GM_Intersection &&
        IntersectionOf(right_trace, right_ctrl) == GM_Intersection &&
        IntersectionOfLine(prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_front_tire[TIRE_OUTSIDE]], right_ctrl) == RELATION_RIGHT) {
        return RIGHT_CTRL_LINE;
    }

    return 0;
    return NONE_CTRL_LINE;
}

static bool EnterParking(const Polygon *map, const car_model *car) {
// 需要库入口线宽计算在内
static bool EnterParking(prime_t &prime) {
    bool succ = false;

    Polygon parking;
    Polygon park_area;
    Polygon car_body;

    car_body.num = car->bodyNum;
    car_body.point = (PointF *) malloc(sizeof(PointF) * car_body.num);
    car_body.num = prime.pModel->body.size();
    car_body.point = new PointF[car_body.num];
    for (int i = 0; i < car_body.num; ++i) {
        car_body.point[i] = car->carXY[car->body[i]];
        car_body.point[i] = prime.pModeling[prime.curr_modeling_index].points[prime.pModel->body[i]];
    }

    MakePolygon(&parking, {map->point[2], map->point[3], map->point[4], map->point[5]});
    PointF p8 = PointExtend(prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[2],
                prime.pMap->park_button_map[prime.curr_exam_map.map_idx].line_width,
                YawOf(prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[2], prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[3]));

    if (IntersectionOf(&car_body, &parking) == GM_Containment) {
    PointF p9 = PointExtend(prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[5],
                prime.pMap->park_button_map[prime.curr_exam_map.map_idx].line_width,
                YawOf(prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[5], prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[4]));

    MakePolygon(&park_area, {p8, prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[3], prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[4], p9});

    if (IntersectionOf(&car_body, &park_area) == GM_Containment) {
        succ = true;
    }

    CleanPolygon(&parking);
    CleanPolygon(&park_area);
    free(car_body.point);

    DEBUG("检查倒库状态 %s", succ ? "成功" : "失败");
@@ -423,84 +345,41 @@
    return succ;
}

static bool CrashRedLine(const Polygon *map, const car_model *car, int &who)
// 4个车轮和车头点不在场地中
bool ExitParkArea(prime_t &prime)
{
    bool ret = false;
    Polygon polygon;

    Line red_line;
    const int red_lines[][2] = {{0, 7}, {1, 2}, {2, 3}, {3, 4}, {4, 5}, {5, 6}};

    Polygon car_body;

    car_body.num = car->bodyNum;
    car_body.point = (PointF *) malloc(sizeof(PointF) * car_body.num);
    for (int i = 0; i < car_body.num; ++i) {
        car_body.point[i] = car->carXY[car->body[i]];
    polygon.num = prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map.size();
    polygon.point = new PointF[polygon.num];
    for (int i = 0; i < polygon.num; ++i) {
        polygon.point[i] = prime.pMap->park_button_map[prime.curr_exam_map.map_idx].map[i];
    }

    for (int i = 0; i < sizeof(red_lines) / sizeof(red_lines[0]); ++i) {
        MakeLine(&red_line, &(map->point[red_lines[i][0]]), &(map->point[red_lines[i][1]]));
        if (IntersectionOf(red_line, &car_body) != GM_None) {
            if (i == 2 || i == 1) {
                who = 1;
            } else if (i == 4 || i == 5) {
                who = 2;
            }
    int num = 0;

            ret = true;
            break;
        }
    if (IntersectionOf(prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_front_tire[TIRE_OUTSIDE]],
                       &polygon) == GM_None) {
        num++;
    }
    if (IntersectionOf(prime.pModeling[prime.curr_modeling_index].points[prime.pModel->right_front_tire[TIRE_OUTSIDE]],
                       &polygon) == GM_None) {
        num++;
    }
    if (IntersectionOf(prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_rear_tire[TIRE_OUTSIDE]],
                       &polygon) == GM_None) {
        num++;
    }
    if (IntersectionOf(prime.pModeling[prime.curr_modeling_index].points[prime.pModel->right_rear_tire[TIRE_OUTSIDE]],
                       &polygon) == GM_None) {
        num++;
    }
    if (IntersectionOf(prime.pModeling[prime.curr_modeling_index].points[prime.pModel->body[prime.pModel->axial[AXIAL_FRONT]]],
                       &polygon) == GM_None) {
        num++;
    }

    free(car_body.point);
    return ret;
}
    delete []polygon.point;

static bool ExitParkArea(const Polygon *map, const car_model *car)
{
// 全车都需不在地图中
    bool ret = false;

    Polygon carBody;

    carBody.num = car->bodyNum;
    carBody.point = (PointF *)malloc(carBody.num * sizeof(PointF));
    for (int i = 0; i < carBody.num; ++i) {
        carBody.point[i] = car->carXY[car->body[i]];
    }

    if (IntersectionOf(&carBody, map) == GM_None) {
        ret = true;
    }

    free(carBody.point);

    return ret;
}

// 双前轮和双后轮不在区域
static bool AllTireExitParkArea(const Polygon *map, const car_model *car)
{
    int tireExitNum = 0;

    if (IntersectionOf(car->carXY[ car->left_front_tire[TIRE_OUTSIDE] ], map) == GM_None) {
        tireExitNum++;
    }

    if (IntersectionOf(car->carXY[ car->right_front_tire[TIRE_OUTSIDE] ], map) == GM_None) {
        tireExitNum++;
    }

    if (IntersectionOf(car->carXY[ car->left_rear_tire[TIRE_OUTSIDE] ], map) == GM_None) {
        tireExitNum++;
    }

    if (IntersectionOf(car->carXY[ car->right_rear_tire[TIRE_OUTSIDE] ], map) == GM_None) {
        tireExitNum++;
    }

    if (tireExitNum >= 4) {
        return true;
    }
    return false;
    return num == 5? true : false;
}

 lib/src/main/cpp/test_items/park_bottom.h

@@ -11,7 +11,8 @@

using namespace std;

void StartParkBottom(int index, int moveStatus, const struct RtkTime *rtkTime);
int TestParkBottom(const Polygon *map, const car_model *car, const car_model *carPrev, double speed, int moveStatus, const struct RtkTime *rtkTime);
void StartParkBottom(prime_t &prime);
void TestParkBottom(prime_t &prime);
bool ExitParkArea(prime_t &prime);

#endif //RTKDRIVERTEST_PARK_BOTTOM_H

 lib/src/main/cpp/test_items/park_edge.cpp

@@ -1,6 +1,17 @@
//
// Created by YY on 2019/10/23.
//
//
//               4  ___________________________ 3
//                  |                         |
//                  |                         |
//                  |                         |
//  ________________|                         |___________1
//  6               5                         2
//
//
//  7______________________________________________________0
//

#include "park_edge.h"
#include "../jni_log.h"
@@ -29,7 +40,6 @@

const uint32_t CHECK_PARK_DELAY = 400;

static int mapIndex = 0;
static bool reportExamTimeout;
static bool reportParkFail;

@@ -45,24 +55,23 @@
static int currGear;
static double odo;

static bool CrashRedLine1(const Polygon *map, const car_model *car);
static bool CrashRedLine2(const Polygon *map, const car_model *car);
static bool CrashRedLine3(const Polygon *map, const car_model *car);
static bool EnterParking(const Polygon *map, const car_model *car);
static bool ExitParkArea(const Polygon *map, const car_model *car);
static bool ExitParkArea2(const Polygon *map, const car_model *car);
static bool CrashRedLine1(const Polygon *map, const car_model_t *car);
static bool CrashRedLine2(const Polygon *map, const car_model_t *car);
static bool CrashRedLine3(const Polygon *map, const car_model_t *car);
static bool EnterParking(const Polygon *map, const car_model_t *car);
static bool ExitParkArea(const Polygon *map, const car_model_t *car);
static bool ExitParkArea2(const Polygon *map, const car_model_t *car);

void StartParkEdge(int index, int moveStatus, const struct RtkTime *rtkTime)
void StartParkEdge(prime_t &prime)
{
    DEBUG("进入侧方停车场地");

    testStatus = TESTING;
    mapIndex = index;

    occurCrashRedLine1 = occurCrashRedLine2 = occurCrashRedLine3 = false;        // 这个科目规定特殊点，发生一次扣10分，而不直接淘汰
    reportExamTimeout = false;
    reportParkFail = false;
    prevMoveStatus = moveStatus;
    prevMoveStatus = prime.pMotion->move;
    parkSuccess = false;
    parkStatus = 0;
    occurMoveBack = false;
@@ -82,7 +91,7 @@
    }*/
}

int TestParkEdge(const Polygon *map, const car_model *car, const car_model *carPrev, double speed, int moveStatus, const struct RtkTime *rtkTime)
int TestParkEdge(prime_t &prime)
{
    vector<double> dtox;
    vector<Line> line_set;
@@ -128,7 +137,7 @@
                occurMoveBack = true;
                moveBackTimePoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss,
                                                      rtkTime->mss * 10);      // 开始计时
                MA_EnterMap(mapIndex, MAP_TYPE_PART_EDGE, 1);
                MA_EnterMap(mapIndex, MAP_TYPE_PARK_EDGE, 1);
            }
        } else {
            if (occurMoveBack && !checkPark) {
@@ -307,7 +316,7 @@
                DEBUG("开始倒车");
                occurMoveBack = true;
                moveBackTimePoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
                MA_EnterMap(mapIndex, MAP_TYPE_PART_EDGE, 1);
                MA_EnterMap(mapIndex, MAP_TYPE_PARK_EDGE, 1);
            }*/
        }

@@ -328,14 +337,43 @@
TEST_END:
    if (testStatus == TEST_FINISH) {
        DEBUG("侧方停车结束");
        MA_EnterMap(mapIndex, MAP_TYPE_PART_EDGE, 0);
        MA_EnterMap(mapIndex, MAP_TYPE_PARK_EDGE, 0);
        return 0;
    }
    return 1;
}

int EnterParkEdge(prime_t &prime) {
    if (prime.prev_modeling_index == -1 || prime.curr_modeling_index == -1) {
        return -1;
    }

    PointF &lp1 = prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_front_tire[TIRE_OUTSIDE]];
    PointF &lp2 = prime.pModeling[prime.prev_modeling_index].points[prime.pModel->left_front_tire[TIRE_OUTSIDE]];
    PointF &rp1 = prime.pModeling[prime.curr_modeling_index].points[prime.pModel->right_front_tire[TIRE_OUTSIDE]];
    PointF &rp2 = prime.pModeling[prime.prev_modeling_index].points[prime.pModel->right_front_tire[TIRE_OUTSIDE]];

    Line left_trace, right_trace;

    MAKE_LINE(left_trace, lp1, lp2);
    MAKE_LINE(right_trace, rp1, rp2);

    // 不同地区左右倒库次序可能不同，所以两个方向都可以进入
    for (int i = 0; i < prime.pMap->park_button_map.size(); ++i) {
        Line left_entrance_line;

        MAKE_LINE(left_entrance_line, prime.pMap->park_button_map[i].map[1], prime.pMap->park_button_map[i].map[0]);

        if (IntersectionOf(left_trace, left_entrance_line) == GM_Intersection
            && IntersectionOfLine(lp1, left_entrance_line) == RELATION_RIGHT) {
            return i;
        }
    }
    return -1;
}

// 车轮是否压道路边线
static bool CrashRedLine1(const Polygon *map, const car_model *car)
static bool CrashRedLine1(const Polygon *map, const car_model_t *car)
{
    bool ret = false;

@@ -360,7 +398,7 @@
}

// 车身是否压库位线
static bool CrashRedLine2(const Polygon *map, const car_model *car)
static bool CrashRedLine2(const Polygon *map, const car_model_t *car)
{
    bool ret = false;

@@ -387,7 +425,7 @@
    return ret;
}

static bool CrashRedLine3(const Polygon *map, const car_model *car) {
static bool CrashRedLine3(const Polygon *map, const car_model_t *car) {
    bool ret = false;

    if (!occurMoveBack) {
@@ -408,62 +446,6 @@

        free(car_body.point);
    }

    return ret;
}

static bool EnterParking(const Polygon *map, const car_model *car) {
    DEBUG("检查停车到位...");

    bool succ = false;

    Polygon parking;
    Polygon car_body;

    car_body.num = car->bodyNum;
    car_body.point = (PointF *) malloc(sizeof(PointF) * car_body.num);
    for (int i = 0; i < car_body.num; ++i) {
        car_body.point[i] = car->carXY[car->body[i]];
    }

    MakePolygon(&parking, {map->point[2], map->point[3], map->point[4], map->point[5]});

    if (IntersectionOf(&car_body, &parking) == GM_Containment) {
        succ = true;
    }

    CleanPolygon(&parking);
    free(car_body.point);

    return succ;
}

// 车头要驶过前库位线
static bool ExitParkArea(const Polygon *map, const car_model *car)
{
    if (IntersectionOfLine(map->point[6], map->point[7], car->carXY[ car->axial[AXIAL_FRONT] ]) == -1)
            return true;
    return false;
}

static bool ExitParkArea2(const Polygon *map, const car_model *car)
{
// 全车都需不在地图中
    bool ret = false;

    Polygon carBody;

    carBody.num = car->bodyNum;
    carBody.point = (PointF *)malloc(carBody.num * sizeof(PointF));
    for (int i = 0; i < carBody.num; ++i) {
        carBody.point[i] = car->carXY[car->body[i]];
    }

    if (IntersectionOf(&carBody, map) == GM_None) {
        ret = true;
    }

    free(carBody.point);

    return ret;
}

 lib/src/main/cpp/test_items/park_edge.h

@@ -12,6 +12,6 @@
using namespace std;

void StartParkEdge(int index, int moveStatus, const struct RtkTime *rtkTime);
int TestParkEdge(const Polygon *map, const car_model *car, const car_model *carPrev, double speed, int moveStatus, const struct RtkTime *rtkTime);
int TestParkEdge(const Polygon *map, const car_model_t *car, const car_model_t *carPrev, double speed, int moveStatus, const struct RtkTime *rtkTime);

#endif //RTKDRIVERTEST_PARK_EDGE_H

 lib/src/main/cpp/test_items/stop_and_start.cpp

@@ -2,6 +2,21 @@
// Created by YY on 2019/10/31.
//

//                         |9
//                         |
//                         |
//                         |
//                         |
//       7-----------------|8
//       6_________________|5
//       3_________________|4
//                         |
//       2-----------------|1
//                         |
//                         |
//                         |
//                         |0

#include <cstdlib>
#include <vector>
#include <cmath>
@@ -21,7 +36,7 @@
using namespace std;


const uint32_t STOP_CAR_TIME = D_SEC(1);


const double EPSILON = 1e-3;

@@ -29,62 +44,66 @@

static PointF stopPoint;

static int mapIndex = 0;
static int prevMoveDirect;
static uint32_t stopTimepoint = 0;
static bool stopCar = false;
static uint32_t stopCarTime;
static bool check1 = false;

static bool stopConfirm;
static bool restartComplete = false;
static bool occurCrashRedLine = false;
static bool slideLongDistance = false;

static bool slideNormalDistance = false;
static bool reportSlideFault = false;
static bool reportStartTimeout = false;
static bool handBreakActive = false;
static bool CrashRedLine(const Polygon *map, const car_model *car);
static double DistanceOfHead2Stopline(const Polygon *map, const car_model *car);
static double DistanceOfTire2Edge(const Polygon *map, const car_model *car);
static bool ExitTestArea(const Polygon *map, const car_model *car);

void StartSAS(int index, const Polygon *map, const car_model *car, int moveDirect, const struct RtkTime *rtkTime)
static bool CrashRedLine(prime_t &prime);
static double DistanceOfHead2Stopline(prime_t &prime);
static double DistanceOfTire2Edge(prime_t &prime);
static bool ExitTestArea(prime_t &prime);

void StartSAS(prime_t &prime)
{
    double yawEdge = YawOf(map->point[8], map->point[0]);

    if (moveDirect < 0 || DeltaYaw(car->yaw, yawEdge) >= 90.0) {
        testing = false;
        return;
    }

    DEBUG("进入坡起项目");

    testing = true;
    mapIndex = index;
    prevMoveDirect = moveDirect;

    if (moveDirect == 0) {
        stopTimepoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
    }

    stopConfirm = false;
    restartComplete = false;
    occurCrashRedLine = false;
    stopCar = false;
    slideLongDistance = false;
    check1 = false;
    slideNormalDistance = false;
    reportSlideFault = false;
    reportStartTimeout = false;
    handBreakActive = false;

    MA_EnterMap(mapIndex, MAP_TYPE_STOP_START, 1);
    MA_EnterMap(prime.curr_exam_map.map_idx, MAP_TYPE_UPHILL, 1);
}

int TestSAS(const Polygon *map, const car_model *car, const car_model *carPrev, double speed, int moveDirect, const struct RtkTime *rtkTime)
static void StoppingTimeout(apptimer_var_t val) {
    // 起步时间超过30秒，不合格
    AddExamFault(20303);
    DEBUG("起步时间超过30秒");
}

static void StopConfirm(apptimer_var_t val) {
    stopConfirm = true;
}

void MotionChange(move_status_t mv)
{
    AppTimer_delete(StopConfirm);

    if (mv == STOP) {
        AppTimer_add(StopConfirm, D_SEC(1));
    } else {

    }
}

int TestSAS(prime_t &prime)
{
    static double distanceToStopLine = 0, distanceToEdge = 0;
    if (!testing)
        return 0;

    if (CrashRedLine(map, car)) {
    if (CrashRedLine(prime)) {
        // 车轮压线，不合格
        if (!occurCrashRedLine) {
            AddExamFault(10116, rtkTime);
            AddExamFault(10116);
            DEBUG("车轮压线");
        }
        occurCrashRedLine = true;
@@ -92,158 +111,125 @@
        occurCrashRedLine = false;
    }

    if (ExitTestArea(map, car)) {
        // 驶离测试区
        if (!stopCar) {
            // 不停车直接离开
            AddExamFault(10103, rtkTime);
        }
        testing = false;
    }
    // 检测到停车
    if (prime.pMotion->move == STOP && stopConfirm) {
        if (!check1) {
            check1 = true;
            // 存储停止点
            stopPoint = prime.pModeling->base_point;
            // 开始停车计时
            AppTimer_delete(StoppingTimeout);
            AppTimer_add(StoppingTimeout, examParam.ramp_start_car_limit_time);

    if (prevMoveDirect != moveDirect) {
        if (moveDirect == 0) {
            stopTimepoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
        } else if (stopCar && !handBreakActive) {
            // 检查是否拉住手刹
            handBreakActive = true;
            AddExamFault(20306, rtkTime);
            DEBUG("没拉手刹");
        }
        prevMoveDirect = moveDirect;
    } else if (moveDirect == 0) {
        uint32_t tp = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);

        if (tp - stopTimepoint >= STOP_CAR_TIME && !stopCar) {
            // 这里判断停车状态
            stopCar = true;
            stopCarTime = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);

            stopPoint = car->carXY[car->body[0]];

            distanceToStopLine = DistanceOfHead2Stopline(map, car);
            distanceToEdge = DistanceOfTire2Edge(map, car);
            // 检查车头和停车带的距离
            distanceToStopLine = DistanceOfHead2Stopline(prime);
            distanceToEdge = DistanceOfTire2Edge(prime);

            DEBUG("DIS1 = %f  DIS2 = %f", distanceToStopLine, distanceToEdge);

            if (distanceToStopLine > examParam.ramp_stoppoint_red_distance) {
                // 距离停止线前后超出50厘米
                AddExamFault(20301, rtkTime);
                AddExamFault(20301);
                DEBUG("距离停止线前后超出50厘米，不合格");
            } else if (fabs(distanceToStopLine) > EPSILON) {
                // 前保险没有位于停止带内，但没有超出50厘米，扣10分
                AddExamFault(20304, rtkTime);
                AddExamFault(20304);
                DEBUG("前保险没有位于停止带内，但没有超出50厘米");
            }

            if (distanceToEdge > examParam.ramp_edge_red_distance) {
                // 距离边线超出50厘米,不合格
                AddExamFault(20302, rtkTime);
                AddExamFault(20302);
                DEBUG("距离边线超出50厘米");
            } else if (distanceToEdge > examParam.ramp_edge_yellow_distance) {
                // 距离边线超出30厘米，扣10分
                AddExamFault(20305, rtkTime);
                AddExamFault(20305);
                DEBUG("距离边线超出30厘米");
            }
        }

        if (stopCar && !handBreakActive && ReadCarStatus(HAND_BREAK) == BREAK_ACTIVE) {
        // 停车后，检查手刹拉起情况
        if (!handBreakActive && ReadCarStatus(HAND_BREAK) == BREAK_ACTIVE) {
            handBreakActive = true;
        }
    }

    if (!stopCar) {
        // 距离检测
            vector<double> dtox;
            vector<Line> line_set;
            Line distance_line;
    if (prime.pMotion->move != STOP && stopConfirm) {
        // 车辆从停止状态再次移动
        double juli = DistanceOf(prime.pModeling->base_point, stopPoint);

            MakeLine(&distance_line, &map->point[0], &map->point[8]);
            line_set.push_back(distance_line);
            DistanceOfTire2X(dtox, car, line_set);
            MA_SendDistance(dtox[0], dtox[1]);
    } else {
        MA_SendDistance(distanceToStopLine, distanceToEdge);
    }

    // 判断起步后滑状态
    if (stopCar) {
        if (IntersectionOfLine(map->point[4], stopPoint, car->carXY[car->axial[AXIAL_FRONT]]) == 1) {
            // 发生后滑
            double slideDistance = DistanceOf(stopPoint, car->carXY[car->axial[AXIAL_FRONT]]);

            if (slideDistance > examParam.ramp_slide_yellow_distance) {
        if (juli > examParam.ramp_slide_yellow_distance) {
            double deg = YawOf(stopPoint, prime.pModeling->base_point);
            deg = fabs(prime.pModeling->yaw - deg);
            if (deg > 180) {
                deg = 360 - deg;
            }
            if (deg < 90) {
                // 车辆上坡
                if (!restartComplete) {
                    restartComplete = true;
                    AppTimer_delete(StoppingTimeout);
                    if (slideNormalDistance && !reportSlideFault) {
                        // 后滑超过10厘米，但没超过30厘米
                        DEBUG("后滑超过10厘米，但没超过30厘米");
                        reportSlideFault = true;
                        AddExamFault(10204);
                    }
                    if (!handBreakActive) {
                        // 检查是否拉住手刹
                        DEBUG("没拉手刹");
                        handBreakActive = true;
                        AddExamFault(20306);
                    }
                }
            } else {
                // 车辆后滑
                slideNormalDistance = true;
            }

            if (slideDistance > examParam.ramp_slide_red_distance && !slideLongDistance && !reportSlideFault) {
                // 后滑超过30厘米, 不合格
                AddExamFault(10106, rtkTime);
                DEBUG("后滑超过30厘米");
                slideLongDistance = true;
                reportSlideFault = true;
            }
        }


        if (!reportStartTimeout && (IntersectionOfLine(map->point[4], stopPoint, car->carXY[car->axial[AXIAL_FRONT]]) != -1 ||
                DistanceOf(stopPoint, car->carXY[car->axial[AXIAL_FRONT]]) < 0.1)) {
            if (TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10) - stopCarTime > examParam.ramp_start_car_limit_time) {
                // 起步时间超过30秒，不合格
                AddExamFault(20303, rtkTime);
                DEBUG("起步时间超过30秒");
                reportStartTimeout = true;
            }
        }

        if (IntersectionOfLine(map->point[5], map->point[6], car->carXY[car->axial[AXIAL_REAR]]) == -1) {
            // 车尾驶过停止杆
            if (slideNormalDistance && !slideLongDistance && !reportSlideFault) {
                reportSlideFault = true;
                // 后滑超过10厘米，但没超过30厘米
                AddExamFault(10204, rtkTime);
                DEBUG("后滑超过10厘米，但没超过30厘米");
                if (juli > examParam.ramp_slide_red_distance && !reportSlideFault) {
                    // 后滑超过30厘米, 不合格
                    DEBUG("后滑超过30厘米");
                    reportSlideFault = true;
                    AddExamFault(10106);
                }
            }
        }
    }

    if (!testing) {
        MA_EnterMap(mapIndex, MAP_TYPE_STOP_START, 0);
    if (ExitTestArea(prime)) {
        // 驶离测试区
        if (!stopConfirm) {
            // 不停车直接离开
            AddExamFault(10103);
        }
        MA_EnterMap(prime.curr_exam_map.map_idx, MAP_TYPE_UPHILL, 0);
    }

    return testing ? 1 : 0;
}

// 车轮是否压边线
static bool CrashRedLine(const Polygon *map, const car_model *car)
static bool CrashRedLine(prime_t &prime)
{
    bool ret = false;

    Line red_line;
    int red_lines[2][2];
    int red_line_num = 0;
    int red_lines[][2] = {{0, 1}, {1, 4}, {4, 5}, {5, 8}, {8, 9}};

    if (map->num == 10) {
        red_lines[0][0] = 0;
        red_lines[0][1] = 8;
        red_lines[1][0] = 8;
        red_lines[1][1] = 9;
        red_line_num = 2;
    } else {
        red_lines[0][0] = 0;
        red_lines[0][1] = 8;
        red_line_num = 1;
    }

    Line frontAxle, rearAxle;

    MakeLine(&frontAxle, &car->carXY[car->left_front_tire[TIRE_OUTSIDE]], &car->carXY[car->right_front_tire[TIRE_OUTSIDE]]);
    MakeLine(&rearAxle, &car->carXY[car->left_rear_tire[TIRE_OUTSIDE]], &car->carXY[car->right_rear_tire[TIRE_OUTSIDE]]);
    // 仅看车轮外侧
    MAKE_LINE(frontAxle, prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_front_tire[TIRE_OUTSIDE]],
              prime.pModeling[prime.curr_modeling_index].points[prime.pModel->right_front_tire[TIRE_OUTSIDE]]);
    MAKE_LINE(rearAxle, prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_rear_tire[TIRE_OUTSIDE]],
              prime.pModeling[prime.curr_modeling_index].points[prime.pModel->right_rear_tire[TIRE_OUTSIDE]]);

    for (int i = 0; i < red_line_num; ++i) {
        MakeLine(&red_line, &map->point[red_lines[i][0]], &map->point[red_lines[i][1]]);
    for (int i = 0; i < sizeof(red_lines) / sizeof(red_lines[0]); ++i) {
        Line red_line;
        MAKE_LINE(red_line, prime.pMap->uphill_map[prime.curr_exam_map.map_idx].map[red_lines[i][0]],
                  prime.pMap->uphill_map[prime.curr_exam_map.map_idx].map[red_lines[i][1]]);

        if (IntersectionOf(red_line, frontAxle) == GM_Intersection ||
                IntersectionOf(red_line, rearAxle) == GM_Intersection) {
            IntersectionOf(red_line, rearAxle) == GM_Intersection) {
            ret = true;
            break;
        }
@@ -252,28 +238,21 @@
    return ret;
}

static double DistanceOfHead2Stopline(const Polygon *map, const car_model *car)
static double DistanceOfHead2Stopline(prime_t &prime)
{
    double dis = 0.0;

    int rel1 = IntersectionOfLine(map->point[4], map->point[3], car->carXY[car->body[0]]);
    int rel2 = IntersectionOfLine(map->point[5], map->point[6], car->carXY[car->body[0]]);
    Line upper_edge, lower_edge;

    DEBUG("%d %d %f, %f", car->body[0], car->axial[AXIAL_FRONT], car->carXY[car->body[0]].X, car->carXY[car->body[0]].Y);
    DEBUG("rel1 %d rel2 %d", rel1, rel2);
    MAKE_LINE(upper_edge, prime.pMap->uphill_map[prime.curr_exam_map.map_idx].map[5],
              prime.pMap->uphill_map[prime.curr_exam_map.map_idx].map[6]);
    MAKE_LINE(lower_edge, prime.pMap->uphill_map[prime.curr_exam_map.map_idx].map[4],
              prime.pMap->uphill_map[prime.curr_exam_map.map_idx].map[3]);

    if (rel1 == 1) {
        Line line1;

        MakeLine(&line1, &map->point[4], &map->point[3]);

        dis = DistanceOf(car->carXY[car->body[0]], line1);
    } else if (rel2 == -1) {
        Line line2;

        MakeLine(&line2, &map->point[5], &map->point[6]);

        dis = DistanceOf(car->carXY[car->body[0]], line2);
    if (IntersectionOfLine(prime.pModeling->points[prime.pModel->axial[AXIAL_FRONT]], upper_edge) == REL_POS_RIGHT) {
        dis = DistanceOf(prime.pModeling->points[prime.pModel->axial[AXIAL_FRONT]], upper_edge);
    } else if (IntersectionOfLine(prime.pModeling->points[prime.pModel->axial[AXIAL_FRONT]], lower_edge) == REL_POS_LEFT) {
        dis = DistanceOf(prime.pModeling->points[prime.pModel->axial[AXIAL_FRONT]], lower_edge);
    }

    DEBUG("DistanceOfHead2Stopline dis %f", dis);
@@ -281,28 +260,27 @@
    return dis;
}

static double DistanceOfTire2Edge(const Polygon *map, const car_model *car)
static double DistanceOfTire2Edge(prime_t &prime)
{
    Line edge;

    MakeLine(&edge,  &map->point[0], &map->point[8]);
    MAKE_LINE(edge, prime.pMap->uphill_map[prime.curr_exam_map.map_idx].map[0],
              prime.pMap->uphill_map[prime.curr_exam_map.map_idx].map[8]);

    double l1 = DistanceOf(car->carXY[car->right_front_tire[TIRE_OUTSIDE]], edge);
    double l1 = DistanceOf(prime.pModeling->points[prime.pModel->right_front_tire[TIRE_OUTSIDE]], edge);

    double l2 = DistanceOf(car->carXY[car->right_rear_tire[TIRE_OUTSIDE]], edge);

//    return (l1+l2)/2.0;
    double l2 = DistanceOf(prime.pModeling->points[prime.pModel->right_rear_tire[TIRE_OUTSIDE]], edge);

    return MAX(l1, l2);     // 取最远的
}

// 整个车辆都要驶离该测试区域
static bool ExitTestArea(const Polygon *map, const car_model *car)
static bool ExitTestArea(prime_t &prime)
{
    // 全车都需不在地图中
    bool ret = false;

    Polygon carBody, map2;
    /*Polygon carBody, map2;
    PointF vPoint = Calc3Point(map->point[8], map->point[0], DistanceOf(map->point[8], map->point[7]), 'R');
    PointF vPoint2;

@@ -329,6 +307,6 @@
    }

    free(carBody.point);
    free(map2.point);
    free(map2.point);*/
    return ret;
}

 lib/src/main/cpp/test_items/stop_and_start.h

@@ -9,7 +9,7 @@

using namespace std;

void StartSAS(int index, const Polygon *map, const car_model *car, int moveDirect, const struct RtkTime *rtkTime);
int TestSAS(const Polygon *map, const car_model *car, const car_model *carPrev, double speed, int moveStatus, const struct RtkTime *rtkTime);
void StartSAS(int index, const Polygon *map, const car_model_t *car, int moveDirect, const struct RtkTime *rtkTime);
int TestSAS(const Polygon *map, const car_model_t *car, const car_model_t *carPrev, double speed, int moveStatus, const struct RtkTime *rtkTime);

#endif //RTKDRIVERTEST_STOP_AND_START_H

 lib/src/main/cpp/test_items/turn_a90.cpp

@@ -2,6 +2,17 @@
// Created by YY on 2019/11/4.
//

// 4----------------------|5
//                        |
//                        |
// 3----------|2          |
//            |           |
//            |           |
//            |           |
//            |           |
//            |           |
//            |1          |0

#include "turn_a90.h"
#include "../test_common/Geometry.h"
#include "../driver_test.h"
@@ -21,90 +32,60 @@
using namespace std;

static bool testing;
static int mapIndex;

static int enterAreaHeading;
static bool turnLeftFinished;
static uint32_t stopTimepoint = 0;

static bool reportStopCarTimeout;
static int prevMoveDirect;
static bool crashRedLine;

static bool CrashRedLine(const Polygon *map, const car_model *car);
static bool ExitTestArea(const Polygon *map, const car_model *car);
static bool CrashRedLine(prime_t &prime);
static bool ExitTestArea(prime_t &prime);

void StartTurnA90(int index, int moveDirect, double heading, const struct RtkTime *rtkTime)
void StartTurnA90(prime_t &prime)
{
    DEBUG("进入直角转弯场地");
    testing = true;
    enterAreaHeading = (int) heading;
    prevMoveDirect = moveDirect;
    if (moveDirect == 0) {
        stopTimepoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
    }
    reportStopCarTimeout = false;
    enterAreaHeading = (int) prime.pModeling->yaw;
    crashRedLine = false;
    turnLeftFinished = false;
    mapIndex = index;

    MA_EnterMap(mapIndex, MAP_TYPE_TURN_90, 1);
    MA_EnterMap(prime.curr_exam_map.map_idx, MAP_TYPE_TURN_90, 1);
}

int TestTurnA90(const Polygon *map, const car_model *car, const car_model *carPrev, double heading, double speed, int moveDirect, const struct RtkTime *rtkTime)
static void StoppingTimeout(apptimer_var_t val) {
    DEBUG("中途停车");
    AddExamFault(20703);
}

void MotionChange(move_status_t mv)
{
    int az = (int) heading;
    if (!testing)
        return;

    AppTimer_delete(StoppingTimeout);

    if (mv == STOP) {
        AppTimer_add(StoppingTimeout, D_SEC(2));
    } else {

    }
}

void TestTurnA90(prime_t &prime)
{
    int az = (int) prime.pModeling->yaw;
    vector<double> dtox;
    vector<Line> line_set;
    Line distance_line;

    if (ExitTestArea(map, car)) {
        testing = false;
        goto TEST_END;
    }

    // 距离检测
    MakeLine(&distance_line, &map->point[0], &map->point[5]);
    line_set.push_back(distance_line);
    MakeLine(&distance_line, &map->point[5], &map->point[4]);
    line_set.push_back(distance_line);
    MakeLine(&distance_line, &map->point[1], &map->point[2]);
    line_set.push_back(distance_line);
    MakeLine(&distance_line, &map->point[2], &map->point[3]);
    line_set.push_back(distance_line);
    DistanceOfTire2X(dtox, car, line_set);
    MA_SendDistance(dtox[0], dtox[1]);

    if (CrashRedLine(map, car)) {
    if (CrashRedLine(prime)) {
        if (!crashRedLine) {
            crashRedLine = true;
            // 碾压道路边缘,不合格
            AddExamFault(20701, rtkTime);
            AddExamFault(20701);
            DEBUG("碾压道路边缘");
        }
    } else {
        crashRedLine = false;
    }

    if (moveDirect != prevMoveDirect) {
        if (moveDirect == 0) {
            stopTimepoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
            reportStopCarTimeout = false;

            DEBUG("停车了 %d %d %d %d %d %d %d", rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss);
        } else {

        }
        prevMoveDirect = moveDirect;
    } else if (moveDirect == 0) {
        uint32_t tp = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);

        if (tp - stopTimepoint >= CorrectPauseCriteria(examParam.turn_a90_pause_criteria) && !reportStopCarTimeout) {
            // 停车超2秒，每次扣5分
            AddExamFault(20703, rtkTime);
            DEBUG("中途停车");
            reportStopCarTimeout = true;
        }
    }

    // 检查转向状态
@@ -117,7 +98,7 @@
    if (az >= 30) {
        if (!turnLeftFinished) {
            char turn_direct;
            if((( ((int)heading) + 360 - enterAreaHeading) % 360) < 180) {
            if((( az + 360 - enterAreaHeading) % 360) < 180) {
                DEBUG("右转");
                turn_direct = 'R';
            } else {
@@ -128,39 +109,38 @@
            if ((turn_direct == 'R' && ReadCarStatus(TURN_SIGNAL_LAMP) != RIGHT_TURN_LIGHT) ||
                    (turn_direct == 'L' && ReadCarStatus(TURN_SIGNAL_LAMP) != LEFT_TURN_LIGHT)) {
                DEBUG("转向灯未开启");
                AddExamFault(20702, rtkTime);
                AddExamFault(20702);
            }
        }
        turnLeftFinished = true;
    }

    if (turnLeftFinished) {

    if (ExitTestArea(prime)) {
        testing = false;
        MA_EnterMap(prime.curr_exam_map.map_idx, MAP_TYPE_TURN_90, 0);
    }

TEST_END:
    if (!testing) {
        MA_EnterMap(mapIndex, MAP_TYPE_TURN_90, 0);
    }
    return testing? 1:0;
}

// 车轮是否压边线
static bool CrashRedLine(const Polygon *map, const car_model *car)
static bool CrashRedLine(prime_t &prime)
{
    bool ret = false;

    Line red_line;
    const int red_lines[][2] = {{0, 5}, {5, 4}, {1, 2}, {2, 3}};

    Line frontAxle, rearAxle;

    // 仅看车轮外侧
    MakeLine(&frontAxle, &car->carXY[car->left_front_tire[TIRE_OUTSIDE]], &car->carXY[car->right_front_tire[TIRE_OUTSIDE]]);
    MakeLine(&rearAxle, &car->carXY[car->left_rear_tire[TIRE_OUTSIDE]], &car->carXY[car->right_rear_tire[TIRE_OUTSIDE]]);
    MAKE_LINE(frontAxle, prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_front_tire[TIRE_OUTSIDE]],
              prime.pModeling[prime.curr_modeling_index].points[prime.pModel->right_front_tire[TIRE_OUTSIDE]]);
    MAKE_LINE(rearAxle, prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_rear_tire[TIRE_OUTSIDE]],
              prime.pModeling[prime.curr_modeling_index].points[prime.pModel->right_rear_tire[TIRE_OUTSIDE]]);

    for (int i = 0; i < sizeof(red_lines) / sizeof(red_lines[0]); ++i) {
        MakeLine(&red_line, &map->point[red_lines[i][0]], &map->point[red_lines[i][1]]);
        Line red_line;
        MAKE_LINE(red_line, prime.pMap->turn_a90_map[prime.curr_exam_map.map_idx].map[red_lines[i][0]],
                  prime.pMap->turn_a90_map[prime.curr_exam_map.map_idx].map[red_lines[i][1]]);

        if (IntersectionOf(red_line, frontAxle) == GM_Intersection ||
            IntersectionOf(red_line, rearAxle) == GM_Intersection) {
            ret = true;
@@ -171,25 +151,41 @@
    return ret;
}

// 整个车辆都要驶离该测试区域
static bool ExitTestArea(const Polygon *map, const car_model *car)
// 4个车轮和车头点不在场地中
static bool ExitTestArea(prime_t &prime)
{
    bool ret = false;
    Polygon polygon;

    // 全车都需不在地图中
    Polygon carBody;

    carBody.num = car->bodyNum;
    carBody.point = (PointF *)malloc(carBody.num * sizeof(PointF));
    for (int i = 0; i < carBody.num; ++i) {
        carBody.point[i] = car->carXY[car->body[i]];
    polygon.num = prime.pMap->turn_a90_map[prime.curr_exam_map.map_idx].map.size();
    polygon.point = new PointF[polygon.num];
    for (int i = 0; i < polygon.num; ++i) {
        polygon.point[i] = prime.pMap->turn_a90_map[prime.curr_exam_map.map_idx].map[i];
    }

    if (IntersectionOf(&carBody, map) == GM_None) {
        ret = true;
    int num = 0;

    if (IntersectionOf(prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_front_tire[TIRE_OUTSIDE]],
                       &polygon) == GM_None) {
        num++;
    }
    if (IntersectionOf(prime.pModeling[prime.curr_modeling_index].points[prime.pModel->right_front_tire[TIRE_OUTSIDE]],
                       &polygon) == GM_None) {
        num++;
    }
    if (IntersectionOf(prime.pModeling[prime.curr_modeling_index].points[prime.pModel->left_rear_tire[TIRE_OUTSIDE]],
                       &polygon) == GM_None) {
        num++;
    }
    if (IntersectionOf(prime.pModeling[prime.curr_modeling_index].points[prime.pModel->right_rear_tire[TIRE_OUTSIDE]],
                       &polygon) == GM_None) {
        num++;
    }
    if (IntersectionOf(prime.pModeling[prime.curr_modeling_index].points[prime.pModel->body[prime.pModel->axial[AXIAL_FRONT]]],
                       &polygon) == GM_None) {
        num++;
    }

    free(carBody.point);
    delete []polygon.point;

    return ret;
    return num == 5? true : false;
}

 lib/src/main/cpp/test_items/turn_a90.h

@@ -11,6 +11,6 @@
using namespace std;

void StartTurnA90(int index, int moveDirect, double heading, const struct RtkTime *rtkTime);
int TestTurnA90(const Polygon *map, const car_model *car, const car_model *carPrev, double heading, double speed, int moveDirect, const struct RtkTime *rtkTime);
int TestTurnA90(const Polygon *map, const car_model_t *car, const car_model_t *carPrev, double heading, double speed, int moveDirect, const struct RtkTime *rtkTime);

#endif //RTKDRIVERTEST_TURN_A90_H

 lib/src/main/cpp/test_items2/car_start.cpp

File was deleted

 lib/src/main/cpp/test_items2/car_start.h

File was deleted

 lib/src/main/cpp/test_items2/change_lane.cpp

File was deleted

 lib/src/main/cpp/test_items2/change_lane.h

File was deleted

 lib/src/main/cpp/test_items2/common_check.cpp

File was deleted

 lib/src/main/cpp/test_items2/common_check.h

File was deleted

 lib/src/main/cpp/test_items2/drive_straight.cpp

File was deleted

 lib/src/main/cpp/test_items2/drive_straight.h

File was deleted

 lib/src/main/cpp/test_items2/dummy_light.cpp

File was deleted

 lib/src/main/cpp/test_items2/dummy_light.h

File was deleted

 lib/src/main/cpp/test_items2/operate_gear.cpp

File was deleted

 lib/src/main/cpp/test_items2/operate_gear.h

File was deleted

 lib/src/main/cpp/test_items2/overtake.cpp

File was deleted

 lib/src/main/cpp/test_items2/overtake.h

File was deleted

 lib/src/main/cpp/test_items2/prepare.cpp

File was deleted

 lib/src/main/cpp/test_items2/prepare.h

File was deleted

 lib/src/main/cpp/test_items2/road_exam.cpp

File was deleted

 lib/src/main/cpp/test_items2/road_exam.h

File was deleted

 lib/src/main/cpp/test_items2/smart_item.cpp

File was deleted

 lib/src/main/cpp/test_items2/smart_item.h

File was deleted

 lib/src/main/cpp/test_items2/stop_car.cpp

File was deleted

 lib/src/main/cpp/test_items2/stop_car.h

File was deleted

 lib/src/main/cpp/test_items2/through_something.cpp

File was deleted

 lib/src/main/cpp/test_items2/through_something.h

File was deleted

 lib/src/main/cpp/utils/xconvert.cpp

@@ -22,6 +22,8 @@
#define DAYS_PER_WEEK       7
#define DAYS_PER_YEAR       365

#define DAYS_UP_TO_1970 ((70*365LU) +17)

double ConvertKMh2Ms(int kmh)
{
    return ((double)kmh) * 1000.0 / 3600.0;
@@ -86,9 +88,20 @@
    }
}

static int TimeMonthOffset( int leapStatus, uint8_t mon )
{
    const int monthOffsets[2][12] =
            {
/*   jan  feb  mar  apr  may  jun  jul  aug  sep  oct  nov  dec */
                    {0, 31, 59, 90, 120, 151, 181, 212, 243, 273, 304, 334},    /* regular year */
                    {0, 31, 60, 91, 121, 152, 182, 213, 244, 274, 305, 335}     /* leap year    */
            };
    return monthOffsets[leapStatus][mon] ;
}

const int LibTimeDays[] = { 31, 28, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31 };

static inline bool TimeYearIsLeap(int year)
static bool TimeYearIsLeap(int year)
{
    if ((year % 4) || year == 2100)
        return false;
@@ -113,7 +126,7 @@
    for (i = 0; i<(month - 1); i++)
        daysThisYear += (int)LibTimeDays[i];

    for (i = 2000; i<year; i++) {
    for (i = 1970; i<year; i++) {
        daysThisYear += 365;
        if (TimeYearIsLeap(i))
            daysThisYear += 1;
@@ -127,22 +140,59 @@
    return (hour*SECONDS_PER_HOUR + minute*SECONDS_PER_MINUTE + second) * 1000 + msecond;
}

/*********************************************************
 * TIME1 - TIME2: msecond
 * @param hour1
 * @param minute1
 * @param second1
 * @param msecond1
 * @param hour2
 * @param minute2
 * @param second2
 * @param msecond2
 * @return
 */
uint32_t TimeGetDiff(int hour1, int minute1, int second1, int msecond1, int hour2, int minute2, int second2, int msecond2)
{
    return (TimeMakeComposite(hour1, minute1, second1, msecond1) + SECONDS_PER_DAY * 1000 -
            TimeMakeComposite(hour2, minute2, second2, msecond2)) % (SECONDS_PER_DAY * 1000);
void TimeBreakdown(uint32_t SecondsSince1970 , struct TimeStructure *pTS) {
    uint16_t yearSince1900;
    uint32_t days;
    uint32_t daysTillYearStarted;
    uint32_t secs;
    bool isLeapYear;
    uint16_t leapDaysSince1900;

    secs = SecondsSince1970;
    days = SecondsSince1970 / SECONDS_PER_DAY;

    // 1970.1.1 is Thursday
    pTS->Wday = (days + 4) % DAYS_PER_WEEK;   /* days since Sunday        */

    yearSince1900 = (days / DAYS_PER_YEAR) + 70;

    leapDaysSince1900 = (yearSince1900 - 1) / 4;

    if (yearSince1900 > 200)
        leapDaysSince1900 -= 1;

    daysTillYearStarted = leapDaysSince1900 + (365L * yearSince1900);

    if (days + DAYS_UP_TO_1970 < daysTillYearStarted) {
        yearSince1900--;

        leapDaysSince1900 = (yearSince1900 - 1) / 4;

        if (yearSince1900 > 200)
            leapDaysSince1900 -= 1;
        daysTillYearStarted = leapDaysSince1900 + (365L * yearSince1900);
    }
    days = days - (daysTillYearStarted - DAYS_UP_TO_1970);

    pTS->Year = yearSince1900 + 1900;
    pTS->Yday = days;

    isLeapYear = TimeYearIsLeap(pTS->Year);

    pTS->Month = 12;
    do {
        pTS->Month--;
    } while (days < TimeMonthOffset(isLeapYear? 1 : 0, pTS->Month));

    pTS->Day = days - TimeMonthOffset(isLeapYear? 1: 0, pTS->Month) + 1;

    pTS->Month++;

    secs %= SECONDS_PER_DAY;
    pTS->Hour = secs / SECONDS_PER_HOUR;
    secs %= SECONDS_PER_HOUR;
    pTS->Minute = secs / 60;
    pTS->Second = secs % 60;
}

vector<string> split(string str, string pattern)

 lib/src/main/cpp/utils/xconvert.h

@@ -9,6 +9,18 @@
#include <string>
#include <vector>

struct TimeStructure
{
    uint8_t  Second;     /* seconds after the minute [0, 59]     */
    uint8_t  Minute;     /* minutes after the hour   [0, 59 ]    */
    uint8_t  Hour;       /* hours since midnight     [0, 23]     */
    uint8_t  Day;        /* day of the month         [1, 31]     */
    uint8_t  Month;      /* month of the year        [1, 12]     */
    uint16_t Year;       /* years since Christ born  [0, lots]   */
    uint8_t  Wday;       /* days since Sunday        [0, 6]      */
    uint16_t Yday;       /* days since January 1     [0, 365]    */
};

double ConvertKMh2Ms(int kmh);
double ConvertMs2KMh(double ms);

@@ -16,6 +28,7 @@
void ConvertHex2String(char *str, const uint8_t *hex, int length);
void ConvertString2Hex(uint8_t *hex, int length, const char *str);
uint32_t TimeMakeComposite(int year, int month, int day, int hour, int minute, int second);
void TimeBreakdown(uint32_t SecondsSince1970 , struct TimeStructure *pTS);
uint32_t TimeMakeComposite(int hour, int minute, int second, int msecond);
uint32_t TimeGetDiff(int hour1, int minute1, int second1, int msecond1, int hour2, int minute2, int second2, int msecond2);