通相检查

yy1717

2020-03-19 3d3a5aa436645e5af1a4877338319ff8274e0346

通相检查

 lib/src/main/cpp/driver_test.cpp

@@ -55,17 +55,6 @@
#define MAP_LIST_SIZE               32

enum {
    TEST_NONE,
    TEST_PARK_EDGE,
    TEST_PARK_BOTTOM,
    TEST_TUNE_90,
    TEST_S_CURVE,
    TEST_SLOPE,
    TEST_SIMPLE,
    TEST_END
};

enum {
    TEST_TYPE_AREA = 2,
    TEST_TYPE_ROAD_DUMMY_LIGHT,
    TEST_TYPE_ROAD_TRUE_LIGHT
@@ -258,8 +247,8 @@

        if (map.triggerLine != NULL) {
            for (int j = 0; j < map.triggerLineNum; ++j) {
                if (map.triggerLine[j].point != NULL)
                    free(map.triggerLine[j].point);
                if (map.triggerLine[j].line.point != NULL)
                    free(map.triggerLine[j].line.point);
            }
            free(map.triggerLine);
        }
@@ -305,6 +294,8 @@
                newMap.redLine[i].point[j] = RoadMapPoints.point[redLines[i][j]];
            }
        }
    } else {
        newMap.redLine = NULL;
    }

    if ((newMap.redAreaNum = redAreas.size()) > 0) {
@@ -318,6 +309,8 @@
                newMap.redArea[i].point[j] = RoadMapPoints.point[redAreas[i][j]];
            }
        }
    } else {
        newMap.redArea = NULL;
    }

    if ((newMap.greenLineNum = greenLines.size()) > 0) {
@@ -331,20 +324,25 @@
                newMap.greenLine[i].point[j] = RoadMapPoints.point[greenLines[i][j]];
            }
        }
    } else {
        newMap.greenLine = NULL;
    }

    if ((newMap.triggerLineNum = triggerLines.size()) > 0) {
        newMap.triggerLine = (Polygon *) malloc(sizeof(Polygon) * newMap.triggerLineNum);
        newMap.triggerLine = (struct trigger_line_t *) malloc(sizeof(struct trigger_line_t) * newMap.triggerLineNum);

        for (int i = 0; i < newMap.triggerLineNum; ++i) {
            newMap.triggerLine[i].num = triggerLines[i].size();
            newMap.triggerLine[i].point = (PointF *) malloc(sizeof(PointF) * newMap.triggerLine[i].num);
            newMap.triggerLine[i].line.num = triggerLines[i].size() - 1;
            newMap.triggerLine[i].line.point = (PointF *) malloc(sizeof(PointF) * newMap.triggerLine[i].line.num);

            for (int j = 0; j < newMap.triggerLine[i].num; ++j) {
                newMap.triggerLine[i].point[j] = RoadMapPoints.point[triggerLines[i][j]];
            newMap.triggerLine[i].triggerMapId = triggerLines[i][0];
            for (int j = 0; j < newMap.triggerLine[i].line.num; ++j) {
                newMap.triggerLine[i].line.point[j] = RoadMapPoints.point[triggerLines[i][j+1]];
            }
        }
    }
    } else {
        newMap.triggerLine = NULL;
    };

    RoadMapList.push_back(newMap);
}
@@ -480,7 +478,7 @@
        return;
    }

    if (MapNum == 0) {
    if (MapNum == 0 && type == TEST_TYPE_AREA) {
        err = true;
        MA_SendExamStatus(0, -1);
    }
@@ -491,6 +489,10 @@
    if (DummyLightContent == NULL && type == TEST_TYPE_ROAD_DUMMY_LIGHT) {
        err = true;
        MA_SendExamStatus(0, -3);
    }
    if (type != TEST_TYPE_AREA && (RoadMapPoints.num == 0 || RoadMapPoints.point == NULL || RoadMapList.size() == 0)) {
        err = true;
        MA_SendExamStatus(0, -1);
    }

    if (!err) {
@@ -657,7 +659,7 @@
    if (ReadCarStatus(ENGINE_START) == ENGINE_START_ACTIVE) {
        if (!engineStart) {
            engineStart = true;
            if (ReadCarStatus(SHIFT) != 'N') {
            if (ReadCarStatus(GEAR) != 'N') {
                // 不是空挡点火，不合格
                if (ExamType == TEST_TYPE_AREA)
                    AddExamFault(3, rtkTime);

 lib/src/main/cpp/driver_test.h

@@ -84,6 +84,11 @@
    int wrong_id;
};

struct trigger_line_t {
    int triggerMapId;
    Polygon line;
};

struct road_exam_map {
    int id;
    int type;
@@ -100,10 +105,11 @@
    Polygon *redArea;

    int triggerLineNum;
    Polygon *triggerLine;
    trigger_line_t *triggerLine;

    Line startLine;
    Line endLine;
    int flagStop;           // 到达开始线前，是否需要停车
};

typedef vector<struct road_exam_map> LIST_ROAD_MAP;

 lib/src/main/cpp/test_common/car_sensor.h

@@ -10,7 +10,7 @@
enum {
    OBD_SPEED,
    ENGINE_RPM,
    SHIFT,
    GEAR,
    TURN_SIGNAL_LAMP,
    DIPPED_BEAM_LAMP,
    FOG_LAMP,
@@ -36,13 +36,13 @@
    FOG_LIGHT,
    INSERT_SEATBELT,
    ENGINE_START_ACTIVE,
    SHIFT_N,
    GEAR_N,
    SHIFT_R,
    SHIFT_1,
    SHIFT_2,
    SHIFT_3,
    SHIFT_4,
    SHIFT_5
    GEAR_1,
    GEAR_2,
    GEAR_3,
    GEAR_4,
    GEAR_5
};

void CarSensorInit(void);

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

@@ -9,6 +9,7 @@
#include "../jni_log.h"
#include "../defs.h"
#include "../test_common/car_sensor.h"
#include "../native-lib.h"

#include <vector>
#include <list>
@@ -37,6 +38,8 @@
static uint32_t stopTimepoint = 0;
static bool reportStopCarOnRedArea;
static PointF stopPoint;
static bool prevGearError = false;
static bool prevGearNSlide = false;

static bool slideLongDistance;
static bool slideNormalDistance;
@@ -49,22 +52,34 @@
    int msec;
} crashGreenRunTime, crashGreenCmpTime, crashGreenStartTime, turnSignalChangeTime;

static struct drive_timer gearErrorTimePoint;
static struct drive_timer gearNSlideTimePoint;

static int gearErrorTime;
static int gearNSlideTime;

static int currExamMapIndex;

static const uint32_t GEAR_N_SLIDE_TIMEOUT = D_SEC(5);
static const uint32_t GEAR_ERROR_TIMEOUT = D_SEC(15);
static const uint32_t STOP_CAR_TIME = D_SEC(2);
static const uint32_t CHANGE_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 const int SPEED_SHIFT_TABLE[][2] = {{0, 20}, {5, 30}, {15, 40}, {25, 10000}, {35, 10000}};
static const int SPEED_GEAR_TABLE[][2] = {{0, 20}, {5, 30}, {15, 40}, {25, 10000}, {35, 10000}};

static void Rtk2DirveTimer(struct drive_timer &tm, const struct RtkTime *rtkTime);
static void Rtk2DriveTimer(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);
static int CrashTriggerLine(LIST_ROAD_MAP &RoadMapList, const car_model *car, LIST_CAR_MODEL &CarModelList);
static int FindMapIndexById(int id, LIST_ROAD_MAP &RoadMapList);

void Init(void)
void InitRoadExam(void)
{
    crashGreenCmpTime.hour = -1;
    occurCrashRedLine = false;
@@ -81,10 +96,17 @@
    occurSlide = false;
    slideLongDistance = false;
    slideNormalDistance = false;
    prevGearError = false;
    gearErrorTime = 0;
    prevGearNSlide = false;
    gearNSlideTime = 0;

    currExamMapIndex = -1;
}

void TestRoadGeneral(LIST_ROAD_MAP &RoadMapList, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime)
{
    // 超速检测
    if (moveDirect != 0 && speed > MAX_SPEED) {
        if (!occurOverSpeed) {
            occurOverSpeed = true;
@@ -96,26 +118,76 @@
    }

    // 挡位匹配检测
    switch (ReadCarStatus(SHIFT)) {
        case SHIFT_N:
    bool currGearError = false;
    bool currGearNSlide = false;

    switch (ReadCarStatus(GEAR)) {
        case GEAR_N:
            if (moveDirect != 0) {

                // 空档滑行
                currGearNSlide = true;
            }
            break;
        case SHIFT_1:
            if (ConvertMs2KMs(speed) < SPEED_SHIFT_TABLE[0][0] || ConvertMs2KMs(speed) > SPEED_SHIFT_TABLE[0][1]) {

        case GEAR_1:
            if (ConvertMs2KMh(speed) < SPEED_GEAR_TABLE[0][0] || ConvertMs2KMh(speed) > SPEED_GEAR_TABLE[0][1]) {
                currGearError = true;
            }
            break;
        case SHIFT_2:
        case GEAR_2:
            if (ConvertMs2KMh(speed) < SPEED_GEAR_TABLE[1][0] || ConvertMs2KMh(speed) > SPEED_GEAR_TABLE[1][1]) {
                currGearError = true;
            }
            break;
        case SHIFT_3:
        case GEAR_3:
            if (ConvertMs2KMh(speed) < SPEED_GEAR_TABLE[2][0] || ConvertMs2KMh(speed) > SPEED_GEAR_TABLE[2][1]) {
                currGearError = true;
            }
            break;
        case SHIFT_4:
        case GEAR_4:
            if (ConvertMs2KMh(speed) < SPEED_GEAR_TABLE[3][0] || ConvertMs2KMh(speed) > SPEED_GEAR_TABLE[3][1]) {
                currGearError = true;
            }
            break;
        case SHIFT_5:
        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 && prevGearNSlide) {
        gearNSlideTime += TimeGetDiff(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10,
                                      gearNSlideTimePoint.hour, gearNSlideTimePoint.min, gearNSlideTimePoint.sec, gearNSlideTimePoint.msec*10);
    }
    if (gearNSlideTime > GEAR_N_SLIDE_TIMEOUT) {
        // 空档滑行超5秒，不合格
        DEBUG("挡位滑行，超过5秒");
        AddExamFault(8, rtkTime);
        gearNSlideTime = 0;
    }

    prevGearNSlide = currGearNSlide;
    if (prevGearNSlide) {
        Rtk2DriveTimer(gearNSlideTimePoint, rtkTime);
    } else {
        gearNSlideTime = 0;
    }
    // 挡位不匹配超时
    if (currGearError && prevGearError) {
        gearErrorTime += TimeGetDiff(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10,
                    gearErrorTimePoint.hour, gearErrorTimePoint.min, gearErrorTimePoint.sec, gearErrorTimePoint.msec*10);
    }
    if (gearErrorTime > GEAR_ERROR_TIMEOUT) {
        // 累计15秒，挡位-车速不匹配，不合格
        DEBUG("挡位错误超过15秒");
        AddExamFault(6, rtkTime);
        gearErrorTime = 0;
    }

    prevGearError = currGearError;
    if (prevGearError) {
        Rtk2DriveTimer(gearErrorTimePoint, rtkTime);
    }

    // 起步后滑
@@ -171,13 +243,13 @@
        case LEFT_TURN_LIGHT:
            if (currTurnSignalStatus != LEFT_TURN_LIGHT) {
                currTurnSignalStatus = LEFT_TURN_LIGHT;
                Rtk2DirveTimer(turnSignalChangeTime, rtkTime);
                Rtk2DriveTimer(turnSignalChangeTime, rtkTime);
            }
            break;
        case RIGHT_TURN_LIGHT:
            if (currTurnSignalStatus != RIGHT_TURN_LIGHT) {
                currTurnSignalStatus = RIGHT_TURN_LIGHT;
                Rtk2DirveTimer(turnSignalChangeTime, rtkTime);
                Rtk2DriveTimer(turnSignalChangeTime, rtkTime);
            }
            break;
        default:
@@ -226,7 +298,7 @@
        reportTurnSignalError = false;
    }


    // 撞红线
    if (CrashRedLine(RoadMapList, car)) {
        if (!occurCrashRedLine) {
            // 车辆行驶中骑轧车道中心实线或者车道边缘实线，不合格
@@ -237,14 +309,14 @@
        occurCrashRedLine = false;
    }

    // 撞绿线
    static PointF p1, p2;

    if (CrashGreenLine(RoadMapList, car, p1, p2)) {
        // 压虚线
        if (moveDirect != 0) {
            if (checkCrashGreenTimeout == 0) {
                checkCrashGreenTimeout = 1;
                Rtk2DirveTimer(crashGreenRunTime, rtkTime);         // 运动中压虚线的开始时间点
                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);
@@ -265,7 +337,7 @@
        if (!occurCrashGreenLine) {
            occurCrashGreenLine = true;
            // 记录开始压线的时间，不确定是否有变道意图，待确认变道后再处理之
            Rtk2DirveTimer(crashGreenStartTime, rtkTime);
            Rtk2DriveTimer(crashGreenStartTime, rtkTime);
            turnSignalStatusWhenCrashGreenLine = currTurnSignalStatus;
        }

@@ -300,7 +372,7 @@
                }

                // 记录本次变道时间点
                Rtk2DirveTimer(crashGreenCmpTime, rtkTime);
                Rtk2DriveTimer(crashGreenCmpTime, rtkTime);

                // 检查变道前，是否提前转向灯
                if (inter == 1) {
@@ -323,9 +395,22 @@
        occurCrashGreenLine = false;
        checkCrashGreenTimeout = 0;
    }

    // 触发线检测
    if (currExamMapIndex == -1) {
        currExamMapIndex = CrashTriggerLine(RoadMapList, car, CarModelList);
        if (currExamMapIndex != -1) {
            DEBUG("进入路考子地图 index = %d id = %d item = %d", currExamMapIndex, RoadMapList[currExamMapIndex].id, RoadMapList[currExamMapIndex].type);
            if (!RoadMapList[currExamMapIndex].tts.empty()) {
                PlayTTS(RoadMapList[currExamMapIndex].tts.c_str(), 0);
            }
        }
    } else {

    }
}

static void Rtk2DirveTimer(struct drive_timer &tm, const struct RtkTime *rtkTime)
static void Rtk2DriveTimer(struct drive_timer &tm, const struct RtkTime *rtkTime)
{
    tm.hour = rtkTime->hh;
    tm.min = rtkTime->mm;
@@ -507,3 +592,82 @@
    }
    return false;
}

/************************************************************
 * 检测车辆是否触发子考项地图
 * @param RoadMapList
 * @param car
 * @param CarModelList
 * @return
 */
static int CrashTriggerLine(LIST_ROAD_MAP &RoadMapList, const car_model *car, LIST_CAR_MODEL &CarModelList)
{
    int map_idx = -1;

    if (CarModelList.size() < 5)
        return map_idx;

    Polygon trace;

    trace.num = 5;
    trace.point = (PointF *) malloc(sizeof(PointF) * trace.num);

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

    int pn = 0;
    while (iter != CarModelList.end() && pn < trace.num) {
        trace.point[pn++] = ((car_model *)(*iter))->carXY[((car_model *)(*iter))->left_front_tire[TIRE_OUTSIDE]];
        ++iter;
    }

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

            // 每条线都检测
            for (int j = 0; j < RoadMapList[i].triggerLineNum; ++j) {
                Line trigger_line;

                int kp = 0;

                // 触发线一般应该只有首尾2点（id, p1, p2）
                for (int k = 1; k < RoadMapList[i].triggerLine[j].line.num; ++k) {
                    MakeLine(&trigger_line, &RoadMapList[i].triggerLine[j].line.point[kp], &RoadMapList[i].triggerLine[j].line.point[k]);

                    int pp = 1;
                    for (int p = 2; p < pn; ++p) {
                        Line trace_line;
                        MakeLine(&trace_line, &trace.point[pp], &trace.point[p]);

                        if (IntersectionOf(trace_line, trigger_line) == GM_Intersection &&
                            IntersectionOfLine(trace.point[pp], trace.point[p], car->carXY[car->left_front_tire[TIRE_OUTSIDE]]) == -1 &&
                            DistanceOf(car->carXY[car->left_front_tire[TIRE_OUTSIDE]], trace_line) > 0.1) {
                            // 碰到触发线
                            map_idx =  FindMapIndexById(RoadMapList[i].triggerLine[j].triggerMapId, RoadMapList);
                            goto SEARCH_TRIGGER_LINE_END;
                        }

                        pp = p;
                    }

                    kp = k;
                }
            }
            break;
        }
    }

SEARCH_TRIGGER_LINE_END:
    free(trace.point);

    return map_idx;
}

static int FindMapIndexById(int id, LIST_ROAD_MAP &RoadMapList)
{
    for (int i = 0; i < RoadMapList.size(); ++i) {
        if (RoadMapList[i].id == id) {
            return i;
        }
    }
    return -1;
}

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

@@ -5,4 +5,9 @@
#ifndef MYAPPLICATION2_ROAD_EXAM_H
#define MYAPPLICATION2_ROAD_EXAM_H

#include "../driver_test.h"

void InitRoadExam(void);
void TestRoadGeneral(LIST_ROAD_MAP &RoadMapList, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime);

#endif //MYAPPLICATION2_ROAD_EXAM_H

 lib/src/main/cpp/utils/xconvert.cpp

@@ -23,7 +23,7 @@
    return ((double)kmh) * 1000.0 / 3600.0;
}

double ConvertMs2KMs(double ms)
double ConvertMs2KMh(double ms)
{
    return ms * 3600.0 / 1000.0;
}

 lib/src/main/cpp/utils/xconvert.h

@@ -8,7 +8,7 @@
#include <cstdint>

double ConvertKMh2Ms(int kmh);
double ConvertMs2KMs(double ms);
double ConvertMs2KMh(double ms);

void ConvertPhoneNum(uint8_t *dst, int length, const char *src);
void ConvertHex2String(char *str, const uint8_t *hex, int length);