加入地图标定模式

yy1717

2020-09-16 611f51ed073de48e83f46d47c82cd5ebe61258d7

加入地图标定模式

 lib/src/main/cpp/driver_test.cpp

@@ -54,7 +54,8 @@
enum {
    TEST_TYPE_AREA = 2,
    TEST_TYPE_ROAD_DUMMY_LIGHT,
    TEST_TYPE_ROAD_TRUE_LIGHT
    TEST_TYPE_ROAD_TRUE_LIGHT,
    TEST_TYPE_ROAD_CALIBRATE
};

static bool ExamStart = false;
@@ -109,7 +110,8 @@

    RoadMap.roads.clear();
    RoadMap.specialAreas.clear();
    RoadMap.triggerLines.clear();
    RoadMap.forbidLines.clear();
    RoadMap.examScheme.clear();

    CarSensorInit();

@@ -192,18 +194,22 @@

    vector<road_t>().swap(RoadMap.roads);
    vector<special_area_t>().swap(RoadMap.specialAreas);
    vector<trigger_line_t>().swap(RoadMap.triggerLines);
    vector<forbid_line_t>().swap(RoadMap.forbidLines);
    vector<scheme_t>().swap(RoadMap.examScheme);
}

void SetRoadMap(road_exam_map &map)
void SetRoadMap(road_exam_map &map, vector<scheme_t> &scheme)
{
    if (ExamStart) return;

    RoadMap.roads.assign(map.roads.begin(), map.roads.end());
    RoadMap.specialAreas.assign(map.specialAreas.begin(), map.specialAreas.end());
    RoadMap.triggerLines.assign(map.triggerLines.begin(), map.triggerLines.end());
//    RoadMap.triggerLines.assign(map.triggerLines.begin(), map.triggerLines.end());
    RoadMap.forbidLines.assign(map.forbidLines.begin(), map.forbidLines.end());
    DEBUG("得到新的路考地图 路数量 %d 特殊区域数量 %d 触发线数量 %d 其他禁止线数量 %d", RoadMap.roads.size(), RoadMap.specialAreas.size(), RoadMap.triggerLines.size(), RoadMap.forbidLines.size());

    RoadMap.examScheme.assign(scheme.begin(), scheme.end());

    DEBUG("得到新的路考地图 路数量 %d 特殊区域数量 %d 其他禁止线数量 %d 项目数量 %d", RoadMap.roads.size(), RoadMap.specialAreas.size(), RoadMap.forbidLines.size(), RoadMap.examScheme.size());
}

void SetCarMeasurePoint(double *basePoint, int *axial, int *left_front_tire,
@@ -402,12 +408,18 @@

            if (type == TEST_TYPE_ROAD_DUMMY_LIGHT) {
                exam_dummy_light = 0;           //0
                RoadMap.calibrate = 0;
            }
            if (type == TEST_TYPE_ROAD_TRUE_LIGHT) {
                RoadMap.calibrate = 0;
                InitRoadExam(RoadMap);
            }
            if (type == TEST_TYPE_AREA) {
                InitAreaExam();
            }
            if (type == TEST_TYPE_ROAD_CALIBRATE) {
                RoadMap.calibrate = 1;
                InitRoadExam(RoadMap);
            }
        }
        MA_SendExamStatus(1, 0);
@@ -865,6 +877,27 @@
    return true;
}

void RoadChange(int road, int status)
{
    struct roadStatusBrief brief;

    brief.road_id = road;
    brief.status = status;

    MA_SendRoadStatus(&brief);
}

void CrossingChange(int road, int crossing, int status)
{
    struct crossingStatusBrief brief;

    brief.road_id = road;
    brief.crossing_index = crossing;
    brief.status = status;

    MA_SendCrossingStatus(&brief);
}

void AddExamFault(int wrong, const struct RtkTime *rtkTime)
{
    struct ExamFault fault;

 lib/src/main/cpp/driver_test.h

@@ -137,16 +137,14 @@
} separate_t;

typedef struct {
    int active;
    string tts;
    bool stopFlag;
    Line line;
    bool centrePointValid;
    PointF centrePoint;
} stop_line_t;

typedef struct {
    int id;
    Line startLine;
//    Line stopLine;
//    int active;         // 到达路口尾部的行进方向
    bool activeBreak;   // 路口刹车减速
@@ -175,15 +173,9 @@
} special_area_t;

typedef struct {
    int id;
    int road;
    int active;
    int time;                   // 项目最大完成时间
    int distance;               // 项目最大完成距离
    string tts;
    bool cmp;
    std::vector<PointF> points;
    std::vector<PointF> leftPoints; // 对应到道路左侧的点
} trigger_line_t;

typedef struct {
@@ -192,11 +184,25 @@
    std::vector<PointF> points;
} forbid_line_t;

typedef struct {
    int road_id;
    int index;
    int active;
} crossing_active_t;

typedef struct {
    std::string name;
    std::vector<crossing_active_t> crossingActive;
    std::vector<trigger_line_t> triggerLines;
} scheme_t;

struct road_exam_map {
    int calibrate;
    std::vector<road_t> roads;
    std::vector<special_area_t> specialAreas;
    std::vector<trigger_line_t> triggerLines;
//    std::vector<trigger_line_t> triggerLines;
    std::vector<forbid_line_t> forbidLines;
    std::vector<scheme_t> examScheme;
};

struct area_exam_map {
@@ -218,7 +224,7 @@


void CleanRoadMap(void);
void SetRoadMap(road_exam_map &map);
void SetRoadMap(road_exam_map &map, vector<scheme_t> &scheme);

void SetCarMeasurePoint(double *basePoint, int *axial, int *left_front_tire,
                        int *right_front_tire, int *left_rear_tire, int *right_rear_tire,
@@ -228,6 +234,8 @@

void UpdateRTKInfo(const rtk_info *s);
void AddExamFault(int wrong, const struct RtkTime *rtkTime);
void RoadChange(int road, int status);
void CrossingChange(int road, int crossing, int status);

void SystemShutdown(int event, int timeout);


 lib/src/main/cpp/master/comm_if.cpp

@@ -64,6 +64,9 @@
#define ID_SM_DISTANCE              0x0020
#define ID_SM_CARSENSOR              0x0013

#define ID_MS_ROAD_BRIEF        0x0015              // 科目三项目标定，进出某条路的提示
#define ID_MS_CROSSING_BRIEF    0x0016              // 科目三项目标定，接近/驶离某各路口的提示

#define MA_OUT_GPS_BRIEF        0x0001
#define MA_OUT_RTK_BRIEF        0x0002
#define MA_OUT_CAR_BRIEF        0x0004
@@ -438,6 +441,38 @@
    writer.EndObject();

    SendMsgToMainProcIndep(ID_SM_DISTANCE, sb.GetString());
}

void MA_SendRoadStatus(const struct roadStatusBrief *brief)
{
    StringBuffer sb;
    Writer<StringBuffer> writer(sb);

    writer.StartObject();
    writer.Key("road_id");
    writer.Int(brief->road_id);
    writer.Key("status");
    writer.Int(brief->status);
    writer.EndObject();

    SendMsgToMainProcIndep(ID_MS_ROAD_BRIEF, sb.GetString());
}

void MA_SendCrossingStatus(const struct crossingStatusBrief *brief)
{
    StringBuffer sb;
    Writer<StringBuffer> writer(sb);

    writer.StartObject();
    writer.Key("road_id");
    writer.Int(brief->road_id);
    writer.Key("crossing_index");
    writer.Int(brief->crossing_index);
    writer.Key("status");
    writer.Int(brief->status);
    writer.EndObject();

    SendMsgToMainProcIndep(ID_MS_CROSSING_BRIEF, sb.GetString());
}

void MA_SendExamWrong(vector<ExamFault> &ExamFaultList)
@@ -844,10 +879,6 @@

                road_exam_map map;

                map.roads.clear();
                map.specialAreas.clear();
                map.triggerLines.clear();

                if (doc.HasMember("road")) {
                    const Value &a = doc["road"];

@@ -860,23 +891,6 @@
                                DEBUG("路id %d", s.GetInt());
                                road.id = s.GetInt();
                            }
                            if (itr->HasMember("start_line")) {
                                const Value &a2 = (*itr)["start_line"];

                                PointF p1, p2;
                                int n = 0;

                                for (Value::ConstValueIterator itr2 = a2.Begin(); itr2 != a2.End(); ++itr2, ++n) {
                                    DEBUG("起始线 %d", (*itr2).GetInt());
                                    if (n == 0) {
                                        p1 = mapPoints[(*itr2).GetInt()];
                                    } else if (n == 1) {
                                        p2 = mapPoints[(*itr2).GetInt()];
                                    }
                                }

                                MakeLine(&road.startLine, &p1, &p2);
                            }
                            if (itr->HasMember("crossing")) {
                                const Value &a2 = (*itr)["crossing"];

@@ -887,20 +901,12 @@
                                    if (!itr2->IsObject()) {
                                        break;
                                    }
                                    if (itr2->HasMember("active")) {
                                        const Value &s = (*itr2)["active"];
                                        temp.active = s.GetInt();
                                        DEBUG("路口动作 %d", temp.active);
                                    }
                                    if (itr2->HasMember("stop_flag")) {
                                        const Value &s = (*itr2)["stop_flag"];
                                        temp.stopFlag = s.GetInt();
                                        DEBUG("路口停车 %d", temp.stopFlag);
                                    }
                                    if (itr2->HasMember("tts")) {
                                        const Value &s = (*itr2)["tts"];
                                        temp.tts = s.GetString();
                                        DEBUG("路口提示 %s", temp.tts.c_str());
                                    } else {
                                        temp.stopFlag = false;
                                    }
                                    if (itr2->HasMember("line")) {
                                        const Value &s = (*itr2)["line"];
@@ -920,8 +926,10 @@
                                    }
                                    if (itr2->HasMember("center_point")) {
                                        const Value &s = (*itr2)["center_point"];

                                        temp.centrePoint = mapPoints[s.GetInt()];
                                        temp.centrePointValid = true;
                                    } else {
                                        temp.centrePointValid = false;
                                    }
                                    crossing.push_back(temp);
                                }
@@ -1092,7 +1100,7 @@
                        map.specialAreas.push_back(specialArea);
                    }
                }
                if (doc.HasMember("trigger_line")) {
                /*if (doc.HasMember("trigger_line")) {
                    const Value &a = doc["trigger_line"];

                    for (Value::ConstValueIterator itr = a.Begin();
@@ -1139,7 +1147,7 @@

                        map.triggerLines.push_back(trigger);
                    }
                }
                }*/
                if (doc.HasMember("red_line")) {
                    const Value &a = doc["red_line"];

@@ -1169,10 +1177,86 @@
                    }
                }

                vector<scheme_t> schemes;

                if (doc.HasMember("scheme")) {
                    const Value &a = doc["scheme"];

                    for (Value::ConstValueIterator itr = a.Begin();
                         itr != a.End(); ++itr) {
                        scheme_t scheme;
                        if (itr->HasMember("name")) {
                            const Value &s = (*itr)["name"];
                            scheme.name = s.GetString();
                        }
                        if (itr->HasMember("crossing_active")) {
                            const Value &a2 = (*itr)["crossing_active"];

                            for (Value::ConstValueIterator itr2 = a2.Begin();
                                 itr2 != a2.End(); ++itr2) {
                                crossing_active_t act;

                                if (itr2->HasMember("road")) {
                                    const Value &s = (*itr2)["road"];
                                    act.road_id = s.GetInt();
                                }
                                if (itr2->HasMember("idx")) {
                                    const Value &s = (*itr2)["idx"];
                                    act.index = s.GetInt();
                                }
                                if (itr2->HasMember("active")) {
                                    const Value &s = (*itr2)["active"];
                                    act.active = s.GetInt();
                                }

                                scheme.crossingActive.push_back(act);
                            }
                        }
                        if (itr->HasMember("trigger_line")) {
                            const Value &a2 = (*itr)["trigger_line"];

                            for (Value::ConstValueIterator itr2 = a2.Begin();
                                 itr2 != a2.End(); ++itr2) {
                                trigger_line_t ins;

                                if (itr2->HasMember("x_y")) {
                                    const Value &a3 = (*itr2)["x_y"];
                                    if (a3.IsArray()) {
                                        PointF point;
                                        int n = 0;
                                        for (Value::ConstValueIterator itr3 = a3.Begin();
                                             itr3 != a3.End(); ++itr3) {
                                            if (n == 0) {
                                                point.X = (*itr3).GetDouble();
                                                n = 1;
                                            } else if (n == 1) {
                                                point.Y = (*itr3).GetDouble();
                                                ins.points.push_back(point);
                                                n = 0;
                                            }
                                        }
                                    }
                                }
                                if (itr2->HasMember("road")) {
                                    const Value &s = (*itr2)["road"];
                                    ins.road = s.GetInt();
                                }
                                if (itr2->HasMember("type")) {
                                    const Value &s = (*itr2)["type"];
                                    ins.active = s.GetInt();
                                }

                                scheme.triggerLines.push_back(ins);
                            }
                        }
                        schemes.push_back(scheme);
                    }
                }

                DEBUG("地图解析完毕");

                CleanRoadMap();
                SetRoadMap(map);
                SetRoadMap(map, schemes);
            } else {
                DEBUG("############## 地图解析出错###################");
            }

 lib/src/main/cpp/master/comm_if.h

@@ -104,6 +104,17 @@
    int pointNum;
};

struct roadStatusBrief {
    int road_id;
    int status;
};

struct crossingStatusBrief {
    int road_id;
    int crossing_index;
    int status;
};

void MA_MainProcMsgEntry(int cmd, const char *value);
void MA_MainProcBinMsgEntry(int cmd, const uint8_t *value, int length);

@@ -131,4 +142,7 @@

void MA_SendDistance(double l, double r);

void MA_SendRoadStatus(const struct roadStatusBrief *brief);
void MA_SendCrossingStatus(const struct crossingStatusBrief *brief);

#endif //MYAPPLICATION2_COMM_IF_H

 lib/src/main/cpp/native-lib.cpp

@@ -27,8 +27,8 @@
const int RTK_PLATFORM_PORT = 12125;
const uint8_t phone[] = {0x20,0x19,0x10,0x15,0x00,0x00,0x00,0x01};

const char *VIRTUAL_RTK_IP = "192.168.1.7";
const int VIRTUAL_RTK_PORT = 9001;
const char *VIRTUAL_RTK_IP = "192.168.16.100";
const int VIRTUAL_RTK_PORT = 9002;

static pthread_mutex_t tts_mutex = PTHREAD_MUTEX_INITIALIZER;


 lib/src/main/cpp/rtk_module/rtk.cpp

@@ -214,7 +214,7 @@
        }*/

        if (RxBufLen > 0) {
#if 1
#if 0
            const uint8_t *ptr = parseGPS(RxBuf, RxBuf + RxBufLen);
            if(ptr != RxBuf) {
                memcpy(RxBuf, ptr, RxBufLen - (ptr - RxBuf));

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

@@ -155,7 +155,7 @@
static void DetectTurn(const car_model *car, int moveDirect, const struct RtkTime *rtkTime);
static bool StopOnRedArea(road_exam_map &RoadMap, const car_model *car);

static int NearbyCrossingGuide(int &stopLineIndex, int roadIndex, road_t &road, const car_model *car);
static int NearbyCrossingGuide(road_exam_map &RoadMap, int &stopLineIndex, int roadIndex, road_t &road, const car_model *car);

static int EntryItem(int index, road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList);

@@ -1109,7 +1109,7 @@
    int stop_line_index;
    PointF nearbyStopPoint;

    NearbyCrossingGuide(stop_line_index, roadIndex, RoadMap.roads[roadIndex], car);
    NearbyCrossingGuide(RoadMap, stop_line_index, roadIndex, RoadMap.roads[roadIndex], car);
    nearbyStopPoint.X = RoadMap.roads[roadIndex].stopLine[stop_line_index].line.X1;
    nearbyStopPoint.Y = RoadMap.roads[roadIndex].stopLine[stop_line_index].line.Y1;

@@ -1159,6 +1159,20 @@
    return rec;
}

int ExamSchemeCrossing(road_exam_map &RoadMap, int roadIndex, int crossIndex)
{
    if (roadIndex < 0 || roadIndex >= RoadMap.roads.size() || RoadMap.examScheme.size() == 0) {
        return -1;
    }
    for (int i = 0; i < RoadMap.examScheme[0].crossingActive.size(); ++i) {
        if (RoadMap.examScheme[0].crossingActive[i].road_id == RoadMap.roads[roadIndex].id &&
                RoadMap.examScheme[0].crossingActive[i].index == crossIndex) {
            return RoadMap.examScheme[0].crossingActive[i].active;
        }
    }
    return -1;
}

static void HintCrossing(road_exam_map &RoadMap, int roadIndex, int stopIndex, double distance) {
    if (roadIndex < 0 || roadIndex >= RoadMap.roads.size())
        return;
@@ -1166,18 +1180,47 @@
    if (distance > 5 && distance < 70) {
        // 提示路口怎么走
        if (GetCrossingStatus(roadIndex, stopIndex) == CROSSING_NOT_HINT) {
            if (!RoadMap.roads[roadIndex].stopLine[stopIndex].tts.empty()) {
                DEBUG("路口提示 %s", RoadMap.roads[roadIndex].stopLine[stopIndex].tts.c_str());
                PlayTTS(RoadMap.roads[roadIndex].stopLine[stopIndex].tts.c_str(), NULL);
            // 项目标定模式下，这里提示标定本路口动作
            if (RoadMap.calibrate) {
                CrossingChange(RoadMap.roads[roadIndex].id, stopIndex, 1);
            } else {
                int act = ExamSchemeCrossing(RoadMap, roadIndex, stopIndex);

                switch (act) {
                    case ROAD_ACTIVE_FORWARD:
                        DEBUG("路口提示 直行");
                        PlayTTS("前方路口请直行", NULL);
                        break;
                    case ROAD_ACTIVE_TURN_LEFT:
                        DEBUG("路口提示 左转");
                        PlayTTS("前方路口请左转", NULL);
                        break;
                    case ROAD_ACTIVE_TURN_RIGHT:
                        DEBUG("路口提示 右转");
                        PlayTTS("前方路口请右转", NULL);
                        break;
                    case ROAD_ACTIVE_TURN_BACKWARD:
                        DEBUG("路口提示 掉头");
                        PlayTTS("前方路口请掉头", NULL);
                        break;
                    default:
                        DEBUG("路口提示 未配置");
                        PlayTTS("前方路口听教练指令", NULL);
                        break;
                }
            }
            ChangeCrossingStatus(roadIndex, stopIndex, CROSSING_HAS_HINT);
        }
    } else if (distance > 75 && GetCrossingStatus(roadIndex, stopIndex) != CROSSING_NOT_HINT) {
        ChangeCrossingStatus(roadIndex, stopIndex, CROSSING_NOT_HINT);

        if (RoadMap.calibrate) {
            CrossingChange(RoadMap.roads[roadIndex].id, stopIndex, 0);
        }
    }
}

static int NearbyCrossingGuide(int &stopLineIndex, int roadIndex, road_t &road, const car_model *car)
static int NearbyCrossingGuide(road_exam_map &RoadMap, int &stopLineIndex, int roadIndex, road_t &road, const car_model *car)
{
    int guide = 0, stopLine = 0;
    double distance;
@@ -1203,7 +1246,7 @@
            struct nearby_crossing temp;

            temp.stopLine = i;
            temp.guide = road.stopLine[i].active;
            temp.guide = ExamSchemeCrossing(RoadMap, roadIndex, i);
            temp.distance = distance;

            set.push_back(temp);
@@ -1253,157 +1296,169 @@
        turnSignalStatus = ReadCarSensorValue(TURN_SIGNAL_LAMP);
    }

    // 刹车提示音
    RingBreak();
    if (RoadMap.calibrate == 0) {
        // 刹车提示音
        RingBreak();

    // 超速检测
    if (speed > MAX_SPEED) {
        if (!occurOverSpeed) {
            occurOverSpeed = true;
            // 超速，不合格
            DEBUG("超速 %f", ConvertMs2KMh(speed));
            AddExamFault(10, rtkTime);
        // 超速检测
        if (speed > MAX_SPEED) {
            if (!occurOverSpeed) {
                occurOverSpeed = true;
                // 超速，不合格
                DEBUG("超速 %f", ConvertMs2KMh(speed));
                AddExamFault(10, rtkTime);
            }
        } else if (speed < DEC_MAX_SPEED) {
            occurOverSpeed = false;
        }
    } else if (speed < DEC_MAX_SPEED ) {
        occurOverSpeed = false;
    }

    // 副刹车检测
    if (ReadCarStatus(SECOND_BREAK) == BREAK_ACTIVE) {
        // 副刹车踩下，不合格
        if (!occurSecondBreak) {
            DEBUG("副刹车动作了");
            occurSecondBreak = true;
            AddExamFault(17, rtkTime);
        // 副刹车检测
        if (ReadCarStatus(SECOND_BREAK) == BREAK_ACTIVE) {
            // 副刹车踩下，不合格
            if (!occurSecondBreak) {
                DEBUG("副刹车动作了");
                occurSecondBreak = true;
                AddExamFault(17, rtkTime);
            }
        } else {
            occurSecondBreak = false;
        }
    } else {
        occurSecondBreak = false;
    }

    // 挡位匹配检测
    bool currGearError = false;
    bool currGearNSlide = false;
        // 挡位匹配检测
        bool currGearError = false;
        bool currGearNSlide = false;

    switch (ReadCarStatus(GEAR)) {
        case GEAR_N:
            if (moveDirect != 0) {
                // 空档滑行
                currGearNSlide = true;
            }
            break;
        case GEAR_1:
            if (ConvertMs2KMh(speed) < SPEED_GEAR_TABLE[0][0] || ConvertMs2KMh(speed) > SPEED_GEAR_TABLE[0][1]) {
                currGearError = true;
            }
            break;
        case GEAR_2:
            if (ConvertMs2KMh(speed) < SPEED_GEAR_TABLE[1][0] || ConvertMs2KMh(speed) > SPEED_GEAR_TABLE[1][1]) {
                currGearError = true;
            }
            break;
        case GEAR_3:
            if (ConvertMs2KMh(speed) < SPEED_GEAR_TABLE[2][0] || ConvertMs2KMh(speed) > SPEED_GEAR_TABLE[2][1]) {
                currGearError = true;
            }
            break;
        case GEAR_4:
            if (ConvertMs2KMh(speed) < SPEED_GEAR_TABLE[3][0] || ConvertMs2KMh(speed) > SPEED_GEAR_TABLE[3][1]) {
                currGearError = true;
            }
            break;
        case GEAR_5:
            if (ConvertMs2KMh(speed) < SPEED_GEAR_TABLE[4][0] || ConvertMs2KMh(speed) > SPEED_GEAR_TABLE[4][1]) {
                currGearError = true;
            }
            break;
        default:break;
    }
    // 空档滑行超时
    if (currGearNSlide) {
        if (GearNSlideStatus == 0) {
            DEBUG("检测到空挡滑行");
            GearNSlideStatus = 1;
            gearNSlideTimePoint = *rtkTime;
        switch (ReadCarStatus(GEAR)) {
            case GEAR_N:
                if (moveDirect != 0) {
                    // 空档滑行
                    currGearNSlide = true;
                }
                break;
            case GEAR_1:
                if (ConvertMs2KMh(speed) < SPEED_GEAR_TABLE[0][0] ||
                    ConvertMs2KMh(speed) > SPEED_GEAR_TABLE[0][1]) {
                    currGearError = true;
                }
                break;
            case GEAR_2:
                if (ConvertMs2KMh(speed) < SPEED_GEAR_TABLE[1][0] ||
                    ConvertMs2KMh(speed) > SPEED_GEAR_TABLE[1][1]) {
                    currGearError = true;
                }
                break;
            case GEAR_3:
                if (ConvertMs2KMh(speed) < SPEED_GEAR_TABLE[2][0] ||
                    ConvertMs2KMh(speed) > SPEED_GEAR_TABLE[2][1]) {
                    currGearError = true;
                }
                break;
            case GEAR_4:
                if (ConvertMs2KMh(speed) < SPEED_GEAR_TABLE[3][0] ||
                    ConvertMs2KMh(speed) > SPEED_GEAR_TABLE[3][1]) {
                    currGearError = true;
                }
                break;
            case GEAR_5:
                if (ConvertMs2KMh(speed) < SPEED_GEAR_TABLE[4][0] ||
                    ConvertMs2KMh(speed) > SPEED_GEAR_TABLE[4][1]) {
                    currGearError = true;
                }
                break;
            default:
                break;
        }
        if (GearNSlideStatus == 1 && TimeGetDiff(rtkTime, &gearNSlideTimePoint) > GEAR_N_SLIDE_TIMEOUT) {
            // 空档滑行超5秒，不合格
            DEBUG("挡位滑行，超过5秒");
            AddExamFault(8, rtkTime);
            GearNSlideStatus = 2;
        }
    } else if (GearNSlideStatus != 0) {
        GearNSlideStatus = 0;
        DEBUG("空挡滑行结束");
    }
    
    // 挡位不匹配超时
    if (currGearError && prevGearError) {
        DEBUG("挡位错误增加 %ld毫秒 当前挡位 %d 时速 %f", TimeGetDiff(rtkTime, &gearErrorTimePoint), ReadCarStatus(GEAR), ConvertMs2KMh(speed));
        gearErrorTime += TimeGetDiff(rtkTime, &gearErrorTimePoint);
    }
    if (gearErrorTime > GEAR_ERROR_TIMEOUT) {
        // 累计15秒，挡位-车速不匹配，不合格
        DEBUG("挡位错误超过15秒");
        AddExamFault(6, rtkTime);
        gearErrorTime = 0;
    }

    prevGearError = currGearError;
    if (prevGearError) {
        gearErrorTimePoint = *rtkTime;
    }

    // 起步后滑
    if (moveDirect != prevMoveDirect) {
        if (moveDirect == 0) {
            stopTimepoint = *rtkTime;
            StopCarOnRedArea = false;

            DEBUG("停车了 %d %d %d %d %d %d %d", rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss);

            if (slideNormalDistance) {
                // 后滑，扣10分
                AddExamFault(18, rtkTime);
                DEBUG("后滑超过10厘米, 但不超过30厘米");
        // 空档滑行超时
        if (currGearNSlide) {
            if (GearNSlideStatus == 0) {
                DEBUG("检测到空挡滑行");
                GearNSlideStatus = 1;
                gearNSlideTimePoint = *rtkTime;
            }

            slideNormalDistance = false;
            slideLongDistance = false;
            occurSlide = false;
        } else if (moveDirect == -1 && prevMoveDirect == 0) {
            DEBUG("开始后滑");
            stopPoint = car->basePoint;
            occurSlide = true;
        }
        prevMoveDirect = moveDirect;
    } else if (moveDirect == 0) {
        // 持续停车
        if (TimeGetDiff(rtkTime, &stopTimepoint) >= STOP_CAR_TIME && !StopCarOnRedArea && StopOnRedArea(RoadMap, car)) {
            // 停车超2秒，停在红区，不合格
            AddExamFault(16, rtkTime);
            DEBUG("禁停区停车");
            StopCarOnRedArea = true;
        }
    } else if (moveDirect == -1) {
        // 持续后滑
        if (occurSlide) {
            double slideDistance = DistanceOf(stopPoint, car->basePoint);

            if (slideDistance > SLIDE_DISTANCE_THRESHOLD_YELLOW) {
                slideNormalDistance = true;
            if (GearNSlideStatus == 1 &&
                TimeGetDiff(rtkTime, &gearNSlideTimePoint) > GEAR_N_SLIDE_TIMEOUT) {
                // 空档滑行超5秒，不合格
                DEBUG("挡位滑行，超过5秒");
                AddExamFault(8, rtkTime);
                GearNSlideStatus = 2;
            }
        } else if (GearNSlideStatus != 0) {
            GearNSlideStatus = 0;
            DEBUG("空挡滑行结束");
        }

            if (slideDistance > SLIDE_DISTANCE_THRESHOLD_RED && !slideLongDistance) {
                // 后滑超过30厘米, 不合格
                AddExamFault(5, rtkTime);
                DEBUG("后滑超过30厘米");
                slideLongDistance = true;
        // 挡位不匹配超时
        if (currGearError && prevGearError) {
            DEBUG("挡位错误增加 %ld毫秒 当前挡位 %d 时速 %f", TimeGetDiff(rtkTime, &gearErrorTimePoint),
                  ReadCarStatus(GEAR), ConvertMs2KMh(speed));
            gearErrorTime += TimeGetDiff(rtkTime, &gearErrorTimePoint);
        }
        if (gearErrorTime > GEAR_ERROR_TIMEOUT) {
            // 累计15秒，挡位-车速不匹配，不合格
            DEBUG("挡位错误超过15秒");
            AddExamFault(6, rtkTime);
            gearErrorTime = 0;
        }

        prevGearError = currGearError;
        if (prevGearError) {
            gearErrorTimePoint = *rtkTime;
        }

        // 起步后滑
        if (moveDirect != prevMoveDirect) {
            if (moveDirect == 0) {
                stopTimepoint = *rtkTime;
                StopCarOnRedArea = false;

                DEBUG("停车了 %d %d %d %d %d %d %d", rtkTime->YY, rtkTime->MM, rtkTime->DD,
                      rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss);

                if (slideNormalDistance) {
                    // 后滑，扣10分
                    AddExamFault(18, rtkTime);
                    DEBUG("后滑超过10厘米, 但不超过30厘米");
                }

                slideNormalDistance = false;
                slideLongDistance = false;
                occurSlide = false;
            } else if (moveDirect == -1 && prevMoveDirect == 0) {
                DEBUG("开始后滑");
                stopPoint = car->basePoint;
                occurSlide = true;
            }
            prevMoveDirect = moveDirect;
        } else if (moveDirect == 0) {
            // 持续停车
            if (TimeGetDiff(rtkTime, &stopTimepoint) >= STOP_CAR_TIME && !StopCarOnRedArea &&
                StopOnRedArea(RoadMap, car)) {
                // 停车超2秒，停在红区，不合格
                AddExamFault(16, rtkTime);
                DEBUG("禁停区停车");
                StopCarOnRedArea = true;
            }
        } else if (moveDirect == -1) {
            // 持续后滑
            if (occurSlide) {
                double slideDistance = DistanceOf(stopPoint, car->basePoint);

                if (slideDistance > SLIDE_DISTANCE_THRESHOLD_YELLOW) {
                    slideNormalDistance = true;
                }

                if (slideDistance > SLIDE_DISTANCE_THRESHOLD_RED && !slideLongDistance) {
                    // 后滑超过30厘米, 不合格
                    AddExamFault(5, rtkTime);
                    DEBUG("后滑超过30厘米");
                    slideLongDistance = true;
                    slideNormalDistance = false;
                    occurSlide = false;
                }
            }
        } else {
            // 前进
        }
    } else {
        // 前进
    }

    // 检测离开此路段，全车需不在范围内
@@ -1415,6 +1470,15 @@
        if (oldid >= 0) {
            ResetCrossingStatus(oldid);
            ResetErrorLaneRpt(oldid);

        }

        if (RoadMap.calibrate) {
            if (currExamMapIndex < 0 && oldid >= 0) {
                RoadChange(oldid, 0);
            } else if (currExamMapIndex >= 0) {
                RoadChange(currExamMapIndex, 1);
            }
        }
        Lane.guide = 0;
    }
@@ -1426,41 +1490,44 @@
    }
    ExitTarget(RoadMap, car, CarModelList, rtkTime);

    oldid = CrashLineType;
    // 检测压线状态
    DetectLine(currExamMapIndex, RoadMap, car, CarModelList, moveDirect, rtkTime);
    if (RoadMap.calibrate == 0) {
        oldid = CrashLineType;
        // 检测压线状态
        DetectLine(currExamMapIndex, RoadMap, car, CarModelList, moveDirect, rtkTime);

    if (oldid != CrashLineType) {
        DEBUG("压线类型切换 %d", CrashLineType);
    }
        if (oldid != CrashLineType) {
            DEBUG("压线类型切换 %d", CrashLineType);
        }


    oldid = Lane.guide;
    DetectLane(RoadMap, car, currExamMapIndex, rtkTime);
        oldid = Lane.guide;
        DetectLane(RoadMap, car, currExamMapIndex, rtkTime);

//    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 (Lane.guide > 0 && currExamMapIndex >= 0) {
            int stop_line_index;
            int act = NearbyCrossingGuide(RoadMap, stop_line_index, currExamMapIndex,
                                          RoadMap.roads[currExamMapIndex], car);

        if (act != 0 && !(act & Lane.guide)) {
            if (!GetErrorLaneRpt(currExamMapIndex, stop_line_index)) {
                DEBUG("不按规定车道标向行驶 %d: %d  期望 = %d guide = %d", currExamMapIndex, stop_line_index, act, Lane.guide);
                AddExamFault(9, rtkTime);
                SetErrorLaneRpt(currExamMapIndex, stop_line_index, true);
            if (act != 0 && !(act & Lane.guide)) {
                if (!GetErrorLaneRpt(currExamMapIndex, stop_line_index)) {
                    DEBUG("不按规定车道标向行驶 %d: %d  期望 = %d guide = %d", currExamMapIndex,
                          stop_line_index, act, Lane.guide);
                    AddExamFault(9, rtkTime);
                    SetErrorLaneRpt(currExamMapIndex, stop_line_index, true);
                }
            }
        }
    }

    if (Lane.guide != oldid) {
        DEBUG("导向类型切换 %d", Lane.guide);
        if (Lane.guide != oldid) {
            DEBUG("导向类型切换 %d", Lane.guide);
        }
    }

    if (currExamMapIndex >= 0 && Lane.guide == 0) {
        BigStraightRoadFree = AnalysisRoad(RoadMap, currExamMapIndex, Lane, car);

        road_end_point_t ep = NearbyRoadEndPoint(currExamMapIndex, RoadMap, car);

        // 提示路口怎么走
        HintCrossing(RoadMap, ep.road_index, ep.stop_line_index, ep.distance);

        double freeSepDis = SeparateLength(RoadMap, Lane, car);
@@ -1476,10 +1543,15 @@
    }

    // 额外的转向检测
    DetectTurn(car, moveDirect, rtkTime);

    ItemExam(RoadMap, currExamMapIndex, car, CarModelList, speed, moveDirect, rtkTime, BigStraightRoadFree, TargetFree > RoadCrossingFree? RoadCrossingFree : TargetFree);
    ItemExam2(RoadMap, currExamMapIndex, car, CarModelList);
    if (RoadMap.calibrate == 0) {
        DetectTurn(car, moveDirect, rtkTime);

        ItemExam(RoadMap, currExamMapIndex, car, CarModelList, speed, moveDirect, rtkTime,
                 BigStraightRoadFree,
                 TargetFree > RoadCrossingFree ? RoadCrossingFree : TargetFree);
        ItemExam2(RoadMap, currExamMapIndex, car, CarModelList);
    }
}

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)
@@ -1605,8 +1677,11 @@
    }
}

void CrossRoadCallback(int road, int stop_line, int active, const car_model *car)
void CrossRoadCallback(road_exam_map &RoadMap, int road, int stop_line, int active, const car_model *car)
{
    if (RoadMap.calibrate) {
        CrossingChange(RoadMap.roads[road].id, stop_line, 0);
    }
    SetErrorLaneRpt(road, stop_line, false);
    if (active != ROAD_ACTIVE_FORWARD) {
        ResetTurnDetect(car);
@@ -1823,12 +1898,12 @@

static int EntryItem(int index, road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList)
{
    if (index < 0 || index >= RoadMap.roads.size())
    if (index < 0 || index >= RoadMap.roads.size() || RoadMap.examScheme.size() == 0)
        return -1;

    for (int i = 0; i < RoadMap.triggerLines.size(); ++i) {
        if (RoadMap.triggerLines[i].road == RoadMap.roads[index].id) {
            Line triggerLine;
    for (int j = 0; j < RoadMap.examScheme[0].triggerLines.size(); ++j) {
        if (RoadMap.examScheme[0].triggerLines[j].road == RoadMap.roads[index].id) {
            /*Line triggerLine;

            PointF p2 = CalcProjectionWithRoadEdge(RoadMap.roads[index].leftEdge, RoadMap.triggerLines[i].points[0]);

@@ -1837,6 +1912,25 @@
            if (CrashTheLine(triggerLine, car, CarModelList)) {
                DEBUG("触发项目 %d %s (%0.4f, %0.4f)-(%0.4f, %0.4f)", RoadMap.triggerLines[i].active, RoadMap.triggerLines[i].tts.c_str(), triggerLine.X1, triggerLine.Y1, triggerLine.X2, triggerLine.Y2);
                return RoadMap.triggerLines[i].active;
            }*/

            vector<double> vec;

            PointF p1 = CalcProjectionWithRoadEdge(RoadMap.roads[index].rightEdge,
                                                   car->carXY[car->axial[AXIAL_FRONT]]);
            PointF p2 = CalcProjectionWithRoadEdge(RoadMap.roads[index].rightEdge,
                                                   RoadMap.examScheme[0].triggerLines[j].points[0]);

            vec.push_back(DistanceOf(car->carXY[car->axial[AXIAL_FRONT]],
                                     RoadMap.examScheme[0].triggerLines[j].points[0]));
            vec.push_back(DistanceOf(p2, RoadMap.examScheme[0].triggerLines[j].points[0]));
            vec.push_back(DistanceOf(p1, car->carXY[car->axial[AXIAL_FRONT]]));

            sort(vec.begin(), vec.end());

            if (fabs(vec[0] + vec[1] - vec[2]) < 0.1) {
                DEBUG("触发项目 %d", RoadMap.examScheme[0].triggerLines[j].active);
                return RoadMap.examScheme[0].triggerLines[j].active;
            }
        }
    }

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

@@ -29,7 +29,8 @@

car_sensor_value_t ReadCarSensorValue(int name);

void CrossRoadCallback(int road, int stop_line, int active, const car_model *car);
void CrossRoadCallback(road_exam_map &RoadMap, int road, int stop_line, int active, const car_model *car);
int ExamSchemeCrossing(road_exam_map &RoadMap, int roadIndex, int crossIndex);

void InitRoadExam(road_exam_map &RoadMap);
void TerminateRoadExam(void);

 lib/src/main/cpp/test_items2/through_something.cpp

@@ -104,6 +104,10 @@
            RemoveTargetReduceRec(TargetReduceRec, key);
        }
    }

    if (RoadMap.calibrate == 1)
        goto GET_DISTANCE_OF_NEARBY_TARGET;

    // 人行道、公交站、学校
    for (int i = 0; i < RoadMap.specialAreas.size(); i++) {
        if (RoadMap.specialAreas[i].type == GRID_AREA || RoadMap.specialAreas[i].road != RoadMap.roads[roadIndex].id)
@@ -165,6 +169,7 @@
        }
    }

    GET_DISTANCE_OF_NEARBY_TARGET:
    if (nearbyTarget.size() > 0) {
        sort(nearbyTarget.begin(), nearbyTarget.end());
        return nearbyTarget[0];
@@ -182,69 +187,73 @@
            if (CrashTheLine(RoadMap.roads[road].stopLine[x].line, car, CarModelList)) {
                DEBUG("越过路口 road %d 路口 %d", road, x);

                CrossRoadCallback(road, x, RoadMap.roads[road].stopLine[x].active, car);
                CrossRoadCallback(RoadMap, road, x, ExamSchemeCrossing(RoadMap, road, x), car);

                if (!(it->second & REDUCE_SPEED)) {
                    // 不按规定减速，不合格
                    DEBUG("不按规定减速");
                if (RoadMap.calibrate == 0) {
                    if (!(it->second & REDUCE_SPEED)) {
                        // 不按规定减速，不合格
                        DEBUG("不按规定减速");

                    if (RoadMap.roads[road].stopLine[x].active == ROAD_ACTIVE_FORWARD) {
                        AddExamFault(41, rtkTime);
                    } else if (RoadMap.roads[road].stopLine[x].active ==
                               ROAD_ACTIVE_TURN_LEFT) {
                        AddExamFault(43, rtkTime);
                    } else if (RoadMap.roads[road].stopLine[x].active ==
                               ROAD_ACTIVE_TURN_RIGHT) {
                        AddExamFault(46, rtkTime);
                        if (ExamSchemeCrossing(RoadMap, road, x) == ROAD_ACTIVE_FORWARD) {
                            AddExamFault(41, rtkTime);
                        } else if (ExamSchemeCrossing(RoadMap, road, x) ==
                                   ROAD_ACTIVE_TURN_LEFT) {
                            AddExamFault(43, rtkTime);
                        } else if (ExamSchemeCrossing(RoadMap, road, x) ==
                                   ROAD_ACTIVE_TURN_RIGHT) {
                            AddExamFault(46, rtkTime);
                        }
                    }
                    if (!(it->second & STOP_CAR) && RoadMap.roads[road].stopLine[x].stopFlag) {
                        // 不停车瞭望，不合格
                        DEBUG("不停车瞭望");
                        if (ExamSchemeCrossing(RoadMap, road, x) == ROAD_ACTIVE_FORWARD) {
                            AddExamFault(42, rtkTime);
                        } else if (ExamSchemeCrossing(RoadMap, road, x) ==
                                   ROAD_ACTIVE_TURN_LEFT) {
                            AddExamFault(44, rtkTime);
                        } else if (ExamSchemeCrossing(RoadMap, road, x) ==
                                   ROAD_ACTIVE_TURN_RIGHT) {
                            AddExamFault(47, rtkTime);
                        }
                    }
                }
                if (!(it->second & STOP_CAR) && RoadMap.roads[road].stopLine[x].stopFlag) {
                    // 不停车瞭望，不合格
                    DEBUG("不停车瞭望");
                    if (RoadMap.roads[road].stopLine[x].active == ROAD_ACTIVE_FORWARD) {
                        AddExamFault(42, rtkTime);
                    } else if (RoadMap.roads[road].stopLine[x].active ==
                               ROAD_ACTIVE_TURN_LEFT) {
                        AddExamFault(44, rtkTime);
                    } else if (RoadMap.roads[road].stopLine[x].active ==
                               ROAD_ACTIVE_TURN_RIGHT) {
                        AddExamFault(47, rtkTime);
                    }
                }

                RemoveTargetReduceRec(TargetReduceRec, it->first);
                goto RECHECK;
            }
    }

    if (RoadMap.calibrate == 1)
        return;

    RECHECK2:
    for (auto it = TargetReduceRec2.begin(); it != TargetReduceRec2.end(); ++it) {
            int x = it->first;
            Line line;
            int roadIndex = 0;
        int x = it->first;
        Line line;
        int roadIndex = 0;

            for (; roadIndex < RoadMap.roads.size(); ++roadIndex) {
                if (RoadMap.roads[roadIndex].id == RoadMap.specialAreas[x].road)
                    break;
            }

            PointF point2 = CalcProjectionWithRoadEdge(RoadMap.roads[roadIndex].leftEdge,
                                                       RoadMap.specialAreas[x].area[0]);
            MakeLine(&line, &RoadMap.specialAreas[x].area[0], &point2);

            if (CrashTheLine(line, car, CarModelList)) {
                if (RoadMap.specialAreas[x].type == ZEBRA_CROSSING &&
                    !(it->second & REDUCE_SPEED)) {
                    DEBUG("人行道 不按规定减速");
                    AddExamFault(48, rtkTime);
                }
                if (RoadMap.specialAreas[x].type == BUS_STATION_AREA &&
                    !(it->second & REDUCE_SPEED)) {
                    DEBUG("公交站 不按规定减速");
                    AddExamFault(50, rtkTime);
                }
                RemoveTargetReduceRec(TargetReduceRec2, it->first);
                goto RECHECK2;
            }
        for (; roadIndex < RoadMap.roads.size(); ++roadIndex) {
            if (RoadMap.roads[roadIndex].id == RoadMap.specialAreas[x].road)
                break;
        }

        PointF point2 = CalcProjectionWithRoadEdge(RoadMap.roads[roadIndex].leftEdge,
                                                   RoadMap.specialAreas[x].area[0]);
        MakeLine(&line, &RoadMap.specialAreas[x].area[0], &point2);

        if (CrashTheLine(line, car, CarModelList)) {
            if (RoadMap.specialAreas[x].type == ZEBRA_CROSSING &&
                !(it->second & REDUCE_SPEED)) {
                DEBUG("人行道 不按规定减速");
                AddExamFault(48, rtkTime);
            }
            if (RoadMap.specialAreas[x].type == BUS_STATION_AREA &&
                !(it->second & REDUCE_SPEED)) {
                DEBUG("公交站 不按规定减速");
                AddExamFault(50, rtkTime);
            }
            RemoveTargetReduceRec(TargetReduceRec2, it->first);
            goto RECHECK2;
        }
    }
}