通用测试

yy1717

2020-03-18 2d0741f8e56e4f24126ea8448551e64560c45b4e

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

@@ -7,27 +7,54 @@
#include "../utils/xconvert.h"
#include "../common/apptimer.h"
#include "../jni_log.h"
#include "../defs.h"
#include "../test_common/car_sensor.h"

#include <vector>
#include <list>
#include <string>

#define DEBUG(fmt, args...)     LOGD("<road_exam> <%s>: " fmt, __func__, ##args)

using namespace std;

#define TURN_CHECK_CNT          5

static const int TURN_THRESHOLD = 10;
static const int TURN_CHECK_INTERVAL = D_SEC(1);

static bool occurCrashRedLine;
static bool occurCrashGreenLine;
static bool occurOverSpeed;
static int checkCrashGreenTimeout;
static char carIntersectionOfGreenLine;
static int currTurnSignalStatus;
static int turnSignalStatusWhenCrashGreenLine;
static bool reportTurnSignalError;
static int prevMoveDirect;
static uint32_t stopTimepoint = 0;
static bool reportStopCarOnRedArea;
static PointF stopPoint;

struct {
static struct drive_timer {
    int hour;
    int min;
    int sec;
    int msec;
} crashGreenRunTime, crashGreenCmpTime;
} crashGreenRunTime, crashGreenCmpTime, crashGreenStartTime, turnSignalChangeTime;

static const uint32_t STOP_CAR_TIME = D_SEC(2);
static const uint32_t CHANGE_ROAD_MIN_INTERVAL = D_SEC(10);
static const uint32_t CRASH_DOTTED_LINE_TIMEOUT = D_SEC(10);
static const uint32_t TURN_SIGNAL_LAMP_ADVANCE = D_SEC(3);

static const double MAX_SPEED = 40.0 * 1000.0 / 3600.0;

static void Rtk2DirveTimer(struct drive_timer &tm, const struct RtkTime *rtkTime);
static char isTurn(int currYaw, int prevYaw);
static char CheckCarTurn(LIST_CAR_MODEL &CarModelList);
static bool CrashRedLine(LIST_ROAD_MAP &RoadMapList, const car_model *car);
static bool CrashRedArea(LIST_ROAD_MAP &RoadMapList, const car_model *car);
static bool CrashGreenLine(LIST_ROAD_MAP &RoadMapList, const car_model *car, PointF &p1, PointF &p2);

void Init(void)
@@ -38,11 +65,17 @@
    occurOverSpeed = false;
    checkCrashGreenTimeout = 0;
    carIntersectionOfGreenLine = 0;

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

    reportStopCarOnRedArea = false;
}

void TestRoadGeneral(LIST_ROAD_MAP &RoadMapList, const car_model *car, double speed, int moveStatus, const struct RtkTime *rtkTime)
void TestRoadGeneral(LIST_ROAD_MAP &RoadMapList, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime)
{
    if (speed > MAX_SPEED) {
    if (moveDirect != 0 && speed > MAX_SPEED) {
        if (!occurOverSpeed) {
            occurOverSpeed = true;
            // 超速，不合格
@@ -51,6 +84,89 @@
    } else {
        occurOverSpeed = false;
    }

    if (moveDirect != prevMoveDirect) {
        if (moveDirect == 0) {
            stopTimepoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
            reportStopCarOnRedArea = false;

            DEBUG("停车了 %d %d %d %d %d %d %d", rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss);
        } else if (moveDirect == -1) {

        }
        prevMoveDirect = moveDirect;
    } else if (moveDirect == 0) {
        uint32_t tp = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);

        if (tp - stopTimepoint >= STOP_CAR_TIME && !reportStopCarOnRedArea && CrashRedArea(RoadMapList, car)) {
            // 停车超2秒，停在红区
            AddExamFault(16, rtkTime);
            DEBUG("中途停车");
            reportStopCarOnRedArea = true;
        }

        stopPoint = car->basePoint;
    }

    switch (ReadCarStatus(TURN_SIGNAL_LAMP)) {
        case LEFT_TURN_LIGHT:
            if (currTurnSignalStatus != LEFT_TURN_LIGHT) {
                currTurnSignalStatus = LEFT_TURN_LIGHT;
                Rtk2DirveTimer(turnSignalChangeTime, rtkTime);
            }
            break;
        case RIGHT_TURN_LIGHT:
            if (currTurnSignalStatus != RIGHT_TURN_LIGHT) {
                currTurnSignalStatus = RIGHT_TURN_LIGHT;
                Rtk2DirveTimer(turnSignalChangeTime, rtkTime);
            }
            break;
        default:
            currTurnSignalStatus = ReadCarStatus(TURN_SIGNAL_LAMP);
            break;
    }


    // 检查是否持续转向
    char turnDirect = CheckCarTurn(CarModelList);
    if (turnDirect == 'L') {
        if (currTurnSignalStatus != LEFT_TURN_LIGHT) {
            if (!reportTurnSignalError) {
                DEBUG("没打左转灯");
                // 没打左转灯，不合格
                AddExamFault(13, rtkTime);
                reportTurnSignalError = true;
            }
        } 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秒，不合格
                AddExamFault(14, rtkTime);
                reportTurnSignalError = true;
            }
        }
    } else if (turnDirect == 'R') {
        if (currTurnSignalStatus != RIGHT_TURN_LIGHT) {
            if (!reportTurnSignalError) {
                DEBUG("没打右转灯");
                // 没打右转灯，不合格
                AddExamFault(13, rtkTime);
                reportTurnSignalError = true;
            }
        } 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秒，不合格
                AddExamFault(14, rtkTime);
                reportTurnSignalError = true;
            }
        }
    } else {
        reportTurnSignalError = false;
    }


    if (CrashRedLine(RoadMapList, car)) {
        if (!occurCrashRedLine) {
@@ -65,13 +181,11 @@
    static PointF p1, p2;

    if (CrashGreenLine(RoadMapList, car, p1, p2)) {
        if (moveStatus != 0) {
        // 压虚线
        if (moveDirect != 0) {
            if (checkCrashGreenTimeout == 0) {
                checkCrashGreenTimeout = 1;
                crashGreenRunTime.hour = rtkTime->hh;
                crashGreenRunTime.min = rtkTime->mm;
                crashGreenRunTime.sec = rtkTime->ss;
                crashGreenRunTime.msec = rtkTime->mss;
                Rtk2DirveTimer(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);
@@ -91,7 +205,9 @@
        // 检测是否3秒前有开启对应之转向灯
        if (!occurCrashGreenLine) {
            occurCrashGreenLine = true;

            // 记录开始压线的时间，不确定是否有变道意图，待确认变道后再处理之
            Rtk2DirveTimer(crashGreenStartTime, rtkTime);
            turnSignalStatusWhenCrashGreenLine = currTurnSignalStatus;
        }

        // p1 ---------------> p2
@@ -104,14 +220,14 @@
            carIntersectionOfGreenLine = 'L';
        }
    } else {
        // 不再压虚线
        if (occurCrashGreenLine) {
            int inter = IntersectionOfLine(p1, p2, car->basePoint);

            // 有过线动作
            // 完成跨线动作
            if ((inter == 1 && carIntersectionOfGreenLine == 'R') ||
                    (inter == -1 && carIntersectionOfGreenLine == 'L')) {
                DEBUG("跨越虚线");

                // 比较上次跨线时间
                if (crashGreenCmpTime.hour >= 0) {
                    uint32_t diff = TimeGetDiff(rtkTime->hh, rtkTime->mm, rtkTime->ss,
                                                rtkTime->mss * 10,
@@ -124,15 +240,102 @@
                    }
                }

                crashGreenCmpTime.hour = rtkTime->hh;
                crashGreenCmpTime.min = rtkTime->mm;
                crashGreenCmpTime.sec = rtkTime->ss;
                crashGreenCmpTime.msec = rtkTime->mss;
                // 记录本次变道时间点
                Rtk2DirveTimer(crashGreenCmpTime, rtkTime);

                // 检查变道前，是否提前转向灯
                if (inter == 1) {
                    // 向左侧变道
                    DEBUG("向左侧变道");
                    if (turnSignalStatusWhenCrashGreenLine != LEFT_TURN_LIGHT) {
                        // 没打灯，不合格
                        AddExamFault(13, rtkTime);
                    }
                } else {
                    // 向右侧变道
                    DEBUG("向右侧变道");
                    if (turnSignalStatusWhenCrashGreenLine != RIGHT_TURN_LIGHT) {
                        // 没打灯，不合格
                        AddExamFault(13, rtkTime);
                    }
                }
            }
        }
        occurCrashGreenLine = false;
        checkCrashGreenTimeout = 0;
    }
}

static void Rtk2DirveTimer(struct drive_timer &tm, const struct RtkTime *rtkTime)
{
    tm.hour = rtkTime->hh;
    tm.min = rtkTime->mm;
    tm.sec = rtkTime->ss;
    tm.msec = rtkTime->mss;
}

static char isTurn(int currYaw, int prevYaw)
{
    if (ABS(currYaw - prevYaw) > 180) {
        currYaw = 360 - ABS(currYaw-prevYaw);
    } else {
        currYaw = ABS(currYaw - prevYaw);
    }

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

    return 0;
}

static char CheckCarTurn(LIST_CAR_MODEL &CarModelList)
{
    // 最近3秒内，每秒的角度差大于10度，且方向相同，连续3秒，认为转向
    if (CarModelList.size() < 1)
        return false;

    list<car_model *>::iterator iter = CarModelList.begin();

    car_model *c1 = *iter, *c2;

    ++iter;

    char turn[TURN_CHECK_CNT] = {0};
    int checkCnt = 0;

    while (iter != CarModelList.end()) {
        c2 = *iter;

        uint32_t tdiff = TimeGetDiff(c1->tm.hh, c1->tm.mm, c1->tm.ss, c1->tm.mss * 10, c2->tm.hh, c2->tm.mm, c2->tm.ss, c2->tm.mss*10);

        if (tdiff >= TURN_CHECK_INTERVAL) {
            turn[checkCnt++] = isTurn((int)c1->yaw, (int)c2->yaw);
            c1 = c2;

            if (checkCnt == TURN_CHECK_CNT)
                break;
        }

        ++iter;
    }

    int i = 0;
    for (; checkCnt == TURN_CHECK_CNT && i < TURN_CHECK_CNT-1; ++i) {
        if (turn[i] != turn[i+1])
            break;
    }

    if (i == TURN_CHECK_CNT-1)
        return turn[0];

    return 0;
}

/*****************************************************
@@ -170,6 +373,42 @@
    return false;
}

/**********************************************************
 * 按整个车身是否覆盖计算
 * @param RoadMapList
 * @param car
 * @return
 */
static bool CrashRedArea(LIST_ROAD_MAP &RoadMapList, const car_model *car)
{
    bool ret = false;

    Polygon carBody;

    carBody.num = car->bodyNum;
    carBody.point = (PointF *)malloc(carBody.num * sizeof(PointF));
    for (int i = 0; i < carBody.num; ++i) {
        carBody.point[i] = car->carXY[car->body[i]];
    }

    for (int i = 0; i < RoadMapList.size(); ++i) {
        if (RoadMapList[i].type == 100) {

            // 每条红区都检测
            for (int j = 0; j < RoadMapList[i].redAreaNum; ++j) {
                if (IntersectionOf(&carBody, &RoadMapList[i].redArea[j]) != GM_None) {
                    ret = true;
                }
            }
            break;
        }
    }

    free(carBody.point);

    return ret;
}

/**************************************************
 * 车轮触碰道路虚线。检测行驶时间超长；变道情况；
 * @param RoadMapList