上坡地图修正

fctom1215

2020-02-19 dc2a01d4764efd33a23afcaf4f1d7543dc35c4fa

 lib/src/main/cpp/driver_test.cpp

@@ -26,6 +26,7 @@
#include "test_items/stop_and_start.h"
#include "master/comm_if.h"
#include "utils/xconvert.h"
#include "test_items/comm_test.h"

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

@@ -87,9 +88,14 @@
} SensorConfig[32];
static int SensorNum = 0;

static int SensorValidLevel;

#define MOV_AVG_SIZE                1
#define RTK_BUFFER_SIZE            100
#define CAR_MODEL_CACHE_SIZE      10

static pthread_mutex_t rtk_clock_mutex = PTHREAD_MUTEX_INITIALIZER;
struct RtkTime rtkClock;

static rtk_info *RtkBuffer = NULL;
static int RtkBufferNum = 0, RtkBufferIn = 0;
@@ -106,14 +112,20 @@

void DriverTestInit(void)
{
    pthread_mutex_init(&rtk_clock_mutex, NULL);
    memset(&rtkClock, 0, sizeof(rtkClock));

    TestStart = false;
    memset(&MapList, 0, sizeof(MapList));
    MapNum = 0;
    CarModel = NULL;
    CarModelPrev = NULL;

    CommTestInit();
    SensorNum = 0;
    memset(SensorConfig, 0, sizeof(SensorConfig));

    SensorValidLevel = 0;

    RtkBuffer = (rtk_info *) malloc(RTK_BUFFER_SIZE * sizeof(rtk_info));
    RtkBufferNum = RtkBufferIn = 0;
@@ -299,22 +311,43 @@

void SetSensorCfg(int (*sensor)[3], int sensorNum)
{
    DEBUG("SetSensorCfg sensorNum %d", sensorNum);
    DEBUG("加入传感器配置 sensorNum %d", sensorNum);
    SensorValidLevel = 0;

    SensorNum = sensorNum;
    for (int i = 0; i < sensorNum; ++i) {
        SensorConfig[i].gpioId = sensor[i][0];
        SensorConfig[i].funId = sensor[i][1];
        SensorConfig[i].validLvl = sensor[i][2];

        if (sensor[i][2] != 0) {
            SensorValidLevel |= BV(i);
        }
    }
}

void GetFuncGpio(int func, int &gpio, int &lvl)
int GetSensorValidLevel(void)
{
    return SensorValidLevel;
}

void GetSensorCfg(int gpio, int &func, bool &lvl)
{
    for (int i = 0; i < SensorNum; ++i) {
        if (SensorConfig[i].gpioId == gpio) {
            func = SensorConfig[i].funId;
            lvl = SensorConfig[i].validLvl == 0 ? false : true;
        }
    }
}

void FindSensorCfg(int func, int &gpio, bool &lvl)
{
    gpio = -1;
    for (int i = 0; i < SensorNum; ++i) {
        if (SensorConfig[i].funId == func) {
            gpio = SensorConfig[i].gpioId;
            lvl = SensorConfig[i].validLvl;
            lvl = SensorConfig[i].validLvl == 0 ? false : true;
        }
    }
}
@@ -326,6 +359,7 @@
    DEBUG("StartDriverExam %d", start);
    if (start == 0) {
        TestStart = false;
        CommTestStart(false);
        MA_SendExamStatus(0, 0);
        return;
    }
@@ -344,13 +378,31 @@
            examFaultIndex = 0;

            TestStart = true;
            CommTestStart(true);
        }
        MA_SendExamStatus(1, 0);
    }
}

void GetRtkClock(struct RtkTime *s)
{
    pthread_mutex_lock(&rtk_clock_mutex);
    *s = rtkClock;
    pthread_mutex_unlock(&rtk_clock_mutex);
}

void UpdateRTKInfo(const rtk_info *s)
{
    pthread_mutex_lock(&rtk_clock_mutex);
    rtkClock.YY = s->YY;
    rtkClock.MM = s->MM;
    rtkClock.DD = s->DD;
    rtkClock.hh = s->hh;
    rtkClock.mm = s->mm;
    rtkClock.ss = s->ss;
    rtkClock.mss = s->dss;
    pthread_mutex_unlock(&rtk_clock_mutex);

    if (s->qf == 3) {
        RtkBuffer[RtkBufferIn] = *s;
        RtkBufferIn = (RtkBufferIn + 1) % RTK_BUFFER_SIZE;
@@ -372,7 +424,7 @@
                RtkBuffer[index].mm, RtkBuffer[index].ss, RtkBuffer[index].dss);

        brief.qf = RtkBuffer[index].qf;
        brief.map_id = 866;//GetMapId(CurrExamMapIndex, MapList, MapNum);
        brief.map_id = 863;//GetMapId(CurrExamMapIndex, MapList, MapNum);
        brief.move = move;
        brief.speed = speed * 3.6;
        brief.heading = RtkBuffer[index].heading;
@@ -431,7 +483,7 @@
                        DEBUG("进入倒车入库场地");
                        MA_SendDebugInfo("进入倒车入库场地 %d", GetMapId(CurrExamMapIndex, MapList, MapNum));

                        StartParkBottom();
                        StartParkBottom(move, &rtkTime);
                        CurrExamStatus = 0;
                        break;
                    case MAP_TYPE_STOP_START:
@@ -465,7 +517,7 @@
                int mtype = GetMapType(CurrExamMapIndex, MapList, MapNum);
                switch (mtype) {
                    case MAP_TYPE_PARK_BUTTOM:
                        CurrExamStatus = TestParkBottom(ErrorList, &MapList[CurrExamMapIndex].map,
                        CurrExamStatus = TestParkBottom(&MapList[CurrExamMapIndex].map,
                                                        CarModel, CarModelPrev, speed, move, &rtkTime);
                        break;
                    case MAP_TYPE_STOP_START:
@@ -712,44 +764,46 @@
{
    // 车的最前点是否进入地图
    for (int i = 0; i < mapNum && car != NULL; ++i) {
        /*if (mapList[i].type == MAP_TYPE_STOP_START) {
            // 构造虚拟的左上角点
            double x9, y9, xo, yo;

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

@@ -760,16 +814,17 @@
                    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;
            }
        }
//        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;
}