s

yy1717

2020-01-14 cc4be69e76709579c80efcc24801004461011c46

s

 lib/src/main/cpp/driver_test.cpp

@@ -25,6 +25,7 @@
#include "test_items/driving_curve.h"
#include "test_items/stop_and_start.h"
#include "master/comm_if.h"
#include "utils/xconvert.h"

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

@@ -95,40 +96,22 @@
    int right_rear_tire[2];
    int pointNum;
    struct car_desc_ *carDesc;
    PointF *carXY;
} car_model;

static car_model *CarModel = NULL;

struct car_coord_ *CarCoord = NULL;
static double currSpeed = 0, currAzimuth = 0;
static PointF currCoord;
static int prevDirect = 0, currDirect = 0;

static int car_coord_num = 0, car_coord_in = 0;

#define MOV_AVG_SIZE                1
#define RTK_HISTORY_SIZE            1000
#define RTK_BUFFER_SIZE            100
#define CAR_MODEL_CACHE_SIZE      10

struct rtk_info_ {
    int qf;
    time_t uptime;
    double azimuth;
    PointF coord;
};
static rtk_info *RtkBuffer = NULL;
static int RtkBufferNum = 0, RtkBufferIn = 0;

static struct rtk_info_ *RTKHistory = NULL;
static int rtk_history_num = 0, rtk_history_in = 0;
static pthread_mutex_t add_rtk_history_mutex = PTHREAD_MUTEX_INITIALIZER;

static carModelDesc_t *carModelDescFile = NULL;

static car_model_cache_t carModelCache[CAR_MODEL_CACHE_SIZE];
static int carModelCacheIn, carModelCacheNum;

static uint32_t CalcTimeDiff(const rtk_info *a, const rtk_info *b);
static void ReadDriverExamPrimerTimeout(union sigval sig);
static void UpdateCarBodyCoord(double azimuth, PointF coord);
static void UpdateCarBodyCoord(double azimuth, PointF coord, car_model_cache_t *carModel, int &carModelIn, int &carModelNum);
static void UpdateCarBodyCoord(double azimuth, PointF main_ant, const car_model *carModel);
static bool UpdateCarCoord(double &spd, int &mov, int &idx);
static bool FrontTireEnterArea(const Polygon *car, const Polygon *map);

void DriverTestInit(void)
@@ -137,142 +120,8 @@
    MapNum = 0;
    CarModel = NULL;

    carModelDescFile = (carModelDesc_t *)malloc(sizeof(carModelDesc_t));

    carModelDescFile->body_num = 6;
    carModelDescFile->body[0] = 0;
    carModelDescFile->body[1] = 1;
    carModelDescFile->body[2] = 2;
    carModelDescFile->body[3] = 3;
    carModelDescFile->body[4] = 4;
    carModelDescFile->body[5] = 5;

    carModelDescFile->front_left_tire[0] = 1;
    carModelDescFile->front_left_tire[1] = 1;
    carModelDescFile->front_right_tire[0] = 5;
    carModelDescFile->front_right_tire[1] = 5;
    carModelDescFile->rear_left_tire[0] = 2;
    carModelDescFile->rear_left_tire[1] = 2;
    carModelDescFile->rear_right_tire[0] = 4;
    carModelDescFile->rear_right_tire[1] = 4;

    memset(&carModelCache, 0, sizeof(carModelCache));
    carModelCacheIn = carModelCacheNum = 0;

    CarDesc = (struct car_desc_ *) malloc(sizeof(struct car_desc_) * 6);
    CarDesc[0].distance = 0.2465;
    CarDesc[0].angle = 0;

    CarDesc[1].distance = 0.2635;
    CarDesc[1].angle = 20.7;

    CarDesc[2].distance = 0.14;
    CarDesc[2].angle = 138.9;

    CarDesc[3].distance = 0.1055;
    CarDesc[3].angle = 180.0;

    CarDesc[4].distance = 0.14;
    CarDesc[4].angle = 221.1;

    CarDesc[5].distance = 0.2635;
    CarDesc[5].angle = 339.3;

    memset(&theParkEdgeMap, 0, sizeof(theParkEdgeMap));
    theParkEdgeMap.num = 8;
    theParkEdgeMap.point = (PointF *)malloc(theParkEdgeMap.num * sizeof(PointF));

    theParkEdgeMap.point[0].Y = 28.013;
    theParkEdgeMap.point[0].X = -11.9669;

    theParkEdgeMap.point[1].Y = 27.137;
    theParkEdgeMap.point[1].X = -11.5114;

    theParkEdgeMap.point[2].Y = 27.5039;
    theParkEdgeMap.point[2].X = -10.8069;

    theParkEdgeMap.point[3].Y = 26.4212;
    theParkEdgeMap.point[3].X = -10.2969;

    theParkEdgeMap.point[4].Y = 26.8894;
    theParkEdgeMap.point[4].X = -9.2102;

    theParkEdgeMap.point[5].Y = 28.0027;
    theParkEdgeMap.point[5].X = -9.6513;

    theParkEdgeMap.point[6].Y = 28.3797;
    theParkEdgeMap.point[6].X = -8.9758;

    theParkEdgeMap.point[7].Y = 29.3232;
    theParkEdgeMap.point[7].X = -9.5057;

    memset(&theTurn90Map, 0, sizeof(theTurn90Map));
    theTurn90Map.num = 6;
    theTurn90Map.point = (PointF *)malloc(theTurn90Map.num * sizeof(PointF));

    theTurn90Map.point[0].Y = 10;
    theTurn90Map.point[0].X = 10;

    theTurn90Map.point[1].Y = 12.5;
    theTurn90Map.point[1].X = 10;

    theTurn90Map.point[2].Y = 12.5;
    theTurn90Map.point[2].X = 12.5;

    theTurn90Map.point[3].Y = 15;
    theTurn90Map.point[3].X = 12.5;

    theTurn90Map.point[4].Y = 15;
    theTurn90Map.point[4].X = 15;

    theTurn90Map.point[5].Y = 10;
    theTurn90Map.point[5].X = 15;

    memset(&theSSMap, 0, sizeof(theSSMap));
    theSSMap.num = 9;
    theSSMap.point = (PointF *)malloc(theSSMap.num * sizeof(PointF));

    theSSMap.point[0].Y = 10;
    theSSMap.point[0].X = 10;

    theSSMap.point[8].Y = 10;
    theSSMap.point[8].X = 15;

    theSSMap.point[1].Y = 10;
    theSSMap.point[1].X = 11.5;

    theSSMap.point[4].Y = 10;
    theSSMap.point[4].X = 12.5;

    theSSMap.point[5].Y = 10;
    theSSMap.point[5].X = 13.5;

    theSSMap.point[2].Y = 12;
    theSSMap.point[2].X = 11.5;

    theSSMap.point[3].Y = 12;
    theSSMap.point[3].X = 12.5;

    theSSMap.point[6].Y = 12;
    theSSMap.point[6].X = 13.5;

    theSSMap.point[7].Y = 12;
    theSSMap.point[7].X = 15;

    pthread_mutex_init(&add_rtk_history_mutex, NULL);

    RTKHistory = (struct rtk_info_ *) malloc(RTK_HISTORY_SIZE * sizeof(struct rtk_info_));
    rtk_history_num = rtk_history_in = 0;

    CarCoord = (struct car_coord_ *) malloc(CAR_COORD_STORE_SIZE * sizeof(struct car_coord_));
    car_coord_num = car_coord_in = 0;

    TestStart = true;
    TestItem = TEST_NONE;

    ErrorList.clear();

    TextSpeak("开始测试");
    RtkBuffer = (rtk_info *) malloc(RTK_BUFFER_SIZE * sizeof(rtk_info));
    RtkBufferNum = RtkBufferIn = 0;
}

static void ReadDriverExamPrimerTimeout(union sigval sig)
@@ -332,6 +181,8 @@
    }

    MapNum++;

    DEBUG("AddMap num %d", MapNum);
}

void SetCarMeasurePoint(double *basePoint, int *axial, int *left_front_tire,
@@ -343,13 +194,15 @@
    if (point == NULL || pointNum == 0) return;

    if (CarModel != NULL) {
        CarModel->carDesc != NULL;
        free(CarModel->carDesc);
        if (CarModel->carDesc != NULL)
            free(CarModel->carDesc);
        if (CarModel->carXY != NULL)
            free(CarModel->carXY);
        free(CarModel);
        CarModel = NULL;
    }

    CarModel = (car_model *)malloc(sizeof(CarModel));
    CarModel = (car_model *)malloc(sizeof(car_model));
    CarModel->basePoint.X = basePoint[0];
    CarModel->basePoint.Y = basePoint[1];
    CarModel->axial[0] = axial[0];
@@ -365,6 +218,7 @@
    CarModel->pointNum = pointNum;

    CarModel->carDesc = (struct car_desc_ *)malloc(sizeof(struct car_desc_) * pointNum);
    CarModel->carXY = (PointF *) malloc(sizeof(PointF) * pointNum);

    // 测量坐标转换为距离-角度形式
    double C02 = (point[0][0]-basePoint[0])*(point[0][0]-basePoint[0]) +
@@ -386,6 +240,8 @@

        if (i > axial[1])
            CarModel->carDesc[i].angle = 360.0 - CarModel->carDesc[i].angle;

        DEBUG("加入点<%d> 距离 %f 角度 %f", i, CarModel->carDesc[i].distance, CarModel->carDesc[i].angle);
    }

    DEBUG("SetCarMeasurePoint Calc Over");
@@ -412,224 +268,161 @@
        MA_SendExamStatus(1, 0);
}

void UpdateRTKInfo(rtk_info *s)
void UpdateRTKInfo(const rtk_info *s)
{
    struct tm test_tm;

    struct timeval tv;
    struct timezone tz;

    gettimeofday(&tv,&tz);

    memset(&test_tm, 0, sizeof(test_tm));

    test_tm.tm_year = 2000 + s->YY - 1900;
    test_tm.tm_mon = s->MM - 1;
    test_tm.tm_mday = s->DD;
    test_tm.tm_hour = s->hh;
    test_tm.tm_min = s->mm;
    test_tm.tm_sec = s->ss;

    pthread_mutex_lock(&add_rtk_history_mutex);
    /*if (s->qf == 3)*/ {
        RTKHistory[rtk_history_in].qf = RTK_FIX;//s->qf;
        RTKHistory[rtk_history_in].uptime = mktime(&test_tm) - tz.tz_minuteswest*60;
        RTKHistory[rtk_history_in].azimuth = s->heading;
        RTKHistory[rtk_history_in].coord.X = s->x;
        RTKHistory[rtk_history_in].coord.Y = s->y;

        rtk_history_in = (rtk_history_in + 1) % RTK_HISTORY_SIZE;
        if (rtk_history_num < RTK_HISTORY_SIZE)
            rtk_history_num++;
    if (s->qf > 0) {
        RtkBuffer[RtkBufferIn] = *s;
        RtkBufferIn = (RtkBufferIn + 1) % RTK_BUFFER_SIZE;
        if (RtkBufferNum < RTK_BUFFER_SIZE)
            RtkBufferNum++;
    }
    pthread_mutex_unlock(&add_rtk_history_mutex);

//    DEBUG("UpdateRTKInfo qf = %d tm %ld", s->qf, mktime(&test_tm) - tz.tz_minuteswest*60);
    double speed;
    int move;
    int index;

    if (UpdateCarCoord(speed, move, index)) {
        struct carBrief brief;

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

        brief.qf = RtkBuffer[index].qf;
        brief.map_id = -1;
        brief.move = move;
        brief.speed = speed;
        brief.heading = RtkBuffer[index].heading;
        brief.main_ant[0] = RtkBuffer[index].x;
        brief.main_ant[1] = RtkBuffer[index].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.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++] = CarModel->carXY[i].X;
            brief.point[j++] = CarModel->carXY[i].Y;
        }

        MA_SendCarPosition(&brief);
    }
}

void UpdateCarCoord(void)
/*************************************************
 * 2次采样相差的时间
 * @param a
 * @param b
 * @return ms
 */
static uint32_t CalcTimeDiff(const rtk_info *a, const rtk_info *b)
{
    double azimuth;
    PointF coord;
    return ABS(TimeMakeComposite(2000 + a->YY, a->MM, a->DD, a->hh, a->mm, a->ss) * 1000 + a->dss*10 -
            (TimeMakeComposite(2000 + b->YY, b->MM, b->DD, b->hh, b->mm, b->ss) * 1000 + b->dss*10));
}

    // 计算最近的几个，滑动平均
    int cnt = 0, valid_cnt = 0;
    int s, sm;
    time_t prev_time = 0;
    time_t last_rtk_time = 0;
static bool UpdateCarCoord(double &spd, int &mov, int &idx)
{
    if (CarModel == NULL)
        return false;

    pthread_mutex_lock(&add_rtk_history_mutex);
    s = rtk_history_in;
    sm = rtk_history_num;
    pthread_mutex_unlock(&add_rtk_history_mutex);
    if (RtkBufferNum < 3)
        return false;

    DEBUG("UpdateCarCoord rtk_history_in %d rtk_history_num %d car_coord_num %d", s, sm, car_coord_num);
    int p1 = ((RtkBufferIn-1)+RTK_BUFFER_SIZE)%RTK_BUFFER_SIZE;
    int p2 = ((RtkBufferIn-2)+RTK_BUFFER_SIZE)%RTK_BUFFER_SIZE;
    int p3 = ((RtkBufferIn-3)+RTK_BUFFER_SIZE)%RTK_BUFFER_SIZE;

    // 如果出现QF不是固定解、GPS报文丢失的情况，就按上次的行驶状态做直线行驶
    for (; cnt < sm && valid_cnt < MOV_AVG_SIZE; cnt++) {
        if (s == 0) {
            s = RTK_HISTORY_SIZE - 1;
        } else {
            s--;
        }
    // 如果一定的时间都没有有效定位，删除之前的值
    uint32_t tmDiff = CalcTimeDiff(&RtkBuffer[p1], &RtkBuffer[p2]);

        if (cnt == 0) {
            last_rtk_time = RTKHistory[s].uptime;
        }

        if (RTKHistory[s].uptime - last_rtk_time <= 10) {
            if (RTKHistory[s].qf == RTK_FIX) {
                if (valid_cnt != 0) {
                    azimuth += RTKHistory[s].azimuth;
                    coord.X += RTKHistory[s].coord.X;
                    coord.Y += RTKHistory[s].coord.Y;
                } else {
                    azimuth = RTKHistory[s].azimuth;
                    coord.X = RTKHistory[s].coord.X;
                    coord.Y = RTKHistory[s].coord.Y;
                }
                valid_cnt++;
            }
        } else {
            break;
        }
        prev_time = RTKHistory[s].uptime;
    if (tmDiff > D_SEC(5)) {
        if (p1 != 0)
            RtkBuffer[0] = RtkBuffer[p1];
        RtkBufferIn = RtkBufferNum = 1;
        return false;
    }

    if (valid_cnt >= MOV_AVG_SIZE) {
        azimuth /= MOV_AVG_SIZE;
        coord.X /= MOV_AVG_SIZE;
        coord.Y /= MOV_AVG_SIZE;
    // 计算车辆轮廓点
    PointF main_ant_coord;
    main_ant_coord.X = RtkBuffer[p1].x;
    main_ant_coord.Y = RtkBuffer[p1].y;

        CarCoord[car_coord_in].uptime = AppTimer_GetTickCount();
    UpdateCarBodyCoord(RtkBuffer[p1].heading, main_ant_coord, CarModel);

        CarCoord[car_coord_in].azimuth = azimuth;
        CarCoord[car_coord_in].coord = coord;
        car_coord_in = (car_coord_in + 1) % CAR_COORD_STORE_SIZE;
        if (car_coord_num < CAR_COORD_STORE_SIZE)
            car_coord_num++;
    } else if (car_coord_num >= 3) {
        // 按上次运行轨迹推算一步
        int p1 = ((car_coord_in-1)+CAR_COORD_STORE_SIZE)%CAR_COORD_STORE_SIZE;
        uint32_t tm = AppTimer_GetTickCount() -  CarCoord[p1].uptime;
        double distance = currSpeed * tm / 1000;
    // 计算速度(米/秒)、前进后退
    double speed = sqrt(pow(RtkBuffer[p1].x - RtkBuffer[p2].x, 2) + pow(RtkBuffer[p1].y - RtkBuffer[p2].y, 2)) * 1000 /
                (double)(tmDiff);
    int move = 0;
    double deg = 0.0;

        PointF xy;

        xy.X = CarCoord[p1].coord.X + distance * sin(toRadians(CarCoord[p1].azimuth)) * currDirect;
        xy.Y = CarCoord[p1].coord.Y + distance * sin(toRadians(CarCoord[p1].azimuth)) * currDirect;

        CarCoord[car_coord_in].uptime = AppTimer_GetTickCount();
        CarCoord[car_coord_in].azimuth =  CarCoord[p1].azimuth;
        CarCoord[car_coord_in].coord = xy;
        car_coord_in = (car_coord_in + 1) % CAR_COORD_STORE_SIZE;
        if (car_coord_num < CAR_COORD_STORE_SIZE)
            car_coord_num++;
    if (speed < 0.05) {
        // 停车
        move = 0;
    } else {
        return;
    }

    // 计算车辆轮廓点、速度、前进后退
    if (car_coord_num >= 3) {
        int p1 = ((car_coord_in-1)+CAR_COORD_STORE_SIZE)%CAR_COORD_STORE_SIZE;
        int p2 = ((car_coord_in-2)+CAR_COORD_STORE_SIZE)%CAR_COORD_STORE_SIZE;
        int p3 = ((car_coord_in-3)+CAR_COORD_STORE_SIZE)%CAR_COORD_STORE_SIZE;

        double speed1 = sqrt(pow(CarCoord[p1].coord.X - CarCoord[p2].coord.X, 2) + pow(CarCoord[p1].coord.Y - CarCoord[p2].coord.Y, 2)) * 1000 /
                (double)(CarCoord[p1].uptime - CarCoord[p2].uptime);
        double speed2 = sqrt(pow(CarCoord[p2].coord.X - CarCoord[p3].coord.X, 2) + pow(CarCoord[p2].coord.Y - CarCoord[p3].coord.Y, 2)) * 1000 /
                        (double)(CarCoord[p2].uptime - CarCoord[p3].uptime);

        currSpeed = speed1;
        currAzimuth = CarCoord[p1].azimuth;
        currCoord = CarCoord[p1].coord;

        DEBUG("方向 spd1 %f spd2 %f", speed1, speed2);

        if (speed1 < 0.05 && speed2 < 0.05) {
            // 停车
            currDirect = 0;
//            TextSpeak("停");
        } else if (speed1 < 0.05) {
             currDirect = prevDirect;
        // 判断前进还是后退
        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 {
            // 判断前进还是后退
            double deg = 0.0;
            deg = atan(fabs(RtkBuffer[p1].x - RtkBuffer[p2].x) /
                       fabs(RtkBuffer[p1].y - RtkBuffer[p2].y));

            if (fabs(CarCoord[p1].coord.Y-CarCoord[p2].coord.Y) <= GLB_EPSILON) {
                if (CarCoord[p1].coord.X > CarCoord[p2].coord.X) {
                    deg = 90;
                } else {
                    deg = 270;
                }
            deg = toDegree(deg);

                DEBUG("方向 deg %f p1x %f p2x %f", deg, CarCoord[p1].coord.X, CarCoord[p2].coord.X);
            } else if (fabs(CarCoord[p1].coord.X - CarCoord[p2].coord.X) <= GLB_EPSILON) {
                if (CarCoord[p1].coord.Y > CarCoord[p2].coord.Y) {
                    deg = 0;
                } else {
                    deg = 180;
                }
            } else {
                deg = atan(fabs(CarCoord[p1].coord.X - CarCoord[p2].coord.X) /
                           fabs(CarCoord[p1].coord.Y - CarCoord[p2].coord.Y));
            if (RtkBuffer[p1].x > RtkBuffer[p2].x &&
                RtkBuffer[p1].y > RtkBuffer[p2].y) {

                deg = toDegree(deg);

                if (CarCoord[p1].coord.X > CarCoord[p2].coord.X &&
                    CarCoord[p1].coord.Y > CarCoord[p2].coord.Y) {

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

                DEBUG("方向 deg %f p1x %f p2x %f p1y %f p2y %f", deg, CarCoord[p1].coord.X,
                      CarCoord[p2].coord.X, CarCoord[p1].coord.Y, CarCoord[p2].coord.Y);
            }

            deg = fabs(CarCoord[p1].azimuth - deg);
            if (deg > 180) {
            } 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;
            }
            if (deg < 90) {
                // 前进
                currDirect = 1;
//                    TextSpeak("进");
            } else {
                // 后退
                currDirect = -1;
//                    TextSpeak("退");
            }

            prevDirect = currDirect;
        }

        DEBUG("speed = %f, azimuth = %f coord.X = %f coord.Y = %f dir = %d", speed1, CarCoord[p1].azimuth,
              CarCoord[p1].coord.X, CarCoord[p1].coord.Y, currDirect);
        deg = fabs(RtkBuffer[p1].heading - deg);
        if (deg > 180) {
            deg = 360 - deg;
        }
        if (deg < 90) {
            // 前进
            move = 1;
        } else {
            // 后退
            move = -1;
        }
    }

//        UpdateCarBodyCoord(CarCoord[p1].azimuth, CarCoord[p1].coord);
        UpdateCarBodyCoord(CarCoord[p1].azimuth, CarCoord[p1].coord, carModelCache, carModelCacheIn, carModelCacheNum);
//        for (int i = 0; i < theCarModel.num; ++i) {
//            DEBUG("n = %d X = %f Y = %f", i, theCarModel.point[i].X, theCarModel.point[i].Y);
//        }
    spd = speed;
    mov = move;
    idx = p1;
    DEBUG("speed = %f m/Sec move = %d", speed, move);


        int c1 = ((carModelCacheIn-1)+CAR_MODEL_CACHE_SIZE)%CAR_MODEL_CACHE_SIZE;
        Polygon py;

        py.num = carModelCache[c1].point_num;
        py.point = carModelCache[c1].points;

        DrawScreen(&theSSMap, &py);

        if (!TestStart) return;
    return true;
/*        if (!TestStart) return;

        if (CarInArea == 0) {
//            if (FrontTireEnterArea(&py, &theParkEdgeMap)) {
@@ -709,16 +502,11 @@
            sprintf(buff, "%s, 总计扣分 %d", list.text_desc, scr);

            TextOsd(1, buff);
        }
    }
        }*/
}

car_model_cache_t *GetCarModelCache(int node)
{
    if (node < carModelCacheNum) {
        node = ((carModelCacheIn-node-1)+CAR_MODEL_CACHE_SIZE)%CAR_MODEL_CACHE_SIZE;
        return &carModelCache[node];
    }
    return NULL;
}

@@ -727,51 +515,17 @@
 * @param azimuth
 * @param coord
 */
static void UpdateCarBodyCoord(double azimuth, PointF coord, car_model_cache_t *carModel, int &in, int &num)
static void UpdateCarBodyCoord(double azimuth, PointF main_ant, const car_model *carModel)
{
    carModel[in].uptime = AppTimer_GetTickCount();
    carModel[in].desc = carModelDescFile;
    carModel[in].point_num = 6;
    for (int i = 0; i < carModel->pointNum; ++i) {
        double tx = main_ant.X + carModel->carDesc[i].distance*sin(toRadians(azimuth));
        double ty = main_ant.Y + carModel->carDesc[i].distance*cos(toRadians(azimuth));

    if (carModel[in].points == NULL) {
        carModel[in].points = (PointF *) malloc(sizeof(PointF) * carModel[in].point_num);
        carModel->carXY[i].X = (tx-main_ant.X)*cos(toRadians(carModel->carDesc[i].angle)) -
                                   (ty-main_ant.Y)*sin(toRadians(carModel->carDesc[i].angle)) + main_ant.X;
        carModel->carXY[i].Y = (tx-main_ant.X)*sin(toRadians(carModel->carDesc[i].angle)) +
                                   (ty-main_ant.Y)*cos(toRadians(carModel->carDesc[i].angle)) + main_ant.Y;
    }

    for (int i = 0; i < 6; ++i) {
        double tx = coord.X + CarDesc[i].distance*sin(toRadians(azimuth));
        double ty = coord.Y + CarDesc[i].distance*cos(toRadians(azimuth));

        carModel[in].points[i].X = (tx-coord.X)*cos(toRadians(CarDesc[i].angle)) -
                                 (ty-coord.Y)*sin(toRadians(CarDesc[i].angle)) + coord.X;
        carModel[in].points[i].Y = (tx-coord.X)*sin(toRadians(CarDesc[i].angle)) +
                                 (ty-coord.Y)*cos(toRadians(CarDesc[i].angle)) + coord.Y;
    }

    in = (in + 1) % CAR_MODEL_CACHE_SIZE;
    if (num < CAR_MODEL_CACHE_SIZE) {
        num++;
    }
}

static void UpdateCarBodyCoord(double azimuth, PointF coord)
{
//    static double az = 0;
//
//    az += 2.0;
//
//    for (int i = 0; i < theCarModel.num; ++i) {
//        double tx = coord.X + CarDesc[i].distance*sin(toRadians(azimuth));
//        double ty = coord.Y + CarDesc[i].distance*cos(toRadians(azimuth));
//
//        theCarModel.point[i].X = (tx-coord.X)*cos(toRadians(CarDesc[i].angle)) -
//                                 (ty-coord.Y)*sin(toRadians(CarDesc[i].angle)) + coord.X;
//        theCarModel.point[i].Y = (tx-coord.X)*sin(toRadians(CarDesc[i].angle)) +
//                                 (ty-coord.Y)*cos(toRadians(CarDesc[i].angle)) + coord.Y;
//    }
//    UpdateParkTest(&theCarModel, &theTireModel, speed);
//    UpdatePark2Test(&theCarModel, &theTireModel, speed);
//    UpdateTuneAngle90Test(&theCarModel, 0);
    //UpdateSlopeTest(&theCarModel, speed, direct);
}

static bool FrontTireEnterArea(const Polygon *car, const Polygon *map)

 lib/src/main/cpp/driver_test.h

@@ -41,8 +41,7 @@
void SetSensorCfg(int (*sensor)[2], int sensorNum);
void StartDriverExam(int start);

void UpdateRTKInfo(rtk_info *s);
void UpdateCarCoord(void);
void UpdateRTKInfo(const rtk_info *s);
car_model_cache_t *GetCarModelCache(int node);

#endif //RTKDRIVERTEST_DRIVER_TEST_H

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

@@ -21,6 +21,7 @@
using namespace std;

#define ID_SM_NDK_START         0x0001
#define ID_MS_NDK_ACK           0x8001
#define ID_SM_REQ_RTK_PLAT_CFG         0x0002
#define ID_MS_RTK_PLAT_CFG         0x8002
#define ID_SM_RTK_PLAT_CONN     0x0003
@@ -41,7 +42,7 @@
#define ID_SM_ENTER_MAP         0x000D
#define ID_SM_CAR               0x000E

static int OnOff = 0;
static int OnOff = 0xFFFF;

void MA_NdkStart(void)
{
@@ -374,7 +375,9 @@
                                    if (map_index == 0) {
                                        int i = 0, j = 0;
                                        pointNum = s2.Size()/2;
                                        map = new double[pointNum][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();
@@ -386,7 +389,9 @@
                                    } else if (map_index == 1) {
                                        int i = 0, j = 0;
                                        point2Num = s2.Size()/2;
                                        map2 = new double[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();
@@ -484,7 +489,8 @@
                    if (s.IsArray()) {
                        int i = 0, j = 0;
                        pointNum = s.Size()/2;
                        point = new double[pointNum][2];
                        point = (double (*)[2]) new double[pointNum][2];
//                        point = (double (*)[2])malloc(pointNum * 2 * sizeof(double));

                        for(Value::ConstValueIterator itr = s.Begin(); itr != s.End(); ++itr) {
                            point[i][j] = itr->GetDouble();
@@ -512,7 +518,7 @@
                if (a.IsArray() && a.Size() > 0) {
                    int n = a.Size();
                    int i = 0;
                    int (*sensor)[2] = new int[n][2];
                    int (*sensor)[2] = (int (*)[2]) new int[n][2];

                    for (Value::ConstValueIterator itr = a.Begin(); itr != a.End(); ++itr) {
                        // a gpio mapping

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

@@ -308,6 +308,7 @@
        sprintf(brief.utc, "%04d%02d%02d%02d%02d%02d.%02d", 2000 + rtk->YY, rtk->MM, rtk->DD, rtk->hh, rtk->mm, rtk->ss, rtk->dss);

        MA_SendRtkBrief(&brief);
        UpdateRTKInfo(rtk);
    }
}


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

@@ -79,7 +79,6 @@

    DEBUG("TestParkBottom speed %f dir %d", speed, run_status);


    if (currTarget > FIRST_TOUCH_CTRL_LINE) {
        // 是否超时
        if (parkTimeout) {

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

@@ -6,6 +6,18 @@
#include <cstdint>
#include <cstring>

#define SECONDS_PER_MINUTE  60
#define SECONDS_PER_HOUR    3600
#define SECONDS_PER_DAY     (86400L)

#define MINUTES_PER_HOUR    60
#define MINUTES_PER_DAY     1440

#define HOURS_PER_DAY       24

#define DAYS_PER_WEEK       7
#define DAYS_PER_YEAR       365

void ConvertPhoneNum(uint8_t *dst, int length, const char *src)
{
    int p = length - 1;
@@ -59,3 +71,39 @@
        }
    }
}

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

static inline bool TimeYearIsLeap(int year)
{
    if ((year % 4) || year == 2100)
        return false;
    else
        return true;
}

uint32_t TimeMakeComposite(int year, int month, int day, int hour, int minute, int second)
{
    int i;
    uint32_t totalSeconds;
    int daysThisYear;

    totalSeconds = second;
    totalSeconds += minute*SECONDS_PER_MINUTE;
    totalSeconds += hour*SECONDS_PER_HOUR;

    daysThisYear = day - 1;
    if (TimeYearIsLeap(year) && month>2)
        daysThisYear += 1;

    for (i = 0; i<(month - 1); i++)
        daysThisYear += (int)LibTimeDays[i];

    for (i = 2000; i<year; i++) {
        daysThisYear += 365;
        if (TimeYearIsLeap(i))
            daysThisYear += 1;
    }
    totalSeconds += daysThisYear * SECONDS_PER_DAY;
    return totalSeconds;
}

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

@@ -10,5 +10,6 @@
void ConvertPhoneNum(uint8_t *dst, int length, const char *src);
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);

#endif //MYAPPLICATION2_XCONVERT_H

 lib/src/main/java/com/anyun/exam/lib/RemoteService.java

@@ -52,8 +52,8 @@

        @Override
        public void SendCmd(int cmd, String value) throws RemoteException {
            Log.d(TAG, "Process " + Process.myPid() + " Thread " + Thread.currentThread().getId() + " RecvMsgFromMainProc cmd = " + String.format(" %04X ", cmd) + " value " + value);
            MainProcMsgEntry(cmd, value);
            Log.d(TAG, "Process " + Process.myPid() + " Thread " + Thread.currentThread().getId() + " RecvMsgFromMainProc cmd = " + String.format(" %04X ", cmd) + " length " + value.length() + " value " + value.trim());
            MainProcMsgEntry(cmd, value.trim());
        }
    };
    @Nullable
@@ -105,10 +105,10 @@
    }

    public void SendMsgToMainProc(int cmd, String value) {
        Log.d(TAG, "SendMsgToMainProc cmd = " + String.format("%04X", cmd) + " json = " + value);
        if (!mIsServiceDestroyed.get()){
            onMessageArrived(cmd, value);
        }
        Log.d(TAG, "SendMsgToMainProc cmd = " + String.format("%04X", cmd) + " json = " + value);
    }

    public String javaDESEncrypt(String plaintext, String key) {