4、磁力计与气压计：HMC5883L磁力计校准、MS5611气压计读取、融合高度数据
飞控系统里，有两个传感器经常被低估——磁力计和气压计。说实话，我刚开始做飞控那会儿，觉得GPS够用了，磁力计就是锦上添花。直到有一次，飞机在悬停时莫名其妙地转圈……嗯，从那以后，我再也不敢轻视这两个小家伙了。
这一章，我们聊聊HMC5883L磁力计的校准、MS5611气压计的读取，以及怎么把高度数据融合起来。你想想看，没有磁力计，你的航向角会漂；没有气压计，你的高度控制就是个笑话。
4.1 HMC5883L磁力计：原理与校准
磁力计，说白了就是电子罗盘。它测量地球磁场，告诉你飞机头朝哪。但问题是，环境中的铁磁物质会干扰它——电机、电池、甚至PCB上的走线都会引入偏差。
我个人习惯，每次装机后第一件事就是校准磁力计。不校准？那你飞出去的飞机，可能自己都不知道在往哪飞。
4.1.1 硬件连接与初始化
HMC5883L通过I2C接口通信，地址是0x1E（7位）。接线很简单：
// HMC5883L 接线
// VCC -> 3.3V
// GND -> GND
// SCL -> PB6 (I2C1_SCL)
// SDA -> PB7 (I2C1_SDA)
// DRDY -> 可选，用于数据就绪中断初始化代码，我习惯这样写：
#define HMC5883L_ADDR 0x1E
void HMC5883L_Init(void) {
uint8_t data;
// 配置寄存器A：采样平均8次，输出速率15Hz
data = 0x70; // 0b01110000
I2C_WriteRegister(HMC5883L_ADDR, 0x00, data);
// 配置寄存器B：增益，±1.3Ga范围
data = 0x20; // 0b00100000
I2C_WriteRegister(HMC5883L_ADDR, 0x01, data);
// 模式寄存器：连续测量模式
data = 0x00;
I2C_WriteRegister(HMC5883L_ADDR, 0x02, data);
printf("HMC5883L 初始化完成\r\n");
}4.1.2 读取原始数据
读取数据时，注意要连续读取6个字节——X、Y、Z各两个字节。我见过有人一个一个读，结果数据不同步，航向角乱跳。
typedef struct {
int16_t x;
int16_t y;
int16_t z;
} MagData;
MagData HMC5883L_Read(void) {
uint8_t buf[6];
MagData mag;
// 从寄存器0x03开始连续读取6字节
I2C_ReadRegisters(HMC5883L_ADDR, 0x03, buf, 6);
// 组合数据，注意是大端格式
mag.x = (int16_t)((buf[0] << 8) | buf[1]);
mag.z = (int16_t)((buf[2] << 8) | buf[3]); // 注意：Z在中间
mag.y = (int16_t)((buf[4] << 8) | buf[5]);
return mag;
}4.1.3 硬铁与软铁校准
磁力计校准，核心就是消除两种误差：
- 硬铁误差： 由永磁体引起，表现为固定偏移。比如电机磁铁、扬声器。
- 软铁误差： 由铁磁材料引起，表现为比例缩放和旋转。比如PCB上的铁氧体、结构件。
最简单的校准方法——旋转法。把飞机在空中转几圈，采集所有方向的数据。理想情况下，数据点应该落在一个球面上。但实际上，它是一个偏移的椭球。
我曾经在项目里遇到过，校准后航向角还是不准。后来发现是电池没固定好，每次飞行时位置会轻微移动。嗯，从那以后，我每次装机都会把电池牢牢固定。
校准步骤：
- 让飞机绕X轴旋转360度
- 绕Y轴旋转360度
- 绕Z轴旋转360度
- 采集至少200个数据点
// 简单的硬铁校准：计算偏移量
void MagCalibrate(float *offset_x, float *offset_y, float *offset_z) {
MagData samples[200];
float max_x = -9999, min_x = 9999;
float max_y = -9999, min_y = 9999;
float max_z = -9999, min_z = 9999;
// 采集数据（略）
// 计算偏移
*offset_x = (max_x + min_x) / 2.0f;
*offset_y = (max_y + min_y) / 2.0f;
*offset_z = (max_z + min_z) / 2.0f;
printf("校准偏移: X=%.2f, Y=%.2f, Z=%.2f\r\n",
*offset_x, *offset_y, *offset_z);
}4.2 MS5611气压计：高精度高度测量
MS5611是气压计里的明星产品。分辨率10cm，更新频率最高可达120Hz。说实话，我对比过BMP280和MS5611，后者在温度稳定性和噪声方面确实更胜一筹。
4.2.1 硬件连接与初始化
MS5611也是I2C接口，地址是0x76（CSB接GND）或0x77（CSB接VCC）。我个人习惯用0x76。
#define MS5611_ADDR 0x76
void MS5611_Init(void) {
// 复位
I2C_WriteByte(MS5611_ADDR, 0x1E);
delay_ms(10);
// 读取校准系数（PROM中有6个系数）
uint16_t C1, C2, C3, C4, C5, C6;
C1 = I2C_ReadRegister16(MS5611_ADDR, 0xA2); // 压力灵敏度
C2 = I2C_ReadRegister16(MS5611_ADDR, 0xA4); // 压力偏移
C3 = I2C_ReadRegister16(MS5611_ADDR, 0xA6); // 温度系数压力灵敏度
C4 = I2C_ReadRegister16(MS5611_ADDR, 0xA8); // 温度系数压力偏移
C5 = I2C_ReadRegister16(MS5611_ADDR, 0xAA); // 参考温度
C6 = I2C_ReadRegister16(MS5611_ADDR, 0xAC); // 温度系数
printf("MS5611 校准系数读取完成\r\n");
}4.2.2 读取气压与温度
读取流程分两步：先启动转换，再读取结果。转换时间取决于精度设置，我一般用OSR=4096，精度最高，但需要等8ms。
float MS5611_ReadPressure(void) {
uint32_t D1, D2;
int32_t dT, TEMP;
int64_t OFF, SENS, P;
// 启动压力转换（OSR=4096）
I2C_WriteByte(MS5611_ADDR, 0x48);
delay_ms(10);
// 读取压力值
D1 = I2C_ReadRegister24(MS5611_ADDR, 0x00);
// 启动温度转换
I2C_WriteByte(MS5611_ADDR, 0x58);
delay_ms(10);
// 读取温度值
D2 = I2C_ReadRegister24(MS5611_ADDR, 0x00);
// 计算实际温度（单位：0.01摄氏度）
dT = D2 - (int32_t)C5 * 256;
TEMP = 2000 + ((int64_t)dT * C6) / 8388608;
// 计算实际压力（单位：Pa）
OFF = (int64_t)C2 * 65536 + ((int64_t)C4 * dT) / 128;
SENS = (int64_t)C1 * 32768 + ((int64_t)C3 * dT) / 256;
P = (D1 * SENS / 2097152 - OFF) / 32768;
return (float)P / 100.0f; // 返回 hPa
}为什么这么复杂？因为MS5611的原始数据需要经过温度补偿才能得到准确的气压值。你想想看，温度变化1度，气压读数可能漂移0.1hPa，对应高度变化约1米。不补偿？那你的高度控制就没法用了。
4.2.3 气压转高度
有了气压值，怎么转成高度？用国际气压公式：
float PressureToAltitude(float pressure, float sea_level_pressure) {
// 标准大气压公式
// h = 44330 * (1 - (P/P0)^(1/5.255))
float ratio = pressure / sea_level_pressure;
float altitude = 44330.0f * (1.0f - powf(ratio, 0.190295f));
return altitude;
}这里有个坑：海平面气压P0不是固定的！它会随天气变化。我建议每次起飞前，用GPS高度或已知高度来校准P0。
4.3 融合高度数据：互补滤波
单独用气压计有个问题——它噪声大，而且响应慢。单独用加速度计积分高度？那漂移更厉害。所以，我们需要融合。
我个人最喜欢用互补滤波。它简单、计算量小、效果也不错。说白了，就是相信气压计的低频部分，相信加速度计的高频部分。
4.3.1 互补滤波器设计
typedef struct {
float altitude; // 融合后的高度
float velocity; // 融合后的速度
float dt; // 采样时间
float tau; // 时间常数（通常0.5-2秒）
} AltitudeFusion;
void AltitudeFusion_Init(AltitudeFusion *fus, float dt, float tau) {
fus->altitude = 0.0f;
fus->velocity = 0.0f;
fus->dt = dt;
fus->tau = tau;
}
void AltitudeFusion_Update(AltitudeFusion *fus,
float baro_alt, float accel_z) {
// 互补滤波公式
// 高度 = 低通(气压高度) + 高通(加速度积分)
float alpha = fus->dt / (fus->tau + fus->dt);
// 用加速度更新速度
fus->velocity += accel_z * fus->dt;
// 融合高度
fus->altitude = (1.0f - alpha) * (fus->altitude + fus->velocity * fus->dt)
+ alpha * baro_alt;
// 修正速度（用气压高度差）
float vel_error = (baro_alt - fus->altitude) / fus->tau;
fus->velocity += vel_error * fus->dt;
}这段代码的核心思想：
- 气压计提供绝对高度，但噪声大——用低通滤波
- 加速度计提供短时变化，但会漂移——用高通滤波
- 两者互补，得到平滑且准确的高度
4.3.2 实际应用中的注意事项
说几个我在项目中遇到的坑：
- 气压计受风影响： 飞机高速飞行时，气压计读数会偏低。我建议加一个动压补偿，或者只在悬停时用气压计。
- 温度突变： 从室内拿到室外，温度变化会导致气压计读数漂移。等几分钟稳定后再起飞。
- 磁力计干扰： 电机大电流时，磁场会变化。我建议在电机空闲时校准，或者用软件滤波。
4.4 本章小结
这一章我们聊了：
- HMC5883L磁力计的校准方法——硬铁和软铁校准
- MS5611气压计的读取和温度补偿
- 互补滤波融合高度数据
说实话，传感器融合是飞控的核心。没有好的数据，再好的控制算法也是白搭。我建议你动手试试，把代码烧进去，看看原始数据和融合后的数据有什么区别。你会发现，融合后的数据平滑多了。
下一章，我们会把这些传感器数据用起来——做姿态解算。到时候，你会看到磁力计和加速度计怎么配合，得到稳定的姿态角。
嗯，今天就到这里。有问题随时找我。