坐标

fctom1215

2020-08-29 d05c25aa4823e9d2aa812e9c0e3d1d66bb9861dd

 lib/src/main/cpp/test_items2/road_exam.cpp

@@ -72,7 +72,7 @@
const double SLIDE_DISTANCE_THRESHOLD_RED = 0.3;
const double SLIDE_DISTANCE_THRESHOLD_YELLOW = 0.1;

const double EXAM_RANGE = 3000.0;                       // 至少驾驶距离
const double EXAM_RANGE = 2700.0;                       // 至少驾驶距离

static bool occurOverSpeed;
static bool occurSecondBreak;
@@ -93,6 +93,8 @@

static struct RtkTime gearErrorTimePoint;
static struct RtkTime gearNSlideTimePoint;

static car_sensor_value_t turnSignalStatus;

static int gearErrorTime;

@@ -127,13 +129,14 @@
} RoadExamItem_t;
static map<int, int> RoadExamItem;
static int RoadExamStatus;
static bool win;

static struct RtkTime beginTurnTime, prevDetectTurnTime;
static int startTurnYaw, prevYaw;
static int turnCnt, turnTimeCnt;
static int prevTurnWise;

static const uint32_t GEAR_N_SLIDE_TIMEOUT = D_SEC(5);
static const uint32_t GEAR_N_SLIDE_TIMEOUT = D_SEC(10);
static const uint32_t GEAR_ERROR_TIMEOUT = D_SEC(15);
static const uint32_t STOP_CAR_TIME = D_SEC(2);
static const uint32_t CHANGE_LANE_MIN_INTERVAL = D_SEC(10);
@@ -199,6 +202,8 @@
    RoadExamItem[ROAD_EXAM_ITEM_CAR_STOP] = ROAD_EXAM_ITEM_NOT_EXEC;

    RoadExamStatus = ROAD_EXAM_READY_NEXT;

    win = false;
}

void TerminateRoadExam(void)
@@ -879,7 +884,7 @@
            checkCrashGreenTimeout = 0;
            // 记录开始压线的时间，不确定是否有变道意图，待确认变道后再处理之
            crashGreenStartTime = *rtkTime;

            turnSignalStatus = ReadCarSensorValue(TURN_SIGNAL_LAMP);
            DEBUG("开始压虚线 %02d-%02d-%02d %02d:%02d:%02d", crashGreenStartTime.YY, crashGreenStartTime.MM, crashGreenStartTime.DD, crashGreenStartTime.hh, crashGreenStartTime.mm, crashGreenStartTime.ss, crashGreenStartTime.mss * 10);
        }
        crashDottedLine = true;
@@ -926,40 +931,45 @@
        if (newLane.road == Lane.road && newLane.sep == Lane.sep) {
            gain = newLane.no - Lane.no;
        } else {
            DEBUG("车道分段切换");
            DEBUG("车道分段切换 Old Lane road = %d sep = %d no = %d New Lane road = %d sep = %d no = %d", Lane.road, Lane.sep, Lane.no, newLane.road, newLane.sep, newLane.no);
            ChangeLane.gain = 0;
        }

        // 检查转向灯
        if (gain != 0) {
            DEBUG("变道 new lane %d, gain %d", newLane.no, gain);
            car_sensor_value_t lamp = ReadCarSensorValue(TURN_SIGNAL_LAMP);
            if (lamp.name == TURN_SIGNAL_LAMP) {
                if (gain < 0) {
                    if (lamp.value != LEFT_TURN_LIGHT) {
                        DEBUG("变调未打灯!!");
                        // 没打灯，不合格
                        AddExamFault(13, rtkTime);
                    } else if (TimeGetDiff(&crashGreenStartTime, &lamp.time) < TURN_SIGNAL_LAMP_ADVANCE) {
                        DEBUG("转向灯时间不足 %02d-%02d-%02d %02d:%02d:%02d.%03d  -> %02d-%02d-%02d %02d:%02d:%02d.%03d",
                              crashGreenStartTime.YY, crashGreenStartTime.MM, crashGreenStartTime.DD, crashGreenStartTime.hh, crashGreenStartTime.mm, crashGreenStartTime.ss, crashGreenStartTime.mss * 10,
                              lamp.time.YY, lamp.time.MM, lamp.time.DD, lamp.time.hh, lamp.time.mm, lamp.time.ss, lamp.time.mss * 10);
            if (gain < 0) {
                if (turnSignalStatus.value != LEFT_TURN_LIGHT) {
                    DEBUG("变调未打灯!!");
                    // 没打灯，不合格
                    AddExamFault(13, rtkTime);
                } else if (TimeGetDiff(&crashGreenStartTime, &turnSignalStatus.time) <
                           TURN_SIGNAL_LAMP_ADVANCE) {
                    DEBUG("转向灯时间不足 %02d-%02d-%02d %02d:%02d:%02d.%03d  -> %02d-%02d-%02d %02d:%02d:%02d.%03d",
                          crashGreenStartTime.YY, crashGreenStartTime.MM, crashGreenStartTime.DD,
                          crashGreenStartTime.hh, crashGreenStartTime.mm, crashGreenStartTime.ss,
                          crashGreenStartTime.mss * 10,
                          turnSignalStatus.time.YY, turnSignalStatus.time.MM, turnSignalStatus.time.DD, turnSignalStatus.time.hh,
                          turnSignalStatus.time.mm, turnSignalStatus.time.ss, turnSignalStatus.time.mss * 10);

                        // 不足3秒，不合格
                        AddExamFault(14, rtkTime);
                    }
                } else {
                    if (lamp.value != RIGHT_TURN_LIGHT) {
                        DEBUG("变调未打灯!!");
                        // 没打灯，不合格
                        AddExamFault(13, rtkTime);
                    } else if (TimeGetDiff(&crashGreenStartTime, &lamp.time) < TURN_SIGNAL_LAMP_ADVANCE) {
                        DEBUG("转向灯时间不足 %02d-%02d-%02d %02d:%02d:%02d.%03d  -> %02d-%02d-%02d %02d:%02d:%02d.%03d",
                              crashGreenStartTime.YY, crashGreenStartTime.MM, crashGreenStartTime.DD, crashGreenStartTime.hh, crashGreenStartTime.mm, crashGreenStartTime.ss, crashGreenStartTime.mss * 10,
                              lamp.time.YY, lamp.time.MM, lamp.time.DD, lamp.time.hh, lamp.time.mm, lamp.time.ss, lamp.time.mss * 10);
                        // 不足3秒，不合格
                        AddExamFault(14, rtkTime);
                    }
                    // 不足3秒，不合格
                    AddExamFault(14, rtkTime);
                }
            } else {
                if (turnSignalStatus.value != RIGHT_TURN_LIGHT) {
                    DEBUG("变调未打灯!!");
                    // 没打灯，不合格
                    AddExamFault(13, rtkTime);
                } else if (TimeGetDiff(&crashGreenStartTime, &turnSignalStatus.time) <
                           TURN_SIGNAL_LAMP_ADVANCE) {
                    DEBUG("转向灯时间不足 %02d-%02d-%02d %02d:%02d:%02d.%03d  -> %02d-%02d-%02d %02d:%02d:%02d.%03d",
                          crashGreenStartTime.YY, crashGreenStartTime.MM, crashGreenStartTime.DD,
                          crashGreenStartTime.hh, crashGreenStartTime.mm, crashGreenStartTime.ss,
                          crashGreenStartTime.mss * 10,
                          turnSignalStatus.time.YY, turnSignalStatus.time.MM, turnSignalStatus.time.DD, turnSignalStatus.time.hh,
                          turnSignalStatus.time.mm, turnSignalStatus.time.ss, turnSignalStatus.time.mss * 10);
                    // 不足3秒，不合格
                    AddExamFault(14, rtkTime);
                }
            }

@@ -1238,6 +1248,11 @@

    UpdataOdo(speed, moveDirect, rtkTime);

    if (!win) {
        win = true;
        turnSignalStatus = ReadCarSensorValue(TURN_SIGNAL_LAMP);
    }

    // 刹车提示音
    RingBreak();

@@ -1323,6 +1338,7 @@
    
    // 挡位不匹配超时
    if (currGearError && prevGearError) {
        DEBUG("挡位错误增加 %ld毫秒 当前挡位 %d 时速 %f", TimeGetDiff(rtkTime, &gearErrorTimePoint), ReadCarStatus(GEAR), ConvertMs2KMh(speed));
        gearErrorTime += TimeGetDiff(rtkTime, &gearErrorTimePoint);
    }
    if (gearErrorTime > GEAR_ERROR_TIMEOUT) {
@@ -1425,10 +1441,11 @@
//    DEBUG("Lane信息 road %d sep %d total %d no %d guide %d", Lane.road, Lane.sep, Lane.total, Lane.no, Lane.guide);
    if (Lane.guide > 0 && currExamMapIndex >= 0) {
        int stop_line_index;
        int act = NearbyCrossingGuide(stop_line_index, currExamMapIndex, RoadMap.roads[currExamMapIndex], car);

        if (!(NearbyCrossingGuide(stop_line_index, currExamMapIndex, RoadMap.roads[currExamMapIndex], car) & Lane.guide)) {
        if (act != 0 && !(act & Lane.guide)) {
            if (!GetErrorLaneRpt(currExamMapIndex, stop_line_index)) {
                DEBUG("不按规定车道标向行驶 %d: %d  guide = %d", currExamMapIndex, stop_line_index, Lane.guide);
                DEBUG("不按规定车道标向行驶 %d: %d  期望 = %d guide = %d", currExamMapIndex, stop_line_index, act, Lane.guide);
                AddExamFault(9, rtkTime);
                SetErrorLaneRpt(currExamMapIndex, stop_line_index, true);
            }
@@ -1547,7 +1564,7 @@
                testing = TestOvertake(Lane.no, rtkTime);
                break;
            case ROAD_EXAM_ITEM_STRAIGHT:
                testing = TestDriveStraight(car, rtkTime);
                testing = TestDriveStraight(RoadMap, roadIndex, car, rtkTime);
                break;
            case ROAD_EXAM_ITEM_CAR_STOP:
                testing = TestStopCar(RoadMap, roadIndex, car, moveDirect, rtkTime);
@@ -1650,6 +1667,8 @@
            turnCnt++;
            beginTurnTime = *rtkTime;
            startTurnYaw = (int) car->yaw;

            turnSignalStatus = ReadCarSensorValue(TURN_SIGNAL_LAMP);
        }
    } else if (turnCnt == 1) {
        if (angle != 0 && moveDirect != 0) {
@@ -1661,6 +1680,8 @@
            } else {
                beginTurnTime = *rtkTime;
                startTurnYaw = (int) car->yaw;

                turnSignalStatus = ReadCarSensorValue(TURN_SIGNAL_LAMP);
            }
        } else {
            turnCnt = 0;
@@ -1676,28 +1697,27 @@
                // 确认转弯行为，检测开始刚转弯时转向灯情况
                turnCnt = -1;

                car_sensor_value_t lamp = ReadCarSensorValue(TURN_SIGNAL_LAMP);
                if (lamp.name == TURN_SIGNAL_LAMP) {
                    if (wise < 0) {
                        if (lamp.value != LEFT_TURN_LIGHT) {
                            DEBUG("变调未打灯!!");
                            // 没打灯，不合格
                            AddExamFault(13, rtkTime);
                        } else if (TimeGetDiff(&beginTurnTime, &lamp.time) < TURN_SIGNAL_LAMP_ADVANCE) {
                            DEBUG("转向灯时间不足");
                            // 不足3秒，不合格
                            AddExamFault(14, rtkTime);
                        }
                    } else {
                        if (lamp.value != RIGHT_TURN_LIGHT) {
                            DEBUG("变调未打灯!!");
                            // 没打灯，不合格
                            AddExamFault(13, rtkTime);
                        } else if (TimeGetDiff(&beginTurnTime, &lamp.time) < TURN_SIGNAL_LAMP_ADVANCE) {
                            DEBUG("转向灯时间不足");
                            // 不足3秒，不合格
                            AddExamFault(14, rtkTime);
                        }
                if (wise < 0) {
                    if (turnSignalStatus.value != LEFT_TURN_LIGHT) {
                        DEBUG("变调未打灯!!");
                        // 没打灯，不合格
                        AddExamFault(13, rtkTime);
                    } else if (TimeGetDiff(&beginTurnTime, &turnSignalStatus.time) <
                               TURN_SIGNAL_LAMP_ADVANCE) {
                        DEBUG("转向灯时间不足");
                        // 不足3秒，不合格
                        AddExamFault(14, rtkTime);
                    }
                } else {
                    if (turnSignalStatus.value != RIGHT_TURN_LIGHT) {
                        DEBUG("变调未打灯!!");
                        // 没打灯，不合格
                        AddExamFault(13, rtkTime);
                    } else if (TimeGetDiff(&beginTurnTime, &turnSignalStatus.time) <
                               TURN_SIGNAL_LAMP_ADVANCE) {
                        DEBUG("转向灯时间不足");
                        // 不足3秒，不合格
                        AddExamFault(14, rtkTime);
                    }
                }
            } else if (turnTimeCnt > D_SEC(10)) {