🚁 STM32 + MPU6050 从零到四轴：一套让你真的"看懂"飞控的教程

一句话说清这篇教程在干嘛：我们用一块 STM32、一个 MPU6050（小陀螺+小加速度计），一步步做出一个"知道自己在哪、怎么保持平衡、怎么飞稳"的四轴飞控核心。从读取传感器原始数据，到算出姿态角，到用 PID 去控制电机，再到用光流传感器实现室内定点——每一步都讲清楚"为什么这么做"，而不是只丢给你一堆代码。

---

🧭 目录

1. 先认识传感器：一个"瞎子"和一个"醉汉"
2. 让 STM32 和它"聊上天"：I2C 读数据
3. 把乱码变成人话：单位换算与校准
4. 姿态到底怎么描述：欧拉角、矩阵、四元数
5. 姿态解算：怎么把两个传感器"合成"一个准的
6. 进阶滤波：卡尔曼 和 DMP（偷懒神器）
7. PID：让姿态"听话"的控制魔法
8. 电机输出：PID 算出的数怎么变成"转多快"
9. 光流传感器：室内没 GPS 时靠"看图"定位
10. 飞控全貌：把前面串成一条完整闭环
11. 新手学习路线 & 参考资源

---

1. 先认识传感器：一个"瞎子"和一个"醉汉"

这就是我们这篇教程的"主角"——一块巴掌大的 MPU6050 模块，以及承载算法的 STM32 单片机：

▲ 图 1 · MPU6050 六轴（3 轴加速度 + 3 轴陀螺）传感器模块，通过 I2C 与单片机通信

▲ 图 2 · STM32 开发板，作为飞控的"大脑"，负责读数据、算姿态、输出 PWM

想象 MPU6050 其实装了两个性格完全相反的小家伙：

🥴 陀螺仪（Gyroscope）——一个"醉汉"

• 它负责测角速度，也就是"我转得有多快"（单位 °/s）。
• 优点：反应极快，你刚转一点点它立刻就知道。
• 缺点：它只能测"转动的速度"，要得到角度必须积分（把速度累加起来）。可它有个坏毛病——会悄悄"跑偏"（零偏漂移）。就像醉汉走路，走几步方向就歪了，时间越久错得越离谱。
• 结论：短时间超准，长时间必漂。

🧘 加速度计（Accelerometer）——一个"坐得稳的老实人"

• 它负责测加速度（单位 g），静止时能"感觉"出重力朝哪边。
• 优点：长期超级稳定，你放平它就知道"地面在这边"，永远不漂。
• 缺点：它特别"怕抖"。飞机一震、一加速，它测出的方向就全是乱颤的噪声，短时间里根本没法信。
• 结论：长期稳，短时间抖。

💡 核心思想：取长补短

这俩一个"快但漂"，一个"稳但抖"——正好互补。姿态解算（姿态融合）的全部秘密，就是想办法让它俩"互相当靠山"，合成一个既快又稳的角度。

🎯 这就是"互补滤波"名字的来源：把两个传感器的长处互补起来。

接线图（就这么简单，一共 4 根线）：

  STM32                         MPU6050 模块
 ┌────────────┐                 ┌────────────┐
 │  3.3V   ───┼── 红色 ────────▶│  VCC       │
 │   GND   ───┼── 黑色 ────────▶│  GND       │
 │  PB6·SCL ──┼── 黄色 ────────▶│  SCL       │
 │  PB7·SDA ──┼── 绿色 ────────▶│  SDA       │
 └────────────┘                 └────────────┘
           （SCL / SDA 都需要接 4.7kΩ 上拉电阻到 3.3V）

⚠️ 新手常踩的三个坑

1. MPU6050 的 AD0 引脚接地 → 地址是 0x68；接 3.3V → 地址变 0x69。地址写错 = 通信失败，最最常见的坑！
2. 别把模块上的 5V 直接怼到 STM32 引脚上（I/O 是 3.3V 电平）。
3. I2C 必须接上拉电阻，否则信号不稳定、时好时坏。

---

2. 让 STM32 和它"聊上天"：I2C 读数据

📬 它是怎么沟通的？

MPU6050 内部有一排"邮箱"（寄存器），STM32 只要用 I2C 协议"写个纸条进去、或取一张纸条出来"就能沟通。芯片底层的电路连接大概是这样：

▲ 图 3 · MPU6050 内部电路原理示意（MCU 通过 I2C 数据线 SDA/SCL 与片内寄存器读写通信）

数据都排好队放在从地址 0x3B 开始的一连串邮箱里，顺序是：

 加速度X → 加速度Y → 加速度Z → 温度 → 陀螺X → 陀螺Y → 陀螺Z
┌────────┐┌────────┐┌────────┐┌──────┐┌──────┐┌──────┐┌──────┐
│  2字节 ││  2字节 ││  2字节 ││ 2字节││ 2字节││ 2字节││ 2字节│
└────────┘└────────┘└────────┘└──────┘└──────┘└──────┘└──────┘
 0x3B     0x3D     0x3F     0x41   0x43    0x45    0x47

所以我们一次突发读取 14 个字节，就把 6 个轴 + 温度全拿回来了，效率最高。

🔌 初始化代码（STM32 HAL 库）

void MPU6050_Init(void){
    HAL_Delay(100); // 等待传感器内部供电稳定
    uint8_t reg = 0x01; // 唤醒并选择时钟源 PLL with X Gyro
    HAL_I2C_Mem_Write(&hi2c1, MPU6050_ADDRESS<<1, 0x6B, I2C_MEMADD_SIZE_8BIT, &reg, 1, 100);
    
    reg = 0x10; // 设陀螺仪量程 ±1000°/s
    HAL_I2C_Mem_Write(&hi2c1, MPU6050_ADDRESS<<1, 0x1B, I2C_MEMADD_SIZE_8BIT, &reg, 1, 100);
    
    reg = 0x00; // 设加速度量程 ±2g (16384 LSB/g)
    HAL_I2C_Mem_Write(&hi2c1, MPU6050_ADDRESS<<1, 0x1C, I2C_MEMADD_SIZE_8BIT, &reg, 1, 100);
}

📖 读取 14 字节原始数据

uint8_t buf[14];
int16_t ax, ay, az, gx, gy, gz, tmp;

void MPU6050_ReadRaw(void){
    // 从 0x3B 开始，一口气读 14 个字节
    HAL_I2C_Mem_Read(&hi2c1, 0x68<<1, 0x3B, I2C_MEMADD_SIZE_8BIT, buf, 14, 100);
    // 每个数据是16位有符号数，高字节在前，低字节在后，拼起来：
    ax = (buf[0] << 8) | buf[1];
    ay = (buf[2] << 8) | buf[3];
    az = (buf[4] << 8) | buf[5];
    tmp= (buf[6] << 8) | buf[7];   // 温度
    gx = (buf[8] << 8) | buf[9];
    gy = (buf[10]<< 8) | buf[11];
    gz = (buf[12]<< 8) | buf[13];
}

✅ 完成后打开串口打印 ax, ay, az, gx, gy, gz，轻轻晃动模块，看数值会不会跟着变。能变 = 你成功了！这是飞控之路的第一个里程碑 🎉。

---

3. 把乱码变成人话：单位换算与校准

上一步读到的只是原始 ADC 数字（比如 16384、-5000），像一堆没单位的乱码。要变成有物理意义的值，得除以"灵敏度"：

物理值 = 原始值 ÷ 灵敏度
MPU6050 芯片内部是一个 16 位的 ADC（模数转换器）。它测到的物理信号（重力、旋转）最终会被转换成一个 16 位有符号整数（int16_t）。

16 位有符号数的最大范围是：
−32768∼+32767
−32768∼+32767。
满量程对应的最大数字就是 32768。

你选择了 ±2g±2*g* 量程（gg 是地球重力加速度，约等于 9.8 m/s2）。

换算推导：

• 芯片感受到的加速度范围是：−2g∼+2g
• 芯片 ADC 输出的数字范围是：−32768∼+32767
• 两者一一对应：
  • +2g+2g 对应数字 +32768
  • +1g+1g 对应数字 +16384（即 32768÷2）
  • 0g0g 对应数字 0
  • −1g−1g 对应数字 −16384
  • −2g−2g 对应数字 −32768

量程	加速度灵敏度 (LSB/g)	陀螺灵敏度 (LSB/°/s)
±2g / ±250°/s	16384	131
±4g / ±500°/s	8192	65.5
±8g / ±1000°/s	4096	32.8
±16g / ±2000°/s	2048	16.4

// 加速度：÷16384 → 单位 g（静止时重力 1g 会落在某个轴上）
float accelX = ax / 16384.0f;
float accelY = ay / 16384.0f;
float accelZ = az / 16384.0f;

// 陀螺仪：÷32.8 → 单位 °/s
float gyroX = gx / 32.8f;
float gyroY = gy / 32.8f;
float gyroZ = gz / 32.8f;

🧊 零偏校准：帮"醉汉"先站直

还记得那个会跑偏的陀螺仪吗？它静止时明明没转，却会输出一个固定的"假速度"（零偏）。如果不扣掉，积分会越来越偏。

解决办法：上电后让它静止几秒，多采几次平均，就得到这个"假速度"，以后每个读数都减掉它。

//
// Created by 31962 on 2026/8/30.
//
#include "Tmpu6050.h"
#include "debug_config.h"

// 存储校准偏移量（全部使用原始有符号 16 位整数 LSB 尺度，避免单位混乱）
static int16_t ax_offset = 0;
static int16_t ay_offset = 0;
static int16_t az_offset = 0;
static int16_t gx_offset = 0;
static int16_t gy_offset = 0;
static int16_t gz_offset = 0;

uint8_t buf[14];

// ===== 初始化 MPU6050 =====
void MPU6050_Init(void){
    HAL_Delay(100); // 等待传感器内部供电稳定
    uint8_t reg = 0x01; // 唤醒并选择时钟源 PLL with X Gyro
    HAL_I2C_Mem_Write(&hi2c1, MPU6050_ADDRESS<<1, 0x6B, I2C_MEMADD_SIZE_8BIT, &reg, 1, 100);

    reg = 0x10; // 设陀螺仪量程 ±1000°/s
    HAL_I2C_Mem_Write(&hi2c1, MPU6050_ADDRESS<<1, 0x1B, I2C_MEMADD_SIZE_8BIT, &reg, 1, 100);

    reg = 0x00; // 设加速度量程 ±2g (16384 LSB/g)
    HAL_I2C_Mem_Write(&hi2c1, MPU6050_ADDRESS<<1, 0x1C, I2C_MEMADD_SIZE_8BIT, &reg, 1, 100);
}

// ===== I2C 扫描 =====
void I2C_Scan(void) {
    printf("Scanning I2C bus...\r\n");
    for (uint8_t i = 1; i < 128; i++) {
        if (HAL_I2C_IsDeviceReady(&hi2c1, (i << 1), 1, 10) == HAL_OK) {
            printf("Found I2C device at address: 0x%02X (8-bit: 0x%02X)\r\n", i, (i << 1));
            return;
        }
    }
    printf("No I2C device found!\r\n");
}

// ===== 自动校准（开机时水平静止放桌上） =====
void MPU6050_Calibrate(void) {
    int32_t sum_ax = 0, sum_ay = 0, sum_az = 0;
    int32_t sum_gx = 0, sum_gy = 0, sum_gz = 0;
    const int samples = 200;

    debug_printf("Calibrating... Keep MPU6050 still and flat!\r\n");

    for (int i = 0; i < samples; i++) {
        if (HAL_I2C_Mem_Read(&hi2c1, MPU6050_ADDRESS<<1, 0x3B, I2C_MEMADD_SIZE_8BIT, buf, 14, 100) == HAL_OK) {
            // 必须强制转换为 (int16_t)，否则负数会出错！
            sum_ax += (int16_t)((buf[0] << 8) | buf[1]);
            sum_ay += (int16_t)((buf[2] << 8) | buf[3]);
            sum_az += (int16_t)((buf[4] << 8) | buf[5]);
            sum_gx += (int16_t)((buf[8] << 8) | buf[9]);
            sum_gy += (int16_t)((buf[10] << 8) | buf[11]);
            sum_gz += (int16_t)((buf[12] << 8) | buf[13]);
        }
        HAL_Delay(5);
    }

    // 计算偏差：
    ax_offset = sum_ax / samples; // X 轴静止理论上是 0
    ay_offset = sum_ay / samples; // Y 轴静止理论上是 0
    // Z 轴静止理论上是 1g，对应 16384 LSB 刻度！用刻度减刻度！
    az_offset = (sum_az / samples) - 16384;

    // 陀螺仪静止理论上全为 0
    gx_offset = sum_gx / samples;
    gy_offset = sum_gy / samples;
    gz_offset = sum_gz / samples;
}

// ===== 输出最终校准并换算后的物理量数据 =====
void MPU6050_GetCalibratedData(float *mpu_value) {
    if (HAL_I2C_Mem_Read(&hi2c1, MPU6050_ADDRESS<<1, 0x3B, I2C_MEMADD_SIZE_8BIT, buf, 14, 100) == HAL_OK) {

        // 1. 读取带符号的原始 16 位整数
        int16_t raw_ax   = (int16_t)((buf[0]  << 8) | buf[1]);
        int16_t raw_ay   = (int16_t)((buf[2]  << 8) | buf[3]);
        int16_t raw_az   = (int16_t)((buf[4]  << 8) | buf[5]);
        int16_t raw_temp = (int16_t)((buf[6]  << 8) | buf[7]);
        int16_t raw_gx   = (int16_t)((buf[8]  << 8) | buf[9]);
        int16_t raw_gy   = (int16_t)((buf[10] << 8) | buf[11]);
        int16_t raw_gz   = (int16_t)((buf[12] << 8) | buf[13]);

        // 2. 减去 LSB 刻度偏差
        int16_t cal_ax = raw_ax - ax_offset;
        int16_t cal_ay = raw_ay - ay_offset;
        int16_t cal_az = raw_az - az_offset;
        int16_t cal_gx = raw_gx - gx_offset;
        int16_t cal_gy = raw_gy - gy_offset;
        int16_t cal_gz = raw_gz - gz_offset;

        // 3. 统一除以灵敏度，转换为物理 float 单位
        mpu_value[0] = (float)cal_ax / 16384.0f;          // 加速度 X (g)
        mpu_value[1] = (float)cal_ay / 16384.0f;          // 加速度 Y (g)
        mpu_value[2] = (float)cal_az / 16384.0f;          // 加速度 Z (g)
        mpu_value[3] = ((float)raw_temp / 340.0f) + 36.53f; // 温度 (℃) 不校准直接换算
        mpu_value[4] = (float)cal_gx / 32.8f;             // 陀螺仪 X (°/s)
        mpu_value[5] = (float)cal_gy / 32.8f;             // 陀螺仪 Y (°/s)
        mpu_value[6] = (float)cal_gz / 32.8f;             // 陀螺仪 Z (°/s)
    }
}

💡 三轴都这样处理。加速度计一般不用大校准（它本身够稳），除非你要很高精度。

我的最后的结果：

---

4. 姿态到底怎么描述：欧拉角、矩阵、四元数

🤔 一个深刻的问题：怎么告诉别人"一个物体现在朝哪"？

先看这张图，一架四轴在空中能绕三个方向转，这就是它的"三个自由度"：

▲ 图 4 · 四轴的三个姿态角：Roll（横滚）、Pitch（俯仰）、Yaw（偏航）——它们定义了飞机"朝哪、歪多少"

有三种主流"语言"：

① 欧拉角（Roll / Pitch / Yaw）——最直观

• Roll 横滚：像飞机压翅膀往左/往右歪。
• Pitch 俯仰：像飞机抬头/低头。
• Yaw 偏航：像飞机在原地转圈（左转/右转）。
• 致命缺点：当 Pitch 转到接近 ±90°（仰头到天顶）时，会陷入"万向锁"（Gimbal Lock）——Roll 和 Yaw 突然分不清了，丢失一个自由度，算法直接崩溃。就像两个铰链叠在一起卡死。

② 旋转矩阵（3×3）——精确但啰嗦

• 信息完整、数学清楚，但有 9 个元素、信息冗余，而且每次运算后数值会"漂移"，要反复做正交化校正，很麻烦。

③ 四元数（Quaternion）——飞控界的"普通话" ⭐

• 只用 4 个数字，没有万向锁，运算量小、稳定性好。
• 飞控和姿态解算的工业标准，几乎所有飞控都用它。

🧮 四元数长什么样？

它可以理解为"绕一个轴 n 转一个角度 θ"的浓缩编码：

q = q0 + q1·i + q2·j + q3·k
  = cos(θ/2) + sin(θ/2)·(nx·i + ny·j + nz·k)

并且它必须满足归一化（模长为 1）：

q0² + q1² + q2² + q3² = 1

🔄 四元数 → 欧拉角（解算结果最终要给人看的）

// 由四元数反解出三个欧拉角
Roll  = atan2f(2*(q0*q1 + q2*q3), 1 - 2*(q1*q1 + q2*q2));
Pitch = asinf (2*(q0*q2 - q1*q3));
Yaw   = atan2f(2*(q0*q3 + q1*q2), 1 - 2*(q2*q2 + q3*q3));

⚙️ 四元数的"心脏"：姿态怎么随时间更新？

知道了角速度 ω，就能推出四元数怎么变。这是一条微分方程：

dq/dt = ½ · q ⊗ ω

在代码里，我们用"一阶积分"把它离散化（halfT = dt/2）：

// 这是 Mahony / Madgwick 都用的四元数一阶积分核心
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;
// 归一化，保证模长为 1
float norm = sqrtf(q0*q0+q1*q1+q2*q2+q3*q3);
q0/=norm; q1/=norm; q2/=norm; q3/=norm;

📌 一句话记住：欧拉角直观但会"卡死"（万向锁），四元数没有这个毛病又省算力，所以飞控都用四元数。

以上是数学研究生需要研究的领域，我们知道学会用现成的算法库或则ai处理收到的数据就行

---

5. 姿态解算：怎么把两个传感器"合成"一个准的

🧠 先懂互补滤波思想（一维版本）

陀螺仪积分出来的角度：快但会漂。
加速度计算出的角度：稳但会抖。

互补滤波就是给它俩各上一道"滤镜"，然后加权相加：

angle = α · (angle + gyro·dt) + (1−α) · angle_acc
       └───── 陀螺仪（权重大，占 0.98）─────┘   └─加速度计（权重小，占 0.02）─┘

• α 通常取 0.95~0.98：大部分信任"快"的陀螺仪，
• 用剩下一点点"稳"的加速度计，偷偷把漂移拉回来。

    陀螺仪 ω ──▶ [高通·α]   ┐
                            ├──▶ 加权求和 ──▶ 姿态角
    加速度计 ──▶ [低通·1−α]  ┘

🚁 三维版：Mahony 算法（四元数互补滤波）

一维互补滤波一次只能算一个轴。把同样的思想推广到三维四元数，就是大名鼎鼎的 Mahony 算法。它流程就三步，非常好懂：

步骤1  由当前四元数，反推"理论重力方向" v
步骤2  拿加速度计实测的重力方向 a，与 v 做叉积 → 得到误差 e = v × a
       （注意：陀螺仪若完全准，a 应该等于 v；差得越多，e 越大）
步骤3  用误差 e 通过 PI 修正陀螺仪：ω = ω + Kp·e + Ki·∫e
       再用修正后的 ω 去积分更新四元数

一句话：加速度计是"老师"，负责挑陀螺仪的毛病（误差），通过 PI 一点点纠正它的漂移。

💻 Mahony 完整代码（可直接用）

// 输入：加速度(单位g)、角速度(rad/s，注意要转成弧度)、采样周期dt
void MahonyAHRSupdate(float gx, float gy, float gz,
                      float ax, float ay, float az,
                      float dt){
    float halfT = dt/2.0f;
    float norm, ex, ey, ez, vx, vy, vz;

    // 1. 归一化加速度计（只关心方向，不关心大小）
    norm = sqrtf(ax*ax+ay*ay+az*az);
    ax/=norm; ay/=norm; az/=norm;

    // 2. 由当前四元数推算"理论重力方向"v（机体坐标系下）
    vx = 2*(q1*q3 - q0*q2);
    vy = 2*(q0*q1 + q2*q3);
    vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;

    // 3. 叉积得到姿态误差 e = v × a
    ex = (ay*vz - az*vy);
    ey = (az*vx - ax*vz);
    ez = (ax*vy - ay*vx);

    // 4. 误差积分 + 比例，修正陀螺仪（Kp比例、Ki积分）
    exInt += ex * Ki * dt;
    eyInt += ey * Ki * dt;
    ezInt += ez * Ki * dt;
    gx += Kp*ex + exInt;
    gy += Kp*ey + eyInt;
    gz += Kp*ez + ezInt;

    // 5. 四元数微分积分更新 + 归一化
    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;
    norm = sqrtf(q0*q0+q1*q1+q2*q2+q3*q3);
    q0/=norm; q1/=norm; q2/=norm; q3/=norm;
}

🎛️ 参数经验（先记个大概，后面再调）

• Kp（纠偏力度）：越大响应越快，但太大引入噪声。常用起步值 2.0。
• Ki（消除陀螺零偏）：用于消除长时间漂移。常用起步值 0.005。
• 采样周期 dt 建议 1~5ms（主循环里跑）。
• 调参方法：把角度通过串口画出来，看响应快不快、会不会震荡，再微调。

---

6. 进阶滤波：卡尔曼 和 DMP（偷懒神器）

🧠 卡尔曼滤波：会"自己学习怎么加权"的高手

互补滤波的权重 α 是写死的。卡尔曼滤波更聪明：它有一个预测 + 校正的两步循环，会根据噪声自动调整权重，是"线性系统的最优估计器"。

        ┌──────────────┐          ┌──────────────┐
  预测  │ 角度=角度+    │─────────▶│ 校正          │
        │ (gyro-bias)dt│          │ 角度 += K·(...)│
        │ P = APAT+Q   │          │ K = 最优增益   │
        └──────────────┘          └──────────────┘
              ▲                        │
              └────────────────────────┘  (循环)

公式（不用背，理解即可）：

预测：x̂k = A·x̂k-1 + B·uk ， Pk = A·Pk-1·Aᵀ + Q
校正：K = Pk·Hᵀ(H·Pk·Hᵀ+R)⁻¹ ， x̂k = x̂k + K·(zk − H·x̂k)

• 优点：更优、能显式建模噪声。
• 缺点：算得多一点，要调 3 个噪声参数（Q_angle、Q_gyro、R_angle），调起来比互补滤波费劲。
• 早期在 51/STM32 上很流行做单轴姿态（一次算一个轴）。

😴 DMP：MPU6050 自带的"偷懒神器"

重点来了！MPU6050 芯片内部自带一个数字运动处理器 DMP，可以直接在硬件里完成姿态解算，输出现成的四元数，都不用你自己写 Mahony/卡尔曼！

// 从 DMP 的 FIFO 读出四元数（q30 定点格式）
long quat[4];
dmp_read_fifo(gyro, accel, quat, &timestamp, &sensors, &more);

// 除以 2^30 转成浮点
q0 = quat[0] / 1073741824.0f;
q1 = quat[1] / 1073741824.0f;
q2 = quat[2] / 1073741824.0f;
q3 = quat[3] / 1073741824.0f;
// 再用上章的公式把四元数换算成欧拉角即可

💡 官方移植包 MPU6050 + dmp6Axis（Jrowberg）是大家最常用的。移植好就能直接拿角度，非常适合快速上手或算力低的 MCU。

⚖️ 四种方案怎么选？（一张表看清）

方案	原理	计算量	稳定性	适合场景
一阶互补滤波	固定权重加权	很小	一般	平衡小车、入门理解
卡尔曼滤波	最优估计（预测+校正）	中	较好，要调参数	单轴角度、要平滑
Mahony/Madgwick	四元数互补滤波	小	好、无万向锁	飞控姿态解算（最推荐）
硬件 DMP	芯片内部硬件解算	最小(MCU)	好，但采样率固定	快速上手、低算力 MCU

以下是对roll和pitch角进行滤波，因为 MPU6050 只有 6 个轴，没有“地磁计”（指南针），所以它在物理上根本无法对 Yaw（偏航角）进行滤波校正

以上都有ai和算法库实现，直接调用api就行，接下来我会对比没有滤波和滤波以后的效果，通过vofa展示

5.1 一阶互补滤波

1. 滤波器头文件 mpu6050_filter.h

创建如下的文件，粘贴复制：

#ifndef __MPU6050_FILTER_H
#define __MPU6050_FILTER_H

#include "main.h"
#include "Tmpu6050.h"
#include <math.h>

// ==================== 姿态角结构体 ====================
typedef struct {
    float pitch;    // 俯仰角 (绕Y轴，抬头低头)
    float roll;     // 翻滚角 (绕X轴，左右倾斜)
    float yaw;      // 偏航角 (绕Z轴，左右转向，仅陀螺仪积分)
} Attitude_t;

// ==================== 卡尔曼滤波器状态结构体 ====================
typedef struct {
    float angle;        // 当前最优角度估计值
    float bias;         // 当前陀螺仪零偏估计值
    float P[2][2];      // 误差协方差矩阵 (2x2)
    float Q_angle;      // 角度过程噪声 (越小越信任陀螺仪)
    float Q_bias;       // 零偏过程噪声
    float R_measure;    // 测量噪声 (越小越信任加速度计)
} Kalman_t;

// ==================== 函数声明 ====================

// --- 通用工具 ---
void MPU6050_GetAccelAngles(float *mpu_data, float *accel_pitch, float *accel_roll);

// --- 方法①：一阶互补滤波 ---
void Filter_Complementary1st_Init(Attitude_t *att);
void Filter_Complementary1st_Update(Attitude_t *att, float *mpu_data, float dt);

// --- 方法②：二阶互补滤波 ---
void Filter_Complementary2nd_Init(Attitude_t *att);
void Filter_Complementary2nd_Update(Attitude_t *att, float *mpu_data, float dt);

// --- 方法③：卡尔曼滤波 ---
void Filter_Kalman_Init(Kalman_t *kf);
float Filter_Kalman_Update(Kalman_t *kf, float accel_angle, float gyro_rate, float dt);
void Filter_Kalman_GetAttitude(Kalman_t *kf_pitch, Kalman_t *kf_roll, 
                                Attitude_t *att, float *mpu_data, float dt);

// --- VOFA+ 输出 (JustFloat 协议) ---
void VOFA_SendData(float *data, uint8_t count);

#endif

---

2. 滤波器实现 mpu6050_filter.c

#include "mpu6050_filter.h"
#include <string.h>
#include <stdio.h>

extern UART_HandleTypeDef huart1; // 根据你的实际串口修改

// ================================================================
//                   通用工具：加速度计算倾角
// ================================================================
// 用 atan2 从加速度直接算出静态角度（只在静止/缓慢运动时准确）
void MPU6050_GetAccelAngles(float *mpu_data, float *accel_pitch, float *accel_roll) {
    float ax = mpu_data[0]; // 单位: g
    float ay = mpu_data[1];
    float az = mpu_data[2];

    // Pitch: 绕Y轴旋转（抬头低头）
    *accel_pitch = atan2f(ax, sqrtf(ay * ay + az * az)) * 57.2957795f; // 弧度转角度

    // Roll: 绕X轴旋转（左右倾斜）
    *accel_roll  = atan2f(ay, sqrtf(ax * ax + az * az)) * 57.2957795f;
}

// ================================================================
//              方法①：一阶互补滤波 (First-Order Complementary)
// ================================================================
// 原理：
//   angle = α * (angle + gyro * dt) + (1-α) * accel_angle
//          ↑ 高通滤波（信任陀螺仪短期变化）  ↑ 低通滤波（信任加速度计长期趋势）
//   α 越大 → 越信任陀螺仪（响应快但会漂移）
//   α 越小 → 越信任加速度计（稳定但有震动噪声）
// ================================================================

#define COMP1_ALPHA  0.98f  // 互补系数，通常取 0.95 ~ 0.99

void Filter_Complementary1st_Init(Attitude_t *att) {
    att->pitch = 0.0f;
    att->roll  = 0.0f;
    att->yaw   = 0.0f;
}

void Filter_Complementary1st_Update(Attitude_t *att, float *mpu_data, float dt) {
    // 1. 从加速度计算静态角度
    float accel_pitch, accel_roll;
    MPU6050_GetAccelAngles(mpu_data, &accel_pitch, &accel_roll);

    // 2. 读取陀螺仪角速度 (°/s)
    float gyro_x = mpu_data[4]; // Roll  方向的角速度
    float gyro_y = mpu_data[5]; // Pitch 方向的角速度
    float gyro_z = mpu_data[6]; // Yaw   方向的角速度

    // 3. 一阶互补滤波核心公式
    //    短期相信陀螺仪积分，长期用加速度计修正漂移
    att->pitch = COMP1_ALPHA * (att->pitch + gyro_y * dt) + (1.0f - COMP1_ALPHA) * accel_pitch;
    att->roll  = COMP1_ALPHA * (att->roll  + gyro_x * dt) + (1.0f - COMP1_ALPHA) * accel_roll;
    att->yaw  += gyro_z * dt; // Yaw 只能靠陀螺仪积分（加速度计无法测偏航）
}


// ================================================================
//              方法②：二阶互补滤波 (Second-Order Complementary)
// ================================================================
// 原理：在一阶互补的基础上，引入加速度计角度的变化率作为第二个修正项，
//       相当于多了一个"微分校正"环节，让滤波器对突变更敏感且更平滑。
//
//   x1 = (accel_angle - angle) * k1
//   x2 += (accel_angle - angle) * k2 * dt
//   angle += (gyro_rate + x1 + x2) * dt
// ================================================================

// 二阶互补滤波内部状态
static float comp2_x1_pitch = 0, comp2_x2_pitch = 0;
static float comp2_x1_roll  = 0, comp2_x2_roll  = 0;

#define COMP2_K1  0.02f   // 比例修正系数（越大越信加速度计）
#define COMP2_K2  0.005f  // 积分修正系数（消除稳态误差）

void Filter_Complementary2nd_Init(Attitude_t *att) {
    att->pitch = 0.0f;
    att->roll  = 0.0f;
    att->yaw   = 0.0f;
    comp2_x1_pitch = 0; comp2_x2_pitch = 0;
    comp2_x1_roll  = 0; comp2_x2_roll  = 0;
}

void Filter_Complementary2nd_Update(Attitude_t *att, float *mpu_data, float dt) {
    float accel_pitch, accel_roll;
    MPU6050_GetAccelAngles(mpu_data, &accel_pitch, &accel_roll);

    float gyro_x = mpu_data[4];
    float gyro_y = mpu_data[5];
    float gyro_z = mpu_data[6];

    // --- Pitch 二阶互补 ---
    float err_pitch = accel_pitch - att->pitch; // 加速度计与当前估计的偏差
    comp2_x1_pitch = err_pitch * COMP2_K1;      // 比例修正
    comp2_x2_pitch += err_pitch * COMP2_K2 * dt; // 积分修正（消除稳态偏差）
    att->pitch += (gyro_y + comp2_x1_pitch + comp2_x2_pitch) * dt;

    // --- Roll 二阶互补 ---
    float err_roll = accel_roll - att->roll;
    comp2_x1_roll = err_roll * COMP2_K1;
    comp2_x2_roll += err_roll * COMP2_K2 * dt;
    att->roll += (gyro_x + comp2_x1_roll + comp2_x2_roll) * dt;

    // --- Yaw 仅陀螺仪积分 ---
    att->yaw += gyro_z * dt;
}


// ================================================================
//              方法③：卡尔曼滤波 (Kalman Filter)
// ================================================================
// 原理（简化版）：
//   预测阶段：用陀螺仪积分预测下一时刻的角度
//   更新阶段：用加速度计的测量值修正预测结果
//   卡尔曼增益 K 会自动调整：
//     - 如果加速度计噪声大 → K 变小 → 更信任陀螺仪
//     - 如果陀螺仪漂移大   → K 变大 → 更信任加速度计
//   最终得到"数学上最优"的角度估计
// ================================================================

void Filter_Kalman_Init(Kalman_t *kf) {
    kf->angle = 0.0f;
    kf->bias  = 0.0f;
    kf->P[0][0] = 1.0f;  kf->P[0][1] = 0.0f;
    kf->P[1][0] = 0.0f;  kf->P[1][1] = 1.0f;

    // 噪声参数（可根据实际传感器噪声调整）
    kf->Q_angle   = 0.001f;  // 角度过程噪声（越小 → 越信任陀螺仪预测）
    kf->Q_bias    = 0.003f;  // 零偏过程噪声
    kf->R_measure = 0.03f;   // 测量噪声（越小 → 越信任加速度计测量）
}

float Filter_Kalman_Update(Kalman_t *kf, float accel_angle, float gyro_rate, float dt) {
    // ========== 第一步：预测 (Predict) ==========
    // 用陀螺仪角速度预测当前角度（减去估计的零偏）
    kf->angle += dt * (gyro_rate - kf->bias);

    // 更新误差协方差矩阵（不确定性会随时间增大）
    kf->P[0][0] += dt * (dt * kf->P[1][1] - kf->P[0][1] - kf->P[1][0] + kf->Q_angle);
    kf->P[0][1] -= dt * kf->P[1][1];
    kf->P[1][0] -= dt * kf->P[1][1];
    kf->P[1][1] += kf->Q_bias * dt;

    // ========== 第二步：更新 (Update) ==========
    // 计算卡尔曼增益 K
    float S = kf->P[0][0] + kf->R_measure; // 创新协方差
    float K[2]; // 卡尔曼增益向量
    K[0] = kf->P[0][0] / S;
    K[1] = kf->P[1][0] / S;

    // 用加速度计测量值修正预测
    float y = accel_angle - kf->angle; // 创新（测量残差）
    kf->angle += K[0] * y;
    kf->bias  += K[1] * y;

    // 更新误差协方差矩阵（不确定性因修正而减小）
    float P00_temp = kf->P[0][0];
    float P01_temp = kf->P[0][1];
    kf->P[0][0] -= K[0] * P00_temp;
    kf->P[0][1] -= K[0] * P01_temp;
    kf->P[1][0] -= K[1] * P00_temp;
    kf->P[1][1] -= K[1] * P01_temp;

    return kf->angle;
}

// 封装：一次性计算 Pitch 和 Roll
void Filter_Kalman_GetAttitude(Kalman_t *kf_pitch, Kalman_t *kf_roll,
                                Attitude_t *att, float *mpu_data, float dt) {
    float accel_pitch, accel_roll;
    MPU6050_GetAccelAngles(mpu_data, &accel_pitch, &accel_roll);

    att->pitch = Filter_Kalman_Update(kf_pitch, accel_pitch, mpu_data[5], dt);
    att->roll  = Filter_Kalman_Update(kf_roll,  accel_roll,  mpu_data[4], dt);
    att->yaw  += mpu_data[6] * dt;
}


// ================================================================
//              VOFA+ JustFloat 协议输出
// ================================================================
// VOFA+ 的 JustFloat 协议：
//   直接发送 float 数据的二进制原始字节（小端序）
//   每组数据结尾追加 4 个字节的帧尾：0x00 0x00 0x80 0x7F
//   VOFA+ 会自动识别并画出波形
// ================================================================

void VOFA_SendData(float *data, uint8_t count) {
    // 帧尾标志（固定为 0x007F8000 的小端表示）
    static const uint8_t tail[4] = {0x00, 0x00, 0x80, 0x7F};

    // 发送所有 float 数据（每个 float 占 4 字节）
    HAL_UART_Transmit(&huart1, (uint8_t *)data, count * sizeof(float), 10);

    // 发送帧尾
    HAL_UART_Transmit(&huart1, (uint8_t *)tail, 4, 10);
}

---

3. 主函数 main.c — 四种滤波对比验证

/* USER CODE BEGIN Header */
/**
  ******************************************************************************
  * @file           : main.c
  * @brief          : Main program body
  ******************************************************************************
  * @attention
  *
  * Copyright (c) 2026 STMicroelectronics.
  * All rights reserved.
  *
  * This software is licensed under terms that can be found in the LICENSE file
  * in the root directory of this software component.
  * If no LICENSE file comes with this software, it is provided AS-IS.
  *
  ******************************************************************************
  */
/* USER CODE END Header */
/* Includes ------------------------------------------------------------------*/
#include "main.h"
#include "i2c.h"
#include "usart.h"
#include "gpio.h"

/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include "debug_config.h"
#include "Tmpu6050.h"
#include "mpu6050_filter_all.h"
/* USER CODE END Includes */

/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */

/* USER CODE END PTD */

/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */

/* USER CODE END PD */

/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */

/* USER CODE END PM */

/* Private variables ---------------------------------------------------------*/

/* USER CODE BEGIN PV */
// 四种滤波方法的姿态角结果
Attitude_t att_comp1;   // 一阶互补
Attitude_t att_comp2;   // 二阶互补
Attitude_t att_kalman;  // 卡尔曼

// 卡尔曼滤波器实例（Pitch 和 Roll 各一个）
Kalman_t kf_pitch, kf_roll;
// 时间步长（10ms = 100Hz）
#define DT  0.01f

// 选择当前验证的滤波方法（改这个数字切换 1/2/3）
#define FILTER_MODE  1   // 1=一阶互补, 2=二阶互补, 3=卡尔曼
/* USER CODE END PV */

/* Private function prototypes -----------------------------------------------*/
void SystemClock_Config(void);
/* USER CODE BEGIN PFP */

/* USER CODE END PFP */

/* Private user code ---------------------------------------------------------*/
/* USER CODE BEGIN 0 */
float mpu_value[7]={0};
/* USER CODE END 0 */

/**
  * @brief  The application entry point.
  * @retval int
  */
int main(void)
{

  /* USER CODE BEGIN 1 */

  /* USER CODE END 1 */

  /* MCU Configuration--------------------------------------------------------*/

  /* Reset of all peripherals, Initializes the Flash interface and the Systick. */
  HAL_Init();

  /* USER CODE BEGIN Init */

  /* USER CODE END Init */

  /* Configure the system clock */
  SystemClock_Config();

  /* USER CODE BEGIN SysInit */

  /* USER CODE END SysInit */

  /* Initialize all configured peripherals */
  MX_GPIO_Init();
  MX_I2C1_Init();
  MX_USART1_UART_Init();
  /* USER CODE BEGIN 2 */
  MPU6050_Init();
  // 保持静止平放 1 秒钟进行自动校准
  MPU6050_Calibrate();
  // 3. 初始化所有滤波器
  Filter_Complementary1st_Init(&att_comp1); // 一阶互补滤波器
  Filter_Complementary2nd_Init(&att_comp2); // 二阶互补滤波器
  Filter_Kalman_Init(&kf_pitch); // pitch 卡尔曼滤波器
  Filter_Kalman_Init(&kf_roll); // roll 卡尔曼滤波器
  att_kalman.pitch = 0;
  att_kalman.roll = 0;
  att_kalman.yaw = 0;
  float accel_pitch_raw, accel_roll_raw;
  /* USER CODE END 2 */

  /* Infinite loop */
  /* USER CODE BEGIN WHILE */
  while (1)
  {
    // 获取校准后的数据
    MPU6050_GetCalibratedData(mpu_value);
    // ===== 同时运行三种软件滤波算法 =====
    Filter_Complementary1st_Update(&att_comp1, mpu_value, DT);
    Filter_Complementary2nd_Update(&att_comp2, mpu_value, DT);
    Filter_Kalman_GetAttitude(&kf_pitch, &kf_roll, &att_kalman, mpu_value, DT);

    // ===== 加速度计直接算出的原始角度（未滤波，作为参考基准线）=====
    MPU6050_GetAccelAngles(mpu_value, &accel_pitch_raw, &accel_roll_raw);

    // ===== VOFA+ 可视化输出 =====
    // 发送 8 个通道的数据到 VOFA+ 同时显示对比：
    //   CH1: 加速度计原始 Pitch（有噪声，作为参考）
    //   CH2: 一阶互补 Pitch
    //   CH3: 二阶互补 Pitch
    //   CH4: 卡尔曼 Pitch
    //   CH5: 加速度计原始 Roll
    //   CH6: 一阶互补 Roll
    //   CH7: 二阶互补 Roll
    //   CH8: 卡尔曼 Roll
    float vofa_buf[8] = {
      accel_pitch_raw,      // CH1 原始加速度计 Pitch
      att_comp1.pitch,      // CH2 一阶互补 Pitch
      att_comp2.pitch,      // CH3 二阶互补 Pitch
      att_kalman.pitch,     // CH4 卡尔曼 Pitch
      accel_roll_raw,       // CH5 原始加速度计 Roll
      att_comp1.roll,       // CH6 一阶互补 Roll
      att_comp2.roll,       // CH7 二阶互补 Roll
      att_kalman.roll,      // CH8 卡尔曼 Roll
  };

    VOFA_SendData(vofa_buf, 8);

    // ===== 也可以切换为 printf 文字输出调试 =====
    // （如果用 VOFA+ 的 FireWater 协议，取消下面注释）
    // debug_printf("%.2f,%.2f,%.2f,%.2f\r\n",
    //        accel_pitch_raw, att_comp1.pitch, att_comp2.pitch, att_kalman.pitch);
    // debug_printf("%.2f,%.2f,%.2f,%.2f\r\n",accel_roll_raw,att_comp1.roll,att_comp2.roll,att_kalman.roll);

    HAL_Delay(10); // 10ms = 100Hz 采样率，与 DT=0.01f 对应
    /* USER CODE END WHILE */

    /* USER CODE BEGIN 3 */
  }
  /* USER CODE END 3 */
}

/**
  * @brief System Clock Configuration
  * @retval None
  */
void SystemClock_Config(void)
{
  RCC_OscInitTypeDef RCC_OscInitStruct = {0};
  RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};

  /** Initializes the RCC Oscillators according to the specified parameters
  * in the RCC_OscInitTypeDef structure.
  */
  RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE;
  RCC_OscInitStruct.HSEState = RCC_HSE_ON;
  RCC_OscInitStruct.HSEPredivValue = RCC_HSE_PREDIV_DIV1;
  RCC_OscInitStruct.HSIState = RCC_HSI_ON;
  RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
  RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE;
  RCC_OscInitStruct.PLL.PLLMUL = RCC_PLL_MUL9;
  if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
  {
    Error_Handler();
  }

  /** Initializes the CPU, AHB and APB buses clocks
  */
  RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK
                              |RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2;
  RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
  RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
  RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV2;
  RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1;

  if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_2) != HAL_OK)
  {
    Error_Handler();
  }
}

/* USER CODE BEGIN 4 */

/* USER CODE END 4 */

/**
  * @brief  This function is executed in case of error occurrence.
  * @retval None
  */
void Error_Handler(void)
{
  /* USER CODE BEGIN Error_Handler_Debug */
  /* User can add his own implementation to report the HAL error return state */
  __disable_irq();
  while (1)
  {
  }
  /* USER CODE END Error_Handler_Debug */
}
#ifdef USE_FULL_ASSERT
/**
  * @brief  Reports the name of the source file and the source line number
  *         where the assert_param error has occurred.
  * @param  file: pointer to the source file name
  * @param  line: assert_param error line source number
  * @retval None
  */
void assert_failed(uint8_t *file, uint32_t line)
{
  /* USER CODE BEGIN 6 */
  /* User can add his own implementation to report the file name and line number,
     ex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */
  /* USER CODE END 6 */
}
#endif /* USE_FULL_ASSERT */

---

4. VOFA+ 配置方法

步骤 1：打开 VOFA+ 软件

• 下载地址：https://www.vofa.plus

步骤 2：配置串口

• 选择你的 COM 口
• 波特率设置为你 CubeMX 里配置的值（常见 115200）

步骤 3：选择协议

• 点击右上角协议选择：JustFloat

步骤 4：开始接收

• 点击"开始"按钮，就能看到 8 条曲线实时滚动

步骤 5：通道命名（便于对比观察）

在 VOFA+ 的通道列表中，按顺序重命名为：

通道	名称	含义
CH1	Raw Pitch	加速度计原始角度（有毛刺噪声）
CH2	Comp1 Pitch	一阶互补滤波
CH3	Comp2 Pitch	二阶互补滤波
CH4	Kalman Pitch	卡尔曼滤波
CH5	Raw Roll	加速度计原始 Roll
CH6	Comp1 Roll	一阶互补 Roll
CH7	Comp2 Roll	二阶互补 Roll
CH8	Kalman Roll	卡尔曼 Roll

我将单片机水平放置后，可以看出：Pitch滤波

---

5. 四种滤波方法对比总结

不是绝对的，在我们的场景中，每个mpu6050的环境以及温度补偿都不一样，所以会有偏差，但是算法没有好坏，只有适合。还有参数的调节好坏

特性	一阶互补	二阶互补	卡尔曼	DMP
复杂度	⭐	⭐⭐	⭐⭐⭐	⭐（硬件自动）
CPU 占用	极低	低	中等	几乎为零
调参难度	只需调 α	调 K1, K2	调 Q, R	无需调参
抗震动	一般	较好	最优	好
长期漂移	小	更小	最小	无
适用场景	入门学习	平衡车	无人机/机器人	快速出成品

---

7. PID：让姿态"听话"的控制魔法

🎯 从"怎么把飞机摆平"说起

姿态解算告诉我们"飞机现在歪了多少"，但光知道不行，得让它自己摆正。这就是 PID 的活。

PID 是个"负反馈闭环"：不停地拿"期望值"和"当前值"比，算出差距（误差），再用差距去指挥电机：

▲ 图 5 · PID 负反馈闭环：期望值 − 测量值 = 误差 → PID → 被控对象 → 输出，输出再反馈回来比较

     期望角度 ──→ [误差e] ──→ [PID控制器] ──→ [电机/机体] ──→ 输出
                    ▲                                    │
                    └───────────── 负反馈 ◀───────────────┘
                              (姿态解算测出当前角度)

🎭 三个字母各是什么脾气？

• P（比例）：误差越大推得越猛，反应快。但光靠 P 消除不了最后那一点"稳态误差"，而且 P 太大容易"抖得像筛糠"。
• I（积分）：把误差攒起来，专门消灭那点"死活消不掉"的稳态偏差（比如飞机老往一边歪，靠 I 一直给个补偿力）。
• D（微分）：看误差变化得多快，有"预见性"，能刹车——抑制超调、让系统更快稳下来（就像开车快撞上前提前松油门）。

u(t) = Kp·e(t) + Ki·∫e(t)dt + Kd·(de(t)/dt)
        比例      积分累计        变化率

🧱 位置式 vs 增量式

// 位置式 PID：一次直接输出一个"绝对控制量"
out = Kp*err + Ki*errSum + Kd*(err - lastErr);

// 增量式 PID：输出"变化量"，再累加（天然带积分效果、更安全）
dOut = Kp*(err - lastErr)+ Ki*err+ Kd*(err - 2*lastErr + lastLastErr);
out += dOut;

特性	位置式 PID	增量式 PID
输出含义	最终绝对控制量	本次输出变化量 Δout
积分存储	保存全部历史误差总和 errSum	仅保存最近 3 次误差，无总累加
积分饱和风险	高	低
故障冲击	大，异常时输出直接跳变	小，只会小幅改动
变量数量	少	略多
典型用途	温控、阀门、定值控制	电机、小车、无人机运动控制

🥪 飞控用的是"串级 PID"（双层嵌套的三明治）

一个 PID 直接控角度通常不够稳。四轴飞控普遍用串级 PID：

      期望角度 ─▶ [外环·角度PID] → 期望角速度 ─▶ [内环·角速度PID] → 电机控制量
                        ▲                              ▲
                姿态解算角度                       陀螺仪实测角速度

• 外环（角度环）：管"稳"，输入角度误差，需要从加速度计积分出来，比较慢，输出"期望角速度"。
• 内环（角速度环）：管"快"，输入角速度误差（陀螺仪直接给），输出电机量。
• 内环快、外环稳，套在一起就又快又稳。

🔧 整定口诀：先内环，后外环。
① 内环只加 P → 加到出现高频抖动再退一点；② 加 D 抑制超调；③ 内环稳了再加外环 P；④ 最后加 I 消稳态误差。每次只改一个参数，用串口画波形判断。

---

8. 电机输出：PID 算出的数怎么变成"转多快"

📶 PWM：靠"占空比"说话

四轴电机（无刷电机 + 电调 ESC）靠 PWM 信号调速。PWM 就像用"手电筒快速闪灯"：高电平占的比例（占空比）越大，电机转得越快。

低速 (占空比25%)   ██░░░░░░██░░░░░░  高电平短 → 转速低
高速 (占空比75%)   ██████░░██████░░  高电平长 → 转速高

• 常见电调"油门脉宽"范围：1000~2000 μs。
• STM32 用定时器输出 PWM 即可，占空比就是"控制量 / 满量程"。

🔀 动力分配（Mixer）：一个控制量怎么分给 4 个电机？

PID 算出来的是"修正量"，得叠加到 4 个电机上。以 X 型机架为例（符号取决于机架和转向约定）：

// thr=基础油门；roll/pitch/yaw = 三个姿态环的输出
M1 = thr + pidRoll.out + pidPitch.out + pidYaw.out;
M2 = thr - pidRoll.out + pidPitch.out - pidYaw.out;
M3 = thr - pidRoll.out - pidPitch.out + pidYaw.out;
M4 = thr + pidRoll.out - pidPitch.out - pidYaw.out;
// 最后限幅到 [油门下限, 上限]，防止某路越界

🔄 为什么相邻电机要反着转？

先看真实四轴的机架布局（X 型），4 个电机分布在四角：

▲ 图 6 · X 型四轴机架布局示意：相邻电机反方向旋转，对角电机同向，用于抵消反扭矩

        M1(顺)        M3(逆)
           \          /
            飞控/IMU
           /          \
        M2(逆)        M4(顺)

因为要让 4 个电机的"反扭矩"互相抵消，飞机才不会自己原地打转。所谓"偏航"控制，正是靠"故意打破这种平衡"——让对角电机一增一减，飞机就转起来了。

---

9. 光流传感器：室内没 GPS 时靠"看图"定位

🖱️ 它其实就是个"倒着放的光学鼠标"

GPS 在室内没信号，那怎么知道飞机有没有水平漂移？答案：光流传感器。

▲ 图 7 · PMW3901 光流传感器模块（常搭配激光测距 VL53L1X 一起用），面向地面安装，原理类似光学鼠标

光流传感器 = 朝下的微型摄像头 + 图像协处理器，原理跟光学鼠标一模一样：

  光流传感器(朝下) ──▶ 连续拍地面 ──▶ 比较相邻两帧的"花纹位置"
                                         │
                                         ▼
                              像素位移 Δx, Δy（花纹动了几格）

📐 像素位移怎么变成"速度"？

关键公式（一定要理解）：

线速度 ≈ 像素位移 × 对地高度 × 比例因子 ÷ 帧间隔

为什么要有高度？ 因为光流测的其实是"角变化"。同样一个像素位移，飞机飞得越高，实际移动得越远。所以光流模块几乎都要搭配测距传感器（TOF/激光），否则测出的速度会有"高度误差被放大"的问题。

🤝 为什么还要融合 IMU（陀螺仪）？

单看光流，数据很"抖"，而且有个大麻烦：飞机晃动/倾斜时，地面画面也会"假动"（机体原地摆，地面明明没动，图像却变了）。

所以标准做法是把三者融合（互补滤波或 EKF）：

光流 Δx,Δy ──┐
测距 高度 h ──┼──▶ [融合: 抵消晃动 + 尺度换算] ──▶ 水平速度 vx,vy ──▶ [位置环PID] ──▶ 定点悬停
IMU 角速度 ──┘

• 用陀螺仪角速度抵消机身晃动带来的虚假光流；
• 用高度把像素换算成真实速度；
• 得到稳定的水平速度/位置后，喂给位置环 PID，实现室内"定点悬停"。

📊 主流光流传感器对比（据 PX4 文档）

传感器	分辨率	帧率	有效高度	接口	特点
PMW3901	30×30	~10000fps	80mm~∞	SPI/UART	低功耗、最流行，DJI Tello 同系
ADNS-9800	30×30	~6400fps	2~30mm	SPI	低空近距离
OV7725+MCU	640×480	~500fps	10cm~5m	SPI/I2C	高度范围大、需计算

---

10. 飞控全貌：把前面串成一条完整闭环

把 1~9 章串起来，就是一条完整的飞控数据链路：

┌─────────────┐    ┌──────────────────┐    ┌────────────────────┐    ┌─────────────┐
│ MPU6050     │    │ 姿态解算·滤波      │    │ 串级PID             │    │ 动力分配     │
│ (加速度+陀螺)│──▶│ Mahony/DMP/卡尔曼 │──▶│ 角度环+角速度环+位置环│──▶│ Mixer→4路PWM│
└─────────────┘    └──────────────────┘    └────────────────────┘    └─────────────┘
┌─────────────┐          ▲                        │                          │
│ 光流+测距    │──────────┘                        ▼                          ▼
└─────────────┘                              机体姿态/位置/速度 ◀──────── 4个电机
       │                                              │
       └─────────────── 反馈回传感器 ◀──────────────────┘

闭环一句话：传感器感受状态 → 解算/滤波算出姿态 → 串级 PID 决定怎么修正 → 动力分配把修正量分给 4 个电机 → 电机改变机体 → 传感器再感受新状态……如此循环，飞机才稳得住。

---

11. 新手学习路线 & 参考资源

🗺️ 由浅入深的学习顺序

1. 先点亮 MPU6050：I2C 读出原始数据，串口打印，确认通信 OK（第 2 章）。
2. 标定 + 换算：做零偏校准，把数据变成 g 和 °/s（第 3 章）。
3. 先算单轴角度：加速度算角度、陀螺仪积分，做一阶互补/卡尔曼，先出 Pitch 或 Roll（第 5、6 章）。
4. 上四元数解算：跑通 Mahony，或直接 DMP，拿到完整 Pitch/Roll/Yaw（第 4、6 章）。
5. 上 PID：先单环调稳一个角度，再升级串级 PID（第 7 章）。
6. 接电机：PWM 驱动电调，理解 Mixer 和电机转向（第 8 章）。
7. 加光流：读像素位移，与 IMU 融合出速度/位置，室内定点悬停（第 9 章）。

📚 参考的优秀网上教程

1. STM32Cube HAL 库 MPU6050 DMP 姿态解算（CSDN）— 寄存器读取与 DMP 移植
   https://blog.csdn.net/weixin_52916289/article/details/135142374
2. STM32 驱动 MPU6050 及四元数姿态解算（Zeeklog）— 驱动与读取代码
   https://zeeklog.com/stm32-wan-zhuan-iiczhi-qu-dong-mpu6050ji-zi-tai-jie-suan
3. MPU6050 姿态解算——Mahony 互补滤波（CSDN）— 算法原理推导
   https://blog.csdn.net/lqj11/article/details/107423334
4. MPU6050 滤波、姿态融合（一阶互补、卡尔曼）（博客园）— 单轴滤波代码
   https://www.cnblogs.com/qsyll0916/p/8030379.html
5. 谈谈 MPU6050 的数据融合：一阶滤波与卡尔曼滤波（CSDN）— 三种融合对比
   https://blog.csdn.net/zsn15702422216/article/details/52223799
6. STM32 MPU6050 姿态解算 Mahony 互补滤波算法（CSDN）— C 语言实现
   https://blog.csdn.net/weixin_44821644/article/details/116893943
7. 无人机算法之 PID（CSDN）— 单级/串级 PID 与调试
   https://blog.csdn.net/u012320127/article/details/104579607
8. 四轴无人机飞行控制原理（PID）（CSDN）— 电机 PWM 与动力分配
   https://blog.csdn.net/weixin_46697509/article/details/133017111
9. 四轴 PID 控制算法详解（单环 PID、串级 PID）（CSDN）— 参数整定
   https://blog.csdn.net/lovely_yoshino/article/details/92789955
10. PX4 官方文档：PMW3901 光流传感器 — 原理、参数与接线
    https://docs.px4.io/main/zh/sensor/pmw3901
11. PX4 光流技术实战教学（openvela/CSDN）— 光流与 IMU/EKF 融合
    https://openvela.csdn.net/694b91185b9f5f31781a3769.html
12. 无人机光流模块 Optical Flow 设置（CSDN）— PMW3901 配置
    https://blog.csdn.net/weixin_60324241/article/details/147270687

---

⚠️ 安全提醒：涉及飞行器的实验，请在合规场地、确保安全的前提下进行，并遵守当地无人机法规。教程中的代码与原理图为教学示意，正式项目请以芯片官方数据手册（MPU6050 / PMW3901 / STM32 参考手册）为准。