update

yy1717

2020-01-15 eced3d013f06b623a49cb9deaba42218c4e37bb8

 lib/src/main/cpp/driver_test.cpp

@@ -57,10 +57,8 @@
int errs = 0;

vector<int> ErrorList;

static Polygon theParkEdgeMap;         // 侧位停车地图
static Polygon theTurn90Map;
static Polygon theSSMap;
vector<ExamFault> ExamFaultList;
static int examFaultIndex = 0;

static struct map_list {
    int id;
@@ -71,35 +69,8 @@

static int MapNum = 0;


#define CAR_COORD_STORE_SIZE        10

struct car_coord_ {
    uint32_t uptime;
    double azimuth;
    PointF coord;
};

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

struct car_desc_ *CarDesc = NULL;

typedef struct {
    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 pointNum;
    struct car_desc_ *carDesc;
    PointF *carXY;
} car_model;

static car_model *CarModel = NULL;
static car_model *CarModelPrev = NULL;

#define MOV_AVG_SIZE                1
#define RTK_BUFFER_SIZE            100
@@ -110,15 +81,17 @@

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

void DriverTestInit(void)
{
    TestStart = false;
    memset(&MapList, 0, sizeof(MapList));
    MapNum = 0;
    CarModel = NULL;
    CarModelPrev = NULL;

    RtkBuffer = (rtk_info *) malloc(RTK_BUFFER_SIZE * sizeof(rtk_info));
    RtkBufferNum = RtkBufferIn = 0;
@@ -140,6 +113,8 @@

void ClearMap(void)
{
    if (TestStart) return;

    for (int i = 0; i < MapNum; ++i) {
        if (MapList[i].map.point != NULL)
            free(MapList[i].map.point);
@@ -156,7 +131,7 @@

    AppTimer_delete(ReadDriverExamPrimerTimeout);

    if (map == NULL || pointNum == 0)
    if (map == NULL || pointNum == 0 || TestStart)
        return;

    MapList[MapNum].id = id;
@@ -187,13 +162,15 @@

void SetCarMeasurePoint(double *basePoint, int *axial, int *left_front_tire,
                    int *right_front_tire, int *left_rear_tire, int *right_rear_tire,
                    double (*point)[2], int pointNum)
                    int *body, int bodyNum,double (*point)[2], int pointNum)
{
    DEBUG("SetCarMeasurePoint pointNum %d", pointNum);

    if (point == NULL || pointNum == 0) return;
    if (point == NULL || pointNum == 0 || TestStart) return;

    if (CarModel != NULL) {
        if (CarModel->body != NULL)
            free(CarModel->body);
        if (CarModel->carDesc != NULL)
            free(CarModel->carDesc);
        if (CarModel->carXY != NULL)
@@ -201,6 +178,23 @@
        free(CarModel);
        CarModel = NULL;
    }

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

    CarModelPrev = (car_model *)malloc(sizeof(car_model));
    CarModelPrev->body = NULL;
    CarModelPrev->carDesc = NULL;
    CarModelPrev->pointNum = 0;
    CarModelPrev->carXY = (PointF *) malloc(sizeof(PointF) * pointNum);

    CarModel = (car_model *)malloc(sizeof(car_model));
    CarModel->basePoint.X = basePoint[0];
@@ -215,8 +209,22 @@
    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->pointNum = pointNum;

    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;
        }
    } else {
        CarModel->body = (int *) malloc(sizeof(int) * CarModel->bodyNum);
        for (int i = 0; i < CarModel->bodyNum; ++i) {
            CarModel->body[i] = body[i];
        }
    }

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

@@ -264,8 +272,15 @@
        err = true;
        MA_SendExamStatus(0, -2);
    }
    if (!err)
    if (!err) {
        if (!TestStart) {
            ExamFaultList.clear();
            examFaultIndex = 0;

            TestStart = true;
        }
        MA_SendExamStatus(1, 0);
    }
}

void UpdateRTKInfo(const rtk_info *s)
@@ -275,6 +290,8 @@
        RtkBufferIn = (RtkBufferIn + 1) % RTK_BUFFER_SIZE;
        if (RtkBufferNum < RTK_BUFFER_SIZE)
            RtkBufferNum++;
    } else {
        return;
    }

    double speed;
@@ -289,9 +306,9 @@
                RtkBuffer[index].mm, RtkBuffer[index].ss, RtkBuffer[index].dss);

        brief.qf = RtkBuffer[index].qf;
        brief.map_id = -1;
        brief.map_id = 863;
        brief.move = move;
        brief.speed = speed;
        brief.speed = speed * 3.6;
        brief.heading = RtkBuffer[index].heading;
        brief.main_ant[0] = RtkBuffer[index].x;
        brief.main_ant[1] = RtkBuffer[index].y;
@@ -307,6 +324,12 @@
        brief.right_rear_tire[0] = CarModel->right_rear_tire[0];
        brief.right_rear_tire[1] = CarModel->right_rear_tire[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];
        }

        brief.pointNum = CarModel->pointNum;
        brief.point = (double *) malloc(CarModel->pointNum * 2 * sizeof(double));
        for (int i = 0, j = 0; i < CarModel->pointNum; ++i) {
@@ -315,6 +338,13 @@
        }

        MA_SendCarPosition(&brief);

        free(brief.body);
        free(brief.point);

        if (TestStart) {
//            TestParkBottom(ErrorList, 0, CarModel, CarModelPrev, speed, move);
        }
    }
}

@@ -345,6 +375,8 @@
    // 如果一定的时间都没有有效定位，删除之前的值
    uint32_t 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];
@@ -357,7 +389,22 @@
    main_ant_coord.X = RtkBuffer[p1].x;
    main_ant_coord.Y = RtkBuffer[p1].y;

    // 保存上一个车身轮廓点
    if (CarModelPrev->pointNum > 0) {
        CarModelPrev->pointNum = CarModel->pointNum;
        CarModelPrev->basePoint = CarModel->basePoint;
        for (int i = 0; i < CarModel->pointNum; ++i) {
            CarModelPrev->carXY[i] = CarModel->carXY[i];
        }
    }
    UpdateCarBodyCoord(RtkBuffer[p1].heading, main_ant_coord, CarModel);
    if (CarModelPrev->pointNum == 0) {
        CarModelPrev->pointNum = CarModel->pointNum;
        CarModelPrev->basePoint = CarModel->basePoint;
        for (int i = 0; i < CarModel->pointNum; ++i) {
            CarModelPrev->carXY[i] = CarModel->carXY[i];
        }
    }

    // 计算速度(米/秒)、前进后退
    double speed = sqrt(pow(RtkBuffer[p1].x - RtkBuffer[p2].x, 2) + pow(RtkBuffer[p1].y - RtkBuffer[p2].y, 2)) * 1000 /
@@ -505,6 +552,21 @@
        }*/
}

void AddExamFault(int wrong, const char *utc)
{
    if (!TestStart) return;

    struct ExamFault fault;

    fault.sn = examFaultIndex++;
    strcpy(fault.utc, utc);
    fault.wrong_id = wrong;

    ExamFaultList.push_back(fault);

    MA_SendExamWrong(ExamFaultList);
}

car_model_cache_t *GetCarModelCache(int node)
{
    return NULL;
@@ -515,8 +577,10 @@
 * @param azimuth
 * @param coord
 */
static void UpdateCarBodyCoord(double azimuth, PointF main_ant, const car_model *carModel)
static void UpdateCarBodyCoord(double azimuth, PointF main_ant, car_model *carModel)
{
    carModel->basePoint = main_ant;

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