起步和直线

yy1717

2020-03-23 682b17ff66dff23e03c6a57de276ea0c3e670c0e

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

@@ -12,6 +12,7 @@
#include "../native-lib.h"
#include "through_something.h"
#include "../master/comm_if.h"
#include "drive_straight.h"

#include <vector>
#include <list>
@@ -23,6 +24,12 @@
using namespace std;

#define TURN_CHECK_CNT          4

enum {
    START_CAR_NOT_DO,
    START_CAR_DOING,
    START_CAR_DONE
};

static const int TURN_THRESHOLD = 5;
static const int TURN_CHECK_INTERVAL = 500;
@@ -40,7 +47,7 @@
static int prevMoveDirect;
static uint32_t stopTimepoint = 0;
static bool reportStopCarOnRedArea;
static PointF stopPoint;
static PointF stopPoint, startPoint;
static bool prevGearError = false;
static bool prevGearNSlide = false;

@@ -61,8 +68,12 @@
static int gearErrorTime;
static int gearNSlideTime;

static int startCar;
static int currExamMapIndex;

static const int MAX_ENGINE_RPM = 2500;
static const double START_CAR_MOVE_DISTANCE = 0.5;//10.0;
static const double START_CAR_CHECK_DOOR_DISTANCE = 0.1;//1.0;
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);
@@ -73,8 +84,9 @@
static const double MAX_SPEED = 40.0 * 1000.0 / 3600.0;
static const int SPEED_GEAR_TABLE[][2] = {{0, 20}, {5, 30}, {15, 40}, {25, 10000}, {35, 10000}};

static void TestRoadStartCar(const car_model *car, double speed, int moveDirect, const struct RtkTime *rtkTime);
static void Rtk2DriveTimer(struct drive_timer &tm, const struct RtkTime *rtkTime);
static char isTurn(int currYaw, int prevYaw);
static char isTurn(int currYaw, int prevYaw, int &ang);
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);
@@ -107,10 +119,69 @@
    gearNSlideTime = 0;

    currExamMapIndex = -1;

    startCar = START_CAR_NOT_DO;
}

static void TestRoadStartCar(const car_model *car, double speed, int moveDirect, const struct RtkTime *rtkTime)
{
    double moveDistance;
    static bool checkDoor = false;
    static bool handBreakActive = false;
    static bool reportRPMOver = false;

    if (startCar == START_CAR_NOT_DO) {
        startPoint = car->basePoint;
        reportRPMOver = false;
        startCar = START_CAR_DOING;

        PlayTTS("请起步");
    } else if (startCar == START_CAR_DOING) {
        moveDistance = DistanceOf(startPoint, car->basePoint);
        if (moveDistance > START_CAR_MOVE_DISTANCE) {
            
            if (ReadCarStatus(HAND_BREAK) == BREAK_ACTIVE) {
                DEBUG("Handbreak active move over 10m");
                // 手刹拉起状态下，行驶了10米以上，不合格
                AddExamFault(25, rtkTime);
            } else if (handBreakActive) {
                // 手刹拉起状态下，行驶了1米以上，扣10分
                DEBUG("Handbreak active move over 1M");
                AddExamFault(26, rtkTime);
            }
            startCar = START_CAR_DONE;
        } else if (moveDistance >= START_CAR_CHECK_DOOR_DISTANCE) {
            if (!checkDoor) {
                checkDoor = true;

                if (ReadCarStatus(DOOR) == DOOR_OPEN) {
                    // 车门未完全关闭，不合格
                    DEBUG("车门未关闭");
                    AddExamFault(23, rtkTime);
                }

                if (ReadCarStatus(HAND_BREAK) == BREAK_ACTIVE) {
                    handBreakActive = true;
                }
            }
        }

        if (ReadCarStatus(ENGINE_RPM) > MAX_ENGINE_RPM && !reportRPMOver) {
            // 转速超标，不合格
            DEBUG("转速超标");
            AddExamFault(29, rtkTime);
            reportRPMOver = true;
        }
    } else {

    }
}

void TestRoadGeneral(LIST_ROAD_MAP &RoadMapList, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime)
{
    // 起步检测
    TestRoadStartCar(car, speed, moveDirect, rtkTime);

    // 超速检测
    if (moveDirect != 0 && speed > MAX_SPEED) {
        if (!occurOverSpeed) {
@@ -266,7 +337,7 @@
    // 检查是否持续转向
    char turnDirect = CheckCarTurn(CarModelList);
    if (turnDirect == 'L') {
        PlayTTS("左1", 5692);
//        PlayTTS("左1");
        if (currTurnSignalStatus != LEFT_TURN_LIGHT) {
            if (!reportTurnSignalError) {
                DEBUG("没打左转灯");
@@ -284,7 +355,7 @@
            }
        }
    } else if (turnDirect == 'R') {
        PlayTTS("右1", 5692);
//        PlayTTS("右1");
        if (currTurnSignalStatus != RIGHT_TURN_LIGHT) {
            if (!reportTurnSignalError) {
                DEBUG("没打右转灯");
@@ -386,7 +457,7 @@

                // 检查变道前，是否提前转向灯
                if (inter == 1) {
                    PlayTTS("左2", 5698);
//                    PlayTTS("左2");
                    // 向左侧变道
                    DEBUG("向左侧变道");
                    if (turnSignalStatusWhenCrashGreenLine != LEFT_TURN_LIGHT) {
@@ -395,7 +466,7 @@
                        AddExamFault(13, rtkTime);
                    }
                } else {
                    PlayTTS("右2", 5698);
//                    PlayTTS("右2");
                    // 向右侧变道
                    DEBUG("向右侧变道");
                    if (turnSignalStatusWhenCrashGreenLine != RIGHT_TURN_LIGHT) {
@@ -410,20 +481,32 @@
        checkCrashGreenTimeout = 0;
    }

    // 触发线检测
    if (currExamMapIndex == -1) {
    // 完成起步后，触发线检测
    if (currExamMapIndex == -1 && startCar == START_CAR_DONE) {
        currExamMapIndex = CrashTriggerLine(RoadMapList, car, CarModelList);
        if (currExamMapIndex != -1) {
            DEBUG("碰撞触发线");

            MA_EnterMap(RoadMapList[currExamMapIndex].id, RoadMapList[currExamMapIndex].type, 1);
            StartThroughExam(currExamMapIndex, RoadMapList);

            if (RoadMapList[currExamMapIndex].type >= THROUGH_INTERSECTION_MAP &&
                    RoadMapList[currExamMapIndex].type <= TURN_AROUND_MAP) {
                StartThroughExam(currExamMapIndex, RoadMapList);
            } else if (RoadMapList[currExamMapIndex].type == DRIVE_STRAIGHT_MAP) {
                StartDriveStraightExam(currExamMapIndex, RoadMapList);
            }
        }
    } else {
    } else if (startCar == START_CAR_DONE) {
        int prevIdx = currExamMapIndex;

        currExamMapIndex = ExecuteThroughExam(currExamMapIndex, RoadMapList, car,
        CarModelList, speed, moveDirect, rtkTime);
        if (currExamMapIndex >= THROUGH_INTERSECTION_MAP && currExamMapIndex <= TURN_AROUND_MAP) {
            currExamMapIndex = ExecuteThroughExam(currExamMapIndex, RoadMapList, car,
                                                  CarModelList, speed, moveDirect, rtkTime);
        }
        else if (currExamMapIndex == DRIVE_STRAIGHT_MAP) {
            currExamMapIndex = ExecuteDriveStraightExam(currExamMapIndex, RoadMapList, car,
                                     CarModelList, speed, moveDirect, rtkTime);
        }

        if (currExamMapIndex == -1) {
            MA_EnterMap(RoadMapList[prevIdx].id, RoadMapList[prevIdx].type, 1);
@@ -521,10 +604,9 @@
    tm.msec = rtkTime->mss;
}

static char isTurn(int currYaw, int prevYaw)
static char isTurn(int currYaw, int prevYaw, int &ang)
{
//    DEBUG("currYaw %d prevYaw %d", currYaw, prevYaw);

    int deltaAng = 0;

    if (ABS(currYaw - prevYaw) > 180) {
@@ -532,6 +614,8 @@
    } else {
        deltaAng = ABS(currYaw - prevYaw);
    }

    ang = deltaAng;

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

@@ -545,12 +629,12 @@
        }
    }

    return 0;
    return 'N';
}

static char CheckCarTurn(LIST_CAR_MODEL &CarModelList)
{
    // 最近3秒内，每秒的角度差大于10度，且方向相同，连续3秒，认为转向
    // 最近2秒内，每0.5秒的角度差大于5度，且方向相同，连续4次；或突现超30度的转向；认为转向。
    if (CarModelList.size() < 1)
        return false;

@@ -571,10 +655,15 @@
        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);
            int ang = 0;
            turn[checkCnt] = isTurn((int)c1->yaw, (int)c2->yaw, ang);
//            DEBUG("%c  角度比较 %02d:%02d:%02d.%03d  %02d:%02d:%02d.%03d", turn[checkCnt], 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 (turn[checkCnt] == 0) {

            if (turn[checkCnt] == 'N') {
                break;
            } else if (ang >= 30) {
                DEBUG("左右转确认 %c", turn[checkCnt]);
                return turn[checkCnt];
            }

            c1 = c2;
@@ -790,3 +879,5 @@
    }
    return -1;
}