MPU6050代码可运行但偏航角无动作递增，偏移量持续增长求助

问题现象 #

使用MPU6050通过陀螺仪计算偏航角时，预期初始角度为0，但角度持续从0递增（0→0.01→0.02……）。代码可正常运行，但偏移校准未生效，导致偏移量持续增长。

原代码 #

#include<avr/io.h>
float x, y, z, k, bam,angle;
////////////////////////////////
///////////////////////////////////////
float baf;
float j = 0.0, i = 0;
unsigned long current_time = 0;
unsigned long previous_time = 0;
unsigned long time_interval = 0;
float gyro_offset_z = 0.0;
void i2cinit(void)
{
  TWSR = 0x00;
  TWBR = 0x42;
  TWCR = (1 << TWEN);
}
void i2cstart(void)
{
  TWCR = (1 << TWINT) | (1 << TWSTA) | (1 << TWEN);
  while (!(TWCR & (1 << TWINT)));
}

void i2cwrite( unsigned char data)
{
  TWDR = data;
  TWCR = (1 << TWINT) | (1 << TWEN);
  while (!(TWCR & (1 << TWINT)));
}

unsigned int i2c_receive(unsigned char isLast) {
  if (isLast == 0)
  {
    TWCR |= (1 << TWINT) | (1 << TWEA) | (1 << TWEN);
  }
  else
  {
    TWCR = (1 << TWINT) | (1 << TWEN);
  }
  while (!(TWCR & (1 << TWINT)));
  return TWDR;
}

void i2cstop(void)
{
  TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWSTO);

}

void mpu6050_init(void) {
  i2cstart();
  i2cwrite(0xD0);
  i2cwrite(0x6B);
  i2cwrite(0);
  i2cstop();
}
void mpu6050f()
{
  i2cstart();
  i2cwrite(0xD0);
  i2cwrite(0x1B);
  i2cwrite(0x18);
  i2cstop();
}
unsigned char mpu6050rbyte(int gyro) {
  i2cstart();
  i2cwrite(0xD0);
  i2cwrite(gyro);
  i2cstart();
  i2cwrite(0xD0 | 0x01);
  unsigned char i = i2c_receive(1);
  i2cstop();
  return i;
}
int mpu6050_whole_data(unsigned char a, unsigned char b) {
  unsigned char msb = mpu6050rbyte(a);
  unsigned char lsb = mpu6050rbyte(b);
  int x_gyro = (msb << 8) | lsb;
  return x_gyro;
}

void calibrateGyroOffset() {
  const int numSamples = 1100;
  for (k = 0; k < numSamples; k++) {
    gyro_offset_z += mpu6050_whole_data(0x47, 0x48);
    if ( k < 100)
    {
      gyro_offset_z = 0;
    }
  }
  gyro_offset_z /= 1000;
}

float mpu6050angle(int woho, float gyro) {
  float angular_velocity = (woho - gyro) / 16.4;
  if (angular_velocity > 0.1 || angular_velocity < -0.1)
  {
    angle = (angular_velocity * time_interval) / 1000.0;
  }
  return angle;
}

问题分析与修复方案 #

1. 校准函数逻辑错误 #

原calibrateGyroOffset函数中，前100次循环强制清零gyro_offset_z，导致前100次采样完全无效，且传感器可能未进入稳定状态；同时缺少采样延时，连续I2C读取易导致数据异常。

修复后的校准函数：

#include <util/delay.h> // 需添加此头文件

void calibrateGyroOffset() {
  const int numSamples = 1000;
  gyro_offset_z = 0.0;
  
  // 等待传感器稳定
  for(int delayCnt = 0; delayCnt < 500; delayCnt++){
    _delay_ms(1);
  }
  
  for (int k = 0; k < numSamples; k++) {
    gyro_offset_z += mpu6050_whole_data(0x47, 0x48);
    _delay_ms(1); // 每次采样后延时，确保数据更新
  }
  gyro_offset_z /= numSamples;
}

2. 角度计算逻辑缺陷 #

原mpu6050angle函数在角速度绝对值小于0.1时，未重置angle值，导致残留偏移持续累积；且角度需通过累加增量计算，而非单次返回增量直接使用。

修复后的角度计算逻辑：

// 新增全局变量存储总偏航角
float yaw_angle = 0.0;

// 需自行实现millis()函数，或用AVR定时器中断获取毫秒级时间
unsigned long millis() {
  // 示例：基于定时器0的毫秒计数，需提前配置定时器
  extern unsigned long timer0_millis;
  return timer0_millis;
}

void updateYawAngle() {
  current_time = millis();
  time_interval = current_time - previous_time;
  previous_time = current_time;

  int gyro_z_raw = mpu6050_whole_data(0x47, 0x48);
  float angular_velocity = (gyro_z_raw - gyro_offset_z) / 16.4;

  // 死区过滤微小噪声
  if(fabs(angular_velocity) < 0.05){
    angular_velocity = 0.0;
  }

  // 累加角度增量
  yaw_angle += angular_velocity * time_interval / 1000.0;
}

3. I2C读取效率与同步问题 #

原mpu6050rbyte每次读取单字节都发起I2C起始/停止，读取陀螺仪MSB和LSB时可能获取不同时刻的数据，导致数值异常。改为一次性读取多字节：

优化后的多字节读取函数：

int mpu6050_read_gyro_z() {
  int gyro_z;
  i2cstart();
  i2cwrite(0xD0);
  i2cwrite(0x47); // 陀螺仪Z轴MSB寄存器地址
  i2cstart();
  i2cwrite(0xD0 | 0x01);
  // 先读MSB（发送ACK继续接收），再读LSB（发送NACK结束）
  unsigned char msb = i2c_receive(0);
  unsigned char lsb = i2c_receive(1);
  i2cstop();
  gyro_z = (msb << 8) | lsb;
  return gyro_z;
}

4. 其他注意事项 #

• 校准过程中需确保MPU6050完全静止，微小震动会导致偏移量计算错误。
• 若使用AVR单片机，需正确配置定时器以实现millis()函数，避免软件延时带来的时间误差。

内容的提问来源于stack exchange，提问作者ShadowKing