传感器变更

yy1717

2020-08-17 21834035950feaadac9d4c7cad82b90ee624c984

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

@@ -74,7 +74,7 @@

static const int INVALID_ROAD = -1;

static const int TURN_THRESHOLD = 1;
static const int TURN_THRESHOLD = 3;
static const int TURN_CHECK_INTERVAL = 500;
const double SLIDE_DISTANCE_THRESHOLD_RED = 0.3;
const double SLIDE_DISTANCE_THRESHOLD_YELLOW = 0.1;
@@ -94,7 +94,7 @@
static char carIntersectionOfGreenLine;
static int currTurnSignalStatus;
static int turnSignalStatusWhenCrashGreenLine;
static bool reportTurnSignalError;

static int prevMoveDirect;
static uint32_t stopTimepoint = 0;
static bool reportStopCarOnRedArea;
@@ -135,6 +135,12 @@

static map<int, car_sensor_value_t> CarSensorValue;
static map<int, int> CrossingHint;
static map<int, bool> ErrorLaneReport;

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

static const int MAX_ENGINE_RPM = 2500;
static const double START_CAR_MOVE_DISTANCE = 10.0;
@@ -157,8 +163,9 @@

static int TestRoadStartCar(const car_model *car, double speed, int moveDirect, const struct RtkTime *rtkTime);

static char isTurn(int currYaw, int prevYaw, int &ang);
static char CheckCarTurn(LIST_CAR_MODEL &CarModelList);
static int isTurn(int currYaw, int prevYaw, int thres);
static void ResetTurnDetect(const car_model *car);
static void DetectTurn(const car_model *car, int moveDirect, const struct RtkTime *rtkTime);


static trigger_line_t * EntryItem(int index, road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList);
@@ -179,7 +186,6 @@

    carIntersectionOfGreenLine = 0;

    reportTurnSignalError = false;
    currTurnSignalStatus = OFF_LIGHT;
    prevMoveDirect = 0;

@@ -211,6 +217,7 @@
    checkTurn = false;

    CrashLineType = -1;
    turnCnt = -1;

    ResetOdo();
    ResetTarget(RoadMap);
@@ -973,7 +980,6 @@
                    } else if (TimeGetDiff(&crashGreenStartTime, &lamp.time) >= D_SEC(3)) {
                        DEBUG("转向灯时间不足");
                        // 不足3秒，不合格
                        reportTurnSignalError = true;
                        AddExamFault(14, rtkTime);
                    }
                } else {
@@ -984,7 +990,6 @@
                    } else if (TimeGetDiff(&crashGreenStartTime, &lamp.time) >= D_SEC(3)) {
                        DEBUG("转向灯时间不足");
                        // 不足3秒，不合格
                        reportTurnSignalError = true;
                        AddExamFault(14, rtkTime);
                    }
                }
@@ -1027,6 +1032,62 @@
    return CROSSING_NOT_HINT;
}

static void ResetCrossingStatus(int roadIndex)
{
    while (true) {
        bool w = false;
        for (auto it = CrossingHint.begin(); it != CrossingHint.end(); ++it) {
            if (it->first / 100 == roadIndex && it->second != CROSSING_NOT_HINT) {
                ChangeCrossingStatus(it->first / 100, it->first % 100, CROSSING_NOT_HINT);
                w = true;
                break;
            }
        }
        if (!w)
            break;
    }
}

static void SetErrorLaneRpt(int roadIndex, int index, bool status)
{
    int key = roadIndex * 100 + index;

    auto it = ErrorLaneReport.find(key);

    if (it != ErrorLaneReport.end()) {
        ErrorLaneReport.erase(it);
    }
    ErrorLaneReport.insert(pair<int, bool>(key, status));
}

static bool GetErrorLaneRpt(int roadIndex, int index)
{
    int key = roadIndex * 100 + index;

    auto it = ErrorLaneReport.find(key);

    if (it != ErrorLaneReport.end()) {
        return it->second;
    }
    return false;
}

static void ResetErrorLaneRpt(int roadIndex)
{
    while (true) {
        bool w = false;
        for (auto it = ErrorLaneReport.begin(); it != ErrorLaneReport.end(); ++it) {
            if (it->first / 100 == roadIndex && it->second == true) {
                SetErrorLaneRpt(it->first / 100, it->first % 100, false);
                w = true;
                break;
            }
        }
        if (!w)
            break;
    }
}

static void HintCrossing(int roadIndex, road_t &road, const car_model *car, LIST_CAR_MODEL &CarModelList)
{
    for (int i = 0; i < road.stopLine.size(); ++i) {
@@ -1053,12 +1114,13 @@
    }
}

static int NearbyCrossingGuide(int roadIndex, road_t &road, const car_model *car, LIST_CAR_MODEL &CarModelList)
static int NearbyCrossingGuide(int &stopLineIndex, int roadIndex, road_t &road, const car_model *car, LIST_CAR_MODEL &CarModelList)
{
    int guide = 0;
    int guide = 0, stopLine = 0;
    double distance;

    struct nearby_crossing {
        int stopLine;
        int guide;
        double distance;
    };
@@ -1077,6 +1139,7 @@
        if (distance > 1e-3) {
            struct nearby_crossing temp;

            temp.stopLine = i;
            temp.guide = road.stopLine[i].active;
            temp.distance = distance;

@@ -1092,10 +1155,12 @@
            if (distance > set[i].distance) {
                distance = set[i].distance;
                guide = set[i].guide;
                stopLine = set[i].stopLine;
            }
        }
    }

    stopLineIndex = stopLine;
    return guide;
}

@@ -1286,6 +1351,15 @@
    // 检测离开此路段，全车需不在范围内
    int oldid = currExamMapIndex;
    currExamMapIndex = CalcRoadIndex(currExamMapIndex, RoadMap, car);
    if (oldid != currExamMapIndex) {
        DEBUG("道路ID切换 %d ======> %d", oldid, currExamMapIndex);

        if (oldid >= 0) {
            ResetCrossingStatus(oldid);
            ResetErrorLaneRpt(oldid);
        }
    }

    if (currExamMapIndex >= 0) {
        car_sensor_value_t brk = ReadCarSensorValue(BREAK);
        HintCrossing(currExamMapIndex, RoadMap.roads[currExamMapIndex], car, CarModelList);
@@ -1294,10 +1368,6 @@
        ApproachTarget(RoadMap, car, currExamMapIndex, (brk.value == BREAK_ACTIVE), speed, moveDirect, rtkTime);
    }
    ExitTarget(RoadMap, car, CarModelList, rtkTime);

    if (oldid != currExamMapIndex) {
        DEBUG("道路ID切换 %d", currExamMapIndex);
    }

    oldid = CrashLineType;

@@ -1311,9 +1381,14 @@
    oldid = Lane.guide;
    DetectLane(RoadMap, car, currExamMapIndex, rtkTime);
    if (Lane.guide > 0 && currExamMapIndex >= 0) {
        if (!(NearbyCrossingGuide(currExamMapIndex, RoadMap.roads[currExamMapIndex], car, CarModelList) & Lane.guide)) {
            DEBUG("不按规定车道标向行驶");
            AddExamFault(9, rtkTime);
        int stop_line;

        if (!(NearbyCrossingGuide(stop_line, currExamMapIndex, RoadMap.roads[currExamMapIndex], car, CarModelList) & Lane.guide)) {
            if (!GetErrorLaneRpt(currExamMapIndex, stop_line)) {
                DEBUG("不按规定车道标向行驶");
                AddExamFault(9, rtkTime);
                SetErrorLaneRpt(currExamMapIndex, stop_line, true);
            }
        }
    }

@@ -1323,6 +1398,9 @@

    // 检测压线状态
    TestRoadStartCar(car, speed, moveDirect, rtkTime);

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

    if (startCar != START_CAR_DONE)
        return;
@@ -1399,7 +1477,15 @@
    tm.msec = rtkTime->mss;
}

static char isTurn(int currYaw, int prevYaw, int &ang)
void CrossRoadCallback(int road, int stop_line, int active, const car_model *car)
{
    SetErrorLaneRpt(road, stop_line, false);
    if (active != ROAD_ACTIVE_FORWARD) {
        ResetTurnDetect(car);
    }
}

static int isTurn(int currYaw, int prevYaw, int thres)
{
//    DEBUG("currYaw %d prevYaw %d", currYaw, prevYaw);
    int deltaAng = 0;
@@ -1410,21 +1496,117 @@
        deltaAng = ABS(currYaw - prevYaw);
    }

    ang = deltaAng;

//    DEBUG("角度差值 %d", deltaAng);

    if (deltaAng >= TURN_THRESHOLD) {
    if (deltaAng >= thres) {
        if((( currYaw + 360 - prevYaw) % 360) < 180) {
//            DEBUG("右转");
            return 'R';
            return deltaAng;
        } else {
//            DEBUG("左转");
            return 'L';
            return 0 - deltaAng;
        }
    }

    return 'N';
    return 0;
}

static void ResetTurnDetect(const car_model *car)
{
    turnCnt = 0;
    turnTimeCnt = 0;
    prevDetectTurnTime = car->tm;
    prevYaw = car->yaw;
}

static void DetectTurn(const car_model *car, int moveDirect, const struct RtkTime *rtkTime)
{
    int angle;

    if (turnCnt < 0 || TimeGetDiff(rtkTime, &prevDetectTurnTime) < 500) {
        return;
    }

    angle = isTurn((int) car->yaw, prevYaw, TURN_THRESHOLD);

    prevYaw = (int) car->yaw;
    prevDetectTurnTime = *rtkTime;

    if (turnCnt == 0) {
        if (angle != 0 && moveDirect != 0) {
            DEBUG("有转向迹象");
            turnCnt++;
            beginTurnTime = *rtkTime;
            startTurnYaw = (int) car->yaw;
        }
    } else if (turnCnt == 1) {
        if (angle != 0 && moveDirect != 0) {
            if (angle * prevTurnWise > 0) {
                DEBUG("确认转向");
                // 同向转动
                turnCnt++;
                turnTimeCnt = 0;
            } else {
                beginTurnTime = *rtkTime;
                startTurnYaw = (int) car->yaw;
            }
        } else {
            turnCnt = 0;
        }
    } else if (turnCnt >= 2) {
        if (moveDirect == 0) {
            // 暂停
        } else {
            turnTimeCnt += TimeGetDiff(rtkTime, &prevDetectTurnTime);
            int wise = isTurn((int) car->yaw, startTurnYaw, TURN_THRESHOLD);
            DEBUG("转动角度 %d", wise);
            if (ABS(wise) > 60) {
                // 确认转弯行为，检测开始刚转弯时转向灯情况
                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) >= D_SEC(3)) {
                            DEBUG("转向灯时间不足");
                            // 不足3秒，不合格
                            AddExamFault(14, rtkTime);
                        }
                    } else {
                        if (lamp.value != RIGHT_TURN_LIGHT) {
                            DEBUG("变调未打灯!!");
                            // 没打灯，不合格
                            AddExamFault(13, rtkTime);
                        } else if (TimeGetDiff(&beginTurnTime, &lamp.time) >= D_SEC(3)) {
                            DEBUG("转向灯时间不足");
                            // 不足3秒，不合格
                            AddExamFault(14, rtkTime);
                        }
                    }
                }
            } else if (turnTimeCnt > D_SEC(10)) {
                // 取消转向检测
                DEBUG("取消转向检测1");
                turnCnt = -1;
            }
        }
    }

    if (turnCnt < 2 && moveDirect != 0) {
        turnTimeCnt += TimeGetDiff(rtkTime, &prevDetectTurnTime);
        if (turnTimeCnt > D_SEC(15)) {
            // 取消转向检测
            DEBUG("取消转向检测2");
            turnCnt = -1;
        }
    }

    prevTurnWise = angle;
}

static char CheckCarTurn(LIST_CAR_MODEL &CarModelList)