s

yy1717

2020-01-10 c64da51ae481dfc2a25e5a6e39bb94f22d65428a

 lib/src/main/cpp/driver_test.cpp

@@ -86,6 +86,19 @@

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

static car_model *CarModel = NULL;

struct car_coord_ *CarCoord = NULL;
static double currSpeed = 0, currAzimuth = 0;
static PointF currCoord;
@@ -122,6 +135,7 @@
{
    memset(&MapList, 0, sizeof(MapList));
    MapNum = 0;
    CarModel = NULL;

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

@@ -259,22 +273,20 @@
    ErrorList.clear();

    TextSpeak("开始测试");

    AppTimer_add(ReadDriverExamPrimerTimeout, D_SEC(2));
    ReadDriverExamPrimer();
}

static void ReadDriverExamPrimerTimeout(union sigval sig)
{
    AppTimer_delete(ReadDriverExamPrimerTimeout);
    AppTimer_add(ReadDriverExamPrimerTimeout, D_SEC(2));
    ReadDriverExamPrimer();
}

void ReadDriverExamPrimer(void)
{
//    MA_ReadMap();
//    MA_ReadCar();
//    MA_ReadSensor();
    MA_ReadMap();
    MA_ReadCar();
    MA_ReadSensor();
}

void ClearMap(void)
@@ -327,6 +339,56 @@
                    double (*point)[2], int pointNum)
{
    DEBUG("SetCarMeasurePoint pointNum %d", pointNum);

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

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

    CarModel = (car_model *)malloc(sizeof(CarModel));
    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->pointNum = pointNum;

    CarModel->carDesc = (struct car_desc_ *)malloc(sizeof(struct car_desc_) * 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;

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

    DEBUG("SetCarMeasurePoint Calc Over");
}

void SetSensorCfg(int (*sensor)[2], int sensorNum)
@@ -334,6 +396,22 @@
    DEBUG("SetSensorCfg sensorNum %d", sensorNum);
}

void StartDriverExam(int start)
{
    bool err = false;

    if (MapNum == 0) {
        err = true;
        MA_SendExamStatus(0, -1);
    }
    if (CarModel == NULL) {
        err = true;
        MA_SendExamStatus(0, -2);
    }
    if (!err)
        MA_SendExamStatus(1, 0);
}

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