九轴解算航向角、俯仰角、滚转角输出f103主控，气压计bmp085，磁力计hmc5883l，dmp解算，卡尔曼滤波矩阵运算，多份代码

前阵子蹲在宿舍焊飞控的时候，突然发现之前抄的九轴解算代码总飘，哪怕把飞控放在桌子上不动，航向角也能自己转个360度，折腾了半天才搞明白是磁力计和气压计没对齐的锅。今天把折腾出来的东西整理一下，顺便把用到的代码片段掏出来唠唠。

首先得明确咱要干啥：用stm32f103当主控，攒一个带九轴姿态解算的小飞控，要输出俯仰角、滚转角、航向角，还要加个bmp085测高度。九轴指的是加速度计+陀螺仪+磁力计，这里我用的是mpu6050（accel+gyro）加hmc5883l磁力计，凑够九个轴。

一开始图省事，直接用了mpu6050自带的DMP解算，毕竟官方例程给得明明白白，复制粘贴就能跑。但问题也来了：DMP默认只用六轴数据，只靠加速度计和陀螺仪解算，陀螺仪本身有漂移，哪怕不动，时间长了角度也会飘。而且DMP输出的航向角完全没用到磁力计，只要转个身，过一会儿就歪到姥姥家。

先贴个当时没加磁力计时的DMP初始化代码：

// 简化的MPU6050 DMP初始化，只开了六轴
mpu_device_init();
// 只配置陀螺仪和加速度计的FIFO
mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL);
mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL);
mpu_set_sample_rate(100);
// 加载DMP固件
dmp_load_motion_driver_firmware();
dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation));
// 只开四元数输出，没加磁力计融合
dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT);
dmp_set_fifo_rate(100);
mpu_set_dmp_state(1);

当时就是这么个代码，跑起来俯仰和滚转还行，但航向角飘得离谱，后来才知道要把hmc5883l的磁力计数据喂进去，让DMP做九轴融合，或者自己写卡尔曼滤波来融合三个传感器的数据。

接下来唠唠hmc5883l这个磁力计，当年我最蠢的坑就是直接按手册读x/y/z，结果航向角一直反的。后来才发现hmc5883l的输出寄存器顺序是x,z,y，不是x,y,z！贴个我后来改对的读取代码：

// 读取HMC5883L原始数据，注意输出顺序是x,z,y
uint8_t hmc5883l_read_data(int16_t *x, int16_t *z, int16_t *y) {
    uint8_t buf[6];
    // 0x03是数据输出寄存器地址，I2C_Read_Buf是自己写的通用I2C读函数
    I2C_Read_Buf(HMC5883L_ADDR, 0x03, buf, 6);
    *x = (buf[0] << 8) | buf[1];
    *z = (buf[2] << 8) | buf[3];
    *y = (buf[4] << 8) | buf[5];
    return 0;
}

而且还要先配置磁力计的量程和采样率，比如我一般配置成8高斯的量程，15Hz的采样率，不然读出来的数据要么饱和要么跳变太厉害。当年没配置的时候，读出来的x轴数据永远是正的，测出来的航向角全是错的。

九轴解算航向角、俯仰角、滚转角输出f103主控，气压计bmp085，磁力计hmc5883l，dmp解算，卡尔曼滤波矩阵运算，多份代码

然后是卡尔曼滤波，一开始我抄了网上的全量九轴卡尔曼代码，矩阵都是9x9的，编译完flash占了一大半，f103的64k flash差点不够用。后来改成了简化版的，把俯仰、滚转、航向角分开处理，每个轴用一个小的卡尔曼滤波器，反而跑起来更稳，还省空间。

贴个单轴的卡尔曼滤波代码，比如用来处理俯仰角的：

// 简化的单轴卡尔曼滤波器，融合陀螺仪和加速度计数据
typedef struct{
    float angle;  // 最终解算出来的角度
    float bias;   // 陀螺仪的零偏偏移
    float P[2][2]; // 协方差矩阵，用来估计误差
} KalmanFilter;

// 更新函数，dt是两次更新的时间间隔，单位秒
float Kalman_Update(KalmanFilter *kf, float gyro_rate, float acc_angle, float dt){
    // 第一步：预测，根据陀螺仪的角速度预测下一刻的角度
    float angle_pred = kf->angle + (gyro_rate - kf->bias) * dt;
    // 更新协方差矩阵的预测值
    float P00_pred = kf->P[0][0] + kf->P[0][1] * dt + kf->P[1][0] * dt + kf->P[1][1] * dt*dt + 0.01;
    float P01_pred = kf->P[0][1] + kf->P[1][1] * dt;
    float P10_pred = kf->P[1][0] + kf->P[1][1] * dt;
    float P11_pred = kf->P[1][1] + 0.001;

    // 第二步：更新，用加速度计的实际角度修正预测值
    float residual = acc_angle - angle_pred; // 预测和实际的残差
    float S = P00_pred + 0.1; // 观测噪声协方差
    float K0 = P00_pred / S;  // 卡尔曼增益
    float K1 = P01_pred / S;

    // 更新角度和陀螺仪偏置
    kf->angle = angle_pred + K0 * residual;
    kf->bias = kf->bias + K1 * residual;

    // 更新协方差矩阵
    kf->P[0][0] = P00_pred - K0 * P00_pred;
    kf->P[0][1] = P01_pred - K0 * P01_pred;
    kf->P[1][0] = P10_pred - K1 * P00_pred;
    kf->P[1][1] = P11_pred - K1 * P01_pred;

    return kf->angle;
}

这个代码里的0.01和0.1是调参调出来的，一开始我用的默认值，结果滤波要么太灵敏要么太迟钝，调了快一周才找到合适的数。而且这个滤波器只处理单轴，三个轴的话就初始化三个这样的结构体就行，比全量矩阵好写多了，f103跑起来也不卡。

接下来是bmp085气压计，一开始我以为这个只是用来测高度的，后来发现飞控里用来做高度锁的话，还要把气压转换成高度，而且要注意校准。比如每次开机的时候，要先测一下当前的大气压作为海平面基准，不然测出来的高度会飘。贴个简化的读取代码：

// BMP085读取温度和气压，省略了气压转高度的部分
uint8_t bmp085_read_data(float *temp, float *press){
    static uint8_t calibrated = 0;
    // 出厂校准参数，要先读出来存起来
    static int16_t ac1, ac2, ac3, b1, b2, mb, mc, md;
    static uint16_t ac4, ac5, ac6;
    if(!calibrated){
        ac1 = I2C_Read_16(BMP085_ADDR, 0xAA);
        ac2 = I2C_Read_16(BMP085_ADDR, 0xAC);
        ac3 = I2C_Read_16(BMP085_ADDR, 0xAE);
        ac4 = I2C_Read_16(BMP085_ADDR, 0xB0);
        ac5 = I2C_Read_16(BMP085_ADDR, 0xB2);
        ac6 = I2C_Read_16(BMP085_ADDR, 0xB4);
        b1 = I2C_Read_16(BMP085_ADDR, 0xB6);
        b2 = I2C_Read_16(BMP085_ADDR, 0xB8);
        mb = I2C_Read_16(BMP085_ADDR, 0xBA);
        mc = I2C_Read_16(BMP085_ADDR, 0xBC);
        md = I2C_Read_16(BMP085_ADDR, 0xBE);
        calibrated = 1;
    }
    // 读取温度
    I2C_Write_Byte(BMP085_ADDR, 0xF4, 0x2E);
    delay_ms(5); // 一定要等转换完成，不然读的是旧数据
    int32_t ut = (I2C_Read_16(BMP085_ADDR, 0xF6) << 8) | I2C_Read_16(BMP085_ADDR, 0xF7);
    // 计算温度，直接抄手册的公式就行
    int32_t x1 = (ut - ac6) * ac5 >> 15;
    int32_t x2 = (mc << 11) / (x1 + md);
    int32_t b5 = x1 + x2;
    *temp = (b5 + 8) >> 4;
    *temp /= 10.0f;
    // 气压读取省略了，和温度差不多，就是多几个步骤
    return 0;
}

当年我踩的坑是delay_ms(1)，结果读出来的温度永远是25度，后来改成5ms就好了，因为bmp085的温度转换需要至少4.5ms的时间。

最后把这些东西串起来，整个主循环大概是这样的：

int main(void){
    HAL_Init();
    SystemClock_Config();
    // 初始化所有传感器
    I2C_Init();
    mpu6050_init();
    hmc5883l_init();
    bmp085_init();
    // 初始化卡尔曼滤波器
    KalmanFilter pitch_filter = {0, 0, {{1,0},{0,1}}};
    KalmanFilter roll_filter = {0, 0, {{1,0},{0,1}}};
    float pitch, roll, yaw;
    uint32_t last_time = HAL_GetTick();
    while(1){
        // 计算两次更新的时间间隔
        uint32_t now_time = HAL_GetTick();
        float dt = (now_time - last_time) / 1000.0f;
        last_time = now_time;

        // 读取各个传感器的数据
        int16_t ax, ay, az, gx, gy, gz;
        mpu6050_read_accel_gyro(&ax, &ay, &az, &gx, &gy, &gz);
        int16_t mx, my, mz;
        hmc5883l_read_data(&mx, &mz, &my);
        float temp, press;
        bmp085_read_data(&temp, &press);

        // 加速度计转角度，俯仰和滚转
        pitch = atan2(ay, sqrt(ax*ax + az*az)) * 180 / PI;
        roll = atan2(-ax, az) * 180 / PI;
        // 磁力计转航向角，这里要注意坐标系转换，不然角度还是反的
        yaw = atan2(my, mx) * 180 / PI;
        if(yaw <0) yaw += 360;

        // 用卡尔曼滤波融合数据
        pitch = Kalman_Update(&pitch_filter, gx, pitch, dt);
        roll = Kalman_Update(&roll_filter, gy, roll, dt);
        // 航向角的卡尔曼滤波同理，省略了

        // 把数据打印到串口
        printf("Pitch: %.2f, Roll: %.2f, Yaw: %.2f, Temp: %.2f\r\n", pitch, roll, yaw, temp);
        HAL_Delay(10);
    }
}

现在这个代码跑起来就稳定多了，放在桌子上不动的话，三个角度都能稳定在初始值附近，不会飘了。当然还有很多坑没唠，比如磁力计的硬铁补偿和软铁补偿，当年我没做这个的时候，放在桌子边靠近电脑的地方，航向角就会飘，后来加了校准代码才好。还有stm32f103的I2C速度，一开始用100kHz没问题，后来加了多个传感器之后，改成400kHz就会出现读错数据的情况，又改回100kHz了。

总的来说，九轴解算看起来简单，其实坑挺多的，抄代码容易，但要搞懂每个部分的原理，才能调对参数。现在我的小飞控终于能稳定悬停了，虽然比起大疆的还差远了，但自己焊的板子跑起来的感觉真的不一样。