如何修改Python代码实现基于I2C磁力计的3D立方体三轴旋转

一、前置准备 #

• 开启树莓派I2C功能：运行sudo raspi-config，进入「Interfacing Options」启用I2C，重启生效
• 安装依赖库：执行pip install smbus2 numpy matplotlib

二、核心修改点 #

1. 新增磁力计数据读取线程，实时获取三轴角度
2. 保留立方体原始顶点数据，避免旋转累积误差
3. 组合X/Y/Z三轴旋转矩阵，实现基于传感器数据的全角度旋转
4. 线程安全更新角度变量，保证动画流畅性

三、完整修改代码 #

import numpy as np
import matplotlib.animation as animation
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
import time
import threading
from smbus2 import SMBus

# ---------------------- 磁力计配置 ----------------------
# HMC5883L磁力计I2C地址，其他型号如QMC5883L为0x0D
MAG_ADDR = 0x1E
# 初始化I2C总线（树莓派I2C总线编号通常为1）
bus = SMBus(1)

# 配置磁力计：设置采样率为15Hz，增益为1.3Ga
bus.write_byte_data(MAG_ADDR, 0x00, 0x70)  # 配置寄存器A：平均8次，采样率15Hz
bus.write_byte_data(MAG_ADDR, 0x01, 0x20)  # 配置寄存器B：增益1.3Ga
bus.write_byte_data(MAG_ADDR, 0x02, 0x00)  # 模式寄存器：连续测量模式

# ---------------------- 全局变量 ----------------------
# 立方体原始顶点（保留副本，避免每次旋转修改原数据）
original_points = np.array([
    [0, 0, 0], [1, 0, 0], [1, 1, 0], [0, 1, 0],
    [0, 0, 1], [1, 0, 1], [1, 1, 1], [0, 1, 1]
])
# 实时旋转角度（俯仰θx、横滚θy、航向θz，单位：度）
theta_x = 0.0
theta_y = 0.0
theta_z = 0.0

# ---------------------- 磁力计数据读取线程 ----------------------
def read_magnetometer():
    global theta_x, theta_y, theta_z
    while True:
        # 读取6字节数据：X轴(2字节)、Z轴(2字节)、Y轴(2字节)
        data = bus.read_i2c_block_data(MAG_ADDR, 0x03, 6)
        # 转换为带符号的16位整数
        x = (data[0] << 8) | data[1]
        z = (data[2] << 8) | data[3]
        y = (data[4] << 8) | data[5]
        # 处理负数（补码转换）
        if x > 32767:
            x -= 65536
        if y > 32767:
            y -= 65536
        if z > 32767:
            z -= 65536

        # 计算欧拉角（此处假设配合加速度计可获取俯仰/横滚，若仅磁力计则仅航向有效）
        # 航向角（绕Z轴）：范围0-360度
        theta_z = np.degrees(np.arctan2(y, x))
        if theta_z < 0:
            theta_z += 360
        # 俯仰角（绕X轴）和横滚角（绕Y轴）：需结合加速度计数据，此处用模拟值示例，实际需替换
        theta_x = np.degrees(np.arctan2(z, np.sqrt(x**2 + y**2)))
        theta_y = np.degrees(np.arctan2(-x, z))

        time.sleep(0.1)  # 10Hz读取频率，匹配动画刷新

# ---------------------- 3D动画绘制 ----------------------
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
plt.axis('off')
# 设置坐标轴范围，避免立方体旋转后超出视野
ax.set_xlim(-2, 2)
ax.set_ylim(-2, 2)
ax.set_zlim(-2, 2)

def animate(i):
    ax.clear()
    plt.axis('off')
    ax.set_xlim(-2, 2)
    ax.set_ylim(-2, 2)
    ax.set_zlim(-2, 2)

    # 将角度转换为弧度
    rx = np.deg2rad(theta_x)
    ry = np.deg2rad(theta_y)
    rz = np.deg2rad(theta_z)

    # 定义三轴旋转矩阵（旋转顺序：Z->Y->X，对应航向->俯仰->横滚）
    rot_z = np.array([
        [np.cos(rz), -np.sin(rz), 0],
        [np.sin(rz), np.cos(rz), 0],
        [0, 0, 1]
    ])
    rot_y = np.array([
        [np.cos(ry), 0, np.sin(ry)],
        [0, 1, 0],
        [-np.sin(ry), 0, np.cos(ry)]
    ])
    rot_x = np.array([
        [1, 0, 0],
        [0, np.cos(rx), -np.sin(rx)],
        [0, np.sin(rx), np.cos(rx)]
    ])

    # 组合旋转矩阵：先绕Z，再绕Y，最后绕X
    rotation_matrix = rot_x @ rot_y @ rot_z

    # 旋转所有顶点（基于原始顶点计算）
    rotated_points = original_points @ rotation_matrix.T

    # 提取旋转后的顶点
    p1, p2, p3, p4, p5, p6, p7, p8 = rotated_points

    # 绘制立方体的12条边
    edges = [
        (p1, p2), (p2, p3), (p3, p4), (p4, p1),
        (p5, p6), (p6, p7), (p7, p8), (p8, p5),
        (p1, p5), (p2, p6), (p3, p7), (p4, p8)
    ]
    for edge in edges:
        ax.plot([edge[0][0], edge[1][0]], [edge[0][1], edge[1][1]], [edge[0][2], edge[1][2]], 'k-')

# 启动磁力计读取线程
mag_thread = threading.Thread(target=read_magnetometer, daemon=True)
mag_thread.start()

# 启动动画
ani = animation.FuncAnimation(fig, animate, interval=100)  # 10Hz刷新，匹配传感器读取频率

plt.show()

四、关键细节说明 #

• 磁力计校准：如果旋转角度偏移明显，需先对磁力计进行校准：将传感器沿X/Y/Z轴各旋转一周，记录最大/最小值，计算偏移量后在读取数据时修正（比如x = x - x_offset）
• 欧拉角旋转顺序：代码中使用Z-Y-X顺序，若需要不同的旋转效果，可调整旋转矩阵的相乘顺序（比如rot_z @ rot_x @ rot_y）
• 加速度计配合：若需准确的俯仰/横滚角，需搭配加速度计（如MPU6050），通过传感器融合算法（如互补滤波）计算更精准的欧拉角，仅磁力计无法单独获取稳定的俯仰/横滚数据
• 线程守护：设置daemon=True保证主线程退出时，传感器线程自动终止

内容的提问来源于stack exchange，提问作者Muhammad Zeeshan Akram