更新地图

yy1717

2020-03-05 6b136af26a51b3d53051cd3e0e7d467ed2ab8a6b

更新地图

 lib/src/main/cpp/driver_test.cpp

@@ -878,40 +878,16 @@
//            }
//        }
        if (mapList[i].type == MAP_TYPE_CURVE) {
            const int step = 10;
            Polygon map;
            bool enter = false;
            Line startLine;
            Line carAxial;

            map.num = 1 + mapList[i].map.num / step + ((mapList[i].map.num % step == 0) ? 0 : 1);
            map.num += 1 + mapList[i].map2.num / step + ((mapList[i].map2.num % step == 0) ? 0 : 1);
            map.point = (PointF *) malloc(map.num * sizeof(PointF));
            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]]);

            int n = 0, m = 0;
            while (n < mapList[i].map.num - 1) {
                map.point[m++] = mapList[i].map.point[n];
                n += step;
            if (IntersectionOf(startLine, carAxial) == GM_Intersection) {
                if (DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], startLine) > 0.1)
                    return i;
            }
            map.point[m++] = mapList[i].map.point[mapList[i].map.num - 1];

            n = mapList[i].map2.num - 1;
            while (n > 0) {
                map.point[m++] = mapList[i].map2.point[n];
                n -= step;
            }
            map.point[m++] = mapList[i].map2.point[0];

            if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &map) == GM_Containment) {
                Line enterLine1;

                MakeLine(&enterLine1, &(mapList[i].map.point[0]), &(mapList[i].map2.point[0]));

                if (DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], enterLine1) > 0.1)
                    enter = true;
            }

            free(map.point);

            if (enter) return i;
        }
    }
    return -1;
@@ -979,6 +955,25 @@
            free(carBody.point);
            free(map.point);
    }
    if (mapList[index].type == MAP_TYPE_CURVE) {
        Line carAxial;
        Line scanLine;

        MakeLine(&carAxial, &car->carXY[car->axial[AXIAL_FRONT]], &car->carXY[car->axial[AXIAL_REAR]]);

        int startL = 0, startR = 0;
        while (startL < mapList[index].map.num || startR < mapList[index].map2.num) {
            MakeLine(&scanLine, &mapList[index].map.point[startL], &mapList[index].map.point[startR]);
            if (IntersectionOf(scanLine, carAxial) == GM_Intersection) {
                break;
            }
            if (startL < mapList[index].map.num)
                startL++;
            if (startR < mapList[index].map2.num)
                startR++;
        }
        ret = true;
    }

    return ret;
}

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

@@ -202,7 +202,7 @@
        }*/

        if (RxBufLen > 0) {
#if 1
#if 0
            const uint8_t *ptr = parseGPS(RxBuf, RxBuf + RxBufLen);
            if(ptr != RxBuf) {
                memcpy(RxBuf, ptr, RxBufLen - (ptr - RxBuf));

 lib/src/main/cpp/test_items/driving_curve.cpp

@@ -26,15 +26,14 @@
static bool reportStopCarTimeout;
static int prevMoveDirect;
static bool crashRedLine;
static struct {
static struct calc_zone_t {
    int leftStart;
    int leftEnd;
    int rightStart;
    int rightEnd;
} calcZone;

static bool CrashRedLine(const Polygon *left, const Polygon *right, const car_model_cache_t *car);
static bool ExitTestArea(const Polygon *left, const Polygon *right, const car_model_cache_t *car);
static bool CrashRedLine(const Polygon *map, const Polygon *map2, const car_model *car, struct calc_zone_t *zone);

void StartDrivingCurve(int moveDirect, const struct RtkTime *rtkTime)
{
@@ -49,104 +48,145 @@
    reportStopCarTimeout = false;
    crashRedLine = false;

    calcZone.leftStart = calcZone.leftEnd = calcZone.rightStart = calcZone.rightEnd = -1;
    calcZone.leftStart = calcZone.leftEnd = calcZone.rightStart = calcZone.rightEnd = 0;
}

int TestDrivingCurve(const Polygon *map, const Polygon *map2, const car_model *car, const car_model *carPrev, double speed, int moveDirect, const struct RtkTime *rtkTime)
{
    Line start, end;
    Polygon zone;
    Line axial;

    if (calcZone.leftStart == -1) {
        calcZone.leftStart = 0;
    }
    if (calcZone.rightStart == -1) {
        calcZone.rightStart = 0;
    MakeLine(&axial, &car->carXY[car->axial[AXIAL_FRONT]], &car->carXY[car->axial[AXIAL_REAR]]);

    MakeLine(&start, &map->point[calcZone.leftStart], &map->point[calcZone.rightStart]);
    MakeLine(&end, &map->point[calcZone.leftEnd], &map->point[calcZone.rightEnd]);

    if (IntersectionOf(start, axial) == GM_None) {
        // 向起点查找
        do {
            if (calcZone.leftStart == 0 && calcZone.rightStart == 0) {
                break;
            }
            if (calcZone.leftStart > 0)
                calcZone.leftStart--;
            if (calcZone.rightStart > 0)
                calcZone.rightStart--;
            MakeLine(&start, &map->point[calcZone.leftStart], &map->point[calcZone.rightStart]);
        } while (IntersectionOf(start, axial) == GM_None);
    } else {
        // 向终点查找
        do {
            if (calcZone.leftStart >= map->num && calcZone.rightStart >= map2->num) {
                break;
            }
            if (calcZone.leftStart < map->num)
                calcZone.leftStart++;
            if (calcZone.rightStart < map2->num)
                calcZone.rightStart++;
            MakeLine(&start, &map->point[calcZone.leftStart], &map->point[calcZone.rightStart]);
        } while (IntersectionOf(start, axial) != GM_None);
    }

    start.X1 = map->point[calcZone.leftStart].X;
    start.Y1 = map->point[calcZone.leftStart].Y;

    start.X2 = map->point[calcZone.rightStart].X;
    start.Y2 = map->point[calcZone.rightStart].Y;

    zone.num = 4;
    zone.point = (PointF *) malloc(zone.num * sizeof(PointF));
    zone.point[0] = car->carXY[car->left_front_tire[TIRE_OUTSIDE]];
    zone.point[1] = car->carXY[car->right_front_tire[TIRE_OUTSIDE]];
    zone.point[2] = car->carXY[car->right_rear_tire[TIRE_OUTSIDE]];
    zone.point[3] = car->carXY[car->left_rear_tire[TIRE_OUTSIDE]];

    if (IntersectionOf(start, &zone) == GM_None) {

    if (IntersectionOf(end, axial) == GM_None) {
        // 向终点查找
        do {
            if (calcZone.leftEnd >= map->num && calcZone.rightEnd >= map2->num) {
                break;
            }
            if (calcZone.leftEnd < map->num)
                calcZone.leftEnd++;
            if (calcZone.rightEnd < map2->num)
                calcZone.rightEnd++;
            MakeLine(&end, &map->point[calcZone.leftEnd], &map->point[calcZone.rightEnd]);
        } while (IntersectionOf(end, axial) == GM_None);
    } else {
        // 向起点查找
        do {
            if (calcZone.leftEnd == 0 && calcZone.rightEnd == 0) {
                break;
            }
            if (calcZone.leftEnd > 0)
                calcZone.leftEnd--;
            if (calcZone.rightEnd > 0)
                calcZone.rightEnd--;
            MakeLine(&end, &map->point[calcZone.leftEnd], &map->point[calcZone.rightEnd]);
        } while (IntersectionOf(end, axial) == GM_None);
    }

    free(zone.point);
    if (calcZone.leftStart <= calcZone.leftEnd || calcZone.rightStart <= calcZone.rightEnd) {
        // 离开场地
        testing = false;
    }

    if (CrashRedLine(map, map2, car, &calcZone)) {
        if (!crashRedLine) {
            crashRedLine = true;
            // 车轮压边线，不合格
            AddExamFault(27, rtkTime);
            DEBUG("车轮压边线");
        }
    } else {
        crashRedLine = false;
    }

    if (moveDirect != prevMoveDirect) {
        if (moveDirect == 0) {
            stopTimepoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
            reportStopCarTimeout = false;

            DEBUG("停车了 %d %d %d %d %d %d %d", rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss);
        } else {

        }
        prevMoveDirect = moveDirect;
    } else if (moveDirect == 0) {
        uint32_t tp = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);

        if (tp - stopTimepoint >= STOP_CAR_TIME && !reportStopCarTimeout) {
            // 停车超2秒，不合格
            AddExamFault(28, rtkTime);
            DEBUG("中途停车");
            reportStopCarTimeout = true;
        }
    }

    return testing ? 1 : 0;
}

// 车轮是否压边线
static bool CrashRedLine(const Polygon *left, const Polygon *right, const car_model_cache_t *car)
static bool CrashRedLine(const Polygon *map, const Polygon *map2, const car_model *car, struct calc_zone_t *zone)
{
    bool ret = false;

    car_model_cache_t *prev_car = GetCarModelCache(1);
    if (prev_car == NULL)
        return false;
    Line frontTireAxial, rearTireAxial;
    Line redLine;

    // 按4个轮子的外侧计算
    Line front_left_tire_track, front_right_tire_track, rear_left_tire_track, rear_right_tire_track;
    MakeLine(&front_left_tire_track,  &car->points[car->desc->front_left_tire[TIRE_OUTSIDE]],  &prev_car->points[car->desc->front_left_tire[TIRE_OUTSIDE]]);
    MakeLine(&front_right_tire_track,  &car->points[car->desc->front_right_tire[TIRE_OUTSIDE]],  &prev_car->points[car->desc->front_right_tire[TIRE_OUTSIDE]]);
    MakeLine(&rear_left_tire_track,  &car->points[car->desc->rear_left_tire[TIRE_OUTSIDE]],  &prev_car->points[car->desc->rear_left_tire[TIRE_OUTSIDE]]);
    MakeLine(&rear_right_tire_track,  &car->points[car->desc->rear_right_tire[TIRE_OUTSIDE]],  &prev_car->points[car->desc->rear_right_tire[TIRE_OUTSIDE]]);
    MakeLine(&frontTireAxial, &car->carXY[car->left_front_tire[TIRE_OUTSIDE]], &car->carXY[car->right_front_tire[TIRE_OUTSIDE]]);
    MakeLine(&rearTireAxial, &car->carXY[car->left_rear_tire[TIRE_OUTSIDE]], &car->carXY[car->right_rear_tire[TIRE_OUTSIDE]]);

    Line line;

    line.X1 = left->point[0].X;
    line.Y1 = left->point[0].Y;
    for (int i = 1; i < left->num; ++i) {
        line.X2 = left->point[i].X;
        line.Y2 = left->point[i].Y;

        if (IntersectionOf(line, front_left_tire_track) == GM_Intersection ||
                IntersectionOf(line, front_right_tire_track) == GM_Intersection ||
                IntersectionOf(line, rear_left_tire_track) == GM_Intersection ||
                IntersectionOf(line, rear_right_tire_track) == GM_Intersection) {
            ret = true;
            break;
    int s = zone->leftStart;
    for (int e = zone->leftStart + 1; e < zone->leftEnd; ++e) {
        MakeLine(&redLine, &map->point[s], &map->point[e]);
        if (IntersectionOf(redLine, frontTireAxial) != GM_None) {
            return true;
        }
        line.X1 = line.X2;
        line.Y1 = line.Y2;
        if (IntersectionOf(redLine, rearTireAxial) != GM_None) {
            return true;
        }
        s = e;
    }

    line.X1 = right->point[0].X;
    line.Y1 = right->point[0].Y;
    for (int i = 1; !ret && i < right->num; ++i) {
        line.X2 = right->point[i].X;
        line.Y2 = right->point[i].Y;

        if (IntersectionOf(line, front_left_tire_track) == GM_Intersection ||
            IntersectionOf(line, front_right_tire_track) == GM_Intersection ||
            IntersectionOf(line, rear_left_tire_track) == GM_Intersection ||
            IntersectionOf(line, rear_right_tire_track) == GM_Intersection) {
            ret = true;
            break;
    s = zone->rightStart;
    for (int e = zone->rightStart + 1; e < zone->rightEnd; ++e) {
        MakeLine(&redLine, &map2->point[s], &map2->point[e]);
        if (IntersectionOf(redLine, frontTireAxial) != GM_None) {
            return true;
        }
        line.X1 = line.X2;
        line.Y1 = line.Y2;
        if (IntersectionOf(redLine, rearTireAxial) != GM_None) {
            return true;
        }
        s = e;
    }

    return ret;
}

// 整个车辆都要驶离该测试区域
static bool ExitTestArea(const Polygon *left, const Polygon *right, const car_model_cache_t *car)
{
    for (int i = 0; i < car->point_num; ++i) {
        if (IntersectionOfLine(left->point[0], right->point[0], car->points[i]) != 1)
            return false;
    }
    return true;
}