MPU6050角度

#include <Wire.h>#include <MPU6050.h>MPU6050 mpu;float gyroAngleX, gyroAngleY, gyroAngleZ;float accAngleX, accAngleY;float pitch, roll;unsigned long timer;void setup() {  Serial.begin(115200);  Wire.begin();  mpu.initialize();  timer = millis();}void loop() {  mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);  // 加速度计角度  accAngleX = atan2(ay, az) * 180 / PI;  accAngleY = atan2(-ax, sqrt(ay * ay + az * az)) * 180 / PI;  // 计算陀螺仪角度增量  float dt = (millis() - timer) / 1000.0;  timer = millis();    gyroAngleX += gx * dt / 131.0; // 陀螺仪灵敏度调整（假设±250度/秒）  gyroAngleY += gy * dt / 131.0;  // 互补滤波  pitch = 0.98 * (pitch + gyroAngleX * dt) + 0.02 * accAngleX;  roll = 0.98 * (roll + gyroAngleY * dt) + 0.02 * accAngleY;  Serial.print("Pitch: "); Serial.print(pitch);  Serial.print(" Roll: "); Serial.println(roll);  delay(10);}php859 Bytes© 菜鸟-创作你的创作

import smbus2import timeimport math# MPU6050寄存器地址MPU6050_ADDR = 0x68PWR_MGMT_1 = 0x6BACCEL_XOUT_H = 0x3BGYRO_XOUT_H = 0x43bus = smbus2.SMBus(1)  # 树莓派I2C bus编号通常是1# 初始化MPU6050bus.write_byte_data(MPU6050_ADDR, 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    if val >= 0x8000:        val = -((65535 - val) + 1)    return valdef get_accel_data():    ax = read_word(ACCEL_XOUT_H)    ay = read_word(ACCEL_XOUT_H + 2)    az = read_word(ACCEL_XOUT_H + 4)    return ax, ay, azdef get_gyro_data():    gx = read_word(GYRO_XOUT_H)    gy = read_word(GYRO_XOUT_H + 2)    gz = read_word(GYRO_XOUT_H + 4)    return gx, gy, gz# 互补滤波参数alpha = 0.98dt = 0.01  # 10ms采样间隔pitch = 0.0roll = 0.0try:    while True:        ax, ay, az = get_accel_data()        gx, gy, gz = get_gyro_data()        # 加速度计转为g值        ax /= 16384.0        ay /= 16384.0        az /= 16384.0        # 陀螺仪转为角速度 (deg/s)，灵敏度为±250度/s，16.4 LSB/deg/s        gx /= 131.0        gy /= 131.0        gz /= 131.0        # 计算加速度计角度 (单位：度)        acc_pitch = math.degrees(math.atan2(ay, math.sqrt(ax*ax + az*az)))        acc_roll = math.degrees(math.atan2(-ax, az))        # 互补滤波计算角度        pitch = alpha * (pitch + gx * dt) + (1 - alpha) * acc_pitch        roll = alpha * (roll + gy * dt) + (1 - alpha) * acc_roll        print(f"Pitch: {pitch:.2f}  Roll: {roll:.2f}")        time.sleep(dt)except KeyboardInterrupt:    print("结束")php1.52 KB© 菜鸟-创作你的创作