9轴imu的yaw进阶c语言代码

以下是一个使用9轴IMU（加速度计、陀螺仪、磁力计）计算Yaw角度的进阶C语言代码：

#include <Wire.h>
#include <math.h>

#define MPU9250_ADDRESS 0x68 // MPU9250地址
#define AK8963_ADDRESS 0x0C // AK8963地址

#define GYRO_FULL_SCALE 250 // 陀螺仪全刻度范围
#define ACC_FULL_SCALE 2 // 加速度计全刻度范围
#define MAG_FULL_SCALE 4912 // 磁力计全刻度范围

int16_t AcX, AcY, AcZ, Tmp, GyX, GyY, GyZ;
int16_t MagX, MagY, MagZ;
float AccX, AccY, AccZ, GyroX, GyroY, GyroZ;
float AccAngleX, AccAngleY, YawAngle, MagAngle;
float GyroAngleX, GyroAngleY, GyroAngleZ;

float Alpha = 0.96; // 滤波系数
float dt = 0.005; // 采样时间
float Yaw = 0.0; // Yaw角度

void setup() {
  Serial.begin(9600);
  Wire.begin();

  // 初始化MPU9250
  Wire.beginTransmission(MPU9250_ADDRESS);
  Wire.write(0x6B); // PWR_MGMT_1寄存器
  Wire.write(0); // 唤醒MPU9250
  Wire.endTransmission(true);

  // 初始化AK8963
  Wire.beginTransmission(AK8963_ADDRESS);
  Wire.write(0x0A); // CNTL寄存器
  Wire.write(0x16); // 连续测量模式2和16位输出
  Wire.endTransmission(true);
}

void loop() {
  // 读取加速度计、陀螺仪和磁力计数据
  Wire.beginTransmission(MPU9250_ADDRESS);
  Wire.write(0x3B); // ACCEL_XOUT_H寄存器
  Wire.endTransmission(false);
  Wire.requestFrom(MPU9250_ADDRESS, 14, true);
  AcX = Wire.read() << 8 | Wire.read();
  AcY = Wire.read() << 8 | Wire.read();
  AcZ = Wire.read() << 8 | Wire.read();
  Tmp = Wire.read() << 8 | Wire.read();
  GyX = Wire.read() << 8 | Wire.read();
  GyY = Wire.read() << 8 | Wire.read();
  GyZ = Wire.read() << 8 | Wire.read();
  
  Wire.beginTransmission(AK8963_ADDRESS);
  Wire.write(0x03); // HXL寄存器
  Wire.endTransmission(false);
  Wire.requestFrom(AK8963_ADDRESS, 7, true);
  MagX = Wire.read() | Wire.read() << 8;
  MagY = Wire.read() | Wire.read() << 8;
  MagZ = Wire.read() | Wire.read() << 8;

  // 加速度计计算角度
  AccAngleX = atan(AcY / sqrt(pow(AcX, 2) + pow(AcZ, 2))) * 180 / PI;
  AccAngleY = atan(-1 * AcX / sqrt(pow(AcY, 2) + pow(AcZ, 2))) * 180 / PI;

  // 低通滤波
  AccX = Alpha * AccX + (1 - Alpha) * AccAngleX;
  AccY = Alpha * AccY + (1 - Alpha) * AccAngleY;

  // 计算Yaw角度
  YawAngle = atan2(AccX, AccY) * 180 / PI;

  // 陀螺仪计算角度
  GyroAngleX = GyroAngleX + GyX * dt;
  GyroAngleY = GyroAngleY + GyY * dt;
  GyroAngleZ = GyroAngleZ + GyZ * dt;

  // 计算Yaw角速度
  GyroZ = GyZ - 1.5; // 陀螺仪零偏校准
  Yaw = Yaw + GyroZ * dt;

  // 磁力计校准
  float MagBiasX = -43.5, MagBiasY = -19.3, MagBiasZ = -4.2;
  MagX = MagX - MagBiasX;
  MagY = MagY - MagBiasY;
  MagZ = MagZ - MagBiasZ;

  // 磁力计计算方向
  MagAngle = atan2(MagY, MagX) * 180 / PI;
  if (MagAngle < 0) {
    MagAngle += 360;
  }

  // 融合Yaw角度
  Yaw = 0.98 * (Yaw + GyroZ * dt) + 0.02 * MagAngle;

  // 输出Yaw角度
  Serial.print("Yaw: ");
  Serial.println(Yaw - YawAngle);

  delay(5);
}

代码中使用了加速度计、陀螺仪和磁力计的数据来计算Yaw角度，并使用低通滤波、零偏校准和磁力计校准来提高精度。其中Yaw角度的计算使用了加速度计、陀螺仪和磁力计的数据，通过将加速度计计算的角度、陀螺仪计算的角度和磁力计计算的方向信息进行融合来计算Yaw角度。代码中还加入了低通滤波、零偏校准和磁力计校准，这些都是为了提高IMU的精度和稳定性。