姿态角度yaw 值限制在 0~2π 的范围内,yaw 数据限制在 -π ~ π 之间的范围内,yaw 数据是nan就f赋值为0.0

姿态角度yaw 值限制在 0~2π 的范围内,yaw 数据限制在 -π ~ π 之间的范围内,yaw 数据是nan就f赋值为0.0

1.yaw 值限制在 0~2π 的范围内

 //Yaw值限制在 0~2π 的范围内
    double normalizeYaw(double yaw) {
        while (yaw < 0.0) {
            yaw += 2 * M_PI;
        }
        while (yaw >= 2 * M_PI) {
            yaw -= 2 * M_PI;
        }
        return yaw;
    }

2.yaw 数据限制在 -π ~ π 之间的范围内

    // yaw 数据限制在 -π ~ π 之间的范围内
    double yawNormalize(double yaw) {
        while (yaw > M_PI) {
            yaw -= 2 * M_PI;
        }
        while (yaw < -M_PI) {
            yaw += 2 * M_PI;
        }
        return yaw;
    }

 3.yaw 数据是nan就f赋值为0.0