基于STM32F405的四轴飞控系统设计

一、系统概述

四轴飞控是无人机（UAV）的核心控制单元，通过传感器融合、姿态解算与闭环控制实现稳定飞行与自主导航。本设计基于STM32F405RGT6（Cortex-M4，168MHz，带FPU，1MB Flash，192KB RAM），集成多传感器（IMU、GPS、气压计）、电机驱动、通信链路，实现姿态稳定（Roll/Pitch/Yaw）、定点悬停、航点飞行、一键返航等功能，适用于消费级航拍无人机、行业巡检无人机等场景。

二、硬件设计

2.1 核心组件选型

模块	型号/参数	功能说明
主控	STM32F405RGT6（Cortex-M4，168MHz，FPU）	姿态解算、PID控制、导航算法、外设驱动
IMU	ICM-20689（6轴，SPI接口，±2000dps/±16g）	加速度计+陀螺仪（姿态解算核心）
磁力计	HMC5883L（I2C，±8高斯）	航向角（Yaw）校准（补偿陀螺仪漂移）
气压计	BMP388（I2C/SPI，±0.12hPa）	海拔高度测量（定点悬停、爬升率控制）
GPS	NEO-M8N（UART，GPS+GLONASS双模）	位置/速度测量（航点飞行、返航定位）
电机驱动	4×BLHeli_32电调（DShot600协议）	驱动4路无刷电机（PWM/DShot信号输出）
通信	DSMX接收机（SBUS协议，UART）	接收遥控器指令（手动/自驾模式切换）
数传	SiK Radio（915MHz，UART，Mavlink协议）	与地面站通信（遥测数据上传、指令下发）
电源	3S锂电池（11.1V，2200mAh）+PMU电源管理	电压监测、过流保护、稳压（3.3V/5V输出）

2.2 硬件架构与连接

graph TD
    A[遥控器 SBUS] -->|UART1| B[STM32F405]
    C[GPS NEO-M8N] -->|UART2| B
    D[IMU ICM-20689] -->|SPI1| B
    E[磁力计 HMC5883L] -->|I2C1| B
    F[气压计 BMP388] -->|I2C1| B
    G[SiK数传] -->|UART3| B
    H[BLHeli_32电调] -->|PWM/DShot| B
    I[PMU电源管理] -->|ADC| B
    B -->|GPIO| J[LED状态指示]

关键引脚分配：

• SPI1：SCK=PA5，MOSI=PA7，MISO=PA6，CS_IMU=PA4（ICM-20689）；
• I2C1：SCL=PB6，SDA=PB7（HMC5883L+BMP388）；
• UART1：PA9(TX)，PA10(RX)（SBUS接收机，波特率100000bps）；
• PWM输出：TIM1_CH1_CH4（PA8PA11，DShot600信号）；
• ADC：PA0（电池电压采样）。

三、软件设计（STM32 HAL库+FreeRTOS）

3.1 系统架构（分层设计）

graph LR
    A[应用层] -->|任务调度| B[导航与控制层]
    B -->|姿态误差| C[算法层]
    C -->|控制量| D[执行器层]
    D -->|PWM/DShot| E[电机/电调]
    F[传感器层] -->|原始数据| C
    G[通信层] -->|遥控/数传| A
    H[电源与安全层] -->|状态监控| A

3.2 核心模块实现

3.2.1 传感器驱动（IMU+磁力计+气压计）

ICM-20689 SPI驱动（姿态解算数据源）：

#include "icm20689.h"
#include "spi.h"

// 读取陀螺仪/加速度计原始数据
void ICM20689_ReadAccelGyro(int16_t *accel, int16_t *gyro) {
  uint8_t tx_buf[14] = {0};
  uint8_t rx_buf[14] = {0};
  tx_buf[0] = 0x3B;  // 起始寄存器（ACCEL_XOUT_H）
  
  HAL_GPIO_WritePin(ICM_CS_GPIO_Port, ICM_CS_Pin, GPIO_PIN_RESET);
  HAL_SPI_TransmitReceive(&hspi1, tx_buf, rx_buf, 14, 100);
  HAL_GPIO_WritePin(ICM_CS_GPIO_Port, ICM_CS_Pin, GPIO_PIN_SET);
  
  // 解析加速度计（X/Y/Z，16位有符号）
  accel[0] = (rx_buf[0] << 8) | rx_buf[1];  // ACCEL_X
  accel[1] = (rx_buf[2] << 8) | rx_buf[3];  // ACCEL_Y
  accel[2] = (rx_buf[4] << 8) | rx_buf[5];  // ACCEL_Z
  // 解析陀螺仪（X/Y/Z，16位有符号）
  gyro[0] = (rx_buf[8] << 8) | rx_buf[9];   // GYRO_X
  gyro[1] = (rx_buf[10] << 8) | rx_buf[11];  // GYRO_Y
  gyro[2] = (rx_buf[12] << 8) | rx_buf[13];  // GYRO_Z
}

HMC5883L I2C驱动（航向角校准）：

// 读取磁力计数据（X/Y/Z，单位：高斯）
void HMC5883L_ReadMag(float *mag_x, float *mag_y, float *mag_z) {
  uint8_t data[6];
  HAL_I2C_Mem_Read(&hi2c1, HMC5883L_ADDR, 0x03, 1, data, 6, 100);
  *mag_x = (int16_t)(data[0] << 8 | data[1]) * 0.92f;  // 分辨率0.92mG/LSB
  *mag_y = (int16_t)(data[2] << 8 | data[3]) * 0.92f;
  *mag_z = (int16_t)(data[4] << 8 | data[5]) * 0.92f;
}

3.2.2 姿态解算（Mahony滤波算法）

融合IMU与磁力计数据，输出四元数或欧拉角（Roll/Pitch/Yaw），核心代码：

// Mahony滤波更新（输入：gyro/accel/mag，输出：四元数q0~q3）
void Mahony_Update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) {
  float q0 = q[0], q1 = q[1], q2 = q[2], q3 = q[3];  // 四元数
  float norm;
  float hx, hy, bx, bz;
  float vx, vy, vz, wx, wy, wz;
  float ex, ey, ez;
  float integralFBx = 0.0f, integralFBy = 0.0f, integralFBz = 0.0f;  // 积分项

  // 1. 归一化加速度计/磁力计数据
  norm = sqrtf(ax*ax + ay*ay + az*az); ax/=norm; ay/=norm; az/=norm;
  norm = sqrtf(mx*mx + my*my + mz*mz); mx/=norm; my/=norm; mz/=norm;

  // 2. 计算参考向量（地球坐标系）
  vx = 2*(q1*q3 - q0*q2); vy = 2*(q0*q1 + q2*q3); vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;  // 重力向量
  hx = 2*mx*(0.5f - q2*q2 - q3*q3) + 2*my*(q1*q3 - q0*q2) + 2*mz*(q0*q1 + q2*q3);
  hy = 2*mx*(q1*q3 + q0*q2) + 2*my*(0.5f - q1*q1 - q3*q3) + 2*mz*(q2*q3 - q0*q1);
  bx = sqrtf(hx*hx + hy*hy);
  bz = 2*mx*(q2*q3 - q0*q1) + 2*my*(q0*q2 + q1*q3) + 2*mz*(0.5f - q1*q1 - q2*q2);

  // 3. 计算误差向量（传感器测量值与参考向量叉积）
  wx = 2*bx*(0.5f - q2*q2 - q3*q3) + 2*bz*(q1*q3 - q0*q2);
  wy = 2*bx*(q1*q3 + q0*q2) + 2*bz*(0.5f - q1*q1 - q3*q3);
  wz = 2*bx*(q2*q3 - q0*q1) + 2*bz*(q0*q2 + q1*q3);
  ex = (ay*vz - az*vy) + (my*wz - mz*wy);
  ey = (az*vx - ax*vz) + (mz*wx - mx*wz);
  ez = (ax*vy - ay*vx) + (mx*wy - my*wx);

  // 4. PI控制器修正陀螺仪漂移
  float ki = 0.01f, kp = 0.5f;  // 积分/比例系数
  integralFBx += ki * ex * dt; integralFBy += ki * ey * dt; integralFBz += ki * ez * dt;
  gx += kp*ex + integralFBx; gy += kp*ey + integralFBy; gz += kp*ez + integralFBz;

  // 5. 更新四元数（陀螺仪积分）
  q0 += (-q1*gx - q2*gy - q3*gz) * 0.5f * dt;
  q1 += (q0*gx + q2*gz - q3*gy) * 0.5f * dt;
  q2 += (q0*gy - q1*gz + q3*gx) * 0.5f * dt;
  q3 += (q0*gz + q1*gy - q2*gx) * 0.5f * dt;

  // 6. 四元数归一化
  norm = sqrtf(q0*q0 + q1*q1 + q2*q2 + q3*q3);
  q0/=norm; q1/=norm; q2/=norm; q3/=norm;
}

3.2.3 串级PID控制（姿态环+位置环）

姿态环（内环，角速度控制）：

// 角速度环PID（输入：目标角速度，实际角速度，输出：电机力矩）
float PID_AngularRate(PID_TypeDef *pid, float target, float actual) {
  float error = target - actual;
  pid->integral += error * dt;
  float derivative = (error - pid->prev_error) / dt;
  float output = pid->Kp*error + pid->Ki*pid->integral + pid->Kd*derivative;
  pid->prev_error = error;
  return output;
}

位置环（外环，角度控制）：

// 角度环PID（输入：目标角度，实际角度，输出：目标角速度）
float PID_Angle(PID_TypeDef *pid, float target, float actual) {
  float error = target - actual;
  pid->integral += error * dt;
  float derivative = (error - pid->prev_error) / dt;
  float output = pid->Kp*error + pid->Ki*pid->integral + pid->Kd*derivative;
  pid->prev_error = error;
  return output;
}

电机混控（X型布局，4路输出）：

// 根据姿态控制量计算4路电机PWM（DShot值）
void Motor_Mixing(float roll_out, float pitch_out, float yaw_out, float throttle) {
  float base = throttle;  // 基础油门
  float m1 = base - roll_out + pitch_out + yaw_out;  // 前右电机
  float m2 = base + roll_out + pitch_out - yaw_out;  // 前左电机
  float m3 = base + roll_out - pitch_out + yaw_out;  // 后左电机
  float m4 = base - roll_out - pitch_out - yaw_out;  // 后右电机
  // 限幅（0~2000，DShot600范围）
  m1 = constrain(m1, 1000, 2000); m2 = constrain(m2, 1000, 2000);
  m3 = constrain(m3, 1000, 2000); m4 = constrain(m4, 1000, 2000);
  // 输出DShot信号（通过TIM1_CH1~CH4）
  DShot_Write(0, m1); DShot_Write(1, m2);
  DShot_Write(2, m3); DShot_Write(3, m4);
}

参考代码 四轴飞控STM32F405 www.youwenfan.com/contentcst/182287.html

四、测试与验证

4.1 测试项目

测试项	方法	预期结果
姿态稳定性	悬停测试（无风环境）	Roll/Pitch波动<±2°，Yaw漂移<±5°/min
定点悬停	GPS模式（开阔场地）	位置误差<±0.5m，高度误差<±0.2m
抗风能力	3级风环境（风速3~5m/s）	姿态恢复时间<2s，无侧翻
续航时间	3S 2200mAh电池，悬停状态	飞行时间>15分钟

4.2 工具链

• 地面站：QGroundControl（Mavlink协议），实时监控姿态、位置、电池电压；
• 日志分析：Flight Review（解析黑匣子日志，优化PID参数）；
• 示波器：测量DShot信号波形，验证电调响应。

六、总结

基于STM32F405实现了四轴飞控的核心功能，核心是多传感器融合（Mahony滤波）、串级PID控制与电机混控。通过FreeRTOS多任务调度（传感器采集、姿态解算、控制输出、通信），确保系统实时性（姿态更新频率500Hz，控制周期2ms）。