uuuu

lizhanwei

2020-03-28 babb05583f59eb5e2c853168490f54fc7261e84a

 lib/src/main/cpp/driver_test.cpp

@@ -474,7 +474,7 @@
        }
    }

    CarModel->antPitch = antPitch;
    CarModel->antPitch = -0.6;//antPitch;/////////////////////////////////////

    CarModel->pointNum = pointNum;
    CarModel->carDesc = (struct car_desc_ *)malloc(sizeof(struct car_desc_) * pointNum);
@@ -730,6 +730,7 @@

static void ExecuteExam(const struct RtkTime* rtkTime)
{
//    DEBUG("ExecuteExam 111111111");
    {
        static const char *NAME[] = {"OBD_SPEED",
                                     "ENGINE_RPM",
@@ -783,12 +784,21 @@
        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 < CAR_STATUS_END; ++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] ]);

@@ -797,7 +807,7 @@
        }

    }

//DEBUG("-------------- 1111111111");

    if (ReadCarStatus(ENGINE_RPM) < ENGINE_MIN_ROTATE) {
        if (engineRuning) {
@@ -854,10 +864,13 @@
            }
        }
    }

//    DEBUG("ExecuteExam 1111111111 ----------------------");
}

static void ExecuteExam(double speed, int move, double azimuth, const struct RtkTime* rtkTime)
{
//    DEBUG("ExecuteExam 2222222");
    if (move != 0) {
        if (ReadCarStatus(SEATBELT) == EJECT_SEATBELT && !reportSeatbeltEject) {
            DEBUG("不系安全带");
@@ -952,6 +965,7 @@
            CurrExamMapIndex = -1;
        }
    }
//    DEBUG("ExecuteExam 2222222 -----------------------");
}

static void EngineStartHold(union sigval sig) {
@@ -1348,16 +1362,20 @@
{
    carModel->basePoint = main_ant;


//    DEBUG("俯仰角 %f", pitch);
    carModel->yaw = azimuth;
    carModel->pitch = pitch;
    carModel->tm = *rtkTime;

    pitch = pitch - carModel->antPitch;

//    DEBUG("xz俯仰角 %f", pitch);

    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)) *
                carModel->carDesc[i].distance * cos(toRadians(carModel->carDesc[i].angle)) /
                cos(toRadians(pitch));

        double projectDistance = sqrt(pow(qrx, 2) + pow(qry, 2));