写一段读取mpu6050数据转换成角度的代码

下面是一段读取mpu6050数据并转换为角度的Python代码：

import smbus
import math

# MPU6050 Registers and Base Address
MPU6050_REG_PWR_MGMT_1 = 0x6B
MPU6050_REG_ACCEL_X = 0x3B
MPU6050_REG_GYRO_X = 0x43
MPU6050_ADDR = 0x68

# Initialize I2C bus and MPU6050
bus = smbus.SMBus(1)
bus.write_byte_data(MPU6050_ADDR, MPU6050_REG_PWR_MGMT_1, 0)

def read_word(reg):
    high = bus.read_byte_data(MPU6050_ADDR, reg)
    low = bus.read_byte_data(MPU6050_ADDR, reg + 1)
    val = (high << 8) + low
    return val

def read_word_2c(reg):
    val = read_word(reg)
    if val >= 0x8000:
        return -((65535 - val) + 1)
    else:
        return val

def get_gyro():
    x = read_word_2c(MPU6050_REG_GYRO_X)
    y = read_word_2c(MPU6050_REG_GYRO_X + 2)
    z = read_word_2c(MPU6050_REG_GYRO_X + 4)
    return (x, y, z)

def get_accel():
    x = read_word_2c(MPU6050_REG_ACCEL_X)
    y = read_word_2c(MPU6050_REG_ACCEL_X + 2)
    z = read_word_2c(MPU6050_REG_ACCEL_X + 4)
    return (x, y, z)

def get_pitch_roll():
    accel_x, accel_y, accel_z = get_accel()

    pitch = math.atan2(accel_y, math.sqrt(accel_x**2 + accel_z**2))
    roll = math.atan2(-accel_x, math.sqrt(accel_y**2 + accel_z**2))

    pitch = math.degrees(pitch)
    roll = math.degrees(roll)

    return (pitch, roll)

# Example usage – prints pitch and roll values every second
while True:
    pitch, roll = get_pitch_roll()
    print("Pitch: {:.2f}, Roll: {:.2f}".format(pitch, roll))
    time.sleep(1)

这段代码首先使用SMBus模块初始化I2C总线和MPU6050，并配置MPU6050的电源管理寄存器以保证读取正常。接着定义了读取16位数值数据的函数read_word(reg)和read_word_2c(reg)。

在get_gyro()和get_accel()函数中，分别读取MPU6050的陀螺仪和加速度计数据，并通过read_word_2c()将读取到的数值转换成有符号整型数值。

最后，get_pitch_roll()函数计算出pitch（俯仰角）和roll（横滚角）的值，并将两个值转换成角度。整个代码的最后一行则使用该函数连续打印出pitch和roll的值。