四轴飞行器DIY：用STM32和MS5611气压计实现定高功能的代码拆解
四轴飞行器DIY：用STM32和MS5611气压计实现定高功能的代码拆解
在无人机和四轴飞行器的开发中，实现稳定的高度控制是一个关键挑战。气压计作为测量高度的核心传感器，其数据精度和稳定性直接影响飞行器的定高性能。本文将深入探讨如何利用STM32微控制器和MS5611气压计构建一个可靠的定高系统，从硬件连接到软件滤波，再到PID控制闭环的实现。
1. MS5611气压计的核心特性与硬件连接
MS5611是一款高精度数字气压传感器，具有以下突出特性：
- 测量范围：10-1200mbar（对应海拔-500m至9000m）
- 温度精度：±0.8°C
- 气压精度：±0.5mbar（对应约±0.5米高度误差）
- 转换时间：0.5-8.2ms（取决于转换精度设置）
硬件连接时需特别注意：
// STM32与MS5611的I2C连接示例 #define MS5611_ADDR 0xEE // CSB接高电平时的I2C地址 // 引脚配置 GPIO_InitTypeDef GPIO_InitStruct; I2C_HandleTypeDef hi2c1; void MS5611_GPIO_Init() { // I2C1_SCL -> PB6 // I2C1_SDA -> PB7 __HAL_RCC_GPIOB_CLK_ENABLE(); GPIO_InitStruct.Pin = GPIO_PIN_6|GPIO_PIN_7; GPIO_InitStruct.Mode = GPIO_MODE_AF_OD; GPIO_InitStruct.Pull = GPIO_PULLUP; GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH; GPIO_InitStruct.Alternate = GPIO_AF4_I2C1; HAL_GPIO_Init(GPIOB, &GPIO_InitStruct); // I2C1初始化 hi2c1.Instance = I2C1; hi2c1.Init.ClockSpeed = 400000; hi2c1.Init.DutyCycle = I2C_DUTYCYCLE_2; hi2c1.Init.OwnAddress1 = 0; hi2c1.Init.AddressingMode = I2C_ADDRESSINGMODE_7BIT; hi2c1.Init.DualAddressMode = I2C_DUALADDRESS_DISABLE; hi2c1.Init.OwnAddress2 = 0; hi2c1.Init.GeneralCallMode = I2C_GENERALCALL_DISABLE; hi2c1.Init.NoStretchMode = I2C_NOSTRETCH_DISABLE; HAL_I2C_Init(&hi2c1); }提示：在四轴飞行器上，建议将MS5611安装在远离电机和螺旋桨的位置，并使用软质硅胶减震垫来降低振动干扰。
2. 数据采集与温度补偿算法实现
MS5611的数据采集流程包括三个关键步骤：发送转换命令、等待转换完成、读取ADC结果。以下是完整的采集流程代码：
typedef struct { uint16_t C[6]; // 校准系数 uint32_t D[2]; // 原始数据(D1:气压, D2:温度) int32_t TEMP; // 补偿后温度(0.01°C) int32_t P; // 补偿后气压(0.01mbar) int64_t OFF; // 偏移量 int64_t SENS; // 灵敏度 } MS5611_HandleTypeDef; void MS5611_ReadData(MS5611_HandleTypeDef *hms5611) { // 读取气压数据(D1) HAL_I2C_Master_Transmit(&hi2c1, MS5611_ADDR, &MS5611_CMD_CONVERT_D1_4096, 1, 100); HAL_Delay(10); // 等待转换完成(4096精度需9ms) HAL_I2C_Master_Transmit(&hi2c1, MS5611_ADDR, &MS5611_CMD_ADC_READ, 1, 100); HAL_I2C_Master_Receive(&hi2c1, MS5611_ADDR, (uint8_t*)&hms5611->D[0], 3, 100); // 读取温度数据(D2) HAL_I2C_Master_Transmit(&hi2c1, MS5611_ADDR, &MS5611_CMD_CONVERT_D2_4096, 1, 100); HAL_Delay(10); HAL_I2C_Master_Transmit(&hi2c1, MS5611_ADDR, &MS5611_CMD_ADC_READ, 1, 100); HAL_I2C_Master_Receive(&hi2c1, MS5611_ADDR, (uint8_t*)&hms5611->D[1], 3, 100); }温度补偿算法的实现需要严格遵循芯片手册中的公式：
void MS5611_Calculate(MS5611_HandleTypeDef *hms5611) { int32_t dT = hms5611->D[1] - ((uint32_t)hms5611->C[4] << 8); int32_t TEMP = 2000 + ((int64_t)dT * hms5611->C[5] >> 23); int64_t OFF = ((int64_t)hms5611->C[1] << 16) + ((int64_t)hms5611->C[3] * dT >> 7); int64_t SENS = ((int64_t)hms5611->C[0] << 15) + ((int64_t)hms5611->C[2] * dT >> 8); // 低温补偿(TEMP < 20°C) if(TEMP < 2000) { int32_t T2 = ((int64_t)dT * dT) >> 31; int64_t OFF2 = 5 * ((int64_t)TEMP - 2000) * ((int64_t)TEMP - 2000) / 2; int64_t SENS2 = 5 * ((int64_t)TEMP - 2000) * ((int64_t)TEMP - 2000) / 4; TEMP -= T2; OFF -= OFF2; SENS -= SENS2; } hms5611->TEMP = TEMP; hms5611->P = (((hms5611->D[0] * SENS) >> 21) - OFF) >> 15; }3. 数据滤波与高度计算
原始气压数据存在噪声，需要采用适当的滤波算法。以下是滑动平均滤波和卡尔曼滤波的对比实现：
| 滤波方法 | 实现复杂度 | 内存需求 | 实时性 | 抗突发干扰能力 |
|---|---|---|---|---|
| 滑动平均 | 低 | 中 | 高 | 中 |
| 卡尔曼滤波 | 高 | 低 | 中 | 高 |
| 一阶低通滤波 | 最低 | 最低 | 最高 | 低 |
滑动平均滤波实现：
#define FILTER_WINDOW_SIZE 10 typedef struct { int32_t buffer[FILTER_WINDOW_SIZE]; uint8_t index; int32_t sum; } MovingAverageFilter; int32_t MovingAverage_Update(MovingAverageFilter *filter, int32_t newValue) { filter->sum -= filter->buffer[filter->index]; filter->sum += newValue; filter->buffer[filter->index] = newValue; filter->index = (filter->index + 1) % FILTER_WINDOW_SIZE; return filter->sum / FILTER_WINDOW_SIZE; }卡尔曼滤波简化实现：
typedef struct { float q; // 过程噪声协方差 float r; // 测量噪声协方差 float x; // 估计值 float p; // 估计误差协方差 float k; // 卡尔曼增益 } KalmanFilter; float Kalman_Update(KalmanFilter *kf, float measurement) { // 预测 kf->p = kf->p + kf->q; // 更新 kf->k = kf->p / (kf->p + kf->r); kf->x = kf->x + kf->k * (measurement - kf->x); kf->p = (1 - kf->k) * kf->p; return kf->x; }气压值转换为高度的公式（国际标准大气模型）：
// 海平面标准大气压(1013.25hPa) #define SEA_LEVEL_PRESSURE 101325.0f float PressureToAltitude(float pressure) { // 简化公式，适用于低空(<11km) return 44330.0f * (1.0f - powf(pressure / SEA_LEVEL_PRESSURE, 0.1903f)); }4. PID控制闭环实现
将高度数据融入PID控制循环是实现定高功能的关键。以下是完整的PID控制器实现：
typedef struct { float kp, ki, kd; // PID系数 float integral; // 积分项 float prev_error; // 上一次误差 float output_limit; // 输出限幅 float integral_limit; // 积分限幅 } PIDController; float PID_Update(PIDController *pid, float setpoint, float measurement, float dt) { float error = setpoint - measurement; // 比例项 float p_term = pid->kp * error; // 积分项(带抗饱和) pid->integral += error * dt; if(pid->integral_limit > 0) { pid->integral = constrain(pid->integral, -pid->integral_limit, pid->integral_limit); } float i_term = pid->ki * pid->integral; // 微分项(避免设定值突变导致的微分冲击) float d_term = 0; if(dt > 0) { d_term = pid->kd * (error - pid->prev_error) / dt; } pid->prev_error = error; // 计算总输出 float output = p_term + i_term + d_term; // 输出限幅 if(pid->output_limit > 0) { output = constrain(output, -pid->output_limit, pid->output_limit); } return output; }将高度控制整合到四轴飞行器的主控制循环中：
void FlightControl_Update(float dt) { // 1. 读取并处理传感器数据 MS5611_ReadData(&hms5611); MS5611_Calculate(&hms5611); float altitude = PressureToAltitude(hms5611.P / 100.0f); // 转换为米 // 2. 高度PID控制 static PIDController alt_pid = {0.8f, 0.2f, 0.5f, 0, 0, 2.0f, 1.0f}; float alt_output = PID_Update(&alt_pid, target_altitude, altitude, dt); // 3. 将高度控制输出融合到姿态控制 motor_output[0] += alt_output; motor_output[1] += alt_output; motor_output[2] += alt_output; motor_output[3] += alt_output; // 4. 电机输出限幅和保护 for(int i = 0; i < 4; i++) { motor_output[i] = constrain(motor_output[i], 0, MAX_THROTTLE); } }注意：实际应用中需要根据飞行器的重量、电机推力等参数仔细调整PID系数。建议先在10cm高度进行测试，逐步增加高度设定值。