角度修正。

fctom1215

2020-03-05 91ed4c9dc7030f45d249192d430d9b56c43ca535

角度修正。

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

@@ -26,7 +26,7 @@
const int RTK_PLATFORM_PORT = 12125;
const uint8_t phone[] = {0x20,0x19,0x10,0x15,0x00,0x00,0x00,0x01};

const char *VIRTUAL_RTK_IP = "192.168.16.100";
const char *VIRTUAL_RTK_IP = "192.168.3.52";
const int VIRTUAL_RTK_PORT = 9001;

static void SendBootIndicate(union sigval sig);

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

@@ -35,6 +35,7 @@
} calcZone;

static bool CrashRedLine(const Polygon *map, const Polygon *map2, const car_model *car, struct calc_zone_t *zone);
static bool ExitArea(const Polygon *map, const Polygon *map2, const car_model *car);

void StartDrivingCurve(int moveDirect, const struct RtkTime *rtkTime)
{
@@ -59,8 +60,8 @@

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

    DEBUG("START 线 %d -- %d", calcZone.leftStart, calcZone.rightStart);
    DEBUG("END 线 %d -- %d", calcZone.leftEnd, calcZone.rightEnd);
//    DEBUG("START 线 %d -- %d", calcZone.leftStart, calcZone.rightStart);
//    DEBUG("END 线 %d -- %d", calcZone.leftEnd, calcZone.rightEnd);

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

@@ -73,7 +74,7 @@
                tempLeft = calcZone.leftStart - 1;
            if (calcZone.rightStart > 0)
                tempRight = calcZone.rightStart - 1;
            DEBUG("START 向起点查找 %d -- %d", tempLeft, tempRight);
//            DEBUG("START 向起点查找 %d -- %d", tempLeft, tempRight);
            MakeLine(&start, &map->point[tempLeft], &map2->point[tempRight]);

            if (IntersectionOf(start, axial) == GM_Intersection) {
@@ -85,21 +86,45 @@

                if (calcZone.leftStart == calcZone.leftEnd && calcZone.rightStart == calcZone.rightEnd) {
                    // 车辆丢失，重新搜索
                    Line cat;
                    int l = 0, r = 0;
                    calcZone.leftStart = calcZone.rightStart = 0;
                    calcZone.leftEnd = calcZone.rightEnd = 0;

                    while (l < map->num || r < map2->num) {
                        MakeLine(&cat, &map->point[l], &map2->point[r]);
                    DEBUG("车辆丢失，重新搜索");

                        if (IntersectionOf(cat, axial) == GM_Intersection) {
                    while (calcZone.leftStart < map->num || calcZone.rightStart < map2->num) {
                        MakeLine(&start, &map->point[calcZone.leftStart], &map2->point[calcZone.rightStart]);

                        if (IntersectionOf(start, axial) == GM_Intersection) {
                            while (calcZone.leftStart < map->num || calcZone.rightStart < map2->num) {
                                MakeLine(&start, &map->point[calcZone.leftStart], &map2->point[calcZone.rightStart]);
                                if (IntersectionOf(start, axial) == GM_None) {
                                    break;
                                }
                                if (calcZone.leftStart < map->num)
                                    calcZone.leftStart++;
                                if (calcZone.rightStart < map2->num)
                                    calcZone.rightStart++;
                            }
                            break;
                        }

                        if (l < map->num)
                            l++;
                        if (r < map2->num)
                            r++;
                        calcZone.leftEnd = calcZone.leftStart;
                        calcZone.rightEnd = calcZone.rightStart;

                        if (calcZone.leftStart < map->num)
                            calcZone.leftStart++;
                        if (calcZone.rightStart < map2->num)
                            calcZone.rightStart++;
                    }

                    if (calcZone.leftStart >= map->num && calcZone.rightStart >= map2->num) {
                        // 离开场地
                        DEBUG("离开曲线场地");
                        testing = false;
                        goto TEST_END;
                    }

                    break;
                }
            }
        }
@@ -113,7 +138,7 @@
                calcZone.leftStart++;
            if (calcZone.rightStart < map2->num)
                calcZone.rightStart++;
            DEBUG("START 向终点查找 %d -- %d", calcZone.leftStart, calcZone.rightStart);
//            DEBUG("START 向终点查找 %d -- %d", calcZone.leftStart, calcZone.rightStart);
            MakeLine(&start, &map->point[calcZone.leftStart], &map2->point[calcZone.rightStart]);
        } while (IntersectionOf(start, axial) == GM_Intersection);
    }
@@ -131,7 +156,7 @@
                tempLeft = calcZone.leftEnd + 1;
            if (calcZone.rightEnd < map2->num)
                tempRight = calcZone.rightEnd + 1;
            DEBUG("END 向终点查找 %d -- %d", tempLeft, tempRight);
//            DEBUG("END 向终点查找 %d -- %d", tempLeft, tempRight);
            MakeLine(&end, &map->point[tempLeft], &map2->point[tempRight]);

            if (IntersectionOf(end, axial) == GM_Intersection) {
@@ -152,18 +177,12 @@
                calcZone.leftEnd--;
            if (calcZone.rightEnd > 0)
                calcZone.rightEnd--;
            DEBUG("END 向起点查找 %d -- %d", calcZone.leftEnd, calcZone.rightEnd);
//            DEBUG("END 向起点查找 %d -- %d", calcZone.leftEnd, calcZone.rightEnd);
            MakeLine(&end, &map->point[calcZone.leftEnd], &map2->point[calcZone.rightEnd]);
        } while (IntersectionOf(end, axial) == GM_Intersection);
    }

    DEBUG("calcZone leftStart %d leftEnd %d rightStart %d rightEnd %d", calcZone.leftStart, calcZone.leftEnd, calcZone.rightStart, calcZone.rightEnd);

    if (calcZone.leftStart <= calcZone.leftEnd || calcZone.rightStart <= calcZone.rightEnd) {
        // 离开场地
        DEBUG("离开曲线场地");
        testing = false;
    }

    if (CrashRedLine(map, map2, car, &calcZone)) {
        if (!crashRedLine) {
@@ -197,7 +216,44 @@
        }
    }

TEST_END:
    return testing ? 1 : 0;
}

static bool ExitArea(const Polygon *map, const Polygon *map2, const car_model *car)
{
// 全车都需不在地图中
    bool ret = false;

    Polygon carBody;
    Polygon bigMap;

    bigMap.num = map->num + map2->num;
    bigMap.point = (PointF *) malloc(bigMap.num * sizeof(PointF));

    int i = 0;
    for (; i < map->num; ++i) {
        bigMap.point[i] = map->point[i];
    }
    for (int j = map2->num; j > 0; --j) {
        bigMap.point[i++] = map2->point[j-1];
    }

    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, &bigMap) == GM_None) {
        ret = true;
    }

    free(carBody.point);
    free(bigMap.point);

    return ret;
}

// 车轮是否压边线
@@ -212,7 +268,7 @@
    MakeLine(&rearTireAxial, &car->carXY[car->left_rear_tire[TIRE_OUTSIDE]], &car->carXY[car->right_rear_tire[TIRE_OUTSIDE]]);

    int s = zone->leftStart;
    for (int e = zone->leftStart + 1; e < zone->leftEnd; ++e) {
    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;
@@ -224,7 +280,7 @@
    }

    s = zone->rightStart;
    for (int e = zone->rightStart + 1; e < zone->rightEnd; ++e) {
    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;