GPS信息合并到RTK中，以RTK包，发送

fctom1215

2020-02-21 4581cc42ba485366603ca8e3f61183f6a50ad72f

 lib/src/main/cpp/driver_test.cpp

@@ -359,14 +359,17 @@
{
    bool err = false;

    DEBUG("StartDriverExam %d", start);
    if (start == 0) {
        DEBUG("-------------结束考试----------------");

        CurrExamMapIndex = -1;
        TestStart = false;
        CommTestStart(false);
        MA_SendExamStatus(0, 0);
        return;
    }

    DEBUG("+++++++++++++++开始考试++++++++++++++++++++");

    if (MapNum == 0) {
        err = true;
@@ -977,6 +980,18 @@
        double tx = carModel->carDesc[i].distance*sin(toRadians(azimuth));
        double ty = carModel->carDesc[i].distance*cos(toRadians(azimuth));

//        double qrx = carModel->carDesc[i].distance * sin(toRadians(carModel->carDesc[i].angle));
//        double qry = carModel->carDesc[i].distance * cos(toRadians(carModel->carDesc[i].angle)) * cos(toRadians(pitch));
//
//        double projDis = sqrt(pow(qrx, 2) + pow(pry, 2));
//        double projAng;
//
//        if (fabs(qry) <= EPSILON) {
//            projAng = 90;
//        } else {
//            projAng = toDegree(atan(qrx / qry));
//        }

        carModel->carXY[i].X = (tx)*cos(toRadians(carModel->carDesc[i].angle)) -
                                   (ty)*sin(toRadians(carModel->carDesc[i].angle)) + main_ant.X;
        carModel->carXY[i].Y = (tx)*sin(toRadians(carModel->carDesc[i].angle)) +