坐标

yy1717

2020-08-12 539bdbbebc2410bbad25af01d9088594d02c95b0

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

@@ -42,6 +42,26 @@
    STOP_CAR_DONE
};

typedef struct {
    int road;
    int sep;
    int no;
    int guide;
} lane_t;

typedef struct {
    int edgeIndex;
    int pointIndex;
    PointF point;
} projection_t;

typedef struct
{
    int name;
    int value;
    struct RtkTime time;
} car_sensor_value_t;

static const int INVALID_ROAD = -1;

static const int TURN_THRESHOLD = 1;
@@ -65,7 +85,7 @@
static bool occurCrashGreenLine;
static bool occurOverSpeed;
static bool occurSecondBreak;
static int checkCrashGreenTimeout;

static char carIntersectionOfGreenLine;
static int currTurnSignalStatus;
static int turnSignalStatusWhenCrashGreenLine;
@@ -82,7 +102,7 @@
static bool occurSlide;
static bool startCarLeftTurnSignal, checkStartCarSignal;

static struct drive_timer crashGreenRunTime, crashGreenCmpTime, crashGreenStartTime, turnSignalChangeTime;
static struct RtkTime crashGreenRunTime, crashGreenStartTime;
static struct drive_timer gearErrorTimePoint;
static struct drive_timer gearNSlideTimePoint;
static struct drive_timer startCarLeftTurnSignalTime;
@@ -106,30 +126,14 @@
static const uint32_t TURN_ERROR_COLD_TIME = D_SEC(10);

static bool turnError13Cold, turnError14Cold;
static struct car_on_lane {
    int road;
    int separate;
    int lane;
    int direct;         // 车道方向限制
    int type;           // 实线，虚线
} CurrentLane;

static lane_t Lane;

static bool laneChanging;
static int changeLaneDirect;
static int CrashLineType;



typedef struct {
    int road;
    int sep;
    int lane;
    int guide;
} lane_t;

typedef struct {
    int edgeIndex;
    int pointIndex;
    PointF point;
} projection_t;
static map<int, car_sensor_value_t> CarSensorValue;

static const int MAX_ENGINE_RPM = 2500;
static const double START_CAR_MOVE_DISTANCE = 10.0;
@@ -147,7 +151,7 @@
static const int CRL_EDGE_DOTTED = 3;
static const int CRL_EDGE_SOLID = 4;

static const double MAX_SPEED = 40.0 * 1000.0 / 3600.0;
static const double MAX_SPEED = 60.0 * 1000.0 / 3600.0;
static const int SPEED_GEAR_TABLE[][2] = {{0, 20}, {5, 30}, {15, 40}, {25, 10000}, {35, 10000}};

static int TestRoadStartCar(const car_model *car, double speed, int moveDirect, const struct RtkTime *rtkTime);
@@ -158,9 +162,8 @@
static void TurnSignalError13ColdTimer(union sigval sig);
static void TurnSignalError14ColdTimer(union sigval sig);
static void ReportTurnSignalError(int err, const struct RtkTime *rtkTime);
static bool UpdateLane(struct car_on_lane &out, road_t &road, const car_model *car);
static int CrashRoadLine(road_t &road, const car_model *car);
static void ArrivedRoadEnd(road_t &road, const car_model *car, LIST_CAR_MODEL &CarModelList);


static trigger_line_t * EntryItem(int index, road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList);
static void ClearAll(road_exam_map &map);
static bool AllCmp(road_exam_map &map);
@@ -174,13 +177,12 @@
{
    DEBUG("Start road_exam");

    crashGreenCmpTime.hour = -1;
    occurCrashRedLine = false;
    occurCrashGreenLine = false;
    occurOverSpeed = false;

    occurSecondBreak = false;
    checkCrashGreenTimeout = 0;

    carIntersectionOfGreenLine = 0;

    reportTurnSignalError = false;
@@ -211,13 +213,15 @@

    turnError13Cold = turnError14Cold = true;

    CurrentLane.road = CurrentLane.separate = CurrentLane.lane = -1;
    Lane.road = Lane.sep = Lane.no = -1;
    laneChanging = false;
    changeLaneDirect = 0;

    nextRoadId = -1;
    checkTurn = false;
    ClearAll(RoadMap);

    CrashLineType = -1;

    ResetOdo();

@@ -489,29 +493,26 @@

/************************************************
 * 车轮压实线，前后轴交叉，前后轮迹交叉
 * @param mode 1 - 道路实线/分道实线  2 - 分道虚线/半实半虚线
 * @param currCrashLineType
 * @param RoadMap
 * @param car
 * @param CarModelList
 * @return
 */
static bool CrashRedLine(int mode, int roadIndex, road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList)
static int CrashRedLine(int currCrashLineType, int roadIndex, road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList)
{
    Line frontTireAxle, rearTireAxle;
    Line frontLeftTireTrack, frontRightTireTrack;
    Line rearLeftTireTrack, rearRightTireTrack;
    bool track = false;
    int lineType = -1;

    PointF p11, p12, p21, p22, p31, p32, p41, p42;

    MakeLine(&frontTireAxle, &car->carXY[car->left_front_tire[TIRE_OUTSIDE]], &car->carXY[car->right_front_tire[TIRE_OUTSIDE]]);
    MakeLine(&rearTireAxle, &car->carXY[car->left_rear_tire[TIRE_OUTSIDE]], &car->carXY[car->right_rear_tire[TIRE_OUTSIDE]]);

    if (CarModelList.size() >= 2) {
        list<car_model *>::iterator iter = CarModelList.begin();

        PointF p11, p12, p21, p22, p31, p32, p41, p42;

        p11 = ((car_model *)(*iter))->carXY[((car_model *)(*iter))->left_front_tire[TIRE_OUTSIDE]];
        p21 = ((car_model *)(*iter))->carXY[((car_model *)(*iter))->right_front_tire[TIRE_OUTSIDE]];
        p31 = ((car_model *)(*iter))->carXY[((car_model *)(*iter))->left_rear_tire[TIRE_OUTSIDE]];
@@ -528,7 +529,7 @@

        struct RtkTime time2 = ((car_model *)(*iter))->tm;

        if (TimeGetDiff(time1.hh, time1.mm, time1.ss, time1.mss, time2.hh, time2.mm, time2.ss, time2.mss) <= D_SEC(1)) {
        if (TimeGetDiff(&time1, &time2) <= D_SEC(1)) {
            MakeLine(&frontLeftTireTrack, &p11, &p12);
            MakeLine(&frontRightTireTrack, &p21, &p22);
            MakeLine(&rearLeftTireTrack, &p31, &p32);
@@ -635,24 +636,19 @@
    }

CRASH_LINE_CONFIRM:
    if (lineType == LINE_HALF_SOLID_LEFT) {
        if (currCrashLineType != LINE_HALF_SOLID_LEFT && track) {
            if (IntersectionOfLine(car->carXY[car->right_front_tire[TIRE_OUTSIDE]], redLine) == -1) {
                // 非法跨线
            }
    if (lineType == LINE_HALF_SOLID_LEFT && currCrashLineType != lineType && track) {
        if (IntersectionOfLine(p21, redLine) == -1 && IntersectionOfLine(p22, redLine) == 1) {
            // 非法跨线
            lineType += 100;
        }
    } else if (lineType == LINE_HALF_SOLID_RIGHT) {
        if (currCrashLineType != LINE_HALF_SOLID_RIGHT && track) {
            if (IntersectionOfLine(car->carXY[car->left_front_tire[TIRE_OUTSIDE]], redLine) == 1) {
                // 非法跨线
            }
    } else if (lineType == LINE_HALF_SOLID_RIGHT && currCrashLineType != lineType && track) {
        if (IntersectionOfLine(p11, redLine) == 1 && IntersectionOfLine(p12, redLine) == -1) {
            // 非法跨线
            lineType += 100;
        }
    }

    return false;
    return lineType;
}



static int GetGuideDirect(road_exam_map &RoadMap, PointF point, int roadIndex, int sepIndex, int laneNo)
{
@@ -673,7 +669,7 @@
 * @param roadIndex
 * @return
 */
static bool GetLane(lane_t &theLane, PointF point, road_exam_map &RoadMap, int roadIndex)
static bool GetLane(lane_t &lane, PointF point, road_exam_map &RoadMap, int roadIndex)
{
    Line leftProjLine, rightProjLine;
    Line sep;
@@ -694,7 +690,7 @@
            char pos;            // 点在车道的左右位置

            bool operator == (const int &x) {
                return(this->lane_no == x);
                return (this->lane_no == x);
            }
        };

@@ -770,22 +766,65 @@
                }
            }

            theLane.road = roadIndex;
            theLane.sep = i;
            theLane.lane = CLP.size();
            lane.road = roadIndex;
            lane.sep = i;
            lane.no = CLP.size();

            for (int x = 0; x < CLP.size(); ++x) {
                if (CLP[x].pos == 'L') {
                    theLane.lane = x;
                    lane.no = x;
                    break;
                }
            }
            theLane.guide = GetGuideDirect(RoadMap, p0, theLane.road, theLane.sep, theLane.lane);
            lane.guide = GetGuideDirect(RoadMap, p0, lane.road, lane.sep, lane.no);
            return true;
        }
    }

    return false;
}

static void WriteCarSensorValue(int name, int value, const struct RtkTime *rtkTime)
{
    car_sensor_value_t nv;

    nv.name = name;
    nv.value = value;
    nv.time = *rtkTime;

    auto it = CarSensorValue.find(name);

    if (it != CarSensorValue.end()) {
        if (it->second.value != value || name == ENGINE_RPM) {
            CarSensorValue[name] = nv;
        }
    } else {
        CarSensorValue.insert(pair<int, car_sensor_value_t>(name, nv));
    }
}

static car_sensor_value_t ReadCarSensorValue(int name)
{
    car_sensor_value_t nv = {.name = -1};

    auto it = CarSensorValue.find(name);

    if (it != CarSensorValue.end()) {
        nv = it->second;
    }

    return nv;
}

static void UpdateCarSensor(const struct RtkTime *rtkTime)
{
    WriteCarSensorValue(TURN_SIGNAL_LAMP, ReadCarStatus(TURN_SIGNAL_LAMP), rtkTime);
    WriteCarSensorValue(HAND_BREAK, ReadCarStatus(HAND_BREAK), rtkTime);
    WriteCarSensorValue(DOOR, ReadCarStatus(DOOR), rtkTime);
    WriteCarSensorValue(ENGINE_RPM, ReadCarStatus(ENGINE_RPM), rtkTime);
    WriteCarSensorValue(SECOND_BREAK, ReadCarStatus(SECOND_BREAK), rtkTime);
    WriteCarSensorValue(GEAR, ReadCarStatus(GEAR), rtkTime);
    WriteCarSensorValue(BREAK, ReadCarStatus(BREAK), rtkTime);
}

static void TurnSignalError13ColdTimer(union sigval sig)
@@ -893,17 +932,119 @@
    return startCar;
}

static void DetectLine(int roadIndex, road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList, int moveDirect, const struct RtkTime *rtkTime)
{
    static int checkCrashGreenTimeout = 0;
    static bool crashDottedLine = false;
    int newLineType = CrashRedLine(CrashLineType, roadIndex, RoadMap, car, CarModelList);

    if (newLineType >= 100) {
        newLineType -= 100;
        DEBUG("非法跨越分道线");
        AddExamFault(11, rtkTime);
    }
    if (newLineType == LINE_SOLID && CrashLineType != LINE_SOLID) {
        // 车辆行驶中骑轧车道中心实线或者车道边缘实线，不合格
        DEBUG("撞道路边缘线");
        AddExamFault(11, rtkTime);
    }
    if (newLineType == LINE_DOTTED || newLineType == LINE_HALF_SOLID_LEFT || newLineType == LINE_HALF_SOLID_RIGHT) {
        if (!crashDottedLine) {
            checkCrashGreenTimeout = 0;
            // 记录开始压线的时间，不确定是否有变道意图，待确认变道后再处理之
            crashGreenStartTime = *rtkTime;
        }
        crashDottedLine = true;
    } else {
        crashDottedLine = false;
    }

    if (crashDottedLine) {
        // 压虚线
        if (moveDirect != 0) {
            if (checkCrashGreenTimeout == 0) {
                checkCrashGreenTimeout = 1;
                crashGreenRunTime = *rtkTime;           // 运动中压虚线的开始时间点
            } else if (checkCrashGreenTimeout == 1) {
                if (TimeGetDiff(rtkTime, &crashGreenRunTime) >= CRASH_DOTTED_LINE_TIMEOUT) {
                    DEBUG("长时间压虚线");
                    checkCrashGreenTimeout = 2;
                    // 长时间骑轧车道分界线行驶，不合格
                    AddExamFault(12, rtkTime);
                }
            }
        } else {
            // 停车就不计时了
            checkCrashGreenTimeout = 0;
        }
    } else {
        // 不再压虚线
        checkCrashGreenTimeout = 0;
    }

     CrashLineType = newLineType;
}

int DetectLane(road_exam_map &RoadMap, const car_model *car, int roadIndex, const struct RtkTime *rtkTime)
{
    lane_t newLane;
    int gain = 0;

    if (CrashLineType == LINE_DOTTED || CrashLineType == LINE_HALF_SOLID_LEFT || CrashLineType == LINE_HALF_SOLID_RIGHT) {
        // 排除跨线中
        return 0;
    }
    if (GetLane(newLane, car->carXY[car->axial[AXIAL_FRONT]], RoadMap, roadIndex)) {
        if (newLane.road == Lane.road && newLane.sep == Lane.sep) {
            gain = newLane.no - Lane.no;
        }

        // 检查转向灯
        if (gain != 0) {
            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("变调未打灯!!");
                        // 没打灯，不合格
                        ReportTurnSignalError(13, rtkTime);
                    } else if (TimeGetDiff(&crashGreenStartTime, &lamp.time) >= D_SEC(3)) {
                        DEBUG("转向灯时间不足");
                        // 不足3秒，不合格
                        reportTurnSignalError = true;
                        ReportTurnSignalError(14, rtkTime);
                    }
                } else {
                    if (lamp.value != RIGHT_TURN_LIGHT) {
                        DEBUG("变调未打灯!!");
                        // 没打灯，不合格
                        ReportTurnSignalError(13, rtkTime);
                    } else {
                        DEBUG("转向灯时间不足");
                        // 不足3秒，不合格
                        reportTurnSignalError = true;
                        ReportTurnSignalError(14, rtkTime);
                    }
                }
            }
        }

        Lane = newLane;
    }
    return gain;
}

void TestRoadGeneral(road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime)
{
    uint32_t cts = AppTimer_GetTickCount();
    int ri = CalcRoadIndex(-1, RoadMap, car);
    bool crash = CrashRedLine(0, 0, RoadMap, car, CarModelList);
    bool crash = CrashRedLine(CrashLineType, 0, RoadMap, car, CarModelList);
    lane_t laneInfo;
    double redist = -1;

    laneInfo.road = -1;
    laneInfo.sep = -1;
    laneInfo.lane = -1;
    laneInfo.no = -1;

    if (ri >= 0) {
        GetLane(laneInfo, car->carXY[car->axial[AXIAL_FRONT]], RoadMap, ri);
@@ -919,7 +1060,9 @@
                              RoadMap.roads[ri].rightEdge);
    }

    DEBUG("当前道路索引 %d, 触发红线 %d lane %d 距离 %f %ld", ri, crash, laneInfo.lane, redist, AppTimer_GetTickCount() - cts);
    DEBUG("当前道路索引 %d, 触发红线 %d lane %d 距离 %f %ld", ri, crash, laneInfo.no, redist, AppTimer_GetTickCount() - cts);

    UpdateCarSensor(rtkTime);

    UpdataOdo(speed, moveDirect, rtkTime);

@@ -1076,24 +1219,6 @@

    }

    // 转向灯读取
    switch (ReadCarStatus(TURN_SIGNAL_LAMP)) {
        case LEFT_TURN_LIGHT:
            if (currTurnSignalStatus != LEFT_TURN_LIGHT) {
                currTurnSignalStatus = LEFT_TURN_LIGHT;
                Rtk2DriveTimer(turnSignalChangeTime, rtkTime);
            }
            break;
        case RIGHT_TURN_LIGHT:
            if (currTurnSignalStatus != RIGHT_TURN_LIGHT) {
                currTurnSignalStatus = RIGHT_TURN_LIGHT;
                Rtk2DriveTimer(turnSignalChangeTime, rtkTime);
            }
            break;
        default:
            currTurnSignalStatus = ReadCarStatus(TURN_SIGNAL_LAMP);
            break;
    }

    // 检测通过路口、人行道等区域时，释放刹车或减速
    CheckBreakActive(RoadMap, car, CarModelList,
@@ -1102,268 +1227,12 @@
    // 检测离开此路段，全车需不在范围内
    currExamMapIndex = CalcRoadIndex(currExamMapIndex, RoadMap, car);

    DetectLine(currExamMapIndex, RoadMap, car, CarModelList, moveDirect, rtkTime);

    DetectLane(RoadMap, car, currExamMapIndex, rtkTime);

    // 检测压线状态
    bool crashRedLineNow = false;
    bool crashGreenLineNow = false;

    if (currExamMapIndex >= 0) {
        int crl = CrashRoadLine(RoadMap.roads[currExamMapIndex], car);

        if (crl == CRL_NONE) {
//            DEBUG("什么都没压");
        } else if (crl == CRL_SEP_DOTTED) {
            DEBUG("压分道虚线");
            crashGreenLineNow = true;
        } else if (crl == CRL_SEP_SOLID) {
            DEBUG("压分道实线");
            crashRedLineNow = true;
        } else if (crl == CRL_EDGE_DOTTED) {
            DEBUG("压边沿虚线");
            crashGreenLineNow = true;
        } else if (crl == CRL_EDGE_SOLID) {
            DEBUG("压边沿实线");
            crashRedLineNow = true;
        }

        // 检查当前车道
        struct car_on_lane lane;

        if (UpdateLane(lane, RoadMap.roads[currExamMapIndex], car)) {

            if (lane.road == CurrentLane.road && lane.separate == CurrentLane.separate &&
                lane.lane == CurrentLane.lane) {
                laneChanging = false;
            }

            if (lane.road == CurrentLane.road && lane.separate == CurrentLane.separate &&
                lane.lane != CurrentLane.lane) {
                if (crl == CRL_SEP_DOTTED) {
                    // 变道进行中
                    if (!laneChanging) {
                        laneChanging = true;
                        DEBUG("发生变道");
                        // 比较上次跨线时间
                        if (crashGreenCmpTime.hour >= 0) {
                            uint32_t diff = TimeGetDiff(rtkTime->hh, rtkTime->mm, rtkTime->ss,
                                                        rtkTime->mss * 10,
                                                        crashGreenCmpTime.hour,
                                                        crashGreenCmpTime.min,
                                                        crashGreenCmpTime.sec,
                                                        crashGreenCmpTime.msec * 10);
                            int laneDirect = 0;

                            if (CurrentLane.lane > lane.lane) {
                                laneDirect = -1;
                            } else {
                                laneDirect = 1;
                            }

                            if (diff < CHANGE_LANE_MIN_INTERVAL && laneDirect == changeLaneDirect) {
                                DEBUG("===================== 连续变道 %d -> %d ============", CurrentLane.lane, lane.lane);
                                // 连续变道，不合格
                                AddExamFault(15, rtkTime);
                            }
                        }

                        // 检查变道前，是否提前转向灯
                        if (lane.lane < CurrentLane.lane) {
                            // 向左侧变道
                            DEBUG("向左侧变道");
                            if (currTurnSignalStatus != LEFT_TURN_LIGHT) {
                                DEBUG("变调未打灯!!");
                                // 没打灯，不合格
                                ReportTurnSignalError(13, rtkTime);
                            } else if (TimeGetDiff(rtkTime->hh, rtkTime->mm, rtkTime->ss,
                                                   rtkTime->mss * 10,
                                                   turnSignalChangeTime.hour,
                                                   turnSignalChangeTime.min,
                                                   turnSignalChangeTime.sec,
                                                   turnSignalChangeTime.msec * 10) <
                                       TURN_SIGNAL_LAMP_ADVANCE) {
                                if (!reportTurnSignalError) {
                                    DEBUG("转向灯时间不足");
                                    // 不足3秒，不合格
                                    reportTurnSignalError = true;
                                    ReportTurnSignalError(14, rtkTime);
                                }
                            }
                        } else {
                            // 向右侧变道
                            DEBUG("向右侧变道");
                            if (currTurnSignalStatus != RIGHT_TURN_LIGHT) {
                                DEBUG("变调未打灯!!");
                                // 没打灯，不合格
                                ReportTurnSignalError(13, rtkTime);
                            } else if (TimeGetDiff(rtkTime->hh, rtkTime->mm, rtkTime->ss,
                                                   rtkTime->mss * 10,
                                                   turnSignalChangeTime.hour,
                                                   turnSignalChangeTime.min,
                                                   turnSignalChangeTime.sec,
                                                   turnSignalChangeTime.msec * 10) <
                                       TURN_SIGNAL_LAMP_ADVANCE) {
                                if (!reportTurnSignalError) {
                                    DEBUG("转向灯时间不足");
                                    // 不足3秒，不合格
                                    reportTurnSignalError = true;
                                    ReportTurnSignalError(14, rtkTime);
                                }
                            }
                        }
                    }
                } else {
                    // 变道完成
                    DEBUG("变道完成 %d -> %d", CurrentLane.lane, lane.lane);

                    if (currRoadItem != NULL && currRoadItem->active == ROAD_ITEM_CHANGE_LANE) {
                        DEBUG("变更车道项目完成");
                        currRoadItem = NULL;
                        PlayTTS("完成变道");
                    } else if (currRoadItem != NULL && currRoadItem->active == ROAD_ITEM_OVERTAKE) {
                        if (CurrentLane.lane > lane.lane) {
                            DEBUG("超车变道完成");
                            overtake = true;
                            Rtk2DriveTimer(overTakeCmpTime, rtkTime);
                            PlayTTS("完成超车");
                        } else {
                            DEBUG("右道超车，错误");
                            AddExamFault(3, rtkTime);
                            currRoadItem = NULL;
                        }
                    }

                    if (CurrentLane.lane > lane.lane) {
                        changeLaneDirect = -1;
                    } else {
                        changeLaneDirect = 1;
                    }

                    CurrentLane = lane;
                    laneChanging = false;
                    // 记录本次变道时间点
                    Rtk2DriveTimer(crashGreenCmpTime, rtkTime);
                }
            }

            if (lane.road != CurrentLane.road || lane.separate != CurrentLane.separate || lane.direct != CurrentLane.direct) {
                // 路或段变更，撤销变道跟踪
                DEBUG("============== 路或段变更 CURR %d, %d dir %d NEW %d, %d, dir %d", CurrentLane.road,
                      CurrentLane.separate, CurrentLane.direct,
                      lane.road, lane.separate, lane.direct);

                CurrentLane = lane;
                laneChanging = false;
            }

            if (CurrentLane.direct != 0 && !(CurrentLane.direct & RoadMap.roads[currExamMapIndex].active)) {
                if (!RoadMap.roads[currExamMapIndex].errorLane) {
                    RoadMap.roads[currExamMapIndex].errorLane = true;
                    DEBUG("不按规定车道标线方向行驶 <%d> %d %d", RoadMap.roads[currExamMapIndex].id, CurrentLane.direct, RoadMap.roads[currExamMapIndex].active);
                    AddExamFault(9, rtkTime);
                }
            }
        }

        ArrivedRoadEnd(RoadMap.roads[currExamMapIndex], car, CarModelList);

//        if (CrashTheLine(RoadMap.roads[currExamMapIndex].stopLine, car, CarModelList)) {
//            DEBUG("下一个目标路 id = %d", RoadMap.roads[currExamMapIndex].targetRoad);
//            nextRoadId = RoadMap.roads[currExamMapIndex].targetRoad;
//            checkTurn = true;
//        }

        if (checkTurn) {
            // 检查是否持续转向
            char turnDirect = CheckCarTurn(CarModelList);
            if (turnDirect == 'L') {
//        PlayTTS("左1");
                if (currTurnSignalStatus != LEFT_TURN_LIGHT) {
                    if (!reportTurnSignalError) {
                        DEBUG("没打左转灯");
                        // 没打左转灯，不合格
                        reportTurnSignalError = true;
                        ReportTurnSignalError(13, rtkTime);
                    }
                } else if (TimeGetDiff(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10,
                                       turnSignalChangeTime.hour, turnSignalChangeTime.min, turnSignalChangeTime.sec, turnSignalChangeTime.msec*10) < TURN_SIGNAL_LAMP_ADVANCE) {
                    if (!reportTurnSignalError) {
                        DEBUG("转向灯时间不足");
                        // 不足3秒，不合格
                        reportTurnSignalError = true;
                        ReportTurnSignalError(14, rtkTime);
                    }
                }
            } else if (turnDirect == 'R') {
//        PlayTTS("右1");
                if (currTurnSignalStatus != RIGHT_TURN_LIGHT) {
                    if (!reportTurnSignalError) {
                        DEBUG("没打右转灯");
                        // 没打右转灯，不合格
                        reportTurnSignalError = true;
                        ReportTurnSignalError(13, rtkTime);
                    }
                } else if (TimeGetDiff(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10,
                                       turnSignalChangeTime.hour, turnSignalChangeTime.min, turnSignalChangeTime.sec, turnSignalChangeTime.msec*10) < TURN_SIGNAL_LAMP_ADVANCE) {
                    if (!reportTurnSignalError) {
                        DEBUG("转向灯时间不足");
                        // 不足3秒，不合格
                        reportTurnSignalError = true;
                        ReportTurnSignalError(14, rtkTime);
                    }
                }
            } else {
                reportTurnSignalError = false;
            }
        }
    }

    // 撞红线
    if (crashRedLineNow) {
        if (!occurCrashRedLine) {
            // 车辆行驶中骑轧车道中心实线或者车道边缘实线，不合格
            DEBUG("撞道路边缘线");
            AddExamFault(11, rtkTime);
            occurCrashRedLine = true;
        }
    } else {
        occurCrashRedLine = false;
    }

    // 压虚线
    if (crashGreenLineNow) {
        // 压虚线
        if (moveDirect != 0) {
            if (checkCrashGreenTimeout == 0) {
                checkCrashGreenTimeout = 1;
                Rtk2DriveTimer(crashGreenRunTime, rtkTime);         // 运动中压虚线的开始时间点
            } else if (checkCrashGreenTimeout == 1) {
                uint32_t diff = TimeGetDiff(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10,
                                            crashGreenRunTime.hour, crashGreenRunTime.min, crashGreenRunTime.sec, crashGreenRunTime.msec*10);

                if (diff >= CRASH_DOTTED_LINE_TIMEOUT) {
                    DEBUG("长时间压虚线");
                    checkCrashGreenTimeout = 2;
                    // 长时间骑轧车道分界线行驶，不合格
                    AddExamFault(12, rtkTime);
                }
            }
        } else {
            // 停车就不计时了
            checkCrashGreenTimeout = 0;
        }

        // 检测当前车辆于虚线的位置，做变道检测;
        // 检测是否3秒前有开启对应之转向灯
        if (!occurCrashGreenLine) {
            occurCrashGreenLine = true;
            // 记录开始压线的时间，不确定是否有变道意图，待确认变道后再处理之
            Rtk2DriveTimer(crashGreenStartTime, rtkTime);
            turnSignalStatusWhenCrashGreenLine = currTurnSignalStatus;
        }
    } else {
        // 不再压虚线
        occurCrashGreenLine = false;
        checkCrashGreenTimeout = 0;
    }

    TestRoadStartCar(car, speed, moveDirect, rtkTime);

@@ -1432,274 +1301,6 @@
            }
        }
    }
}

/*************************************************************
 * 检测当前车道，以车头中点为基准
 * @param road
 * @param car
 */
static bool UpdateLane(struct car_on_lane &out, road_t &road, const car_model *car)
{
    bool leftExt = false, rightExt = false;
    Line leftExtLine, rightExtLine;
    struct car_on_lane lane;

    for (int i = 0; i < road.leftEdge.size(); ++i) {
        PointF p1, p2;
        Line edge;

        p1 = road.leftEdge[i].points[0];
        for (int j = 1; j < road.leftEdge[i].points.size(); ++j) {
            p2 = road.leftEdge[i].points[j];
            MakeLine(&edge, &p1, &p2);

            PointF vp;
            if (VerticalPointOnLine(car->carXY[car->axial[AXIAL_FRONT]], edge, vp)) {
                leftExt = true;
                MakeLine(&leftExtLine, &car->carXY[car->axial[AXIAL_FRONT]], &vp);
                goto LEFT_EXT_CMP;
            }
            p1 = p2;
        }
    }
LEFT_EXT_CMP:
    for (int i = 0; i < road.rightEdge.size(); ++i) {
        PointF p1, p2;
        Line edge;

        p1 = road.rightEdge[i].points[0];
        for (int j = 1; j < road.rightEdge[i].points.size(); ++j) {
            p2 = road.rightEdge[i].points[j];
            MakeLine(&edge, &p1, &p2);

            PointF vp;
            if (VerticalPointOnLine(car->carXY[car->axial[AXIAL_FRONT]], edge, vp)) {
                rightExt = true;
                MakeLine(&rightExtLine, &car->carXY[car->axial[AXIAL_FRONT]], &vp);
                goto RIGHT_EXT_CMP;
            } else {
//                DEBUG("右侧不垂点 %d p1(%f,%f) p2(%f,%f) (%f,%f)", j, p1.X, p1.Y, p2.X, p2.Y, vp.X, vp.Y);
            }
            p1 = p2;
        }
    }
RIGHT_EXT_CMP:

    if (!leftExt || !rightExt) {
        DEBUG("左右边界不匹配");
        return false;
    }

    bool orthogonalInSegment = false;

    for (int i = 0; i < road.separate.size(); ++i) {        // 段
        PointF p1, p2;
        Line sep;

        map<int, vector<int>> orthogonal;

        // 一段分道组内，有一条正交，就必须保证同组的全都正交，否则直接退出
        for (int j = 0; j < road.separate[i].lines.size(); ++j) {   // 线组
            bool intersection = false;

            for (int k = 0; !intersection && k < road.separate[i].lines[j].size(); ++k) {   // 节

                p1 = road.separate[i].lines[j][k].points[0];
                for (int m = 1; m < road.separate[i].lines[j][k].points.size(); ++m) {
                    p2 = road.separate[i].lines[j][k].points[m];
                    MakeLine(&sep, &p1, &p2);

                    if (IntersectionOf(leftExtLine, sep) == GM_Intersection) {
                        vector<int> stor(4);

                        stor[0] = 1;
                        stor[1] = road.separate[i].lines[j][k].character;
                        stor[2] = road.separate[i].lines[j][k].left_lane_direct;
                        stor[3] = road.separate[i].lines[j][k].right_lane_direct;

                        orthogonal.insert(pair<int, vector<int>>(j, stor));
                        orthogonalInSegment = true;
                        intersection = true;

                        break;
                    } else if (IntersectionOf(rightExtLine, sep) == GM_Intersection) {
                        vector<int> stor(4);

                        stor[0] = 2;
                        stor[1] = road.separate[i].lines[j][k].character;
                        stor[2] = road.separate[i].lines[j][k].left_lane_direct;
                        stor[3] = road.separate[i].lines[j][k].right_lane_direct;

                        orthogonal.insert(pair<int, vector<int>>(j, stor));
                        orthogonalInSegment = true;
                        intersection = true;

                        break;
                    }
                    p1 = p2;
                }
            }
        }
//        DEBUG("目标 %d 当前 %d", road.separate[i].lines.size(), orthogonal.size());

        if (orthogonal.size() > 0) {
            if (orthogonal.size() == road.separate[i].lines.size()) {
                // 得到当前在第几个车道
                int x = 0;
                int left_char = LINE_SOLID;               // 道路左边线
                int right_direct = 0;

                for (x = 0; x < orthogonal.size(); ++x) {
                    auto itx = orthogonal.find(x);
                    if (itx != orthogonal.end()) {
                        if (itx->second[0] != 1) {          // 在车辆右侧
                            lane.road = road.id;
                            lane.separate = i;
                            lane.lane = itx->first;

//                            DEBUG("left_char %d second %d", left_char, itx->second[1]);

                            if ((left_char == LINE_SOLID || left_char == LINE_HALF_SOLID_RIGHT) &&
                                    (itx->second[1] == LINE_SOLID || itx->second[1] == LINE_HALF_SOLID_LEFT))    // 车道左右均是实线
                                lane.direct = itx->second[2];
                            else
                                lane.direct = 0;
//                            DEBUG("路 %d 段 %d 车道 %d 限定 %d", lane.road, lane.separate, lane.lane, lane.direct);
                            break;
                        } else {
                            right_direct = itx->second[3];
                            left_char = itx->second[2];
                        }
                    }
                }
                if (x >= orthogonal.size()) {
                    lane.road = road.id;
                    lane.separate = i;
                    lane.lane = orthogonal.size();

//                    DEBUG("left_char %d right_direct %d", left_char, right_direct);

                    // 最后车道的右侧限定
                    if (left_char == LINE_SOLID || left_char == LINE_HALF_SOLID_RIGHT)
                        lane.direct = right_direct;
                    else
                        lane.direct = 0;
//                    DEBUG("路 %d 段 %d 车道 %d 限定 %d", lane.road, lane.separate, lane.lane, lane.direct);
                }
                out = lane;
                return true;
            } else {
                // 不完全正交，直接退出
            }
            DEBUG("分道线长短不同，退出");
            return false;
        }
    }
    DEBUG("段匹配失败");
    return false;
}

/*************************************************************************
 * 碰触车道线型
 * @param road
 * @param car
 */
static int CrashRoadLine(road_t &road, const car_model *car)
{
    Line frontAxle, rearAxle;

    MakeLine(&frontAxle, &car->carXY[car->left_front_tire[TIRE_OUTSIDE]], &car->carXY[car->right_front_tire[TIRE_OUTSIDE]]);
    MakeLine(&rearAxle, &car->carXY[car->left_rear_tire[TIRE_OUTSIDE]], &car->carXY[car->right_rear_tire[TIRE_OUTSIDE]]);

    // 检查道路边缘
    for (int i = 0; i < road.leftEdge.size(); ++i) {
        PointF p1, p2;
        Line edge;

        p1 = road.leftEdge[i].points[0];
        for (int j = 1; j < road.leftEdge[i].points.size(); ++j) {
            p2 = road.leftEdge[i].points[j];
            MakeLine(&edge, &p1, &p2);

            if (IntersectionOf(edge, frontAxle) == GM_Intersection ||
                IntersectionOf(edge, rearAxle) == GM_Intersection) {
                // 压道路左边线
                if (road.leftEdge[i].character != LINE_SOLID) {
                    return CRL_EDGE_DOTTED;
                }
                return CRL_EDGE_SOLID;
            }
            p1 = p2;
        }
    }

    for (int i = 0; i < road.rightEdge.size(); ++i) {
        PointF p1, p2;
        Line edge;

        p1 = road.rightEdge[i].points[0];
        for (int j = 1; j < road.rightEdge[i].points.size(); ++j) {
            p2 = road.rightEdge[i].points[j];
            MakeLine(&edge, &p1, &p2);

            if (IntersectionOf(edge, frontAxle) == GM_Intersection ||
                IntersectionOf(edge, rearAxle) == GM_Intersection) {
                // 压道路右边线
                if (road.rightEdge[i].character != LINE_SOLID) {
                    return CRL_EDGE_DOTTED;
                }
                return CRL_EDGE_SOLID;
            }
            p1 = p2;
        }
    }

    // 检查分道线
    for (int i = 0; i < road.separate.size(); ++i) {
        // 段
        PointF p1, p2;
        Line sep;

        for (int j = 0; j < road.separate[i].lines.size(); ++j) {
            // 线
            for (int k = 0; k < road.separate[i].lines[j].size(); ++k) {
                // 节
                p1 = road.separate[i].lines[j][k].points[0];

                for (int m = 1; m < road.separate[i].lines[j][k].points.size(); ++m) {
                    p2 = road.separate[i].lines[j][k].points[m];
                    MakeLine(&sep, &p1, &p2);

                    if (IntersectionOf(sep, frontAxle) == GM_Intersection ||
                        IntersectionOf(sep, rearAxle) == GM_Intersection) {
                        // 检查道路分隔线类型
                        if (road.separate[i].lines[j][k].character == LINE_DOTTED) {
                            // 压虚线
                            return CRL_SEP_DOTTED;
                        } else if (road.separate[i].lines[j][k].character == LINE_SOLID) {
                            // 压实线
                            return CRL_SEP_SOLID;
                        } else if (road.separate[i].lines[j][k].character == LINE_HALF_SOLID_LEFT) {
                            if (LaneIsValid(CurrentLane) && CurrentLane.lane <= j) {
                                return CRL_SEP_SOLID;
                            }
                            return CRL_SEP_DOTTED;
                        } else if (road.separate[i].lines[j][k].character == LINE_HALF_SOLID_RIGHT) {
                            if (LaneIsValid(CurrentLane) && CurrentLane.lane > j) {
                                return CRL_SEP_SOLID;
                            }
                            return CRL_SEP_DOTTED;
                        }
                    }

                    p1 = p2;
                }
            }
        }
    }

    return CRL_NONE;
}

void Rtk2DriveTimer(struct drive_timer &tm, const struct RtkTime *rtkTime)
@@ -2006,17 +1607,17 @@
 */
static void ClearAll(road_exam_map &map)
{
    for (int i = 0; i < map.roads.size(); ++i) {
        map.roads[i].activeStop = map.roads[i].activeBreak = false;
        map.roads[i].arrivedTail = false;
        map.roads[i].errorLane = false;
    }
    for (int i = 0; i < map.specialAreas.size(); i++) {
        map.specialAreas[i].overSpeed = map.specialAreas[i].activeBreak = false;
    }
    for (int i = 0; i < map.triggerLines.size(); ++i) {
        map.triggerLines[i].cmp = false;
    }
//    for (int i = 0; i < map.roads.size(); ++i) {
//        map.roads[i].activeStop = map.roads[i].activeBreak = false;
//        map.roads[i].arrivedTail = false;
//        map.roads[i].errorLane = false;
//    }
//    for (int i = 0; i < map.specialAreas.size(); i++) {
//        map.specialAreas[i].overSpeed = map.specialAreas[i].activeBreak = false;
//    }
//    for (int i = 0; i < map.triggerLines.size(); ++i) {
//        map.triggerLines[i].cmp = false;
//    }
}

static void CheckBreakActive(road_exam_map &map, const car_model *car, LIST_CAR_MODEL &CarModelList,