坐标

yy1717

2020-04-08 59abff0d03403344619420aa0bcd9c2b28ff3522

 lib/src/main/cpp/driver_test.cpp

@@ -18,7 +18,7 @@

#include "driver_test.h"
#include "defs.h"
#include "Geometry.h"
#include "test_common/Geometry.h"
#include "common/apptimer.h"
#include "jni_log.h"
#include "test_items/park_edge.h"
@@ -35,6 +35,7 @@
#include "mcu/mcu_if.h"
#include "test_common/car_sensor.h"
#include "test_items2/road_exam.h"
#include "test_items/area_exam.h"

#define DEBUG(fmt, args...)     LOGD("<driver_test> <%s>: " fmt, __func__, ##args)

@@ -59,29 +60,16 @@
static int ExamType;
static bool reportSeatbeltEject;

static int CarInArea = 0;
int errs = 0;

vector<int> ErrorList;
vector<ExamFault> ExamFaultList;
static int examFaultIndex = 0;

static struct map_list {
    int id;
    int type;
    Polygon map;
    Polygon map2;
} MapList[MAP_LIST_SIZE];
static LIST_AREA_MAP AreaMapList;

static Polygon RoadMapPoints;

static LIST_ROAD_MAP RoadMapList;

static int MapNum = 0;
static int CurrExamMapIndex = -1;
static int CurrEnterMapIndex = -1;

static int CurrExamStatus = EXAM_AREA_NONE;      // 1 测试完成 0 测试中 -1 测试错误退出
static int exam_dummy_light;

static car_model *CarModel = NULL;
@@ -102,7 +90,6 @@
static rtk_info *RtkBuffer = NULL;
static int RtkBufferNum = 0, RtkBufferIn = 0;

static void DetectEnterOrExitMap(void);
static void EngineStartHold(union sigval sig);
static void ExecuteExam(const struct RtkTime* rtkTime);
static void ExecuteExam(double speed, int move, double azimuth, const struct RtkTime* rtkTime);
@@ -111,19 +98,15 @@
static void UpdateCarBodyCoord(struct RtkTime *rtkTime, double azimuth, double pitch, double roll, PointF main_ant, car_model *carModel);
static bool UpdateCarCoord(double &spd, int &mov, int &idx);

static bool CrashTriggerLine(Line triggerLine, const car_model *car, LIST_CAR_MODEL &CarModelList);
static int EnterMap(const car_model *car, LIST_CAR_MODEL &CarModelList, 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 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)
{
    ExamStart = false;
    memset(&MapList, 0, sizeof(MapList));
    MapNum = 0;

    CarModel = NULL;
    CarModelList.clear();

    AreaMapList.clear();

    RoadMapPoints.num = 0;
    RoadMapPoints.point = NULL;
@@ -137,8 +120,6 @@

    RtkBuffer = (rtk_info *) malloc(RTK_BUFFER_SIZE * sizeof(rtk_info));
    RtkBufferNum = RtkBufferIn = 0;

    CurrExamStatus = EXAM_AREA_NONE;
}

static void ReadDriverExamPrimerTimeout(union sigval sig)
@@ -155,54 +136,54 @@
    MA_ReadSensor();
}

void ClearMap(void)
void ClearAreaMap(void)
{
    if (ExamStart) return;

    for (int i = 0; i < MapNum; ++i) {
        if (MapList[i].map.point != NULL)
            free(MapList[i].map.point);
        if (MapList[i].map2.point != NULL)
            free(MapList[i].map2.point);
    }
    memset(&MapList, 0, sizeof(MapList));
    MapNum = 0;
}

void AddMap(int id, int type, const double (*map)[2], int pointNum, const double (*map2)[2], int pointNum2)
{
    DEBUG("加入地图信息 id %d type %d pointNum %d point2Num %d", id, type, pointNum, pointNum2);

    AppTimer_delete(ReadDriverExamPrimerTimeout);

    if (map == NULL || pointNum == 0 || ExamStart)
    if (ExamStart)
        return;

    MapList[MapNum].id = id;
    for (int i = 0; i < AreaMapList.size(); ++i) {
        if (AreaMapList[i].map.point != NULL)
            free(AreaMapList[i].map.point);
        if (AreaMapList[i].map2.point != NULL)
            free(AreaMapList[i].map2.point);
    }

    MapList[MapNum].type = type;
    AreaMapList.clear();
}

    MapList[MapNum].map.num = pointNum;
void AddAreaMap(int id, int type, const double (*map)[2], int pointNum, const double (*map2)[2], int pointNum2)
{
    if (map == NULL || pointNum == 0 || ExamStart)
        return;
    DEBUG("加入地图信息 id %d type %d pointNum %d point2Num %d", id, type, pointNum, pointNum2);

    struct area_exam_map newMap;

    newMap.id = id;
    newMap.type = type;
    newMap.map.num = pointNum;
    newMap.map2.num = 0;
    newMap.map.point = NULL;
    newMap.map2.point = NULL;

    if (pointNum > 0) {
        MapList[MapNum].map.point = (PointF *)malloc(sizeof(PointF) * pointNum);
        newMap.map.point = (PointF *) malloc(pointNum * sizeof(PointF));
        for (int i = 0; i < pointNum; ++i) {
            MapList[MapNum].map.point[i].X = map[i][0];
            MapList[MapNum].map.point[i].Y = map[i][1];
            newMap.map.point[i].X = map[i][0];
            newMap.map.point[i].Y = map[i][1];
        }
    }

    MapList[MapNum].map2.num = pointNum2;
    if (pointNum2 > 0 && map2 != NULL) {
        MapList[MapNum].map2.point = (PointF *)malloc(sizeof(PointF) * pointNum2);
        newMap.map2.num = pointNum2;
        newMap.map2.point = (PointF *) malloc(pointNum2 * sizeof(PointF));
        for (int i = 0; i < pointNum2; ++i) {
            MapList[MapNum].map2.point[i].X = map2[i][0];
            MapList[MapNum].map2.point[i].Y = map2[i][1];
            newMap.map2.point[i].X = map2[i][0];
            newMap.map2.point[i].Y = map2[i][1];
        }
    }

    MapNum++;

    DEBUG("AddMap num %d", MapNum);
    AreaMapList.push_back(newMap);
}

void CleanRoadMap(void)
@@ -550,14 +531,15 @@
        DEBUG("结束考试");

        TerminateRoadExam();
        TerminateAreaExam();

        CurrExamMapIndex = -1;

        ExamStart = false;
        MA_SendExamStatus(0, 0);
        return;
    }

    if (MapNum == 0 && type == TEST_TYPE_AREA) {
    if (AreaMapList.size() == 0 && type == TEST_TYPE_AREA) {
        DEBUG("没有场考地图");
        err = true;
        MA_SendExamStatus(0, -1);
@@ -590,21 +572,12 @@

            if (type == TEST_TYPE_ROAD_DUMMY_LIGHT) {
                exam_dummy_light = 0;

//                InitRoadExam();         ////////////////////
            }
            if (type == TEST_TYPE_AREA) {
                InitAreaExam();
            }
        }
        MA_SendExamStatus(1, 0);
    }
}

void StartMapExam(int map_id, int exam)
{
    DEBUG("测试该场地 %d: %d", map_id, exam);

    if (map_id >= 0 && exam == 0) {
        CurrExamMapIndex = map_id;
        CurrExamStatus = EXAM_AREA_START;
    }
}

@@ -692,35 +665,8 @@
        rtkTime.ss = RtkBuffer[index].ss;
        rtkTime.mss = RtkBuffer[index].dss;

        DetectEnterOrExitMap();

        if (ExamStart) {
            ExecuteExam(speed, move, azimuth, &rtkTime);
        }
    }
}

static void DetectEnterOrExitMap(void)
{
    if (ExamType == TEST_TYPE_AREA) {

    }
    if (CurrExamMapIndex < 0) {
        if (CurrEnterMapIndex < 0) {
            CurrEnterMapIndex = EnterMap(CarModel, CarModelList, MapList, MapNum);
            if (CurrEnterMapIndex >= 0) {
//                DEBUG("发送进入场地报告 %d", GetMapId(CurrEnterMapIndex, MapList, MapNum));
//                MA_EnterMap(GetMapId(CurrEnterMapIndex, MapList, MapNum), GetMapType(CurrEnterMapIndex, MapList, MapNum), 1);

                CurrExamMapIndex = CurrEnterMapIndex;
                CurrExamStatus = EXAM_AREA_START;
            }
        } else {
            if (ExitMap(CarModel, CurrEnterMapIndex, MapList, MapNum)) {
//                DEBUG("发送离开场地报告 %d", GetMapId(CurrEnterMapIndex, MapList, MapNum));
//                MA_EnterMap(GetMapId(CurrEnterMapIndex, MapList, MapNum), GetMapType(CurrEnterMapIndex, MapList, MapNum), 0);
                CurrEnterMapIndex = -1;
            }
        }
    }
}
@@ -875,87 +821,8 @@
        if (exam_dummy_light == 2) {
            TestRoadGeneral(RoadMapList, CarModel, CarModelList, speed, move, rtkTime);
        }
        return;
    }

    if (CurrExamMapIndex >= 0) {
        int mtype = GetMapType(CurrExamMapIndex, MapList, MapNum);

        if (CurrExamStatus == EXAM_AREA_START) {
            DEBUG("CurrExamMapIndex %d mtype %d", GetMapId(CurrExamMapIndex, MapList, MapNum), mtype);

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

                    StartParkBottom(GetMapId(CurrExamMapIndex, MapList, MapNum), move, rtkTime);
                    CurrExamStatus = EXAM_AREA_RUN;
                    break;
                case MAP_TYPE_STOP_START:
                    DEBUG("进入上坡起步场地");
                    MA_SendDebugInfo("进入上坡起步场地 %d", GetMapId(CurrExamMapIndex, MapList, MapNum));
                    StartSAS(GetMapId(CurrExamMapIndex, MapList, MapNum), move, rtkTime);
                    CurrExamStatus = EXAM_AREA_RUN;
                    break;
                case MAP_TYPE_PART_EDGE:
                    DEBUG("进入侧方位停车场地");
                    MA_SendDebugInfo("进入侧方位停车场地 %d", GetMapId(CurrExamMapIndex, MapList, MapNum));
                    StartParkEdge(GetMapId(CurrExamMapIndex, MapList, MapNum), move, rtkTime);

                    CurrExamStatus = EXAM_AREA_RUN;
                    break;
                case MAP_TYPE_CURVE:
                    DEBUG("进入曲线行驶场地");
                    MA_SendDebugInfo("进入曲线行驶场地 %d", GetMapId(CurrExamMapIndex, MapList, MapNum));
                    StartDrivingCurve(GetMapId(CurrExamMapIndex, MapList, MapNum), move, rtkTime);

                    CurrExamStatus = EXAM_AREA_RUN;
                    break;
                case MAP_TYPE_TURN_90:
                    DEBUG("进入直角转弯场地");
                    MA_SendDebugInfo("进入直角转弯场地 %d", GetMapId(CurrExamMapIndex, MapList, MapNum));
                    StartTurnA90(GetMapId(CurrExamMapIndex, MapList, MapNum), move, azimuth, rtkTime);
                    CurrExamStatus = EXAM_AREA_RUN;
                    break;
                default:break;
            }
        } else if (CurrExamStatus == EXAM_AREA_RUN) {
            int testing = 0;
            switch (mtype) {
                case MAP_TYPE_PARK_BUTTOM:
                    testing = TestParkBottom(&MapList[CurrExamMapIndex].map,
                                             CarModel, NULL, speed, move, rtkTime);
                    break;
                case MAP_TYPE_STOP_START:
                    testing = TestSAS(&MapList[CurrExamMapIndex].map, CarModel, NULL, speed, move, rtkTime);
                    break;
                case MAP_TYPE_PART_EDGE:
                    testing = TestParkEdge(&MapList[CurrExamMapIndex].map, CarModel, NULL, speed, move, rtkTime);
                    break;
                case MAP_TYPE_CURVE:
                    testing = TestDrivingCurve(&MapList[CurrExamMapIndex].map, &MapList[CurrExamMapIndex].map2, CarModel, NULL, speed, move, rtkTime);
                    break;
                case MAP_TYPE_TURN_90:
                    testing = TestTurnA90(&MapList[CurrExamMapIndex].map, CarModel, NULL, azimuth, speed, move, rtkTime);
                    break;
                default:
                    break;
            }

            if (testing > 0) {
                CurrExamStatus = EXAM_AREA_RUN;
            } else {
                CurrExamStatus = EXAM_AREA_END;
            }
        }
        if (CurrExamStatus != EXAM_AREA_RUN) {
            // 某项结束
            //DEBUG("退出场地 %d", GetMapId(CurrExamMapIndex, MapList, MapNum));
            //MA_SendDebugInfo("退出场地 %d", GetMapId(CurrExamMapIndex, MapList, MapNum));
            CurrExamStatus = EXAM_AREA_NONE;
            CurrExamMapIndex = -1;
        }
    } else {
        TestAreaGeneral(AreaMapList, CarModel, CarModelList, speed, move, azimuth, rtkTime);
    }
}

@@ -1140,225 +1007,6 @@
    return true;
}

static bool CrashTriggerLine(Line triggerLine, const car_model *car, LIST_CAR_MODEL &CarModelList)
{
    bool trigger = false;

    if (CarModelList.size() < 5)
        return trigger;

    Polygon trace, trace2;
    int pn = 0;

    trace2.num = trace.num = 5;
    trace.point = (PointF *) malloc(sizeof(PointF) * trace.num);
    trace2.point = (PointF *) malloc(sizeof(PointF) * trace2.num);

    list<car_model *>::iterator iter = CarModelList.begin();

    car_model *c1 = *iter;

    trace.point[pn] = c1->carXY[c1->left_front_tire[TIRE_OUTSIDE]];
    trace2.point[pn++] = c1->carXY[c1->left_rear_tire[TIRE_OUTSIDE]];

    ++iter;

    while (iter != CarModelList.end() && pn < trace.num) {
        car_model *c2 = *iter;

        uint32_t tdiff = TimeGetDiff(c1->tm.hh, c1->tm.mm, c1->tm.ss, c1->tm.mss * 10, c2->tm.hh, c2->tm.mm, c2->tm.ss, c2->tm.mss*10);

        if (tdiff >= D_SEC(1)) {
            trace.point[pn] = c2->carXY[c2->left_front_tire[TIRE_OUTSIDE]];
            trace2.point[pn++] = c2->carXY[c2->left_rear_tire[TIRE_OUTSIDE]];
            c1 = c2;
        }
        ++iter;
    }

    PointF p1, p2;

    p1.X = triggerLine.X1;
    p1.Y = triggerLine.Y1;
    p2.X = triggerLine.X2;
    p2.Y = triggerLine.Y2;

    int pp = 0;
    for (int p = 1; p < pn; ++p) {
        Line trace_line, trace2_line;

        MakeLine(&trace_line, &trace.point[pp], &trace.point[p]);
        MakeLine(&trace2_line, &trace2.point[pp], &trace2.point[p]);

        if ((IntersectionOf(trace_line, triggerLine) == GM_Intersection || IntersectionOf(trace2_line, triggerLine) == GM_Intersection) &&
            IntersectionOfLine(p1, p2, car->carXY[car->left_front_tire[TIRE_OUTSIDE]]) == -1 &&
            DistanceOf(car->carXY[car->left_front_tire[TIRE_OUTSIDE]], triggerLine) > 0.1) {
            // 碰到触发线
            DEBUG("碰撞触发线 引发地图");
            trigger = true;
            goto SEARCH_TRIGGER_LINE_END;
        }
    }

SEARCH_TRIGGER_LINE_END:
    free(trace.point);

    return trigger;
}

static int EnterMap(const car_model *car, LIST_CAR_MODEL &CarModelList, const struct map_list *mapList, int mapNum)
{
    for (int i = 0; i < mapNum && car != NULL; ++i) {
        // 车前轮或后轮轨迹越过触发线
        if (mapList[i].type == MAP_TYPE_STOP_START) {
            // 构造虚拟的左上角点
            double x9, y9, xo, yo;

            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;

            Line triggerLine;

            triggerLine.X1 = mapList[i].map.point[0].X;
            triggerLine.Y1 = mapList[i].map.point[0].Y;
            triggerLine.X2 = x9;
            triggerLine.Y2 = y9;

            if (CrashTriggerLine(triggerLine, car, CarModelList))
                return i;
        }
        if (mapList[i].type == MAP_TYPE_PARK_BUTTOM) {
            // 车头顶点在场地内
            if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &mapList[i].map) == GM_Containment) {
                Line enterLine1, enterLine2;

                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 (DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], enterLine1) > 0.1 &&
                    DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], enterLine2) > 0.1)
                    return i;
            }
        }
        if (mapList[i].type == MAP_TYPE_PART_EDGE) {
            // 车头顶点在场地内
            if (IntersectionOf(car->carXY[ car->axial[AXIAL_FRONT] ], &mapList[i].map) == GM_Containment) {
                Line enterLine;

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

                if (DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], enterLine) > 0.1)
                    return i;
            }
        }
        if (mapList[i].type == MAP_TYPE_TURN_90) {
            // 车前轮或后轮轨迹越过触发线
            Line triggerLine;

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

            if (CrashTriggerLine(triggerLine, car, CarModelList))
                return i;
        }
        if (mapList[i].type == MAP_TYPE_CURVE) {
            Line triggerLine;

            MakeLine(&triggerLine, &mapList[i].map2.point[0], &mapList[i].map.point[0]);
            if (CrashTriggerLine(triggerLine, car, CarModelList))
                return i;
        }
    }
    return -1;
}

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 ||
        mapList[index].type == MAP_TYPE_PART_EDGE ||
        mapList[index].type == MAP_TYPE_TURN_90) {
        // 全车都需不在地图中
        Polygon carBody;

        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);
    }
    if (mapList[index].type == MAP_TYPE_STOP_START) {
            // 构造虚拟的左上角点
            double x9, y9, xo, yo;

            bool enter = false;

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

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

            Polygon map;

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

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

            // 全车都需不在地图中
            Polygon carBody;

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

            free(carBody.point);
            free(map.point);
    }
    if (mapList[index].type == MAP_TYPE_CURVE) {
        ret = ExitDrivingCurveArea(&mapList[index].map, &mapList[index].map2, car);
    }

    return ret;
}

static int GetMapId(int index, const struct map_list *mapList, int mapNum)
{
    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)
{
    struct ExamFault fault;
@@ -1383,11 +1031,6 @@
    MA_SendExamWrong(ExamFaultList);

    ExamFaultList.clear();
}

car_model_cache_t *GetCarModelCache(int node)
{
    return NULL;
}

/*******************************************************************