更新地图

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


 lib/src/main/cpp/master/comm_if.cpp

@@ -50,8 +50,8 @@
#define ID_SM_RTCM_IND           0x000F
#define ID_SM_DEBUG_INFO        0x0010
#define ID_MS_FILE              0x8100
#define ID_MS_READ_CARD         0x800F
#define ID_SM_PUT_CARD          0x000F
#define ID_MS_READ_CARD         0x8011
#define ID_SM_PUT_CARD          0x0011
#define ID_MS_SYS_SHUTDOWN      0x8010

#define MA_OUT_GPS_BRIEF        0x0001

 lib/src/main/cpp/native-lib.cpp

@@ -245,7 +245,7 @@
    InitPlatform(phone, RTK_PLATFORM_IP, RTK_PLATFORM_PORT);
    AppTimer_add(SendBootIndicate, 500);

    InitVirtualDevice("192.168.3.52", 9001);
    InitVirtualDevice("192.168.86.101", 9001);
}

static void SendBootIndicate(union sigval sig) {

 lib/src/main/cpp/rtk_module/rtk.cpp

@@ -202,17 +202,18 @@
        }*/

        if (RxBufLen > 0) {
//            const uint8_t *ptr = parseGPS(RxBuf, RxBuf + RxBufLen);
//            if(ptr != RxBuf) {
//                memcpy(RxBuf, ptr, RxBufLen - (ptr - RxBuf));
//                RxBufLen -= ptr - RxBuf;
//            } else if(RxBufLen == PARSE_BUFF_SIZE) {        //填满了，且没有一个\r，都抛弃
//                DEBUG("Parse GPS error");
//                RxBufLen = 0;
//            }


#if 0
            const uint8_t *ptr = parseGPS(RxBuf, RxBuf + RxBufLen);
            if(ptr != RxBuf) {
                memcpy(RxBuf, ptr, RxBufLen - (ptr - RxBuf));
                RxBufLen -= ptr - RxBuf;
            } else if(RxBufLen == PARSE_BUFF_SIZE) {        //填满了，且没有一个\r，都抛弃
                DEBUG("Parse GPS error");
                RxBufLen = 0;
            }
#else
            RxBufLen = 0;           //PC模拟用时
#endif
        }
    }
    if (res == 0) {

 lib/src/main/cpp/rtk_module/virtual_rtk.cpp

@@ -24,7 +24,7 @@

void InitVirtualDevice(const char *domain_name, int port)
{
    DEBUG("InitVirtualDevice");
    DEBUG("InitVirtualDevice %s: %d", domain_name, port);

    struct vSocket *ptr = (struct vSocket *)malloc(sizeof(struct vSocket));

@@ -57,6 +57,8 @@

    if (fd > 0) {
        DEBUG("虚拟平台连接成功");
    } else {
        DEBUG("虚拟平台连接失败");
    }

    while (fd > 0) {