用c语言写imu963ra地磁针融合偏航角代码

以下是一个使用C语言编写的IMU963RA地磁针融合偏航角代码示例：

#include <stdio.h>
#include <math.h>

#define PI 3.14159265

// 地磁场参考值
#define MAG_REF_X 25.0
#define MAG_REF_Y 0.0
#define MAG_REF_Z 0.0

// 地磁场偏差校准值
#define MAG_OFFSET_X -1.5
#define MAG_OFFSET_Y 0.0
#define MAG_OFFSET_Z 1.5

// 偏航角计算参考值
#define REFERENCE_ANGLE 90.0

// 角度转弧度
double to_radian(double degree) {
    return degree * PI / 180.0;
}

// 弧度转角度
double to_degree(double radian) {
    return radian * 180.0 / PI;
}

int main() {
    // 读取IMU数据
    double acc_x = 0.0, acc_y = 0.0, acc_z = 0.0;
    double gyro_x = 0.0, gyro_y = 0.0, gyro_z = 0.0;
    double mag_x = 0.0, mag_y = 0.0, mag_z = 0.0;
    // TODO: 从IMU读取数据

    // 加速度计和磁力计融合计算偏航角
    double roll = atan2(acc_y, acc_z);
    double pitch = atan2(-acc_x, sqrt(acc_y * acc_y + acc_z * acc_z));
    double mag_yaw = atan2(mag_y - MAG_OFFSET_Y, mag_x - MAG_OFFSET_X);
    double yaw = to_degree(mag_yaw) - REFERENCE_ANGLE;

    // 陀螺仪积分计算偏航角
    double dt = 1.0 / 100.0; // 假设采样周期为10ms
    double gyro_yaw = gyro_z * dt;
    yaw += gyro_yaw;

    // 输出偏航角
    printf("Yaw: %.2f\n", yaw);

    return 0;
}

请注意，此示例代码仅供参考。实际实现中还需要根据具体的硬件和传感器数据进行调整和优化。