updatecrrDOC1

yy1717

2020-01-22 eb645b6bf08ec50459c737821e63ee20dea4486d

 lib/src/main/cpp/driver_test.cpp

@@ -296,6 +296,11 @@
    bool err = false;

    DEBUG("StartDriverExam %d", start);
    if (start == 0) {
        TestStart = false;
        MA_SendExamStatus(0, 0);
        return;
    }

    if (MapNum == 0) {
        err = true;
@@ -318,7 +323,7 @@

void UpdateRTKInfo(const rtk_info *s)
{
    if (s->qf > 0) {
    if (s->qf == 3) {
        RtkBuffer[RtkBufferIn] = *s;
        RtkBufferIn = (RtkBufferIn + 1) % RTK_BUFFER_SIZE;
        if (RtkBufferNum < RTK_BUFFER_SIZE)
@@ -434,6 +439,7 @@
                }
            }
            if (CurrExamStatus != 0) {
                // 某项结束，
                if (ExitMap(CarModel, CurrExamMapIndex, MapList, MapNum)) {
                    DEBUG("退出场地 %d", GetMapId(CurrExamMapIndex, MapList, MapNum));
                    MA_SendDebugInfo("退出场地 %d", GetMapId(CurrExamMapIndex, MapList, MapNum));
@@ -458,18 +464,28 @@

static bool UpdateCarCoord(double &spd, int &mov, int &idx)
{
    long tmDiff;

    if (CarModel == NULL)
        return false;

    if (RtkBufferNum < 3)
    if (RtkBufferNum < 2)
        return false;

    int p1 = ((RtkBufferIn-1)+RTK_BUFFER_SIZE)%RTK_BUFFER_SIZE;
    int p2 = ((RtkBufferIn-2)+RTK_BUFFER_SIZE)%RTK_BUFFER_SIZE;
    int p3 = ((RtkBufferIn-3)+RTK_BUFFER_SIZE)%RTK_BUFFER_SIZE;
    int p1 = ((RtkBufferIn-1)+RTK_BUFFER_SIZE)%RTK_BUFFER_SIZE;               // 最近采样值
    int p2 = ((RtkBufferIn-2)+RTK_BUFFER_SIZE)%RTK_BUFFER_SIZE;               // 找到1秒前的采样值
    int pn = 0;

    for (pn = 1; pn < RtkBufferNum; ++pn) {
        p2 = ((RtkBufferIn-1-pn)+RTK_BUFFER_SIZE)%RTK_BUFFER_SIZE;
        if ((tmDiff = CalcTimeDiff(&RtkBuffer[p1], &RtkBuffer[p2])) >= D_SEC(1)) break;
    }

    if (pn == RtkBufferNum)
        return false;

    // 如果一定的时间都没有有效定位，删除之前的值
    uint32_t tmDiff = CalcTimeDiff(&RtkBuffer[p1], &RtkBuffer[p2]);
    /*tmDiff = CalcTimeDiff(&RtkBuffer[p1], &RtkBuffer[p2]);

    DEBUG("tmDiff = %ld, p1 = %d p2 = %d dss = %d dss2 = %d", tmDiff, p1, p2, RtkBuffer[p1].dss, RtkBuffer[p2].dss);

@@ -478,7 +494,7 @@
            RtkBuffer[0] = RtkBuffer[p1];
        RtkBufferIn = RtkBufferNum = 1;
        return false;
    }
    }*/

    // 计算车辆轮廓点
    PointF main_ant_coord;
@@ -562,7 +578,7 @@
    spd = speed;
    mov = move;
    idx = p1;
    DEBUG("speed = %f m/Sec move = %d", speed, move);
    DEBUG("tmDiff = %ld speed = %f m/Sec move = %d", tmDiff, speed, move);

    return true;
/*        if (!TestStart) return;
@@ -674,23 +690,27 @@

static bool ExitMap(const car_model *car, int index, const struct map_list *mapList, int mapNum)
{
    // 车的最后点是否进入地图
    bool ret = false;
    if (index < 0 || mapList == NULL || mapNum == 0) return true;

    if (mapList[index].type == MAP_TYPE_PARK_BUTTOM) {
        if (IntersectionOf(car->carXY[car->axial[AXIAL_REAR]], &mapList[index].map) == GM_None) {
            Line enterLine1, enterLine2;
        // 全车都需不在地图中
        Polygon carBody;

            MakeLine(&enterLine1, &(mapList[index].map.point[0]), &(mapList[index].map.point[1]));
            MakeLine(&enterLine2, &(mapList[index].map.point[6]), &(mapList[index].map.point[7]));

            if (DistanceOf(car->carXY[car->axial[AXIAL_REAR]], enterLine1) > 0.1 &&
                DistanceOf(car->carXY[car->axial[AXIAL_REAR]], enterLine2) > 0.1)
                return true;
        carBody.num = car->bodyNum;
        carBody.point = (PointF *)malloc(carBody.num * sizeof(PointF));
        for (int i = 0; i < carBody.num; ++i) {
            carBody.point[i] = car->carXY[car->body[i]];
        }

        if (IntersectionOf(&carBody, &mapList[index].map) == GM_None) {
            ret = true;
        }

        free(carBody.point);
    }

    return true;
    return ret;
}

static int GetMapId(int index, const struct map_list *mapList, int mapNum)