自适应触发和个子项修改。

yy1717

2020-08-21 8a09b209f1c546a2fa15329e8f69b4a4f89557c9

自适应触发和个子项修改。

 lib/src/main/cpp/CMakeLists.txt

@@ -47,6 +47,8 @@
        test_items2/operate_gear.cpp
        test_items2/smart_item.cpp
        test_items2/car_start.cpp
        test_items2/overtake.cpp
        test_items2/change_lane.cpp

        rtk_module/rtk.cpp
        rtk_module/virtual_rtk.cpp

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

@@ -377,9 +377,9 @@
        }
        case SENSOR_DOOR: {
            if (value == 0) {
                WriteCarStatus(DOOR, DOOR_OPEN);
            } else {
                WriteCarStatus(DOOR, DOOR_CLOSE);
            } else {
                WriteCarStatus(DOOR, DOOR_OPEN);
            }
            break;
        }

 lib/src/main/cpp/test_items2/change_lane.cpp

New file
@@ -0,0 +1,50 @@
//
// Created by YY on 2020/8/21.
//

#include "change_lane.h"
#include "../test_common/odo_graph.h"
#include "../native-lib.h"
#include "../jni_log.h"
#include "../driver_test.h"

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

static double maxMoveDistance;
static int originalLane;
static bool start;

static void TtsBack(int seq)
{
    maxMoveDistance = ReadOdo();
    start = true;
}

void StartChaneLaneExam(int ori_lane)
{
    DEBUG("变更车道");

    start = false;
    originalLane = ori_lane;
    PlayTTS("前方请变更车道", TtsBack);
}

bool TestChangeLane(int currLane, const struct RtkTime *rtkTime)
{
    if (!start)
        return true;
    if (originalLane != currLane) {
        DEBUG("完成变道");
        PlayTTS("完成变道", NULL);
        return false;
    }

    if (ReadOdo() - maxMoveDistance > 100) {
        // 超车未完成
        DEBUG("超车固定距离内未完成");
        AddExamFault(3, rtkTime);
        return false;
    }

    return true;
}

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

New file
@@ -0,0 +1,12 @@
//
// Created by YY on 2020/8/21.
//

#ifndef MYAPPLICATION2_CHANGE_LANE_H
#define MYAPPLICATION2_CHANGE_LANE_H

void StartChaneLaneExam(int ori_lane);

bool TestChangeLane(int currLane, const struct RtkTime *rtkTime);

#endif //MYAPPLICATION2_CHANGE_LANE_H

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

@@ -9,6 +9,7 @@
#include "road_exam.h"
#include "../master/comm_if.h"
#include "../common/apptimer.h"
#include "../test_common/odo_graph.h"
#include <cmath>

#define DEBUG(fmt, args...)     LOGD("<drive_straight> <%s>: " fmt, __func__, ##args)
@@ -16,148 +17,56 @@
static const double CHECK_STAGE_DISTANCE = 100.0;
static const double MAX_OFFSET_DISTANCE = 0.3;

static int ttsPlayEnd;
static bool crossStartLine;
static bool reportOffsetOver;
static double edgeDistance;
static double distanceToStartSum;
static int examTtsSeq;
static int setup;
static double beginOdo;
static int yaw_stat;

static double CalcDistance2Edge(road_t &road,  const car_model *car);
static void PlayTTSTimeout(union sigval sig);
static void TtsBack(int seq)
{
    setup = 1;
}

void StartDriveStraightExam(std::string tts) {
void StartDriveStraightExam(void) {
    DEBUG("开始直线行驶");

    ttsPlayEnd = 0;
    if (!tts.empty()) {
        examTtsSeq = PlayTTS(tts.c_str(), NULL);
    } else {
        examTtsSeq = PlayTTS("请保持直线行驶", NULL);
    }

    distanceToStartSum = 0;
    reportOffsetOver = false;

    AppTimer_delete(PlayTTSTimeout);
    AppTimer_add(PlayTTSTimeout, D_SEC(5));
    setup = 0;
    yaw_stat = 0;
    PlayTTS("请保持直线行驶", TtsBack);
}

int ExecuteDriveStraightExam(road_t &road,  const car_model *car,
                             const trigger_line_t *item, const struct RtkTime *rtkTime) {
    static PointF startPoint;
bool TestDriveStraight(const car_model *car, const struct RtkTime *rtkTime) {
    static double yaw;
    static Line baseLine;

    double dis2roadEdge = 0;
    if (setup == 1) {
        // 偏航角平均值
        if (yaw_stat == 0) {
            yaw = car->yaw;
        } else {
            yaw += car->yaw;
        }
        yaw_stat++;
        if (yaw_stat == 5) {
            yaw = fmod(yaw, 360) / 5;

    if (ttsPlayEnd == 1) {
        ttsPlayEnd = 2;
        startPoint = car->basePoint;
        edgeDistance = CalcDistance2Edge(road, car);           // 基准边距

        DEBUG("当前基准路边间距 %f", edgeDistance);
            PointF extPoint = PointExtend(car->basePoint, 100, yaw);
            MakeLine(&baseLine, &car->basePoint, &extPoint);
            beginOdo = ReadOdo();
            setup = 2;
        }
    } else if (setup == 2) {
        if (DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], baseLine) > MAX_OFFSET_DISTANCE) {
            DEBUG("直线偏移大于30厘米");
            // 偏移大于30厘米，不合格
            AddExamFault(30, rtkTime);
            return false;
        }
    }

    if (ttsPlayEnd != 2)
        return 1;

    double distanceToStart = DistanceOf(car->basePoint, startPoint);
    dis2roadEdge = CalcDistance2Edge(road, car);

    DEBUG("路边间距 %f --- %f", dis2roadEdge, edgeDistance);

    if (!reportOffsetOver && fabs(dis2roadEdge - edgeDistance) > MAX_OFFSET_DISTANCE) {
        DEBUG("直线偏移大于30厘米");
        // 偏移大于30厘米，不合格
        AddExamFault(30, rtkTime);
        reportOffsetOver = true;

        //////////////////////////////////////////////
//        startPoint = car->basePoint;
//        edgeDistance = dis2roadEdge;
//        reportOffsetOver = false;
    }

    if (distanceToStart > CHECK_STAGE_DISTANCE) {
        DEBUG("复位边距偏移量");
        startPoint = car->basePoint;
        edgeDistance = dis2roadEdge;
        reportOffsetOver = false;
        distanceToStartSum += distanceToStart;
        distanceToStart = 0;
    }

    if (distanceToStart + distanceToStartSum > 105) {
        DEBUG("离开直线行驶区域");
    if (setup == 2 && ReadOdo() - beginOdo > CHECK_STAGE_DISTANCE) {
        DEBUG("直线行驶结束");
        PlayTTS("直线行驶结束", NULL);
        return -1;
    }
    return 1;
}

void DriveStraightTTSDone(int id)
{
    // 等语音播报完毕后计时
    if (id == examTtsSeq) {
        DEBUG("StopCarTTSDone %d", id);
        ttsPlayEnd = 1;
        AppTimer_delete(PlayTTSTimeout);
    }
}

void TerminateDriveStraightExam(void)
{
    AppTimer_delete(PlayTTSTimeout);
}

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

    ttsPlayEnd = 1;
}

static double CalcDistance2Edge(road_t &road,  const car_model *car)
{
    PointF vp;
    bool get_vp = false;
    double distance = 0;

    // 检测道路边缘线
    for (int i = 0; i < road.leftEdge.size(); ++i) {
        PointF p1, p2;
        Line edge;

        p1 = road.leftEdge[i].points[0];
        for (int j = 1; j < road.leftEdge[i].points.size(); ++j) {
            p2 = road.leftEdge[i].points[j];
            MakeLine(&edge, &p1, &p2);

            if (VerticalPointOnLine(car->basePoint, edge, vp)) {
                get_vp = true;
                goto FIND_VP_END;
            }

            p1 = p2;
        }
        return false;
    }

FIND_VP_END:
    if (get_vp) {
        DEBUG("得到垂点 %d: %f, %f -- %f, %f", road.id, car->basePoint.X, car->basePoint.Y, vp.X, vp.Y);
        distance = DistanceOf(car->basePoint, vp);
    } else {
        // 没有找到匹配线端，按最小距离顶点计算
        DEBUG("无垂点");
        distance = 100;
        for (int i = 0; i < road.leftEdge.size(); ++i) {
            for (int j = 0; j < road.leftEdge[i].points.size(); ++j) {
                double x;
                if (distance > (x = DistanceOf(car->basePoint, road.leftEdge[i].points[j]))) {
                    distance = x;
                }
            }
        }
    }

    return distance;
    return true;
}

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

@@ -7,10 +7,7 @@

#include "../driver_test.h"

void StartDriveStraightExam(std::string tts);
int ExecuteDriveStraightExam(road_t &road,  const car_model *car,
                             const trigger_line_t *item, const struct RtkTime *rtkTime);
void DriveStraightTTSDone(int id);
void TerminateDriveStraightExam(void);
void StartDriveStraightExam(void);
bool TestDriveStraight(const car_model *car, const struct RtkTime *rtkTime);

#endif //MYAPPLICATION2_DRIVE_STRAIGHT_H

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

@@ -19,7 +19,6 @@
static int setup;

static double maxMoveDistance;
static double gearMoveDistance;

static void TtsBack(int seq)
{
@@ -37,6 +36,8 @@

bool TestOperateGear(const struct RtkTime *rtkTime)
{
    static struct RtkTime shiftTime;

    car_sensor_value_t sensor = ReadCarSensorValue(GEAR);

    if (sensor.name != GEAR)
@@ -92,11 +93,11 @@
            return false;
        } else {
            // 在此挡位行驶一定距离，再执行下一个
            gearMoveDistance = ReadOdo();
            shiftTime = *rtkTime;
            setup = 3;
        }
    } else if (setup == 3) {
        if (ReadOdo() - gearMoveDistance > 10) {
        if (TimeGetDiff(&shiftTime, rtkTime) >= D_SEC(5)) {
            setup = 4;
            char buff[128];
            expectGear += 0 - upDownShift;
@@ -113,11 +114,11 @@
            return false;
        } else {
            // 在此挡位行驶一定距离，再执行下一个
            gearMoveDistance = ReadOdo();
            shiftTime = *rtkTime;
            setup = 5;
        }
    } else if (setup == 5) {
        if (ReadOdo() - gearMoveDistance > 10) {
        if (TimeGetDiff(&shiftTime, rtkTime) >= D_SEC(5)) {
            PlayTTS("加减挡位操作结束", NULL);
            return false;
        }

 lib/src/main/cpp/test_items2/overtake.cpp

New file
@@ -0,0 +1,70 @@
//
// Created by YY on 2020/8/21.
//

#include "overtake.h"
#include "../native-lib.h"
#include "../jni_log.h"
#include "../test_common/odo_graph.h"
#include "../driver_test.h"

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

static double maxMoveDistance;
static int setup;
static int originalLane;

static void TtsBack(int seq)
{
    maxMoveDistance = ReadOdo();
    setup = 1;
}

void StartOvertakeExam(int ori_lane)
{
    DEBUG("超越前方车辆");

    PlayTTS("请超越前方车辆", TtsBack);

    setup = 0;
    originalLane = ori_lane;
}

bool TestOvertake(int currLane, const struct RtkTime *rtkTime)
{
    if (setup == 0) {
        return true;
    }
    if (setup == 1) {
        if (originalLane == currLane) {

        } else if (currLane + 1 == originalLane) {
            setup = 2;
            originalLane = currLane;
        } else {
            DEBUG("右侧超车");
            AddExamFault(3, rtkTime);
            return false;
        }
    } else if (setup == 2) {
        if (originalLane == currLane) {

        } else if (currLane == originalLane + 1) {
            PlayTTS("完成超车", NULL);
            return false;
        } else {
            DEBUG("超车违规变道");
            AddExamFault(3, rtkTime);
            return false;
        }
    }

    if (ReadOdo() - maxMoveDistance > 150) {
        // 超车未完成
        DEBUG("超车固定距离内未完成");
        AddExamFault(3, rtkTime);
        return false;
    }

    return true;
}

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

New file
@@ -0,0 +1,12 @@
//
// Created by YY on 2020/8/21.
//

#ifndef MYAPPLICATION2_OVERTAKE_H
#define MYAPPLICATION2_OVERTAKE_H

void StartOvertakeExam(int ori_lane);

bool TestOvertake(int currLane, const struct RtkTime *rtkTime);

#endif //MYAPPLICATION2_OVERTAKE_H

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

@@ -17,6 +17,8 @@
#include "operate_gear.h"
#include "../test_common/odo_graph.h"
#include "car_start.h"
#include "overtake.h"
#include "change_lane.h"

#include <cmath>
#include <vector>
@@ -30,19 +32,6 @@

using namespace std;

#define TURN_CHECK_CNT          6

enum {
    START_CAR_NOT_DO,
    START_CAR_DOING,
    START_CAR_DONE
};

enum {
    STOP_CAR_NOT_DO,
    STOP_CAR_DOING,
    STOP_CAR_DONE
};

enum {
    CROSSING_NOT_HINT,
@@ -79,25 +68,15 @@

static const int CROSSING_TURN_THRESHOLD = 45;
static const int TURN_THRESHOLD = 3;
static const int TURN_CHECK_INTERVAL = 500;

const double SLIDE_DISTANCE_THRESHOLD_RED = 0.3;
const double SLIDE_DISTANCE_THRESHOLD_YELLOW = 0.1;
const double CHANGE_LANE_RANGE = 100.0;
const double OVERTAKE_RANGE = 150.0;
const int OVERTAKE_HOLD_TIME = D_SEC(3);                // 在超车道行驶的一段时间

const double EXAM_RANGE = 3000.0;                       // 至少驾驶距离

static const double NEXT_ROAD_TIP = 100.0;                  // 到达路口前提示下一个怎么走


static bool occurCrashRedLine;
static bool occurCrashGreenLine;
static bool occurOverSpeed;
static bool occurSecondBreak;

static char carIntersectionOfGreenLine;
static int currTurnSignalStatus;
static int turnSignalStatusWhenCrashGreenLine;

static int prevMoveDirect;
static uint32_t stopTimepoint = 0;
@@ -114,19 +93,11 @@

static struct drive_timer gearErrorTimePoint;
static struct drive_timer gearNSlideTimePoint;
static struct drive_timer startCarLeftTurnSignalTime;
static struct drive_timer overTakeCmpTime;

static int gearErrorTime;
static int gearNSlideTime;

static int stopCar;
static int currExamMapIndex;
static trigger_line_t *currRoadItem;
static PointF roadItemStartPoint;
static struct drive_timer roadItemStartTime;
static bool overtake = false;
static bool checkTurn = false;

static lane_t Lane;
static change_lane_t ChangeLane;
@@ -170,16 +141,12 @@
static const uint32_t CRASH_DOTTED_LINE_TIMEOUT = D_SEC(10);
static const uint32_t TURN_SIGNAL_LAMP_ADVANCE = D_SEC(3);

static const int CRL_NONE = 0;
static const int CRL_SEP_DOTTED = 1;
static const int CRL_SEP_SOLID = 2;
static const int CRL_EDGE_DOTTED = 3;
static const int CRL_EDGE_SOLID = 4;

static const double MAX_SPEED = 60.0 * 1000.0 / 3600.0;
static const double MAX_SPEED = 60.0 * 1000.0 / 3600.0;         // 超速确认
static const double DEC_MAX_SPEED = 55.0 * 1000.0 / 3600.0;     // 超速取消
static const int SPEED_GEAR_TABLE[][2] = {{0, 20}, {5, 30}, {15, 40}, {25, 10000}, {35, 10000}};

static void ItemExam(road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime, double straight, double road_end);
static void ItemExam(road_exam_map &RoadMap, int roadIndex, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime, double straight, double road_end);
static int isTurn(int currYaw, int prevYaw, int thres);
static void ResetTurnDetect(const car_model *car);
static void DetectTurn(const car_model *car, int moveDirect, const struct RtkTime *rtkTime);
@@ -187,8 +154,6 @@
static int NearbyCrossingGuide(int &stopLineIndex, int roadIndex, road_t &road, const car_model *car);

static trigger_line_t * EntryItem(int index, road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList);

static bool AllCmp(road_exam_map &map);

static int CalcRoadIndex(int currRoadIndex, road_exam_map &RoadMap, const car_model *car);

@@ -198,15 +163,10 @@
{
    DEBUG("Start road_exam");

    occurCrashRedLine = false;
    occurCrashGreenLine = false;
    occurOverSpeed = false;

    occurSecondBreak = false;

    carIntersectionOfGreenLine = 0;

    currTurnSignalStatus = OFF_LIGHT;
    prevMoveDirect = 0;

    reportStopCarOnRedArea = false;
@@ -220,14 +180,9 @@

    currExamMapIndex = -1;

    stopCar = STOP_CAR_NOT_DO;

    currRoadItem = NULL;

    Lane.road = Lane.sep = Lane.no = -1;
    Lane.guide = 0;

    checkTurn = false;

    CrashLineType = -1;
    turnCnt = -1;
@@ -934,7 +889,7 @@
                        DEBUG("变调未打灯!!");
                        // 没打灯，不合格
                        AddExamFault(13, rtkTime);
                    } else if (TimeGetDiff(&crashGreenStartTime, &lamp.time) >= D_SEC(3)) {
                    } else if (TimeGetDiff(&crashGreenStartTime, &lamp.time) >= TURN_SIGNAL_LAMP_ADVANCE) {
                        DEBUG("转向灯时间不足");
                        // 不足3秒，不合格
                        AddExamFault(14, rtkTime);
@@ -944,7 +899,7 @@
                        DEBUG("变调未打灯!!");
                        // 没打灯，不合格
                        AddExamFault(13, rtkTime);
                    } else if (TimeGetDiff(&crashGreenStartTime, &lamp.time) >= D_SEC(3)) {
                    } else if (TimeGetDiff(&crashGreenStartTime, &lamp.time) >= TURN_SIGNAL_LAMP_ADVANCE) {
                        DEBUG("转向灯时间不足");
                        // 不足3秒，不合格
                        AddExamFault(14, rtkTime);
@@ -1206,9 +1161,6 @@
    return guide;
}

static uint8_t itemExec[4] = {0};
static double odo;

void TestRoadGeneral(road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime)
{
    double BigStraightRoadFree = 0, RoadCrossingFree = 0;
@@ -1218,14 +1170,14 @@
    UpdataOdo(speed, moveDirect, rtkTime);

    // 超速检测
    if (moveDirect != 0 && speed > MAX_SPEED) {
    if (speed > MAX_SPEED) {
        if (!occurOverSpeed) {
            occurOverSpeed = true;
            // 超速，不合格
            DEBUG("超速 %f", speed);
            DEBUG("超速 %f", ConvertMs2KMh(speed));
            AddExamFault(10, rtkTime);
        }
    } else {
    } else if (speed < DEC_MAX_SPEED ) {
        occurOverSpeed = false;
    }

@@ -1433,7 +1385,7 @@

        RoadCrossingFree = freeSepDis;

        DEBUG("直道剩余距离 %f, 车道剩余距离 %f", BigStraightRoadFree, RoadCrossingFree);
//        DEBUG("累计行驶距离 %f, 直道剩余距离 %f, 车道剩余距离 %f", ReadOdo(), BigStraightRoadFree, RoadCrossingFree);
    }

    // 检测压线状态
@@ -1441,75 +1393,14 @@
    // 额外的转向检测
    DetectTurn(car, moveDirect, rtkTime);

    ItemExam(RoadMap, car, CarModelList, speed, moveDirect, rtkTime, BigStraightRoadFree, RoadCrossingFree);

    if (ReadOdo() > EXAM_RANGE && currRoadItem == NULL && AllCmp(RoadMap) && stopCar == STOP_CAR_NOT_DO) {
        // 在合适条件下停车结束考试
        StartStopCarExam("请靠边停车");
        stopCar = STOP_CAR_DOING;
    } else if (stopCar == STOP_CAR_DOING) {
        if (ExecuteStopCarExam(RoadMap.roads[currExamMapIndex], car, CarModelList, speed, moveDirect, rtkTime) < 0)
            stopCar = STOP_CAR_DONE;
    }

    // 执行某个项目
    /*if (currRoadItem != NULL) {
        if (currRoadItem->active == ROAD_ITEM_CHANGE_LANE) {
            if (DistanceOf(car->basePoint, roadItemStartPoint) > CHANGE_LANE_RANGE) {
                DEBUG("变道距离超标");
                AddExamFault(3, rtkTime);
                currRoadItem = NULL;
            }
        } else if (currRoadItem->active == ROAD_ITEM_OVERTAKE) {
            if (!overtake && DistanceOf(car->basePoint, roadItemStartPoint) > OVERTAKE_RANGE) {
                DEBUG("超车距离超标");
                AddExamFault(3, rtkTime);
                currRoadItem = NULL;
            } else if (overtake && TimeGetDiff(rtkTime->hh, rtkTime->mm, rtkTime->ss,
                                               rtkTime->mss * 10,
                                               overTakeCmpTime.hour, overTakeCmpTime.min,
                                               overTakeCmpTime.sec, overTakeCmpTime.msec * 10) > OVERTAKE_HOLD_TIME) {
                DEBUG("回原车道");
                PlayTTS("请返回原车道");
                currRoadItem = NULL;
            }
        } else if (currRoadItem->active == ROAD_ITEM_OPERATE_GEAR) {
            if (ExecuteOperateGearExam(rtkTime) < 0) {
                currRoadItem = NULL;
            }
        } else if (currRoadItem->active == ROAD_ITEM_STRAIGHT) {
            if (ExecuteDriveStraightExam(RoadMap.roads[currExamMapIndex], car, currRoadItem, rtkTime) < 0) {
                currRoadItem = NULL;
            }
        }
    } // 检测由触发线控制的项目
    else if (currExamMapIndex >= 0) {
        trigger_line_t *new_item = EntryItem(currExamMapIndex, RoadMap, car, CarModelList);

        if (new_item != NULL && !new_item->cmp) {
            currRoadItem = new_item;
            if (!currRoadItem->tts.empty())
                PlayTTS(currRoadItem->tts.c_str());

            // 初始时间和距离限制
            roadItemStartPoint = car->basePoint;
            Rtk2DriveTimer(roadItemStartTime, rtkTime);

            if (currRoadItem->active == ROAD_ITEM_OVERTAKE) {
                overtake = false;
            } else if (currRoadItem->active == ROAD_ITEM_OPERATE_GEAR) {
                StartOperateGearExam(rtkTime);
            } else if (currRoadItem->active == ROAD_ITEM_STRAIGHT) {
                StartDriveStraightExam(currRoadItem->tts);
            }
        }
    }*/
    ItemExam(RoadMap, currExamMapIndex, car, CarModelList, speed, moveDirect, rtkTime, BigStraightRoadFree, RoadCrossingFree);
}

static void ItemExam(road_exam_map &RoadMap, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime, double straight, double road_end)
static void ItemExam(road_exam_map &RoadMap, int roadIndex, const car_model *car, LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime, double straight, double road_end)
{
    static double freeRunDistance;
    static double totalRunDistance;
    static double freeRunExceptDistance = 50.0;

    if (RoadExamStatus == ROAD_EXAM_READY_NEXT) {
        if (RoadExamItem[ROAD_EXAM_ITEM_CAR_START] == ROAD_EXAM_ITEM_NOT_EXEC) {
@@ -1517,28 +1408,59 @@
            totalRunDistance = ReadOdo();
            RoadExamStatus = ROAD_EXAM_ITEM_CAR_START;
        } else {
            if (RoadExamItem[ROAD_EXAM_ITEM_STRAIGHT] == ROAD_EXAM_ITEM_NOT_EXEC) {
                if (straight > 170 && road_end > 170) {
            bool not_complete = false;

            if (RoadExamItem[ROAD_EXAM_ITEM_STRAIGHT] == ROAD_EXAM_ITEM_NOT_EXEC) {
                not_complete = true;
                if (straight > 170 && road_end > 170) {
                    StartDriveStraightExam();
                    RoadExamStatus = ROAD_EXAM_ITEM_STRAIGHT;
                    return;
                }
            }
            if (RoadExamItem[ROAD_EXAM_ITEM_OP_GEAR] == ROAD_EXAM_ITEM_NOT_EXEC) {
                if (straight > 150 && road_end > 150) {
                not_complete = true;
                if (road_end > 170) {
                    StartOperateGearExam();
                    RoadExamStatus = ROAD_EXAM_ITEM_OP_GEAR;
                    return;
                }
            }
            if (RoadExamItem[ROAD_EXAM_ITEM_CHANGE_LANE] == ROAD_EXAM_ITEM_NOT_EXEC) {

                not_complete = true;
                if (road_end > 150 && Lane.total > 1) {
                    StartChaneLaneExam(Lane.no);
                    RoadExamStatus = ROAD_EXAM_ITEM_CHANGE_LANE;
                    return;
                }
            }
            if (RoadExamItem[ROAD_EXAM_ITEM_OVER_TAKE] == ROAD_EXAM_ITEM_NOT_EXEC) {

                not_complete = true;
                if (road_end > 200 && Lane.total > 1) {
                    if (Lane.no == 0) {
                        // 已在最左车道，先变更车道？
                        StartChaneLaneExam(Lane.no);
                        RoadExamStatus = ROAD_EXAM_ITEM_CHANGE_LANE;
                    } else {
                        StartOvertakeExam(Lane.no);
                        RoadExamStatus = ROAD_EXAM_ITEM_OVER_TAKE;
                    }
                    return;
                }
            }

            if (!not_complete) {
                if (road_end > 200 && ReadOdo() > EXAM_RANGE) {
                    RoadExamStatus = ROAD_EXAM_ITEM_CAR_STOP;
                    StartStopCarExam();
                    return;
                }
            }
        }
    } else if (RoadExamStatus == ROAD_EXAM_FREE_RUN) {
        if (ReadOdo() - freeRunDistance > 300) {
        if (ReadOdo() - freeRunDistance > freeRunExceptDistance) {
            RoadExamStatus = ROAD_EXAM_READY_NEXT;

        }
    } else {
        bool testing = false;
@@ -1550,17 +1472,33 @@
            case ROAD_EXAM_ITEM_OP_GEAR:
                testing = TestOperateGear(rtkTime);
                break;
            case ROAD_EXAM_ITEM_CHANGE_LANE:
                testing = TestChangeLane(Lane.no, rtkTime);
                break;
            case ROAD_EXAM_ITEM_OVER_TAKE:
                testing = TestOvertake(Lane.no, rtkTime);
                break;
            case ROAD_EXAM_ITEM_STRAIGHT:
                testing = TestDriveStraight(car, rtkTime);
                break;
            case ROAD_EXAM_ITEM_CAR_STOP:
                testing = TestStopCar(RoadMap, roadIndex, car, moveDirect, rtkTime);
                break;
            default:break;
        }

        if (!testing) {
            RoadExamItem[RoadExamStatus] = ROAD_EXAM_ITEM_EXECED;
            if (RoadExamStatus == ROAD_EXAM_ITEM_CAR_START) {
                freeRunExceptDistance = 60.0;
            } else {
                freeRunExceptDistance = 200.0;
            }

            RoadExamStatus = ROAD_EXAM_FREE_RUN;
            freeRunDistance = ReadOdo();
        }
    }

}

void Rtk2DriveTimer(struct drive_timer &tm, const struct RtkTime *rtkTime)
@@ -1666,7 +1604,7 @@
                            DEBUG("变调未打灯!!");
                            // 没打灯，不合格
                            AddExamFault(13, rtkTime);
                        } else if (TimeGetDiff(&beginTurnTime, &lamp.time) >= D_SEC(3)) {
                        } else if (TimeGetDiff(&beginTurnTime, &lamp.time) >= TURN_SIGNAL_LAMP_ADVANCE) {
                            DEBUG("转向灯时间不足");
                            // 不足3秒，不合格
                            AddExamFault(14, rtkTime);
@@ -1676,7 +1614,7 @@
                            DEBUG("变调未打灯!!");
                            // 没打灯，不合格
                            AddExamFault(13, rtkTime);
                        } else if (TimeGetDiff(&beginTurnTime, &lamp.time) >= D_SEC(3)) {
                        } else if (TimeGetDiff(&beginTurnTime, &lamp.time) >= TURN_SIGNAL_LAMP_ADVANCE) {
                            DEBUG("转向灯时间不足");
                            // 不足3秒，不合格
                            AddExamFault(14, rtkTime);
@@ -1790,15 +1728,6 @@
    }

    return NULL;
}

static bool AllCmp(road_exam_map &map)
{
    for (int i = 0; i < map.triggerLines.size(); ++i) {
        if (!map.triggerLines[i].cmp)
            return false;
    }
    return true;
}

static double AnalysisRoad(road_exam_map &RoadMap, int roadIndex, lane_t lane, const car_model *car)

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

@@ -11,221 +11,132 @@
#include "../common/apptimer.h"
#include "../test_common/car_sensor.h"
#include "../defs.h"
#include "../test_common/odo_graph.h"

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

static bool inactiveBreakHandbreakTogether, notCloseEngine, inactiveHandBreakAfterOpenDoor, occurOpenDoor, doorNotClose, checkRoadDistance;
static int examTtsSeq = 0;
static int ttsPlayEnd;
static double moveDistance;
static double prevSpeed;
static struct drive_timer prevPointTime;
static int prevMoveDirect;
static uint32_t stopTimepoint = 0;
static uint32_t openDoorTimepoint;
static bool BreakHandbreakReleaseSametime, OpenDoor;

const int ENGINE_MIN_ROTATE = 200;
static const int ENGINE_MIN_ROTATE = 200;
static const double MAX_STOP_DISTANCE = 150;

static const uint32_t STOP_CAR_TIME = D_SEC(2);
static const uint32_t OPEN_DOOR_TIMEOUT = D_SEC(15);
static const double DISTANCE_TO_ROAD_EDGE_1 = 0.5;
static const double DISTANCE_TO_ROAD_EDGE_2 = 0.3;

static void PlayTTSTimeout(union sigval sig);
static double beginOdo;
static int setup;

void StartStopCarExam(std::string tts) {
static void TtsBack(int seq)
{
    setup = 1;
}

void StartStopCarExam(void) {
    DEBUG("靠边停车");
    ttsPlayEnd = 0;
    moveDistance = 0;
    prevMoveDirect = 0;
    inactiveBreakHandbreakTogether = false;
    notCloseEngine = false;
    inactiveHandBreakAfterOpenDoor = false;
    occurOpenDoor = false;
    doorNotClose = false;
    checkRoadDistance = false;

    if (!tts.empty()) {
        examTtsSeq = PlayTTS(tts.c_str(), NULL);
    } else {
        examTtsSeq = PlayTTS("请靠边停车", NULL);
    }

    AppTimer_delete(PlayTTSTimeout);
    AppTimer_add(PlayTTSTimeout, D_SEC(8));
    BreakHandbreakReleaseSametime = false;
    setup = 0;
    OpenDoor = false;
    PlayTTS("请靠边停车", TtsBack);
}

void StopCarTTSDone(int id)
{
    // 等语音播报完毕后计时
    if (id == examTtsSeq) {
        DEBUG("StopCarTTSDone %d", id);
        ttsPlayEnd = 1;
    }
}
bool TestStopCar(road_exam_map &RoadMap, int roadIndex, const car_model *car, int moveDirect, const struct RtkTime *rtkTime) {
    struct RtkTime time;

void TerminateStopCarExam(void)
{
    AppTimer_delete(PlayTTSTimeout);
}
    if (setup == 0)
        return true;

int ExecuteStopCarExam(road_t &road, const car_model *car,
                             LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime) {
    if (ttsPlayEnd == 1) {
        ttsPlayEnd = 2;
        prevSpeed = speed;
        Rtk2DriveTimer(prevPointTime, rtkTime);
    }

    if (ttsPlayEnd != 2)
        return 1;

    uint32_t diff = TimeGetDiff(rtkTime->hh, rtkTime->mm, rtkTime->ss,
                                rtkTime->mss * 10,
                                prevPointTime.hour, prevPointTime.min,
                                prevPointTime.sec, prevPointTime.msec * 10);
    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米内未停车，不合格
    if (roadIndex < 0) {
        DEBUG("停车距离超标，靠边停车结束");
        AddExamFault(33, rtkTime);
        return -1;
        return false;
    }

    if (moveDirect != prevMoveDirect) {
    if (setup == 1) {
        beginOdo = ReadOdo();
        setup = 2;
    } else if (setup == 2) {
        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);
            time = *rtkTime;
            setup = 3;
        }
        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 && !checkRoadDistance) {
    } else if (setup == 3) {
        if (moveDirect != 0) {
            setup = 2;
        } else if (TimeGetDiff(rtkTime, &time) >= D_SEC(2)) {
            // 停车超2秒，开始判断
            DEBUG("检测和路边的距离");
            PointF pc, vp;
            bool get_vp = false;
            double dis2roadEdge = 0;

            checkRoadDistance = true;
            // 前后轮的中点
            pc.X = (car->carXY[car->right_front_tire[TIRE_OUTSIDE]].X + car->carXY[car->right_rear_tire[TIRE_OUTSIDE]].X) / 2;
            pc.Y = (car->carXY[car->right_front_tire[TIRE_OUTSIDE]].Y + car->carXY[car->right_rear_tire[TIRE_OUTSIDE]].Y) / 2;
            PointF p1 = CalcProjectionWithRoadEdge(RoadMap.roads[roadIndex].rightEdge, car->carXY[ car->right_front_tire[TIRE_OUTSIDE] ]);
            PointF p2 = CalcProjectionWithRoadEdge(RoadMap.roads[roadIndex].rightEdge, car->carXY[ car->right_rear_tire[TIRE_OUTSIDE] ]);

            // 检测道路边缘线
            for (int i = 0; i < road.rightEdge.size(); ++i) {
                PointF p1, p2;
                Line edge;

                p1 = road.rightEdge[i].points[0];
                for (int j = 1; j < road.rightEdge[i].points.size(); ++j) {
                    p2 = road.rightEdge[i].points[j];
                    MakeLine(&edge, &p1, &p2);


                    if (VerticalPointOnLine(pc, edge, vp)) {
                        get_vp = true;
                        goto FIND_VP_END;
                    }

                    p1 = p2;
                }
            }

FIND_VP_END:
            if (get_vp) {
                dis2roadEdge = DistanceOf(pc, vp);
            } else {
                // 没有找到匹配线端，按最小距离顶点计算
                dis2roadEdge = 100;
                for (int i = 0; i < road.rightEdge.size(); ++i) {
                    for (int j = 0; j < road.rightEdge[i].points.size(); ++j) {
                        double dis;
                        if (dis2roadEdge > (dis = DistanceOf(pc, road.rightEdge[i].points[j]))) {
                            dis2roadEdge = dis;
                        }
                    }
                }
            }

            DEBUG("停车距路边距离 %f", dis2roadEdge);

            if (dis2roadEdge > DISTANCE_TO_ROAD_EDGE_1) {
            if (DistanceOf(p1, car->carXY[ car->right_front_tire[TIRE_OUTSIDE] ]) > DISTANCE_TO_ROAD_EDGE_1 ||
                    DistanceOf(p2, car->carXY[ car->right_rear_tire[TIRE_OUTSIDE] ]) > DISTANCE_TO_ROAD_EDGE_1) {
                DEBUG("停车超出路边0.5米");
                // 停车距离超过50厘米，不合格
                AddExamFault(36, rtkTime);
            } else if (dis2roadEdge > DISTANCE_TO_ROAD_EDGE_2) {
                return false;
            } else if (DistanceOf(p1, car->carXY[ car->right_front_tire[TIRE_OUTSIDE] ]) > DISTANCE_TO_ROAD_EDGE_2 ||
                       DistanceOf(p2, car->carXY[ car->right_rear_tire[TIRE_OUTSIDE] ]) > DISTANCE_TO_ROAD_EDGE_2) {
                DEBUG("停车超出路边0.3米");
                // 停车距离超过30厘米，扣10分
                AddExamFault(37, rtkTime);
                return false;
            }

            setup = 4;
        }
    }
    } else if (setup == 4) {
        car_sensor_value_t brk = ReadCarSensorValue(BREAK);
        car_sensor_value_t hbrk = ReadCarSensorValue(HAND_BREAK);
        car_sensor_value_t door = ReadCarSensorValue(DOOR);
        car_sensor_value_t rpm = ReadCarSensorValue(ENGINE_RPM);

    if (!inactiveBreakHandbreakTogether && ReadCarStatus(BREAK) == BREAK_INACTIVE && ReadCarStatus(HAND_BREAK) == BREAK_INACTIVE) {
        // 拉手刹前，松脚刹，扣10分
        DEBUG("拉手刹前，松脚刹");
        AddExamFault(39, rtkTime);
        inactiveBreakHandbreakTogether = true;
    }
        if (!BreakHandbreakReleaseSametime && brk.value == BREAK_INACTIVE && hbrk.value == BREAK_INACTIVE) {
            // 拉手刹前，松脚刹，扣10分
            DEBUG("拉手刹前，松脚刹");
            AddExamFault(39, rtkTime);
            BreakHandbreakReleaseSametime = true;
        }

    if (!notCloseEngine && ReadCarStatus(ENGINE_RPM) < ENGINE_MIN_ROTATE && ReadCarStatus(DOOR) == DOOR_OPEN) {
        // 下车前，不熄火，扣5分
        DEBUG("下车前，不熄火");
        AddExamFault(40, rtkTime);
        if (door.value == DOOR_OPEN) {
            if (!OpenDoor) {
                if (rpm.value < ENGINE_MIN_ROTATE) {
                    // 下车前，不熄火，扣5分
                    DEBUG("下车前，不熄火");
                    AddExamFault(40, rtkTime);
                }
                if (hbrk.value == BREAK_INACTIVE) {
                    // 开门前，未拉手刹， 扣10分
                    DEBUG("开门前，未拉手刹");
                    AddExamFault(38, rtkTime);
                }
                time = *rtkTime;
                OpenDoor = true;
            }

        notCloseEngine = true;
    }

    if (!inactiveHandBreakAfterOpenDoor && ReadCarStatus(DOOR) == DOOR_OPEN && ReadCarStatus(HAND_BREAK) == BREAK_INACTIVE) {
        // 开门前，未拉手刹， 扣10分
        DEBUG("开门前，未拉手刹");
        AddExamFault(38, rtkTime);

        inactiveHandBreakAfterOpenDoor = true;
    }

    if (ReadCarStatus(DOOR) == DOOR_OPEN) {
        if (!occurOpenDoor) {
            occurOpenDoor = true;
            openDoorTimepoint = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);
        } else {
            uint32_t tp = TimeMakeComposite(rtkTime->hh, rtkTime->mm, rtkTime->ss, rtkTime->mss*10);

            if (!doorNotClose && tp - openDoorTimepoint >= OPEN_DOOR_TIMEOUT) {
            if (TimeGetDiff(rtkTime, &time) > OPEN_DOOR_TIMEOUT) {
                // 开门时间超过15秒，不合格
                DEBUG("开门时间超过15秒");
                AddExamFault(35, rtkTime);
                doorNotClose = true;
                return false;
            }
        }
    } else if (occurOpenDoor) {
        DEBUG("开车门，靠边停车结束");
        return -1;
    }

    if (occurOpenDoor) {
        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;
        if (OpenDoor && door.value == DOOR_CLOSE) {
            DEBUG("完成停车");
            PlayTTS("靠边停车结束", NULL);
            return false;
        }
    }

    return 1;
}
    if (ReadOdo() - beginOdo > MAX_STOP_DISTANCE) {
        // 150米内未停车，不合格
        DEBUG("停车距离超标，靠边停车结束");
        AddExamFault(33, rtkTime);
        return false;
    }

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

    ttsPlayEnd = 1;
    return true;
}

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

@@ -7,10 +7,7 @@

#include "../driver_test.h"

void StartStopCarExam(std::string tts);
int ExecuteStopCarExam(road_t &road, const car_model *car,
                       LIST_CAR_MODEL &CarModelList, double speed, int moveDirect, const struct RtkTime *rtkTime);
void TerminateStopCarExam(void);
void StopCarTTSDone(int id);
void StartStopCarExam(void);
bool TestStopCar(road_exam_map &RoadMap, int roadIndex, const car_model *car, int moveDirect, const struct RtkTime *rtkTime);

#endif //MYAPPLICATION2_STOP_CAR_H