角度修正。

fctom1215

2020-03-27 53be5cac481ddc7927ca3f8fe75a5180f0d6567a

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

@@ -25,7 +25,7 @@

using namespace std;

#define TURN_CHECK_CNT          4
#define TURN_CHECK_CNT          6

enum {
    START_CAR_NOT_DO,
@@ -33,7 +33,7 @@
    START_CAR_DONE
};

static const int TURN_THRESHOLD = 5;
static const int TURN_THRESHOLD = 1;
static const int TURN_CHECK_INTERVAL = 500;
const double SLIDE_DISTANCE_THRESHOLD_RED = 0.3;
const double SLIDE_DISTANCE_THRESHOLD_YELLOW = 0.1;
@@ -57,10 +57,12 @@
static bool slideLongDistance;
static bool slideNormalDistance;
static bool occurSlide;
static bool startCarLeftTurnSignal, checkStartCarSignal;

static struct drive_timer crashGreenRunTime, crashGreenCmpTime, crashGreenStartTime, turnSignalChangeTime;
static struct drive_timer gearErrorTimePoint;
static struct drive_timer gearNSlideTimePoint;
static struct drive_timer startCarLeftTurnSignalTime;

static int gearErrorTime;
static int gearNSlideTime;
@@ -71,6 +73,10 @@
static bool checkDoor = false;
static bool handBreakActive = false;
static bool reportRPMOver = false;

static const uint32_t TURN_ERROR_COLD_TIME = D_SEC(10);

static bool turnError13Cold, turnError14Cold;

static const int MAX_ENGINE_RPM = 2500;
static const double START_CAR_MOVE_DISTANCE = 10.0;
@@ -94,6 +100,10 @@
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);

static void TurnSignalError13ColdTimer(union sigval sig);
static void TurnSignalError14ColdTimer(union sigval sig);
static void ReportTurnSignalError(int err, const struct RtkTime *rtkTime);

void InitRoadExam(void)
{
@@ -127,6 +137,10 @@
    checkDoor = false;
    handBreakActive = false;
    reportRPMOver = false;

    checkStartCarSignal = startCarLeftTurnSignal = false;

    turnError13Cold = turnError14Cold = true;
}

void TerminateRoadExam(void)
@@ -134,6 +148,42 @@
    TerminateDummyLightExam();
    TerminateStopCarExam();
    TerminateOperateGearExam();

    AppTimer_delete(TurnSignalError13ColdTimer);
    AppTimer_delete(TurnSignalError14ColdTimer);
}

static void TurnSignalError13ColdTimer(union sigval sig)
{
    AppTimer_delete(TurnSignalError13ColdTimer);

    turnError13Cold = true;
}

static void TurnSignalError14ColdTimer(union sigval sig)
{
    AppTimer_delete(TurnSignalError14ColdTimer);

    turnError14Cold = true;
}

static void ReportTurnSignalError(int err, const struct RtkTime *rtkTime)
{
    if (err == 13 && turnError13Cold) {
        DEBUG("起步不开转向灯");
        AddExamFault(13, rtkTime);

        turnError13Cold = false;
        AppTimer_delete(TurnSignalError13ColdTimer);
        AppTimer_add(TurnSignalError13ColdTimer, TURN_ERROR_COLD_TIME);
    } else if (err == 14 && turnError14Cold) {
        DEBUG("起步转向灯不足3秒");
        AddExamFault(14, rtkTime);

        turnError14Cold = false;
        AppTimer_delete(TurnSignalError14ColdTimer);
        AppTimer_add(TurnSignalError14ColdTimer, TURN_ERROR_COLD_TIME);
    }
}

static void TestRoadStartCar(const car_model *car, double speed, int moveDirect, const struct RtkTime *rtkTime)
@@ -148,8 +198,24 @@
        PlayTTS("请起步");
    } else if (startCar == START_CAR_DOING) {
        moveDistance = DistanceOf(startPoint, car->basePoint);
        DEBUG("起步行驶距离 %f", moveDistance);

        if (!startCarLeftTurnSignal && ReadCarStatus(TURN_SIGNAL_LAMP) == LEFT_TURN_LIGHT) {
            startCarLeftTurnSignal = true;
            Rtk2DriveTimer(startCarLeftTurnSignalTime, rtkTime);
        }

        if (!checkStartCarSignal && moveDirect == 1) {
            checkStartCarSignal = true;
            if (!startCarLeftTurnSignal) {
                ReportTurnSignalError(13, rtkTime);
            } else if (TimeGetDiff(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10,
                                   startCarLeftTurnSignalTime.hour, startCarLeftTurnSignalTime.min, startCarLeftTurnSignalTime.sec, startCarLeftTurnSignalTime.msec*10) < TURN_SIGNAL_LAMP_ADVANCE) {
                ReportTurnSignalError(14, rtkTime);
            }
        }

        if (moveDistance > START_CAR_MOVE_DISTANCE) {
            
            if (ReadCarStatus(HAND_BREAK) == BREAK_ACTIVE) {
                DEBUG("Handbreak active move over 10m");
                // 手刹拉起状态下，行驶了10米以上，不合格
@@ -159,6 +225,9 @@
                DEBUG("Handbreak active move over 1M");
                AddExamFault(26, rtkTime);
            }

            PlayTTS("完成起步");
            DEBUG("############# 完成起步 ############");
            startCar = START_CAR_DONE;
        } else if (moveDistance >= START_CAR_CHECK_DOOR_DISTANCE) {
            if (!checkDoor) {
@@ -363,16 +432,16 @@
            if (!reportTurnSignalError) {
                DEBUG("没打左转灯");
                // 没打左转灯，不合格
                AddExamFault(13, rtkTime);
                reportTurnSignalError = true;
                ReportTurnSignalError(13, rtkTime);
            }
        } 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;
                ReportTurnSignalError(14, rtkTime);
            }
        }
    } else if (turnDirect == 'R') {
@@ -381,16 +450,16 @@
            if (!reportTurnSignalError) {
                DEBUG("没打右转灯");
                // 没打右转灯，不合格
                AddExamFault(13, rtkTime);
                reportTurnSignalError = true;
                ReportTurnSignalError(13, rtkTime);
            }
        } 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;
                ReportTurnSignalError(14, rtkTime);
            }
        }
    } else {
@@ -484,7 +553,7 @@
                    if (turnSignalStatusWhenCrashGreenLine != LEFT_TURN_LIGHT) {
                        DEBUG("变调未打灯!!");
                        // 没打灯，不合格
                        AddExamFault(13, rtkTime);
                        ReportTurnSignalError(13, rtkTime);
                    }
                } else {
//                    PlayTTS("右2");
@@ -493,7 +562,7 @@
                    if (turnSignalStatusWhenCrashGreenLine != RIGHT_TURN_LIGHT) {
                        DEBUG("变调未打灯!!");
                        // 没打灯，不合格
                        AddExamFault(13, rtkTime);
                        ReportTurnSignalError(14, rtkTime);
                    }
                }
            }