坐标

yy1717

2020-08-24 fb7b0660a319a9c54ff35c3944548348fae11b60

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

@@ -45,17 +45,22 @@
            yaw += car->yaw;
        }
        yaw_stat++;

        DEBUG("角度矫正 car %f yaw %f", car->yaw, yaw);

        if (yaw_stat == 5) {
            yaw = fmod(yaw, 360) / 5;

            DEBUG("创建基线 yaw %f", yaw);
            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厘米");
        double offset = DistanceOf(car->carXY[car->axial[AXIAL_FRONT]], baseLine);

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