updatecrrDOC1

yy1717

2020-01-21 7c66884c800b0b48c2b3aac2ac29ff0b4529b95e

 lib/src/main/cpp/driver_test.cpp

@@ -74,7 +74,7 @@
} MapList[MAP_LIST_SIZE];

static int MapNum = 0;
static int CurrExamMapId = -1;
static int CurrExamMapIndex = -1;
static int CurrExamStatus = 0;      // 1 测试完成 0 测试中 -1 测试错误退出

static car_model *CarModel = NULL;
@@ -94,7 +94,8 @@
static bool FrontTireEnterArea(const Polygon *car, const Polygon *map);
static int EnterMap(const car_model *car, const struct map_list *mapList, int mapNum);
static bool ExitMap(const car_model *car, int mapId, const struct map_list *mapList, int mapNum);
static int GetMapType(int id, const struct map_list *mapList, int mapNum);
static int GetMapId(int index, const struct map_list *mapList, int mapNum);
static int GetMapType(int index, const struct map_list *mapList, int mapNum);

void DriverTestInit(void)
{
@@ -201,6 +202,7 @@
        CarModelPrev = NULL;
    }

    // 仅分配carXY, body和carDesc均没有
    CarModelPrev = (car_model *)malloc(sizeof(car_model));
    CarModelPrev->body = NULL;
    CarModelPrev->carDesc = NULL;
@@ -337,7 +339,7 @@
                RtkBuffer[index].mm, RtkBuffer[index].ss, RtkBuffer[index].dss);

        brief.qf = RtkBuffer[index].qf;
        brief.map_id = 863;//CurrExamMapId;
        brief.map_id = 863;//GetMapId(CurrExamMapIndex, MapList, MapNum);
        brief.move = move;
        brief.speed = speed * 3.6;
        brief.heading = RtkBuffer[index].heading;
@@ -374,16 +376,16 @@
        free(brief.point);

        if (TestStart) {
            if (CurrExamMapId < 0) {
                CurrExamMapId = EnterMap(CarModel, MapList, MapNum);
                int mtype = GetMapType(CurrExamMapId, MapList, MapNum);
            if (CurrExamMapIndex < 0) {
                CurrExamMapIndex = EnterMap(CarModel, MapList, MapNum);
                int mtype = GetMapType(CurrExamMapIndex, MapList, MapNum);

                DEBUG("CurrExamMapId %d mtype %d", CurrExamMapId, mtype);
                DEBUG("CurrExamMapIndex %d mtype %d", GetMapId(CurrExamMapIndex, MapList, MapNum), mtype);

                switch (mtype) {
                    case MAP_TYPE_PARK_BUTTOM:
                        DEBUG("进入倒车入库场地");
                        MA_SendDebugInfo("进入倒车入库场地 %d", CurrExamMapId);
                        MA_SendDebugInfo("进入倒车入库场地 %d", GetMapId(CurrExamMapIndex, MapList, MapNum));

                        StartParkBottom();
                        CurrExamStatus = 0;
@@ -411,12 +413,12 @@
        rtkTime.ss = RtkBuffer[index].ss;
        rtkTime.mss = RtkBuffer[index].dss;

        if (CurrExamMapId >= 0) {
        if (CurrExamMapIndex >= 0) {
            if (CurrExamStatus == 0) {
                int mtype = GetMapType(CurrExamMapId, MapList, MapNum);
                int mtype = GetMapType(CurrExamMapIndex, MapList, MapNum);
                switch (mtype) {
                    case MAP_TYPE_PARK_BUTTOM:
                        CurrExamStatus = TestParkBottom(ErrorList, &MapList[CurrExamMapId].map,
                        CurrExamStatus = TestParkBottom(ErrorList, &MapList[CurrExamMapIndex].map,
                                                        CarModel, CarModelPrev, speed, move, &rtkTime);
                        break;
                    case MAP_TYPE_STOP_START:
@@ -432,10 +434,10 @@
                }
            }
            if (CurrExamStatus != 0) {
                if (ExitMap(CarModel, CurrExamMapId, MapList, MapNum)) {
                    DEBUG("退出场地 %d", CurrExamMapId);
                    MA_SendDebugInfo("退出场地 %d", CurrExamMapId);
                    CurrExamMapId = -1;
                if (ExitMap(CarModel, CurrExamMapIndex, MapList, MapNum)) {
                    DEBUG("退出场地 %d", GetMapId(CurrExamMapIndex, MapList, MapNum));
                    MA_SendDebugInfo("退出场地 %d", GetMapId(CurrExamMapIndex, MapList, MapNum));
                    CurrExamMapIndex = -1;
                }
            }
        }
@@ -450,8 +452,8 @@
 */
static uint32_t CalcTimeDiff(const rtk_info *a, const rtk_info *b)
{
    return ABS(TimeMakeComposite(2000 + a->YY, a->MM, a->DD, a->hh, a->mm, a->ss) * 1000 + a->dss*10 -
            (TimeMakeComposite(2000 + b->YY, b->MM, b->DD, b->hh, b->mm, b->ss) * 1000 + b->dss*10));
    return ABS((long)(TimeMakeComposite(2000 + a->YY, a->MM, a->DD, a->hh, a->mm, a->ss) * 1000 + a->dss*10) -
               (long)(TimeMakeComposite(2000 + b->YY, b->MM, b->DD, b->hh, b->mm, b->ss) * 1000 + b->dss*10));
}

static bool UpdateCarCoord(double &spd, int &mov, int &idx)
@@ -663,42 +665,48 @@

                if (DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], enterLine1) > 0.1 &&
                    DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], enterLine2) > 0.1)
                    return mapList[i].id;
                    return i;
            }
        }
    }
    return -1;
}

static bool ExitMap(const car_model *car, int mapId, const struct map_list *mapList, int mapNum)
static bool ExitMap(const car_model *car, int index, const struct map_list *mapList, int mapNum)
{
    // 车的最后点是否进入地图
    for (int i = 0; i < mapNum && car != NULL; ++i) {
        if (mapList[i].id == mapId) {
            if (mapList[i].type == MAP_TYPE_PARK_BUTTOM) {
                if (IntersectionOf(car->carXY[ car->axial[AXIAL_REAR] ], &mapList[i].map) == GM_None) {
                    Line enterLine1, enterLine2;
    if (index < 0 || mapList == NULL || mapNum == 0) return true;

                    MakeLine(&enterLine1, &(mapList[i].map.point[0]), &(mapList[i].map.point[1]));
                    MakeLine(&enterLine2, &(mapList[i].map.point[6]), &(mapList[i].map.point[7]));
    if (mapList[index].type == MAP_TYPE_PARK_BUTTOM) {
        if (IntersectionOf(car->carXY[car->axial[AXIAL_REAR]], &mapList[index].map) == GM_None) {
            Line enterLine1, enterLine2;

                    if (DistanceOf(car->carXY[car->axial[AXIAL_REAR]], enterLine1) > 0.1 &&
                        DistanceOf(car->carXY[car->axial[AXIAL_REAR]], enterLine2) > 0.1)
                        return true;
                }
            }
            return false;
            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;
        }
    }

    return true;
}

static int GetMapType(int id, const struct map_list *mapList, int mapNum)
static int GetMapId(int index, const struct map_list *mapList, int mapNum)
{
    for (int i = 0; i < mapNum && id >= 0; ++i) {
        if (mapList[i].id == id) return mapList[i].type;
    }
    return -1;
    if (index < 0 || mapList == NULL || mapNum == 0)
        return -1;

    return mapList[index].id;
}

static int GetMapType(int index, const struct map_list *mapList, int mapNum)
{
    if (index < 0 || mapList == NULL || mapNum == 0)
        return -1;

    return mapList[index].type;
}

void AddExamFault(int wrong, const struct RtkTime *rtkTime)