适应河南测试反馈，倒库，侧方，判定由挡位改变触发，坡起增加上方点。

fctom1215

2021-04-26 08e28643083ff1bee05ad872c98c708dc16dda35

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

@@ -11,6 +11,7 @@
#include "../utils/xconvert.h"
#include "../master/comm_if.h"
#include "area_exam.h"
#include "../test_common/car_sensor.h"
#include <vector>
#include <cstdlib>

@@ -27,7 +28,14 @@
    THIRD_TOUCH_CTRL_LINE
};

static bool testing = false, reverseCar = false;
enum {
    TESTING,
    TEST_FAIL,          // 因触发某些规则，在车身未完全立场情况下，提前终止部分测试
    TEST_FINISH
};

static int testStatus;
static bool reverseCar = false;
static int mapIndex = 0;

const uint32_t CHECK_PARK_DELAY = 400;
@@ -44,15 +52,21 @@
static int darray[3];
static int parkStatus[3];

static int gearAtStop;
static int currGear;
static char prevCrossedCtrlLine;

static char CrossCtrlLine(const Polygon *map, const car_model *car, const car_model *prev_car);
static bool EnterParking(const Polygon *map, const car_model *car);
static bool CrashRedLine(const Polygon *map, const car_model *car, int &who);
static bool ExitParkArea(const Polygon *map, const car_model *car);
static bool AllTireExitParkArea(const Polygon *map, const car_model *car);

void StartParkBottom(int index, int moveDirect, const struct RtkTime *rtkTime)
{
    DEBUG("StartParkBottom");
    testing = true;
    testStatus = TESTING;

    reverseCar = false;
    mapIndex = index;
    memset(carray, 0, sizeof(carray));
@@ -67,6 +81,11 @@
    crossCtrlLineSw = false;
    reportParkFail = false;
    occurCrashRedLine = false;

    currGear = ReadCarStatus(GEAR);
    prevCrossedCtrlLine = 0;

    PlayTTS("您已进入倒车入库区域", NULL);
}

int TestParkBottom(const Polygon *map, const car_model *car, const car_model *carPrev, double speed, int moveDirect, const struct RtkTime *rtkTime)
@@ -78,16 +97,83 @@
    vector<Line> line_set;
    Line distance_line;

    bool gear_change = false;
    int gear = ReadCarStatus(GEAR);

    if (gear == GEAR_R) {
        if (currGear != GEAR_R) {
            gear_change = true;
            currGear = GEAR_R;
        }
    } else if (currGear == GEAR_R) {
        gear_change = true;
        currGear = gear;
    }

    if (testStatus == TESTING && gear_change) {
        if (currGear == GEAR_R) {
            // 挂倒挡,检测是否过控制线
            DEBUG("开始挂倒挡");
            if (!reverseCar) {
                DEBUG("开始首轮入库");
                reverseCar = true;
                MA_EnterMap(mapIndex, MAP_TYPE_PARK_BUTTOM, 1);
                firstReverseTimepoint = tp;             // 开始210秒计时
            }
            crossCtrlLine = CrossCtrlLine(map, car, carPrev);

            if (parkCount >= 2) {
                DEBUG("开始次轮入库");
                parkCount = 0;
                MA_EnterMap(mapIndex, MAP_TYPE_PARK_BUTTOM, 1);
                firstReverseTimepoint = tp;             // 开始210秒计时
            }

            if (crossCtrlLine == 0) {
                // 倒车前，前轮未驶过控制线
                AddExamFault(20104, rtkTime);
                DEBUG("倒车前，前轮未驶过控制线");
            } else if (crossCtrlLine == prevCrossedCtrlLine) {
                // 重复跨越同一控制线，不按规定线路，顺序形式，不合格
                AddExamFault(20101, rtkTime);
                DEBUG("不按规定线路，顺序形式， 同 %c 侧", prevCrossedCtrlLine);
            } else {
                prevCrossedCtrlLine = crossCtrlLine;
                DEBUG("开始 %c 侧 倒库", prevCrossedCtrlLine);
            }
        } else {
            // 从倒挡移出，检测是否入库
            DEBUG("从倒挡移出");

            parkCount++;

            DEBUG("库位检查次数 = %d", parkCount);

            if (EnterParking(map, car)) {
                DEBUG("倒库成功");
            } else {
                AddExamFault(20103, rtkTime);
                DEBUG("倒库不入");
            }
        }
    }
    
    if (testStatus == TESTING && parkCount < 2 && AllTireExitParkArea(map, car)) {
        testStatus = TEST_FAIL;
        AddExamFault(10103, rtkTime);
        DEBUG("直接驶离测试区,不按考试员指令驾驶");
    }
    
    if (ExitParkArea(map, car)) {
        DEBUG("离开场地");
        // 离开场地
        testing = false;
        if ((parkStatus[0] != 1 || parkStatus[1] != 1) && !reportParkFail && reverseCar) {
        testStatus = TEST_FINISH;
        /*if ((parkStatus[0] != 1 || parkStatus[1] != 1) && !reportParkFail && reverseCar) {
            // 倒库不入，不合格
            reportParkFail = true;
            AddExamFault(20103, rtkTime);
            DEBUG("倒库不入");
        }
        }*/
        goto TEST_END;
    }

@@ -108,22 +194,45 @@
    MA_SendDistance(dtox[0], dtox[1]);

    if (CrashRedLine(map, car, who)) {
        if (!occurCrashRedLine && reverseCar) {
        if (!occurCrashRedLine /*&& reverseCar*/) {
            occurCrashRedLine = true;
            // 车身出线，不合格
            AddExamFault(10116, rtkTime);
            DEBUG("车轮压线");
            if (who == 1) {
            /*if (who == 1) {
                PlayTTS("压左库位线", NULL);
            } else if (who == 2) {
                PlayTTS("压右库位线", NULL);
            }
            }*/
        }
    } else {
        occurCrashRedLine = false;
    }

    crossCtrlLine = CrossCtrlLine(map, car, carPrev);
    if (moveDirect != prevMoveDirect) {
        if (moveDirect == 0) {
            stopTimepoint = tp;
            gearAtStop = (currGear == GEAR_R ? 1 : 0);
            DEBUG("停车了 %d %d %d %d %d %d %d", rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss);
            DEBUG("停车时挡位 = %d", gearAtStop);
        } else if (prevMoveDirect == 0) {
            DEBUG("继续行驶 %d %d %d %d %d %d %d", rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss);

            DEBUG("停车时间 %ld", tp - stopTimepoint);
            DEBUG("再次移动时挡位 = %d", currGear == GEAR_R ? 1 : 0);

            if (tp - stopTimepoint >= CorrectPauseCriteria(examParam.park_bottom_pause_criteria)
                && gearAtStop == (currGear == GEAR_R ? 1 : 0)) {
                // 停车超2秒，每次扣5分
                AddExamFault(20106, rtkTime);
                DEBUG("中途停车");
            }
        }

        prevMoveDirect = moveDirect;
    }

/*    crossCtrlLine = CrossCtrlLine(map, car, carPrev);
    if (crossCtrlLine > 0 && !crossCtrlLineSw) {
        crossCtrlLineSw = true;
        if (parkCount == 0) {
@@ -143,14 +252,14 @@
            } else {
                // 离开测试区，停止计时
                DEBUG("离开测试区，停止计时");
                testing = false;
                testStatus = false;
                goto TEST_END;
            }
            carray[2] = crossCtrlLine;
        }
    }

    if (testing && darray[0] > 0 && tp - firstReverseTimepoint >= examParam.park_bottom_limit_time) {
    if (testStatus && darray[0] > 0 && tp - firstReverseTimepoint >= examParam.park_bottom_limit_time) {
        // 完成超时，不合格
        if (!reportExamTimeout) {
            reportExamTimeout = true;
@@ -235,13 +344,15 @@
            }
            checkPartStatus = false;
        }
    }
    }*/

TEST_END:
    if (!testing && reverseCar) {
    if (testStatus == TEST_FINISH) {
        DEBUG("倒库结束");
        MA_EnterMap(mapIndex, MAP_TYPE_PARK_BUTTOM, 0);
        return 0;
    }
    return testing ? 1 : 0;
    return 1;
}

// 检测2前轮是否正向越过左右控制线
@@ -344,3 +455,30 @@

    return ret;
}

// 双前轮和任一后轮不在区域
static bool AllTireExitParkArea(const Polygon *map, const car_model *car)
{
    int tireExitNum = 0;

    if (IntersectionOf(car->carXY[ car->left_front_tire[TIRE_OUTSIDE] ], map) == GM_None) {
        tireExitNum++;
    }

    if (IntersectionOf(car->carXY[ car->right_front_tire[TIRE_OUTSIDE] ], map) == GM_None) {
        tireExitNum++;
    }

    if (IntersectionOf(car->carXY[ car->left_rear_tire[TIRE_OUTSIDE] ], map) == GM_None) {
        tireExitNum++;
    }

    if (IntersectionOf(car->carXY[ car->right_rear_tire[TIRE_OUTSIDE] ], map) == GM_None) {
        tireExitNum++;
    }

    if (tireExitNum >= 3) {
        return true;
    }
    return false;
}