updatecrrDOC1

yy1717

2020-01-22 eb645b6bf08ec50459c737821e63ee20dea4486d

updatecrrDOC1

 lib/src/main/cpp/driver_test.cpp

@@ -296,6 +296,11 @@
    bool err = false;

    DEBUG("StartDriverExam %d", start);
    if (start == 0) {
        TestStart = false;
        MA_SendExamStatus(0, 0);
        return;
    }

    if (MapNum == 0) {
        err = true;
@@ -318,7 +323,7 @@

void UpdateRTKInfo(const rtk_info *s)
{
    if (s->qf > 0) {
    if (s->qf == 3) {
        RtkBuffer[RtkBufferIn] = *s;
        RtkBufferIn = (RtkBufferIn + 1) % RTK_BUFFER_SIZE;
        if (RtkBufferNum < RTK_BUFFER_SIZE)
@@ -434,6 +439,7 @@
                }
            }
            if (CurrExamStatus != 0) {
                // 某项结束，
                if (ExitMap(CarModel, CurrExamMapIndex, MapList, MapNum)) {
                    DEBUG("退出场地 %d", GetMapId(CurrExamMapIndex, MapList, MapNum));
                    MA_SendDebugInfo("退出场地 %d", GetMapId(CurrExamMapIndex, MapList, MapNum));
@@ -458,18 +464,28 @@

static bool UpdateCarCoord(double &spd, int &mov, int &idx)
{
    long tmDiff;

    if (CarModel == NULL)
        return false;

    if (RtkBufferNum < 3)
    if (RtkBufferNum < 2)
        return false;

    int p1 = ((RtkBufferIn-1)+RTK_BUFFER_SIZE)%RTK_BUFFER_SIZE;
    int p2 = ((RtkBufferIn-2)+RTK_BUFFER_SIZE)%RTK_BUFFER_SIZE;
    int p3 = ((RtkBufferIn-3)+RTK_BUFFER_SIZE)%RTK_BUFFER_SIZE;
    int p1 = ((RtkBufferIn-1)+RTK_BUFFER_SIZE)%RTK_BUFFER_SIZE;               // 最近采样值
    int p2 = ((RtkBufferIn-2)+RTK_BUFFER_SIZE)%RTK_BUFFER_SIZE;               // 找到1秒前的采样值
    int pn = 0;

    for (pn = 1; pn < RtkBufferNum; ++pn) {
        p2 = ((RtkBufferIn-1-pn)+RTK_BUFFER_SIZE)%RTK_BUFFER_SIZE;
        if ((tmDiff = CalcTimeDiff(&RtkBuffer[p1], &RtkBuffer[p2])) >= D_SEC(1)) break;
    }

    if (pn == RtkBufferNum)
        return false;

    // 如果一定的时间都没有有效定位，删除之前的值
    uint32_t tmDiff = CalcTimeDiff(&RtkBuffer[p1], &RtkBuffer[p2]);
    /*tmDiff = CalcTimeDiff(&RtkBuffer[p1], &RtkBuffer[p2]);

    DEBUG("tmDiff = %ld, p1 = %d p2 = %d dss = %d dss2 = %d", tmDiff, p1, p2, RtkBuffer[p1].dss, RtkBuffer[p2].dss);

@@ -478,7 +494,7 @@
            RtkBuffer[0] = RtkBuffer[p1];
        RtkBufferIn = RtkBufferNum = 1;
        return false;
    }
    }*/

    // 计算车辆轮廓点
    PointF main_ant_coord;
@@ -562,7 +578,7 @@
    spd = speed;
    mov = move;
    idx = p1;
    DEBUG("speed = %f m/Sec move = %d", speed, move);
    DEBUG("tmDiff = %ld speed = %f m/Sec move = %d", tmDiff, speed, move);

    return true;
/*        if (!TestStart) return;
@@ -674,23 +690,27 @@

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) {
        if (IntersectionOf(car->carXY[car->axial[AXIAL_REAR]], &mapList[index].map) == GM_None) {
            Line enterLine1, enterLine2;
        // 全车都需不在地图中
        Polygon carBody;

            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;
        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);
    }

    return true;
    return ret;
}

static int GetMapId(int index, const struct map_list *mapList, int mapNum)

 lib/src/main/cpp/rtk_platform/platform.cpp

@@ -53,6 +53,13 @@
    uint32_t downloadRtk : 1;
} platformStatus;

static struct event_queue_t {
    uint32_t event;
    int length;
    void *data;
    struct event_queue_t *next;
} *eventQueue;

struct platformSocket exceptSocket, currSocket;
static uint32_t eventMask;
static void *eventData[32];
@@ -82,6 +89,24 @@
static void TriggerHeartbeat(union sigval sig);
static void RequestRtkNoResp(union sigval sig);

void AddEvnet(uint32_t event, const uint8_t *data, int length)
{
    struct event_queue_t *p = eventQueue;

    while (p != NULL) {
        struct event_queue_t *nw = (struct event_queue_t *)malloc(sizeof(struct event_queue_t));

        nw->event = event;
        nw->next = NULL;
        nw->data = NULL;
        nw->length = 0;
        if (data != NULL && length > 0) {
            nw->data = malloc(length);
            nw->length = length;
        }
    }
}

void InitPlatform(const uint8_t *phone, const char *domain_name, int port)
{
    DEBUG("InitPlatform");
@@ -89,6 +114,8 @@
    pthread_mutex_init(&platform_tx_mutex, NULL);
    pthread_mutex_init(&events_mutex, NULL);

    eventQueue = NULL;

    eventMask = 0;
    memset(eventData, 0, sizeof(eventData));
    memset(eventDataLength, 0, sizeof(eventDataLength));

 lib/src/main/cpp/test_items/park_bottom.cpp

@@ -83,6 +83,7 @@
        // 是否超时
        if (parkTimeout) {
            // 不合格：动作超时
            DEBUG("不合格：动作超时");
            err.push_back(10);
            AddExamFault(10, rtkTime);
            status = -1;
@@ -90,6 +91,7 @@
        // 是否压线
        if (CrashRedLine(map, car)) {
            // 不合格：车身出线
            DEBUG("不合格：车身出线");
            err.push_back(7);
            AddExamFault(7, rtkTime);
            status = -1;
@@ -110,6 +112,7 @@
        if (run_status > 0) {
            if (stopCar2S && currTarget != FIRST_TOUCH_CTRL_LINE) {
                // 扣5分：中途停车超过2秒
                DEBUG("扣5分：中途停车超过2秒");
                err.push_back(11);
                AddExamFault(11, rtkTime);
            }
@@ -127,6 +130,7 @@
                        DEBUG("来道场玩的");
                    } else {
                        // 不合格：未按规定线路行驶（直接跑出测试区了）
                        DEBUG("不合格：未按规定线路行驶（直接跑出测试区了）");
                        err.push_back(6);
                        AddExamFault(6, rtkTime);
                        status = -1;
@@ -166,6 +170,7 @@
                    status = 1;
                } else if (the_ctrl_line_crossed != 0) {
                    // 不合格：未按规定线路行驶（未回到起始点）
                    DEBUG("不合格：未按规定线路行驶（未回到起始点）");
                    err.push_back(6);
                    AddExamFault(6, rtkTime);
                    status = -1;
@@ -187,6 +192,7 @@

            if (first_ctrl_line_id > 0 && first_ctrl_line_id == the_ctrl_line_crossed) {
                // 不合格：未按规定线路行驶（试图做2次同方向的倒库）
                DEBUG("不合格：未按规定线路行驶（试图做2次同方向的倒库）");
                err.push_back(6);
                AddExamFault(6, rtkTime);
                status = -1;
@@ -208,6 +214,7 @@
                DEBUG("第二次倒库");
            } else if (currTarget != THIRD_TOUCH_CTRL_LINE) {
                // 不合格：倒车前，2前轮没驶过控制线
                DEBUG("不合格：倒车前，2前轮没驶过控制线");
                err.push_back(9);
                AddExamFault(9, rtkTime);
                status = -1;
@@ -227,6 +234,7 @@
        if (run_status < 0) {
            if (stopCar2S) {
                // 扣5分：中途停车
                DEBUG("扣5分：中途停车");
                err.push_back(11);
                AddExamFault(11, rtkTime);
            }
@@ -245,6 +253,7 @@
            if (carStopEvent) {
                if (!carParkSuccess) {
                    // 不合格：倒库不入
                    DEBUG("不合格：倒库不入");
                    err.push_back(8);
                    AddExamFault(8, rtkTime);
                    status = -1;
@@ -301,14 +310,12 @@
    p2.Y = (prev_car->carXY[car->left_front_tire[TIRE_OUTSIDE]].Y + prev_car->carXY[car->left_front_tire[TIRE_INSIDE]].Y) / 2;
    MakeLine(&track1, &p1, &p2);
    if (IntersectionOf(track1, leftCtrlLine) == GM_Intersection &&
            IntersectionOfLine(map->point[0], map->point[1], p1) == -1
        /*IntersectionOf(p1, map) == GM_None*/) {
            IntersectionOfLine(map->point[0], map->point[1], p1) == -1) {
        leftTireCrossLeftLine = true;
        DEBUG("左轮跨越左控制线");
    }
    if (IntersectionOf(track1, rightCtrlLine) == GM_Intersection &&
            IntersectionOfLine(map->point[6], map->point[7], p1) == -1
        /*IntersectionOf(p1, map) == GM_None*/) {
            IntersectionOfLine(map->point[6], map->point[7], p1) == -1) {
        leftTireCrossRightLine = true;
        DEBUG("左轮跨越右控制线");
    }
@@ -319,14 +326,12 @@
    p2.Y = (prev_car->carXY[car->right_front_tire[TIRE_OUTSIDE]].Y + prev_car->carXY[car->right_front_tire[TIRE_INSIDE]].Y) / 2;
    MakeLine(&track1, &p1, &p2);
    if (IntersectionOf(track1, leftCtrlLine) == GM_Intersection &&
            IntersectionOfLine(map->point[0], map->point[1], p1) == -1
        /*IntersectionOf(p1, map) == GM_None*/) {
            IntersectionOfLine(map->point[0], map->point[1], p1) == -1) {
        rightTireCrossLeftLine = true;
        DEBUG("右轮跨越左控制线");
    }
    if (IntersectionOf(track1, rightCtrlLine) == GM_Intersection &&
            IntersectionOfLine(map->point[6], map->point[7], p1) == -1
        /*IntersectionOf(p1, map) == GM_None*/) {
            IntersectionOfLine(map->point[6], map->point[7], p1) == -1) {
        rightTireCrossRightLine = true;
        DEBUG("右轮跨越右控制线");
    }