STM32 四旋翼飞行器

🛠️ STM32F407 · MPU6050 · PID · FreeRTOS GitHub

STM32FreeRTOSPIDMPU6050

项目简介

基于 STM32F407VGT6 的自制四旋翼飞行器飞控系统。使用 MPU6050 进行姿态解算，通过 PID 控制器实现自稳悬停。

硬件架构

组件	型号	说明
主控	STM32F407VGT6	Cortex-M4, 168MHz
姿态传感器	MPU6050	六轴 IMU
气压计	BMP280	高度测量
遥控接收机	SBUS 协议	8 通道
电调	BLHeli_S 30A	4 路 PWM

软件架构

┌─────────────────────────────────┐
│            任务调度              │  FreeRTOS
├────────────────┬────────────────┤
│   姿态解算任务  │    PID 控制任务 │
│ (1000Hz, 高优) │  (500Hz, 高优) │
├────────────────┼────────────────┤
│   遥控接收任务  │   数据回传任务  │
│ (50Hz, 中优)   │  (10Hz, 低优)  │
└────────────────┴────────────────┘

关键技术要点

姿态解算

使用 Mahony 互补滤波算法融合加速度计和陀螺仪数据：

void MahonyAHRSupdate(float gx, float gy, float gz,
                      float ax, float ay, float az) {
    float recipNorm;
    float halfvx, halfvy, halfvz;
    float halfex, halfey, halfez;

    // 计算重力参考方向
    halfvx = q1q3 - q0q2;
    halfvy = q0q1 + q2q3;
    halfvz = q0q0 - q1q1 - q2q2 + q3q3;

    // 误差 = 测量值 × 参考值
    halfex = ay * halfvz - az * halfvy;
    halfey = az * halfvx - ax * halfvz;
    halfez = ax * halfvy - ay * halfvx;

    // PI 补偿
    integralFBx += Ki * halfex;
    integralFBy += Ki * halfey;
    integralFBz += Ki * halfez;

    gx += integralFBx; gy += integralFBy; gz += integralFBz;

    // 四元数更新（一阶龙格-库塔）
    q0 += (-q1*gx - q2*gy - q3*gz) * halfT;
    q1 += ( q0*gx + q2*gz - q3*gy) * halfT;
    q2 += ( q0*gy - q1*gz + q3*gx) * halfT;
    q3 += ( q0*gz + q1*gy - q2*gx) * halfT;

    recipNorm = 1.0f / sqrtf(q0*q0 + q1*q1 + q2*q2 + q3*q3);
    q0 *= recipNorm; q1 *= recipNorm;
    q2 *= recipNorm; q3 *= recipNorm;
}

PID 控制

外环角度环 + 内环角速度环的串级 PID 结构：

• 角度环（外环）：P = 4.5, I = 0.02, D = 0.1
• 角速度环（内环）：P = 0.15, I = 0.005, D = 0.01

待优化项

• [ ] 添加光流模块实现定点悬停
• [ ] 移植 EKF 替代互补滤波
• [ ] 添加 SD 卡日志记录

---

💬 评论