角度修正。

fctom1215

2020-03-27 53be5cac481ddc7927ca3f8fe75a5180f0d6567a

角度修正。

 lib/src/main/cpp/test_items2/drive_straight.cpp

@@ -56,23 +56,28 @@
            crossStartLine = true;
            startPoint = car->basePoint;
            edgeDistance = DistanceOf(car->basePoint, road_edge);

            DEBUG("当前基准路边间距 %f", edgeDistance);
        }
    } else {
        double distanceToStart = DistanceOf(car->basePoint, startPoint);

        if (distanceToStart > CHECK_STAGE_DISTANCE) {
            if (!reportOffsetOver && fabs(DistanceOf(car->basePoint, road_edge) - edgeDistance) > MAX_OFFSET_DISTANCE) {
                DEBUG("直线偏移大于30厘米");
                // 偏移大于30厘米，不合格
                AddExamFault(30, rtkTime);
                reportOffsetOver = true;
        DEBUG("路边间距 %f --- %f", DistanceOf(car->basePoint, road_edge), edgeDistance);

                //////////////////////////////////////////////
                startPoint = car->basePoint;
                edgeDistance = DistanceOf(car->basePoint, road_edge);
                reportOffsetOver = false;
            }
        } else {
        if (!reportOffsetOver && fabs(DistanceOf(car->basePoint, road_edge) - edgeDistance) > MAX_OFFSET_DISTANCE) {
            DEBUG("直线偏移大于30厘米");
            // 偏移大于30厘米，不合格
            AddExamFault(30, rtkTime);
            reportOffsetOver = true;

            //////////////////////////////////////////////
            startPoint = car->basePoint;
            edgeDistance = DistanceOf(car->basePoint, road_edge);
            reportOffsetOver = false;
        }

        if (distanceToStart > CHECK_STAGE_DISTANCE) {
            DEBUG("复位边距偏移量");
            startPoint = car->basePoint;
            edgeDistance = DistanceOf(car->basePoint, road_edge);
            reportOffsetOver = false;

 lib/src/main/cpp/test_items2/dummy_light.cpp

@@ -58,9 +58,9 @@

void DummyLightTTSDone(int id)
{
    DEBUG("DummyLightTTSDone %d", id);
    // 等语音播报完毕后计时
    if (id == examTtsSeq) {
        DEBUG("DummyLightTTSDone %d", id);
        for (int i = 0; i < contentNum; ++i) {
            if (content[i].itemStatus == TTS_DOING) {
                content[i].itemStatus = TTS_DONE;

 lib/src/main/cpp/test_items2/operate_gear.cpp

@@ -156,9 +156,9 @@

void OperateGearTTSDone(int id)
{
    DEBUG("升降挡指令发出完毕 %d", id);
    // 等语音播报完毕后计时
    if (id == examTtsSeq) {
        DEBUG("升降挡指令发出完毕 %d", id);
        opGear = CHECK_OP;
        AppTimer_add(CheckGear, D_SEC(7));
    }

 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);
                    }
                }
            }

 lib/src/main/cpp/test_items2/stop_car.cpp

@@ -61,9 +61,9 @@

void StopCarTTSDone(int id)
{
    DEBUG("StopCarTTSDone %d", id);
    // 等语音播报完毕后计时
    if (id == examTtsSeq) {
        DEBUG("StopCarTTSDone %d", id);
        ttsPlayEnd = 1;
    }
}
@@ -88,14 +88,16 @@
                                rtkTime->mss * 10,
                                prevPointTime.hour, prevPointTime.min,
                                prevPointTime.sec, prevPointTime.msec * 10);

    moveDistance += (double)diff * (speed + prevSpeed) / 2;
    prevSpeed = speed;
    Rtk2DriveTimer(prevPointTime, rtkTime);
    if (diff >= D_SEC(1)) {
        moveDistance += (double)diff * (speed + prevSpeed) / 2.0 / 1000.0;
        prevSpeed = speed;
        Rtk2DriveTimer(prevPointTime, rtkTime);
        DEBUG("停车已行驶距离 %f", moveDistance);
    }

    if (moveDistance > 150) {
        // 150米内未停车，不合格
        DEBUG("停车距离超标");
        DEBUG("停车距离超标，靠边停车结束");
        AddExamFault(33, rtkTime);
        return -1;
    }
@@ -103,7 +105,7 @@
    if (moveDirect != prevMoveDirect) {
        if (moveDirect == 0) {
            stopTimepoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
            DEBUG("停车了 %d %d %d %d %d %d %d", rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss);
            DEBUG("####### 靠边停车，停车了 %d %d %d %d %d %d %d ######", rtkTime->YY, rtkTime->MM, rtkTime->DD, rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss);
        }
        prevMoveDirect = moveDirect;
    } else if (moveDirect == 0) {
@@ -189,6 +191,7 @@
            }
        }
    } else if (occurOpenDoor) {
        DEBUG("开车门，靠边停车结束");
        return -1;
    }

@@ -196,6 +199,7 @@
        uint32_t tp = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);

        if (tp - openDoorTimepoint >= OPEN_DOOR_TIMEOUT + D_SEC(5)) {
            DEBUG("开门时间大于20秒，靠边停车结束");
            return -1;
        }
    }

 lib/src/main/cpp/test_items2/through_something.cpp

@@ -25,7 +25,7 @@
        return;
    DEBUG("进入路考通过something地图 index = %d id = %d item = %d", index, RoadMapList[index].id, RoadMapList[index].type);
    if (!RoadMapList[index].tts.empty()) {
        DEBUG("播放TTS");
        DEBUG("播放TTS %s", RoadMapList[index].tts.c_str());
        PlayTTS(RoadMapList[index].tts.c_str());
    } else {
        DEBUG("没有TTS");