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

yy1717

2020-08-21 8a09b209f1c546a2fa15329e8f69b4a4f89557c9

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