坐标

fctom1215

2020-08-29 d05c25aa4823e9d2aa812e9c0e3d1d66bb9861dd

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

@@ -20,6 +20,7 @@
static int setup;
static double beginOdo;
static int yaw_stat;
static double offsetBase;

static void TtsBack(int seq)
{
@@ -33,9 +34,13 @@
    PlayTTS("请保持直线行驶", TtsBack);
}

bool TestDriveStraight(const car_model *car, const struct RtkTime *rtkTime) {
bool TestDriveStraight(road_exam_map &RoadMap, int roadIndex, const car_model *car, const struct RtkTime *rtkTime) {
    static double yaw;
    static Line baseLine;
    static double offset1, offset2;

    if (roadIndex < 0 || roadIndex >= RoadMap.roads.size())
        return false;

    if (setup == 1) {
        // 偏航角平均值
@@ -47,7 +52,7 @@
        yaws.push_back(car->yaw);
        yaw_stat++;

        if (yaw_stat == 5) {
        if (yaw_stat == 15) {
            yaw = AvgYaw(yaws);
            vector<double>().swap(yaws);

@@ -55,13 +60,28 @@
            PointF extPoint = PointExtend(car->basePoint, 100, yaw);
            MakeLine(&baseLine, &car->basePoint, &extPoint);
            beginOdo = ReadOdo();

            PointF px = CalcProjectionWithRoadEdge(RoadMap.roads[roadIndex].rightEdge, car->carXY[car->axial[AXIAL_FRONT]]);
            offsetBase = DistanceOf(px, car->carXY[car->axial[AXIAL_FRONT]]);
            DEBUG("起点偏离 %f", offsetBase);

            setup = 2;
        }
    } else if (setup == 2) {
        double offset = DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], baseLine);
        offset1 = DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], baseLine);

        if (offset > MAX_OFFSET_DISTANCE) {
            DEBUG("直线偏移大于30厘米 offset = %f", offset);
        if (offset1 > MAX_OFFSET_DISTANCE) {
            DEBUG("虚拟直线偏移大于30厘米 offset1 = %f", offset1);
//            // 偏移大于30厘米，不合格
//            AddExamFault(30, rtkTime);
//            return false;
        }

        PointF px = CalcProjectionWithRoadEdge(RoadMap.roads[roadIndex].rightEdge, car->carXY[car->axial[AXIAL_FRONT]]);
        offset2 = DistanceOf(px, car->carXY[car->axial[AXIAL_FRONT]]);

        if (fabs(offset2 - offsetBase) > MAX_OFFSET_DISTANCE) {
            DEBUG("直线偏移大于30厘米 offset2 = %f", fabs(offset2 - offsetBase));
            // 偏移大于30厘米，不合格
            AddExamFault(30, rtkTime);
            return false;
@@ -69,7 +89,8 @@
    }

    if (setup == 2 && ReadOdo() - beginOdo > CHECK_STAGE_DISTANCE) {
        DEBUG("直线行驶结束");
        DEBUG("直线行驶结束 offset1 = %f offset2 = %f", offset1, fabs(offset2 - offsetBase));

        PlayTTS("直线行驶结束", NULL);
        return false;
    }