通过pygame 来编写一个能够展示 yaw, pitch ,roll 效果的程序

1️⃣ 环境准备（树莓派 or PC 均可）

# 安装依赖
pip install pygame pyserial numpy

2️⃣ ESP32 端（已跑通的 DMP 代码）

在 loop() 里把打印改成 固定格式 方便 Python 解析：

// 替换原来的 3 行 Serial.print
Serial.printf("Y%.2fP%.2fR%.2f\n",   // 以 Y 开头，固定大写
              ypr[0] * 180 / M_PI,
              ypr[1] * 180 / M_PI,
              ypr[2] * 180 / M_PI);
  • 波特率 115200，每秒输出约 50 帧即可（DMP 默认 100 Hz，可加 delay(20) 降速）。

3️⃣ Python 端（Pygame 读取 + 3D 展示）

保存为 imu3d.py：

#!/usr/bin/env python3
import sys, serial, re, threading
import pygame as pg
import numpy as np

# ---------------- 参数 -----------------
SERIAL_PORT = '/dev/ttyUSB0'   # 改成你的串口
BAUD        = 115200
WINDOW      = (800, 600)
FPS         = 60
# -------------------------------------

# 全局变量
yaw, pitch, roll = 0, 0, 0
lock = threading.Lock()

def serial_thread():
    global yaw, pitch, roll
    ser = serial.Serial(SERIAL_PORT, BAUD, timeout=1)
    while True:
        try:
            line = ser.readline().decode(errors='ignore').strip()
            # 匹配 Yxx.xxPxx.xxRxx.xx
            m = re.search(r'Y([-+]?\d*\.\d+)P([-+]?\d*\.\d+)R([-+]?\d*\.\d+)', line)
            if m:
                with lock:
                    yaw, pitch, roll = map(float, m.groups())
        except Exception as e:
            print(e)

# 把欧拉角→旋转矩阵（ZYX顺序）
def euler_to_rot(y, p, r):
    cy, sy = np.cos(np.radians(y)), np.sin(np.radians(y))
    cp, sp = np.cos(np.radians(p)), np.sin(np.radians(p))
    cr, sr = np.cos(np.radians(r)), np.sin(np.radians(r))
    Rz = np.array([[cy,-sy,0],[sy,cy,0],[0,0,1]])
    Ry = np.array([[cp,0,sp],[0,1,0],[-sp,0,cp]])
    Rx = np.array([[1,0,0],[0,cr,-sr],[0,sr,cr]])
    return Rz @ Ry @ Rx

# 绘制立方体
def draw_cube(screen, rot):
    # 1. 立方体顶点（边长 = 1）
    v = np.array([[-1, -1, -1],
                  [ 1, -1, -1],
                  [ 1,  1, -1],
                  [-1,  1, -1],
                  [-1, -1,  1],
                  [ 1, -1,  1],
                  [ 1,  1,  1],
                  [-1,  1,  1]], dtype=float)

    # 2. 放大 + 旋转
    v *= 120                       # 尺寸
    v = v @ rot.T                  # 旋转

    # 3. 透视投影：把摄像机放在 z=300，看向原点
    cam_z = 300
    v2d = v[:, :2] / (cam_z - v[:, 2, None])   # 透视除法
    v2d = v2d * 200 + np.array(WINDOW) / 2     # 放大并移到屏幕中心

    # 4. 12 条边
    edges = [(0,1),(1,2),(2,3),(3,0),
             (4,5),(5,6),(6,7),(7,4),
             (0,4),(1,5),(2,6),(3,7)]
    for a, b in edges:
        pg.draw.line(screen, (0, 255, 0), v2d[a], v2d[b], 3)


def main():
    pg.init()
    screen = pg.display.set_mode(WINDOW)
    pg.display.set_caption("ESP32 MPU6050 3D")
    clock = pg.time.Clock()
    threading.Thread(target=serial_thread, daemon=True).start()

    while True:
        for e in pg.event.get():
            if e.type == pg.QUIT: return
        with lock:
            rot = euler_to_rot(yaw, pitch, roll)
            print(f'{yaw=:.1f} {pitch=:.1f} {roll=:.1f}')
        screen.fill((0,0,0))
        draw_cube(screen, rot)
        pg.display.flip()
        clock.tick(FPS)

if __name__ == '__main__':
    main()

4️⃣ 运行

python3 imu3d.py
  • 窗口出现黑色背景的绿色立方体；
  • 转动 ESP32，立方体会实时跟着 yaw / pitch / roll 旋转；
  • 串口数据格式不对时终端会打印异常，方便调试。

5️⃣ 可玩扩展

目标 快速做法
背景网格/坐标轴 在 draw_cube 里加 draw_axis() 画三段彩色线
双缓冲抗锯齿 pygame.transform.smoothscale 或 OpenGL
记录/回放 把 ypr 写 CSV，再读文件模拟串口
手机可视化 用 UDP 把 ypr 发到手机 → Three.js 网页

视频效果：