python代码解算mpu6050姿态角

以下是Python代码，使用MPU6050传感器计算姿态角：

import math
import time
from MPU6050 import MPU6050

mpu = MPU6050()

# 初始化MPU6050
mpu.dmp_initialize()

# 设置采样频率
mpu.set_rate(5)

# 设置滤波器截止频率
mpu.set_lpf(42)

# 获取加速度计和陀螺仪偏移量
accel_offset = mpu.get_accel_offset()
gyro_offset = mpu.get_gyro_offset()

# 计算重力加速度
gravity = math.sqrt(accel_offset[0] ** 2 + accel_offset[1] ** 2 + accel_offset[2] ** 2)

# 设置角速度单位
gyro_scale = 131

# 设置加速度单位
accel_scale = 16384

# 计算初始姿态角
pitch = math.atan2(accel_offset[0], math.sqrt(accel_offset[1] ** 2 + accel_offset[2] ** 2))
roll = math.atan2(accel_offset[1], math.sqrt(accel_offset[0] ** 2 + accel_offset[2] ** 2))
yaw = 0

# 计算姿态角的权重
alpha = 0.5

# 循环读取数据并计算姿态角
while True:
    # 获取加速度计和陀螺仪数据
    accel_data = mpu.get_accel_data()
    gyro_data = mpu.get_gyro_data()

    # 修正加速度计和陀螺仪偏移量
    accel_data[0] -= accel_offset[0]
    accel_data[1] -= accel_offset[1]
    accel_data[2] -= accel_offset[2]
    gyro_data[0] -= gyro_offset[0]
    gyro_data[1] -= gyro_offset[1]
    gyro_data[2] -= gyro_offset[2]

    # 计算加速度计和陀螺仪数据的角度变化
    accel_pitch = math.atan2(accel_data[0], math.sqrt(accel_data[1] ** 2 + accel_data[2] ** 2))
    accel_roll = math.atan2(accel_data[1], math.sqrt(accel_data[0] ** 2 + accel_data[2] ** 2))
    gyro_pitch = gyro_data[0] / gyro_scale
    gyro_roll = gyro_data[1] / gyro_scale
    gyro_yaw = gyro_data[2] / gyro_scale

    # 计算姿态角
    pitch = alpha * (pitch + gyro_pitch) + (1 - alpha) * accel_pitch
    roll = alpha * (roll + gyro_roll) + (1 - alpha) * accel_roll
    yaw += gyro_yaw * 0.01

    # 打印姿态角
    print("Pitch: %f, Roll: %f, Yaw: %f" % (pitch, roll, yaw))

    # 延时
    time.sleep(0.01)

请注意，此代码使用MPU6050.py模块，您需要下载此模块并将其与此代码放在同一目录中。此外，您还需要安装smbus和numpy模块。