四轴飞行器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高度进行测试逐步增加高度设定值。