九轴解算航向角、俯仰角、滚转角输出f103主控气压计bmp085磁力计hmc5883ldmp解算卡尔曼滤波矩阵运算多份代码前阵子蹲在宿舍焊飞控的时候突然发现之前抄的九轴解算代码总飘哪怕把飞控放在桌子上不动航向角也能自己转个360度折腾了半天才搞明白是磁力计和气压计没对齐的锅。今天把折腾出来的东西整理一下顺便把用到的代码片段掏出来唠唠。首先得明确咱要干啥用stm32f103当主控攒一个带九轴姿态解算的小飞控要输出俯仰角、滚转角、航向角还要加个bmp085测高度。九轴指的是加速度计陀螺仪磁力计这里我用的是mpu6050accelgyro加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磁力计hmc5883ldmp解算卡尔曼滤波矩阵运算多份代码然后是卡尔曼滤波一开始我抄了网上的全量九轴卡尔曼代码矩阵都是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了。总的来说九轴解算看起来简单其实坑挺多的抄代码容易但要搞懂每个部分的原理才能调对参数。现在我的小飞控终于能稳定悬停了虽然比起大疆的还差远了但自己焊的板子跑起来的感觉真的不一样。