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

yy1717

2020-08-21 8a09b209f1c546a2fa15329e8f69b4a4f89557c9

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