yy1717
2020-10-23 c484cbb09d445e2ab30ea011c6d2ffd87202bb26
lib/src/main/cpp/test_items2/road_exam.cpp
@@ -69,11 +69,6 @@
static const int CROSSING_TURN_THRESHOLD = 35;
static const int TURN_THRESHOLD = 3;
const double SLIDE_DISTANCE_THRESHOLD_RED = 0.3;
const double SLIDE_DISTANCE_THRESHOLD_YELLOW = 0.1;
const double EXAM_RANGE = 2700.0;                       // 至少驾驶距离
static bool occurOverSpeed;
static bool occurSecondBreak;
@@ -137,18 +132,6 @@
static int startTurnYaw, prevYaw;
static int turnCnt, turnTimeCnt;
static int prevTurnWise;
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);
static const uint32_t CRASH_DOTTED_LINE_TIMEOUT = D_SEC(10);
static const uint32_t TURN_SIGNAL_LAMP_ADVANCE = D_SEC(3);
static const double MAX_SPEED = 60.0 * 1000.0 / 3600.0;         // 超速确认
static const double DEC_MAX_SPEED = 55.0 * 1000.0 / 3600.0;     // 超速取消
static const int SPEED_GEAR_TABLE[][2] = {{0, 20}, {5, 30}, {15, 40}, {25, 10000}, {35, 10000}};
static void ItemExam(road_exam_map &RoadMap, int roadIndex, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime, double straight, double road_end);
static void ItemExam2(road_exam_map &RoadMap, int roadIndex, const car_model *car, LIST_CAR_MODEL &CarModelList);
@@ -901,7 +884,7 @@
                checkCrashGreenTimeout = 1;
                crashGreenRunTime = *rtkTime;           // 运动中压虚线的开始时间点
            } else if (checkCrashGreenTimeout == 1) {
                if (TimeGetDiff(rtkTime, &crashGreenRunTime) >= CRASH_DOTTED_LINE_TIMEOUT) {
                if (TimeGetDiff(rtkTime, &crashGreenRunTime) >= examParam.crash_dotted_line_cumulative_time) {
                    DEBUG("长时间压虚线");
                    checkCrashGreenTimeout = 2;
                    // 长时间骑轧车道分界线行驶,不合格
@@ -946,7 +929,7 @@
                    // 没打灯,不合格
                    AddExamFault(13, rtkTime);
                } else if (TimeGetDiff(&crashGreenStartTime, &turnSignalStatus.time) <
                           TURN_SIGNAL_LAMP_ADVANCE) {
                           examParam.turn_signal_min_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,
@@ -963,7 +946,7 @@
                    // 没打灯,不合格
                    AddExamFault(13, rtkTime);
                } else if (TimeGetDiff(&crashGreenStartTime, &turnSignalStatus.time) <
                           TURN_SIGNAL_LAMP_ADVANCE) {
                        examParam.turn_signal_min_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,
@@ -975,7 +958,7 @@
                }
            }
            if (((ChangeLane.gain < 0 && gain < 0) || (ChangeLane.gain > 0 && gain > 0)) && TimeGetDiff(rtkTime, &ChangeLane.time) < CHANGE_LANE_MIN_INTERVAL) {
            if (((ChangeLane.gain < 0 && gain < 0) || (ChangeLane.gain > 0 && gain > 0)) && TimeGetDiff(rtkTime, &ChangeLane.time) < examParam.continuous_change_lane_min_time) {
                DEBUG("连续变道");
                AddExamFault(15, rtkTime);
            }
@@ -1196,23 +1179,23 @@
                switch (act) {
                    case ROAD_ACTIVE_FORWARD:
                        DEBUG("路口提示 直行");
                        PlayTTS("前方路口请直行", NULL);
                        PlayTTS(examParam.crossing_go_straight_tts, NULL);
                        break;
                    case ROAD_ACTIVE_TURN_LEFT:
                        DEBUG("路口提示 左转");
                        PlayTTS("前方路口请左转", NULL);
                        PlayTTS(examParam.crossing_turn_left_tts, NULL);
                        break;
                    case ROAD_ACTIVE_TURN_RIGHT:
                        DEBUG("路口提示 右转");
                        PlayTTS("前方路口请右转", NULL);
                        PlayTTS(examParam.crossing_turn_right_tts, NULL);
                        break;
                    case ROAD_ACTIVE_TURN_BACKWARD:
                        DEBUG("路口提示 掉头");
                        PlayTTS("前方路口请掉头", NULL);
                        PlayTTS(examParam.crossing_turn_back_tts, NULL);
                        break;
                    default:
                        DEBUG("路口提示 未配置");
                        PlayTTS("前方路口听教练指令", NULL);
                        PlayTTS(examParam.crossing_turn_unknown_tts, NULL);
                        break;
                }
            }
@@ -1308,14 +1291,14 @@
        RingBreak();
        // 超速检测
        if (speed > MAX_SPEED) {
        if (ConvertMs2KMh(speed) > examParam.road_max_speed) {
            if (!occurOverSpeed) {
                occurOverSpeed = true;
                // 超速,不合格
                DEBUG("超速 %f", ConvertMs2KMh(speed));
                AddExamFault(10, rtkTime);
            }
        } else if (speed < DEC_MAX_SPEED) {
        } else if (ConvertMs2KMh(speed) < examParam.road_max_speed - 5) {
            occurOverSpeed = false;
        }
@@ -1358,32 +1341,32 @@
                }
                break;
            case GEAR_1:
                if (ConvertMs2KMh(speed) < SPEED_GEAR_TABLE[0][0] ||
                    ConvertMs2KMh(speed) > SPEED_GEAR_TABLE[0][1]) {
                if (ConvertMs2KMh(speed) < examParam.gear_speed_table[0][0] ||
                    ConvertMs2KMh(speed) > examParam.gear_speed_table[0][1]) {
                    currGearError = true;
                }
                break;
            case GEAR_2:
                if (ConvertMs2KMh(speed) < SPEED_GEAR_TABLE[1][0] ||
                    ConvertMs2KMh(speed) > SPEED_GEAR_TABLE[1][1]) {
                if (ConvertMs2KMh(speed) < examParam.gear_speed_table[1][0] ||
                    ConvertMs2KMh(speed) > examParam.gear_speed_table[1][1]) {
                    currGearError = true;
                }
                break;
            case GEAR_3:
                if (ConvertMs2KMh(speed) < SPEED_GEAR_TABLE[2][0] ||
                    ConvertMs2KMh(speed) > SPEED_GEAR_TABLE[2][1]) {
                if (ConvertMs2KMh(speed) < examParam.gear_speed_table[2][0] ||
                    ConvertMs2KMh(speed) > examParam.gear_speed_table[2][1]) {
                    currGearError = true;
                }
                break;
            case GEAR_4:
                if (ConvertMs2KMh(speed) < SPEED_GEAR_TABLE[3][0] ||
                    ConvertMs2KMh(speed) > SPEED_GEAR_TABLE[3][1]) {
                if (ConvertMs2KMh(speed) < examParam.gear_speed_table[3][0] ||
                    ConvertMs2KMh(speed) > examParam.gear_speed_table[3][1]) {
                    currGearError = true;
                }
                break;
            case GEAR_5:
                if (ConvertMs2KMh(speed) < SPEED_GEAR_TABLE[4][0] ||
                    ConvertMs2KMh(speed) > SPEED_GEAR_TABLE[4][1]) {
                if (ConvertMs2KMh(speed) < examParam.gear_speed_table[4][0] ||
                    ConvertMs2KMh(speed) > examParam.gear_speed_table[4][1]) {
                    currGearError = true;
                }
                break;
@@ -1398,7 +1381,7 @@
                gearNSlideTimePoint = *rtkTime;
            }
            if (GearNSlideStatus == 1 &&
                TimeGetDiff(rtkTime, &gearNSlideTimePoint) > GEAR_N_SLIDE_TIMEOUT) {
                TimeGetDiff(rtkTime, &gearNSlideTimePoint) > examParam.gear_n_allow_time) {
                // 空档滑行超5秒,不合格
                DEBUG("挡位滑行,超过5秒");
                AddExamFault(8, rtkTime);
@@ -1415,7 +1398,7 @@
                  ReadCarStatus(GEAR), ConvertMs2KMh(speed));
            gearErrorTime += TimeGetDiff(rtkTime, &gearErrorTimePoint);
        }
        if (gearErrorTime > GEAR_ERROR_TIMEOUT) {
        if (gearErrorTime > examParam.gear_speed_error_cumulative_time) {
            // 累计15秒,挡位-车速不匹配,不合格
            DEBUG("挡位错误超过15秒");
            AddExamFault(6, rtkTime);
@@ -1453,7 +1436,7 @@
            prevMoveDirect = moveDirect;
        } else if (moveDirect == 0) {
            // 持续停车
            if (TimeGetDiff(rtkTime, &stopTimepoint) >= STOP_CAR_TIME && !StopCarOnRedArea &&
            if (TimeGetDiff(rtkTime, &stopTimepoint) >= examParam.road_pause_criteria && !StopCarOnRedArea &&
                StopOnRedArea(RoadMap, car)) {
                // 停车超2秒,停在红区,不合格
                AddExamFault(16, rtkTime);
@@ -1465,11 +1448,11 @@
            if (occurSlide) {
                double slideDistance = DistanceOf(stopPoint, car->basePoint);
                if (slideDistance > SLIDE_DISTANCE_THRESHOLD_YELLOW) {
                if (slideDistance > examParam.road_slide_yellow_distance) {
                    slideNormalDistance = true;
                }
                if (slideDistance > SLIDE_DISTANCE_THRESHOLD_RED && !slideLongDistance) {
                if (slideDistance > examParam.road_slide_red_distance && !slideLongDistance) {
                    // 后滑超过30厘米, 不合格
                    AddExamFault(5, rtkTime);
                    DEBUG("后滑超过30厘米");
@@ -1629,7 +1612,7 @@
            }
            if (!not_complete) {
                if (road_end > 200 && ReadOdo() > EXAM_RANGE) {
                if (road_end > 200 && ReadOdo() > examParam.road_total_distance) {
                    RoadExamStatus = ROAD_EXAM_ITEM_CAR_STOP;
                    StartStopCarExam();
                    return;
@@ -1818,7 +1801,7 @@
                        // 没打灯,不合格
                        AddExamFault(13, rtkTime);
                    } else if (TimeGetDiff(&beginTurnTime, &turnSignalStatus.time) <
                               TURN_SIGNAL_LAMP_ADVANCE) {
                            examParam.turn_signal_min_advance) {
                        DEBUG("转向灯时间不足");
                        // 不足3秒,不合格
                        AddExamFault(14, rtkTime);
@@ -1829,7 +1812,7 @@
                        // 没打灯,不合格
                        AddExamFault(13, rtkTime);
                    } else if (TimeGetDiff(&beginTurnTime, &turnSignalStatus.time) <
                               TURN_SIGNAL_LAMP_ADVANCE) {
                            examParam.turn_signal_min_advance) {
                        DEBUG("转向灯时间不足");
                        // 不足3秒,不合格
                        AddExamFault(14, rtkTime);