修改了上坡起步。

fctom1215

2020-02-12 7b6dd5603277ac8fddba735c456f99d4c07e2e9b

 lib/src/main/cpp/driver_test.cpp

@@ -148,8 +148,6 @@

    MapList[MapNum].id = id;

    if (type == 1) type = 3;            //////////////////////////////////////

    MapList[MapNum].type = type;

    MapList[MapNum].map.num = pointNum;
@@ -347,7 +345,7 @@
                RtkBuffer[index].mm, RtkBuffer[index].ss, RtkBuffer[index].dss);

        brief.qf = RtkBuffer[index].qf;
        brief.map_id = 863;//GetMapId(CurrExamMapIndex, MapList, MapNum);
        brief.map_id = 865;//GetMapId(CurrExamMapIndex, MapList, MapNum);
        brief.move = move;
        brief.speed = speed * 3.6;
        brief.heading = RtkBuffer[index].heading;
@@ -384,6 +382,7 @@
        free(brief.point);

        struct RtkTime rtkTime;
        double azimuth = RtkBuffer[index].heading;

        rtkTime.YY = RtkBuffer[index].YY;
        rtkTime.MM = RtkBuffer[index].MM;
@@ -409,6 +408,10 @@
                        CurrExamStatus = 0;
                        break;
                    case MAP_TYPE_STOP_START:
                        DEBUG("进入上坡起步场地");
                        MA_SendDebugInfo("进入上坡起步场地 %d", GetMapId(CurrExamMapIndex, MapList, MapNum));
                        StartSAS(move, &rtkTime);
                        CurrExamStatus = 0;
                        break;
                    case MAP_TYPE_PART_EDGE:
                        DEBUG("进入侧方位停车场地");
@@ -420,6 +423,10 @@
                    case MAP_TYPE_CURVE:
                        break;
                    case MAP_TYPE_TURN_90:
                        DEBUG("进入直角转弯场地");
                        MA_SendDebugInfo("进入直角转弯场地 %d", GetMapId(CurrExamMapIndex, MapList, MapNum));
                        StartTurnA90(move, azimuth, &rtkTime);
                        CurrExamStatus = 0;
                        break;
                    default:break;
                }
@@ -435,6 +442,7 @@
                                                        CarModel, CarModelPrev, speed, move, &rtkTime);
                        break;
                    case MAP_TYPE_STOP_START:
                        CurrExamStatus = TestSAS(&MapList[CurrExamMapIndex].map, CarModel, CarModelPrev, speed, move, &rtkTime);
                        break;
                    case MAP_TYPE_PART_EDGE:
                        CurrExamStatus = TestParkEdge(&MapList[CurrExamMapIndex].map, CarModel, CarModelPrev, speed, move, &rtkTime);
@@ -442,6 +450,7 @@
                    case MAP_TYPE_CURVE:
                        break;
                    case MAP_TYPE_TURN_90:
                        CurrExamStatus = TestTurnA90(&MapList[CurrExamMapIndex].map, CarModel, CarModelPrev, azimuth, speed, move, &rtkTime);
                        break;
                    default:
                        break;
@@ -678,8 +687,41 @@
    // 车的最前点是否进入地图
    for (int i = 0; i < mapNum && car != NULL; ++i) {
        if (mapList[i].type == MAP_TYPE_STOP_START) {
            // 构造虚拟的左上角点
            double x9, y9, xo, yo;

        } else if (mapList[i].type == MAP_TYPE_CURVE) {
            bool enter = false;

            xo = (mapList[i].map.point[0].X + mapList[i].map.point[7].X) / 2;
            yo = (mapList[i].map.point[0].Y + mapList[i].map.point[7].Y) / 2;

            x9 = 2*xo - mapList[i].map.point[8].X;
            y9 = 2*yo - mapList[i].map.point[8].Y;

            Polygon map;

            map.num = 4;
            map.point = (PointF *) malloc(map.num * sizeof(PointF));

            map.point[0] = mapList[i].map.point[0];
            map.point[1] = mapList[i].map.point[8];
            map.point[2] = mapList[i].map.point[7];
            map.point[3].X = x9;
            map.point[3].Y = y9;

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

                MakeLine(&enterLine1, &(map.point[0]), &(map.point[3]));

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

            free(map.point);

            if (enter) return i;
        } /*else if (mapList[i].type == MAP_TYPE_CURVE) {

        } else if (mapList[i].type == MAP_TYPE_PARK_BUTTOM || mapList[i].type == MAP_TYPE_PART_EDGE) {
            if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &mapList[i].map) == GM_Containment) {
@@ -692,7 +734,16 @@
                    DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], enterLine2) > 0.1)
                    return i;
            }
        }
        } else if (mapList[i].type == MAP_TYPE_TURN_90) {
            if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &mapList[i].map) == GM_Containment) {
                Line enterLine1;

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

                if (DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], enterLine1) > 0.1)
                    return i;
            }
        }*/
    }
    return -1;
}