坐标

yy1717

2020-08-25 0701276b4fec856d5427e4776eec3cc7c56ec065

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

@@ -39,17 +39,18 @@

    if (setup == 1) {
        // 偏航角平均值
        static vector<double> yaws;

        if (yaw_stat == 0) {
            yaw = car->yaw;
        } else {
            yaw += car->yaw;
            yaws.clear();
        }
        yaws.push_back(car->yaw);
        yaw_stat++;

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

        if (yaw_stat == 5) {
            yaw = fmod(yaw, 360) / 5;
            yaw = AvgYaw(yaws);
            vector<double>().swap(yaws);

            DEBUG("创建基线 yaw %f", yaw);
            PointF extPoint = PointExtend(car->basePoint, 100, yaw);
            MakeLine(&baseLine, &car->basePoint, &extPoint);