坡道坐标修正、发往主进程的消息线程独立

yy1717

2020-03-31 79f2500657c85c84aceed46ec00f9a703eae395b

 lib/src/main/cpp/driver_test.cpp

@@ -429,7 +429,7 @@

void SetCarMeasurePoint(double *basePoint, int *axial, int *left_front_tire,
                    int *right_front_tire, int *left_rear_tire, int *right_rear_tire,
                    int *body, int bodyNum,double (*point)[2], int pointNum, double antPitch)
                    int *body, int bodyNum, double (*point)[2], int pointNum, double antPitch, double antHeight, double groundHeight)
{
    DEBUG("加入车辆信息 pointNum %d", pointNum);

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

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

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

static void ExecuteExam(const struct RtkTime* rtkTime)
{
//    DEBUG("ExecuteExam 111111111");
    {
        static const char *NAME[] = {"OBD_SPEED",
                                     "ENGINE_RPM",
@@ -754,7 +755,7 @@
                                     "CAR_STATUS_END"};

        static const char *VALUE[] = {
                "0",
                "INACTIVE",
                "HAZARD_LIGHTS",
                "LEFT_TURN_LIGHT",
                "RIGHT_TURN_LIGHT",
@@ -807,7 +808,6 @@
        }

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

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

//    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("不系安全带");
@@ -965,7 +962,6 @@
            CurrExamMapIndex = -1;
        }
    }
//    DEBUG("ExecuteExam 2222222 -----------------------");
}

static void EngineStartHold(union sigval sig) {
@@ -1226,10 +1222,10 @@
            Line carAxial;

            MakeLine(&startLine, &mapList[i].map.point[0], &mapList[i].map2.point[0]);
            MakeLine(&carAxial, &car->carXY[car->axial[AXIAL_FRONT]], &car->carXY[car->axial[AXIAL_REAR]]);
            MakeLine(&carAxial, &car->carXY[car->left_front_tire[TIRE_OUTSIDE]], &car->carXY[car->left_rear_tire[TIRE_OUTSIDE]]);

            if (IntersectionOf(startLine, carAxial) == GM_Intersection) {
                if (DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], startLine) > 0.1)
                if (DistanceOf(car->carXY[car->left_front_tire[TIRE_OUTSIDE]], startLine) > 0.1)
                    return i;
            }
        }
@@ -1360,22 +1356,23 @@
 */
static void UpdateCarBodyCoord(struct RtkTime *rtkTime, double azimuth, double pitch, double roll, PointF main_ant, car_model *carModel)
{
    carModel->basePoint = main_ant;


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

    pitch = pitch - carModel->antPitch;
//    DEBUG("yaw = %f 修正俯仰角 %f", azimuth, pitch);

//    DEBUG("xz俯仰角 %f", 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));

    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));
@@ -1391,11 +1388,14 @@
        carModel->carXY[i].X =
                projectDistance * sin(toRadians(azimuth)) * cos(toRadians(projectAngle)) -
                projectDistance * cos(toRadians(azimuth)) * sin(toRadians(projectAngle)) +
                main_ant.X;
                carModel->basePoint.X;
        carModel->carXY[i].Y =
                projectDistance * sin(toRadians(azimuth)) * sin(toRadians(projectAngle)) +
                projectDistance * cos(toRadians(azimuth)) * cos(toRadians(projectAngle)) +
                main_ant.Y;
                carModel->basePoint.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);
    }
}