更新地图

yy1717

2020-02-26 9ca8ca86beb09186ff3aecefceeddaea80b2409f

 lib/src/main/cpp/driver_test.cpp

@@ -291,23 +291,23 @@
        DEBUG("加入点<%d> 距离 %f 角度 %f", i, CarModel->carDesc[i].distance, CarModel->carDesc[i].angle);
    }

    CarModel->carDesc[0].distance = 0.2465;
    CarModel->carDesc[0].angle = 0;

    CarModel->carDesc[1].distance = 0.2635;
    CarModel->carDesc[1].angle = 20.7;

    CarModel->carDesc[2].distance = 0.14;
    CarModel->carDesc[2].angle = 138.9;

    CarModel->carDesc[3].distance = 0.1055;
    CarModel->carDesc[3].angle = 180.0;

    CarModel->carDesc[4].distance = 0.14;
    CarModel->carDesc[4].angle = 221.1;

    CarModel->carDesc[5].distance = 0.2635;
    CarModel->carDesc[5].angle = 339.3;
//    CarModel->carDesc[0].distance = 0.2465;
//    CarModel->carDesc[0].angle = 0;
//
//    CarModel->carDesc[1].distance = 0.2635;
//    CarModel->carDesc[1].angle = 20.7;
//
//    CarModel->carDesc[2].distance = 0.14;
//    CarModel->carDesc[2].angle = 138.9;
//
//    CarModel->carDesc[3].distance = 0.1055;
//    CarModel->carDesc[3].angle = 180.0;
//
//    CarModel->carDesc[4].distance = 0.14;
//    CarModel->carDesc[4].angle = 221.1;
//
//    CarModel->carDesc[5].distance = 0.2635;
//    CarModel->carDesc[5].angle = 339.3;

    DEBUG("SetCarMeasurePoint Calc Over");
}
@@ -362,10 +362,10 @@
    if (start == 0) {
        DEBUG("-------------结束考试----------------");

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

@@ -397,7 +397,7 @@

    if (map_id >= 0 && exam == 0) {
        CurrExamMapIndex = map_id;
        CurrExamStatus = 0;
        CurrExamStatus = 2;
    }
}

@@ -495,6 +495,9 @@
                    if (CurrEnterMapIndex >= 0) {
                        DEBUG("发送进入场地报告 %d", GetMapId(CurrEnterMapIndex, MapList, MapNum));
                        MA_EnterMap(GetMapId(CurrEnterMapIndex, MapList, MapNum), 1);

                        CurrExamMapIndex = CurrEnterMapIndex;
                        CurrExamStatus = 2;
                    }
                } else {
                    if (ExitMap(CarModel, CurrEnterMapIndex, MapList, MapNum)) {
@@ -510,7 +513,7 @@
        if (CurrExamMapIndex >= 0) {
            int mtype = GetMapType(CurrExamMapIndex, MapList, MapNum);

            if (CurrExamStatus == 0) {
            if (CurrExamStatus == 2) {
                DEBUG("CurrExamMapIndex %d mtype %d", GetMapId(CurrExamMapIndex, MapList, MapNum), mtype);

                switch (mtype) {
@@ -544,9 +547,7 @@
                        break;
                    default:break;
                }

                CurrExamStatus = 1;
            } else if (CurrExamStatus == 1) {
            } else if (CurrExamStatus == 0) {
                switch (mtype) {
                    case MAP_TYPE_PARK_BUTTOM:
                        CurrExamStatus = TestParkBottom(&MapList[CurrExamMapIndex].map,
@@ -564,12 +565,12 @@
                        CurrExamStatus = TestTurnA90(&MapList[CurrExamMapIndex].map, CarModel, CarModelPrev, azimuth, speed, move, &rtkTime);
                        break;
                    default:
                        CurrExamStatus = -1;
                        CurrExamStatus = 1;
                        break;
                }
            }

            if (CurrExamStatus < 0) {
            if (CurrExamStatus == 1) {
                // 某项结束
                //DEBUG("退出场地 %d", GetMapId(CurrExamMapIndex, MapList, MapNum));
                //MA_SendDebugInfo("退出场地 %d", GetMapId(CurrExamMapIndex, MapList, MapNum));
@@ -707,7 +708,7 @@
    spd = speed;
    mov = move;
    idx = p1;
    DEBUG("tmDiff = %ld speed = %f m/Sec move = %d", tmDiff, speed, move);
//    DEBUG("tmDiff = %ld speed = %f m/Sec move = %d", tmDiff, speed, move);

    return true;
/*        if (!TestStart) return;
@@ -836,7 +837,7 @@
//        if (mapList[i].type == MAP_TYPE_CURVE) {
//
//        }
        if (mapList[i].type == MAP_TYPE_PARK_BUTTOM /*|| mapList[i].type == MAP_TYPE_PART_EDGE*/) {
        if (mapList[i].type == MAP_TYPE_PARK_BUTTOM) {
            if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &mapList[i].map) == GM_Containment) {
                Line enterLine1, enterLine2;

@@ -848,6 +849,16 @@
                    return i;
            }
        }
//        if (mapList[i].type == MAP_TYPE_PART_EDGE) {
//            if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &mapList[i].map) == GM_Containment) {
//                Line enterLine;
//
//                MakeLine(&enterLine, &(mapList[i].map.point[0]), &(mapList[i].map.point[1]));
//
//                if (DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], enterLine) > 0.1)
//                    return i;
//            }
//        }
//        if (mapList[i].type == MAP_TYPE_TURN_90) {
//            if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &mapList[i].map) == GM_Containment) {
//                Line enterLine1;
@@ -987,21 +998,13 @@
            projectAngle = 360 - projectAngle;
        }

        double tx = projectDistance*sin(toRadians(azimuth));
        double ty = projectDistance*cos(toRadians(azimuth));
//        double tx = projectDistance*sin(toRadians(azimuth));
//        double ty = projectDistance*cos(toRadians(azimuth));

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

//        double tx = carModel->carDesc[i].distance*sin(toRadians(azimuth));
//        double ty = carModel->carDesc[i].distance*cos(toRadians(azimuth));
//
//        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)) +
//                                   (ty)*cos(toRadians(carModel->carDesc[i].angle)) + main_ant.Y;
        carModel->carXY[i].X = projectDistance*sin(toRadians(azimuth)) * cos(toRadians(projectAngle)) -
                projectDistance*cos(toRadians(azimuth)) * sin(toRadians(projectAngle)) + main_ant.X;
        carModel->carXY[i].Y = projectDistance*sin(toRadians(azimuth)) * sin(toRadians(projectAngle)) +
                projectDistance*cos(toRadians(azimuth)) * cos(toRadians(projectAngle)) + main_ant.Y;
    }
}