传感器_02：MPU6050 六轴 IMU 姿态入门——从一串看不懂的数，到一个不漂的角度

适用人群：已经能用 I2C 读通一个温湿度传感器，但第一次碰"运动传感器"的同学。你会调 i2c_master_transmit_receive()，可面对 MPU6050 吐出来的 14 个字节，完全不知道从哪下嘴。

没看过本专栏 01 篇也能读：01 篇讲的 SHT30/BME280 属于"读一个数、换算一下就完事"的环境传感器；这篇的 MPU6050 属于"读出来的数还得再算好几步才有意义"的运动传感器------这是两类传感器最大的分水岭，也是新手第一次感到"传感器怎么这么难"的地方。

读完你能得到：

① 彻底搞清 MPU6050 的 I2C 地址、WHO_AM_I、量程与灵敏度、采样率与 DLPF 各自管什么；

② 一份 ESP-IDF v5.x i2c_master 下能编过、每一步都查返回值的读取 + 换算代码；

③ 想明白"加速度计算出的角度"和"陀螺仪积分出的角度"为什么单独用都不行；

④ 一阶互补滤波的公式怎么来的 + 20 行代码，让角度既不抖也不漂；

⑤ 9 个我真踩过的坑，外加两个"故意写错看现象"的实验。

一、为什么 IMU 让人头大

先说我第一次用它的惨状。

大三下我想做个两轮自平衡小车，网上教程都说"用 MPU6050 读姿态角"。我买了块 GY-521 模块（就是那块蓝色的、六个引脚的小板子），照着某篇博客复制了初始化代码，串口一开，屏幕上唰唰滚出这样的东西：

text 复制代码

-284  132  16250  -37  12  -5
-290  128  16244  -41  9   -3
-276  140  16255  -35  14  -7

我盯着这六个数看了十分钟，脑子里全是问号：

哪个是角度？ 教程说"读出姿态角"，可我把板子摆平放着，没有任何一个数字长得像"0 度"。
16250 是什么单位？ 米？度？还是某种玄学计数？
我把板子竖起来，前三个数变了，后三个数只在转动的那一瞬间跳一下就回到零附近。 后三个数到底在测什么？

更崩溃的在后面。我按另一篇博客的做法，把后三个数"积分"起来当角度用，板子明明一动不动放在桌上，串口里的角度却像脱缰野马一样，一分钟漂出十几度，十分钟能跑出去一百多度。我当时的原话是："这传感器是不是坏了？"

传感器没坏，是我压根不知道自己在读什么。 这篇要解决的就是这三个具体困惑：

text 复制代码

  困惑一：读出来的 6 个数分别是什么？          → 第二、三章：量程 + 灵敏度 + 换算
  困惑二：为什么没有一个数直接是"角度"？        → 第四章：角度是算出来的，不是读出来的
  困惑三：为什么一积分就漂？                    → 第四章：零偏 + 积分的本质

1.1 一句话先扫盲：六轴到底是哪六轴

"六轴 IMU"（Inertial Measurement Unit，惯性测量单元）= 三轴加速度计 + 三轴陀螺仪。MPU6050 一次读回来的 6 个数，就是这两个器件各 3 个轴：

输出	器件	它到底在测什么	单位（换算后）	静止平放时的典型值
ax, ay, az	加速度计	比力（加速度 + 重力的合成）	g（1 g ≈ 9.8 m/s²）	ax≈0, ay≈0, az≈±1
gx, gy, gz	陀螺仪	角速度（转得多快）	°/s（度每秒）	三个都≈0

回头看我那串数：-284 132 16250 是加速度计原始值，16250 除以 16384 正好约等于 1.0------那就是重力，板子平放着，Z 轴正对天，测到 1 g。后三个 -37 12 -5 是陀螺仪，板子不转，所以在零附近晃（不是正好 0，这叫零偏，第四章的主角）。

⚠️ 这里有个几乎所有新手都会卡住的认知门槛，我用一句话点破：

加速度计和陀螺仪，谁都不直接输出"角度"。

加速度计输出"重力往哪个方向拽"，陀螺仪输出"转得多快"。角度是你自己算出来的中间量。教程里那句"读姿态角"是偷懒的说法，MPU6050 的寄存器里根本没有一个叫 ANGLE 的东西（DMP 除外，那是另一个话题，本文不碰）。

1.2 为什么还非得两个都要

只用一个行不行？先把结论摆这，第四章会用代码验证：

	加速度计	陀螺仪
算角度的原理	看重力矢量指向哪，反三角函数算倾角	角速度对时间积分
静止时	✅ 准，长时间不跑偏	❌ 零偏被积分，越积越偏
运动时	❌ 自身加速度混进来，算出的角完全不可信	✅ 快、跟手、不受平移影响
噪声表现	❌ 抖（振动直接进数据）	✅ 平滑
一句话	稳但只在静态时可信	动态好但一定会漂

看出来了吗？两个器件的缺点正好错开。 加速度计的缺点（动态不可信）恰好是陀螺仪的优点，陀螺仪的缺点（长期漂移）恰好是加速度计的优点。把两个的优点各取一半拼起来，就是第四章要讲的互补滤波------这也是为什么这两个器件会被封进同一颗芯片里卖。

二、MPU6050 基础：四件事必须先搞清

写代码之前，有四个东西必须先弄明白，否则后面每一步都会踩空。

2.1 I2C 地址：0x68 还是 0x69

MPU6050 的 7 位从机地址是 b110100X，最低位由 AD0 引脚 的电平决定（手册 7.10 节："The LSB of the I2C slave address is set by pin 9 (AD0)"）：

AD0 引脚	7 位地址	什么时候用
接 GND	0x68	默认，绝大多数情况
接 VCC	0x69	一根 I2C 总线上要挂两片 MPU6050 时

（常见的 GY-521 模块一般把 AD0 通过电阻下拉到 GND，所以悬空时也是 0x68；但别把"悬空=0x68"当成通用规律，以你手上模块的原理图为准------裸芯片悬空的 AD0 是浮空输入，行为不确定。）

7 位地址 vs 8 位地址 ，这个坑我在 01 篇讲 SHT30 时就踩过一次，这里再强调：0x68 是 7 位地址 ，实际在总线上跑的第一个字节是 (0x68 << 1) | R/W，也就是写 0xD0、读 0xD1。但是------

💡 ESP-IDF 的 i2c_master 驱动要你填的是 7 位原始地址，不要自己左移。

官方对 i2c_device_config_t::device_address 的说明是 "I2C device raw address. (The 7/10 bit address without read/write bit)" 。填 0x68 就对了，填 0xD0 只会得到一个 ESP_ERR_INVALID_ARG 或者永远 NACK。移位这件事驱动帮你干了。

如果你用的是 STM32 HAL，那正好相反：HAL_I2C_Mem_Read() 要的是已经左移过的 8 位地址 0xD0。两个平台约定不一样，换平台时这是第一个炸点。

2.2 WHO_AM_I：花 5 行代码省你 5 小时

WHO_AM_I（寄存器 0x75）是 MPU6050 的"身份证"。手册对它的描述有两句话特别值得抠：

"The contents of WHO_AM_I are the upper 6 bits of the MPU-60X0's 7-bit I2C address. The least significant bit ... is determined by the value of the AD0 pin. The value of the AD0 pin is not reflected in this register."

"The default value of the register is 0x68. Bits 0 and 7 are reserved. (Hard coded to 0)"

翻成人话，两个结论：

正常情况下，读 0x75 应该得到 0x68。
即使你把 AD0 拉高、器件地址变成 0x69，WHO_AM_I 读出来还是 0x68------因为它只包含地址的高 6 位，AD0 那一位不体现在这个寄存器里。所以千万别写成"地址 0x69 就该读回 0x69"，那是错的。

为什么值得专门花 5 行代码去校验？因为它能一次性区分开三类完全不同的故障：

text 复制代码

  i2c_master_probe() 就失败（NACK）  →  接线/供电/上拉/地址错，芯片根本没应答
  probe 通过但 WHO_AM_I 读回 0x00 或 0xFF →  通信通了但数据全错，多半是接触不良或电平不匹配
  WHO_AM_I 读回其它值（不是 0x68）   →  你买到的可能不是 MPU6050（市面上有标着 6050 实为兼容片的模块）
  WHO_AM_I == 0x68                   →  芯片认了，后面出问题就是你代码的事了

我当年就是跳过这一步，抱着"读数不对"的现象，在换算公式里查了整整一晚上，最后发现是杜邦线接触不良------SDA 那根线插松了，读回来的全是 0xFF。先证明芯片活着，再怀疑自己的算法，这个顺序能省掉大量无用功。

2.3 量程与灵敏度：一一对应，错配就差好几倍

这是全篇最容易出错、也最容易被教程含糊带过的地方。

**量程（Full Scale Range）**决定"最大能测多少"，由两个寄存器设置：

寄存器	地址	字段	位置	可选值
GYRO_CONFIG	0x1B	FS_SEL $1:0$	bit4:3	0→±250 °/s，1→±500，2→±1000，3→±2000
ACCEL_CONFIG	0x1C	AFS_SEL $1:0$	bit4:3	0→±2 g，1→±4 g，2→±8 g，3→±16 g

注意 FS_SEL / AFS_SEL 都在 bit4:3 ，不是 bit1:0。所以设置 ±500 °/s 要写 1 << 3 = 0x08，不是写 0x01。写错了的后果是------什么都不报错，量程还是默认的 ±250，然后你按 ±500 的系数去换算，读数直接差一半。

**灵敏度（LSB Sensitivity）**是量程的"影子"，两者严格一一对应，手册原表：

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

这几个数不用背，它们是算出来的，理解了就再也不会记错：

text 复制代码

  ADC 是 16 位有符号（补码），能表示 -32768 ~ +32767
  量程 ±2 g 意味着：+32768 这个满量程刻度 = +2 g
  所以 1 g 占多少个刻度 = 32768 / 2 = 16384 LSB/g   ✅ 和手册一致
  同理 ±250 °/s：32768 / 250 = 131.072 ≈ 131 LSB/(°/s)  ✅
  ±500 °/s：32768 / 500 = 65.536 ≈ 65.5                 ✅

换算公式因此就是一句除法（注意用 float，第三章细说精度）：

text 复制代码

  加速度 (g)   = 原始值 / 灵敏度      例：16250 / 16384.0f = 0.992 g
  角速度 (°/s) = 原始值 / 灵敏度      例：  -37 /   131.0f = -0.28 °/s

⚠️ 怎么选量程？ 别无脑拉满。量程越大，同样的 16 位 ADC 要覆盖更大范围，分辨率就越差 ：±2 g 时 1 个 LSB = 1/16384 g ≈ 0.061 mg；±16 g 时 1 个 LSB ≈ 0.49 mg，精度差了 8 倍。

我的经验取法：测静态倾角/姿态 → 加速度 ±2 g、陀螺 ±250 °/s （灵敏度最高，够用）；做自平衡车/云台这种会快速转动的 → 陀螺至少 ±500 甚至 ±1000 ，因为手一挥角速度就能过 250 °/s，一旦超量程数据会削顶（饱和在 ±32767），积分出来的角度直接失真。

2.4 采样率与数字低通 DLPF：两个互相牵扯的旋钮

这两个东西经常被教程一句"照抄就行"糊弄过去，但它们直接决定你的数据有多干净、以及你的积分周期该填多少。

先看 DLPF（Digital Low Pass Filter，数字低通滤波器） ，由 CONFIG 寄存器（0x1A）的 DLPF_CFG[2:0] 设置。手册原表（我把两栏合并了）：

DLPF_CFG	加速度带宽 / 延迟	陀螺带宽 / 延迟	陀螺输出率 Fs
0	260 Hz / 0 ms	256 Hz / 0.98 ms	8 kHz
1	184 Hz / 2.0 ms	188 Hz / 1.9 ms	1 kHz
2	94 Hz / 3.0 ms	98 Hz / 2.8 ms	1 kHz
3	44 Hz / 4.9 ms	42 Hz / 4.8 ms	1 kHz
4	21 Hz / 8.5 ms	20 Hz / 8.3 ms	1 kHz
5	10 Hz / 13.8 ms	10 Hz / 13.4 ms	1 kHz
6	5 Hz / 19.0 ms	5 Hz / 18.6 ms	1 kHz
7	保留	保留	8 kHz

读这张表要抓住一个权衡：带宽越低 → 数据越平滑，但延迟越大 。DLPF_CFG=6（5 Hz）时数据非常干净，可延迟接近 19 ms------对自平衡车来说这 19 ms 足以让它先摔一跤再告诉你"你歪了"。我一般选 DLPF_CFG=3（约 44 Hz，延迟 5 ms） 作为起点：既滤掉了电机振动的高频噪声，延迟又还能接受。

再看采样率 ，由 SMPRT_DIV（0x19）分频得到，手册公式：

text 复制代码

  Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV)

  其中 Gyroscope Output Rate =
       8 kHz  当 DLPF 关闭（DLPF_CFG = 0 或 7）
       1 kHz  当 DLPF 开启（DLPF_CFG = 1~6）

所以这两个旋钮是联动的，写代码时一定要一起看：

text 复制代码

  DLPF_CFG=3（1 kHz） + SMPLRT_DIV=9  →  1000/(1+9)  = 100 Hz  ✅ 常用
  DLPF_CFG=3（1 kHz） + SMPLRT_DIV=0  →  1000/(1+0)  = 1000 Hz
  DLPF_CFG=0（8 kHz） + SMPLRT_DIV=9  →  8000/(1+9)  = 800 Hz  ⚠️ 和你以为的 100 Hz 差 8 倍！

最后那行就是经典翻车现场：抄了别人 SMPLRT_DIV=9 的代码，却忘了人家 DLPF 开着而你关着，实际采样率变成 800 Hz。

💡 手册里还有一句容易漏掉的话："Note: The accelerometer output rate is 1kHz." 加速度计的内部输出率恒定 1 kHz ，跟陀螺的 8 kHz 无关。所以当你把采样率设到 1 kHz 以上时，同一个加速度样本会被重复输出多次------采样率拉再高，加速度数据也不会更新更快。想靠提高采样率来"提高加速度精度"是白费力气。

另外提醒：采样率 ≠ 你读取的频率 。采样率是芯片内部刷新寄存器的速度；你的 MCU 什么时候去读是另一回事。第四章互补滤波用的 dt，取决于你读取的实际间隔，不是这里设的采样率。这俩搞混是新手漂移的常见原因之一。

2.5 上电第一件事：把它叫醒

MPU6050 有一个非常"阴险"的默认状态。手册第 3 节写着：

"The reset value is 0x00 for all registers other than the registers below: Register 107: 0x40. Register 117: 0x68."

寄存器 107 就是 PWR_MGMT_1（0x6B），复位值 0x40 = 0b0100_0000，也就是 SLEEP 位（bit6）= 1，芯片开机默认在睡觉。

这时候会出现一个特别迷惑的现象：I2C 通信一切正常，WHO_AM_I 也能读回 0x68，但所有数据寄存器读出来全是 0。 很多人到这一步会怀疑传感器坏了，其实它只是没醒。

PWR_MGMT_1 的位定义：

bit7	bit6	bit5	bit4	bit3	bit2:0
DEVICE_RESET	SLEEP	CYCLE	保留	TEMP_DIS	CLKSEL $2:0$

CLKSEL 是时钟源选择，手册这段话很关键：

"Upon power up, the MPU-60X0 clock source defaults to the internal oscillator. However, it is highly recommended that the device be configured to use one of the gyroscopes (or an external clock source) as the clock reference for improved stability."

手册明确推荐用陀螺仪作为时钟参考 （CLKSEL=1，PLL with X axis gyroscope reference），因为内部 8 MHz RC 振荡器精度较差，而你后面要拿时间去做积分------时钟不准，积分自然不准。所以初始化时应该写 PWR_MGMT_1 = 0x01（SLEEP=0 且 CLKSEL=1），而不是无脑写 0x00。

三、把它读出来：ESP-IDF i2c_master 完整代码

3.1 接线（ESP32 DevKit + GY-521 模块）

text 复制代码

   GY-521 模块            ESP32 DevKitC
   ┌──────────┐          ┌──────────┐
   │ VCC ─────┼──────────┼─ 3V3     │   模块板载 LDO，接 3.3 V 或 5 V 都行
   │ GND ─────┼──────────┼─ GND     │   ★ 必须共地
   │ SCL ─────┼──────────┼─ GPIO22  │   多数模块板载 4.7 kΩ 上拉，以实物为准
   │ SDA ─────┼──────────┼─ GPIO21  │
   │ XDA      │          │          │   辅助 I2C（接磁力计用），本文不接
   │ XCL      │          │          │
   │ AD0 ─────┼── 接 GND │          │   → 地址 0x68（模块上有下拉时可悬空）
   │ INT      │          │          │   数据就绪中断，进阶用法，本文不接
   └──────────┘          └──────────┘

⚠️ 裸芯片和模块不一样 ：MPU6050 芯片本身的 VDD 范围是 2.375 V ~ 3.46 V （手册电气特性表），不能直接接 5 V。GY-521 模块之所以能接 5 V，是因为板上有个 LDO 帮你降到 3.3 V。如果你是自己画板直接焊裸片，5 V 接上去就是一颗芯片的钱。

3.2 需要用到的寄存器地图

地址	名字	干什么
0x19	SMPRT_DIV	采样率分频（手册寄存器表写作 `SMPRT_DIV`，公式里写作 `SMPLRT_DIV`，是同一个东西）
0x1A	CONFIG	DLPF 设置
0x1B	GYRO_CONFIG	陀螺量程（FS_SEL，占 bit4:3）
0x1C	ACCEL_CONFIG	加速度量程（AFS_SEL，占 bit4:3）
0x3B	ACCEL_XOUT_H	加速度 X 高字节（连续 6 字节到 0x40）
0x41	TEMP_OUT_H	温度高字节（2 字节）
0x43	GYRO_XOUT_H	陀螺 X 高字节（连续 6 字节到 0x48）
0x6B	PWR_MGMT_1	复位 / 唤醒 / 时钟源
0x75	WHO_AM_I	身份校验，应为 0x68

注意这三段数据（加速度 6 字节 + 温度 2 字节 + 陀螺 6 字节）在地址上是连着的 ：0x3B ~ 0x48 共 14 个字节。这不是巧合，是芯片设计者故意的------一次 burst read 全部读回来。手册明确说明了这么做的好处：

"...the user-facing read register set duplicates the internal register set's data values whenever the serial interface is idle. This guarantees that a burst read of sensor registers will read measurements from the same sampling instant. Note that if burst reads are not used, the user is responsible for ensuring a set of single byte reads correspond to a single sampling instant..."

翻译：一次 burst 读到的 14 字节保证来自同一个采样时刻；如果你分 6 次单独读，中间可能夹进新的采样，读到的 ax 和 az 就不属于同一瞬间了------静止时看不出来，一晃动算出来的角度就会莫名其妙地跳。所以下面代码坚持一次读 14 字节。

3.3 初始化：每一步都查返回值

先说环境：ESP-IDF v5.x 的新 i2c_master 驱动 （头文件 driver/i2c_master.h）。老的 driver/i2c.h 官方已明确标注废弃："the legacy driver is now deprecated and will be removed in future"，新代码别再用了。

c 复制代码

// mpu6050.c ------ 平台：ESP32 / ESP-IDF v5.x（新 i2c_master 驱动）
#include <math.h>
#include <stdbool.h>
#include <stdint.h>
#include <string.h>
#include "driver/gpio.h"
#include "driver/i2c_master.h"
#include "esp_log.h"
#include "esp_check.h"
#include "esp_timer.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"

static const char *TAG = "mpu6050";

#define MPU_I2C_ADDR        0x68        // 7 位原始地址，AD0 接地；不要自己左移
#define I2C_SDA_IO          GPIO_NUM_21
#define I2C_SCL_IO          GPIO_NUM_22
#define I2C_FREQ_HZ         100000      // 先用 100 kHz，稳了再考虑 400 kHz（芯片上限）
#define I2C_TIMEOUT_MS      100         // 有限超时，绝不写 -1（-1 = 永久等待）

#define REG_SMPRT_DIV       0x19
#define REG_CONFIG          0x1A
#define REG_GYRO_CONFIG     0x1B
#define REG_ACCEL_CONFIG    0x1C
#define REG_ACCEL_XOUT_H    0x3B
#define REG_PWR_MGMT_1      0x6B
#define REG_WHO_AM_I        0x75

#define WHO_AM_I_EXPECTED   0x68        // 手册：本寄存器默认值 0x68，且不反映 AD0

static i2c_master_dev_handle_t s_mpu = NULL;

static esp_err_t mpu_write_reg(uint8_t reg, uint8_t val)
{
    uint8_t buf[2] = { reg, val };
    return i2c_master_transmit(s_mpu, buf, sizeof(buf), I2C_TIMEOUT_MS);
}

static esp_err_t mpu_read_regs(uint8_t reg, uint8_t *out, size_t len)
{
    // 写寄存器地址 + 重复起始 + 读，中间不发 STOP ------ 这正是 transmit_receive 干的事
    return i2c_master_transmit_receive(s_mpu, &reg, 1, out, len, I2C_TIMEOUT_MS);
}

量程和灵敏度我强制写在一起，这是我被坑过之后养成的习惯------两个宏挨着放，改一个的时候眼睛一定会看见另一个：

c 复制代码

/* ★★ 这四行必须成套修改：改了量程，灵敏度必须同步改，否则读数差好几倍 ★★ */
#define GYRO_FS_SEL             0           // FS_SEL=0 → ±250 °/s
#define ACCEL_AFS_SEL           0           // AFS_SEL=0 → ±2 g
#define GYRO_SENS_LSB_PER_DPS   131.0f      // 必须对应 FS_SEL=0（±500 时改 65.5f）
#define ACCEL_SENS_LSB_PER_G    16384.0f    // 必须对应 AFS_SEL=0（±4g 时改 8192.0f）

初始化函数。注意 ESP_RETURN_ON_ERROR / ESP_RETURN_ON_FALSE 是 ESP-IDF esp_check.h 提供的宏，出错时自动打日志并 return------它不是"省略错误检查"，恰恰是强制你每一步都检查：

c 复制代码

esp_err_t mpu6050_init(i2c_master_bus_handle_t bus)
{
    /* 1) 把 MPU6050 挂到已建好的 I2C 总线上 */
    i2c_device_config_t dev_cfg = {
        .dev_addr_length = I2C_ADDR_BIT_LEN_7,
        .device_address  = MPU_I2C_ADDR,     // 填 0x68，不是 0xD0
        .scl_speed_hz    = I2C_FREQ_HZ,
    };
    ESP_RETURN_ON_ERROR(i2c_master_bus_add_device(bus, &dev_cfg, &s_mpu),
                        TAG, "add device failed");

    /* 2) 先探一探：芯片到底在不在总线上（能把"接线问题"和"代码问题"分开） */
    ESP_RETURN_ON_ERROR(i2c_master_probe(bus, MPU_I2C_ADDR, I2C_TIMEOUT_MS),
                        TAG, "probe 0x%02X 无应答：查接线/供电/上拉/AD0", MPU_I2C_ADDR);

    /* 3) 软复位：把可能被上次运行改乱的寄存器全恢复默认 */
    ESP_RETURN_ON_ERROR(mpu_write_reg(REG_PWR_MGMT_1, 0x80), TAG, "reset failed");
    vTaskDelay(pdMS_TO_TICKS(100));   // 手册说 DEVICE_RESET 位复位完成后自动清零，这里给足时间

    uint8_t pwr = 0xFF;
    ESP_RETURN_ON_ERROR(mpu_read_regs(REG_PWR_MGMT_1, &pwr, 1), TAG, "read pwr failed");
    ESP_RETURN_ON_FALSE((pwr & 0x80) == 0, ESP_ERR_TIMEOUT, TAG,
                        "复位未完成，PWR_MGMT_1=0x%02X", pwr);
    // 复位后 PWR_MGMT_1 应为 0x40（SLEEP=1），也就是芯片仍在睡觉

    /* 4) 校验身份：5 行代码，省 5 小时 */
    uint8_t who = 0;
    ESP_RETURN_ON_ERROR(mpu_read_regs(REG_WHO_AM_I, &who, 1), TAG, "read WHO_AM_I failed");
    ESP_RETURN_ON_FALSE(who == WHO_AM_I_EXPECTED, ESP_ERR_NOT_FOUND, TAG,
                        "WHO_AM_I=0x%02X，期望 0x%02X：这可能不是 MPU6050",
                        who, WHO_AM_I_EXPECTED);
    ESP_LOGI(TAG, "WHO_AM_I = 0x%02X, OK", who);

    /* 5) 唤醒 + 时钟源选 X 轴陀螺 PLL（手册 highly recommended，比内部 RC 稳） */
    ESP_RETURN_ON_ERROR(mpu_write_reg(REG_PWR_MGMT_1, 0x01), TAG, "wakeup failed");
    vTaskDelay(pdMS_TO_TICKS(10));    // 等 PLL 锁定再配置后面的寄存器

    /* 6) DLPF_CFG=3：加速度 44 Hz / 陀螺 42 Hz，延迟约 5 ms，陀螺输出率 1 kHz */
    ESP_RETURN_ON_ERROR(mpu_write_reg(REG_CONFIG, 0x03), TAG, "dlpf failed");

    /* 7) 采样率 = 1000/(1+9) = 100 Hz。★ 这一行的算法依赖上一行 DLPF 开着 ★ */
    ESP_RETURN_ON_ERROR(mpu_write_reg(REG_SMPRT_DIV, 9), TAG, "smprt failed");

    /* 8) 量程：注意 FS_SEL / AFS_SEL 在 bit4:3，所以要左移 3 位 */
    ESP_RETURN_ON_ERROR(mpu_write_reg(REG_GYRO_CONFIG,  (GYRO_FS_SEL   << 3)), TAG, "gyro cfg");
    ESP_RETURN_ON_ERROR(mpu_write_reg(REG_ACCEL_CONFIG, (ACCEL_AFS_SEL << 3)), TAG, "accel cfg");

    /* 9) 回读验证：写进去了不代表生效了（我遇到过接触不良导致写丢失） */
    uint8_t gc = 0xFF, ac = 0xFF;
    ESP_RETURN_ON_ERROR(mpu_read_regs(REG_GYRO_CONFIG,  &gc, 1), TAG, "readback gyro");
    ESP_RETURN_ON_ERROR(mpu_read_regs(REG_ACCEL_CONFIG, &ac, 1), TAG, "readback accel");
    ESP_LOGI(TAG, "GYRO_CONFIG=0x%02X ACCEL_CONFIG=0x%02X", gc, ac);

    return ESP_OK;
}

总线本身在 app_main() 里建一次就行（如果你 01 篇的 SHT30 也在这条总线上，总线只建一次，两个设备各 add_device 一次）：

c 复制代码

void app_main(void)
{
    i2c_master_bus_config_t bus_cfg = {
        .clk_source                   = I2C_CLK_SRC_DEFAULT,
        .i2c_port                     = I2C_NUM_0,
        .sda_io_num                   = I2C_SDA_IO,
        .scl_io_num                   = I2C_SCL_IO,
        .glitch_ignore_cnt            = 7,      // 官方例程的典型值
        .flags.enable_internal_pullup = true,   // 内部上拉较弱，模块自带 4.7k 时可不依赖它
    };
    i2c_master_bus_handle_t bus = NULL;
    ESP_ERROR_CHECK(i2c_new_master_bus(&bus_cfg, &bus));

    ESP_ERROR_CHECK(mpu6050_init(bus));

    /* attitude_task 见 4.3；实际工程里记得在 app_main 之前加前置声明或放进头文件。
       栈给 4096 字节而不是拍脑袋写 2048：任务里有 float 运算 + ESP_LOGI 的 %f 格式化，
       浮点格式化本身就要吃掉一两 KB 栈。栈够不够别猜，
       用 uxTaskGetStackHighWaterMark() 跑一会儿看剩余水位再定。 */
    xTaskCreate(attitude_task, "attitude", 4096, NULL, 5, NULL);
}

⚠️ xfer_timeout_ms 千万别填 -1。 ESP-IDF 文档写得很清楚："xfer_timeout_ms -- Wait timeout, in ms. Note: -1 means wait forever." 填 -1 意味着 SDA 一旦被从机拉死（I2C 总线锁死是真实存在的故障），你的任务就永久卡在这里，看门狗都不一定救得回来。给个 100 ms 的有限超时，出错了至少你知道它出错了。

3.4 一次读 14 字节 + 换算成物理量

c 复制代码

typedef struct {
    float ax, ay, az;   // 单位：g
    float gx, gy, gz;   // 单位：°/s
    float temp_c;       // 单位：℃（芯片内部温度，不是环境温度）
} mpu6050_data_t;

esp_err_t mpu6050_read(mpu6050_data_t *out)
{
    if (out == NULL) {
        return ESP_ERR_INVALID_ARG;
    }

    uint8_t buf[14];    // 0x3B ~ 0x48：accel 6 + temp 2 + gyro 6
    ESP_RETURN_ON_ERROR(mpu_read_regs(REG_ACCEL_XOUT_H, buf, sizeof(buf)),
                        TAG, "burst read failed");

    /* MPU6050 高字节在前（大端）：先拼成 uint16 再整体转 int16，
       不要把 buf 直接强转成 int16_t* ------ 字节序和对齐两个坑同时踩（见下文①） */
    int16_t ax_raw = (int16_t)(((uint16_t)buf[0]  << 8) | buf[1]);
    int16_t ay_raw = (int16_t)(((uint16_t)buf[2]  << 8) | buf[3]);
    int16_t az_raw = (int16_t)(((uint16_t)buf[4]  << 8) | buf[5]);
    int16_t t_raw  = (int16_t)(((uint16_t)buf[6]  << 8) | buf[7]);
    int16_t gx_raw = (int16_t)(((uint16_t)buf[8]  << 8) | buf[9]);
    int16_t gy_raw = (int16_t)(((uint16_t)buf[10] << 8) | buf[11]);
    int16_t gz_raw = (int16_t)(((uint16_t)buf[12] << 8) | buf[13]);

    /* 换算：原始计数 ÷ 灵敏度 = 物理量。除数写成 float 常量，别写整数除法 */
    out->ax = (float)ax_raw / ACCEL_SENS_LSB_PER_G;
    out->ay = (float)ay_raw / ACCEL_SENS_LSB_PER_G;
    out->az = (float)az_raw / ACCEL_SENS_LSB_PER_G;

    out->gx = (float)gx_raw / GYRO_SENS_LSB_PER_DPS;
    out->gy = (float)gy_raw / GYRO_SENS_LSB_PER_DPS;
    out->gz = (float)gz_raw / GYRO_SENS_LSB_PER_DPS;

    /* 手册给的温度公式：Temperature in degrees C = TEMP_OUT / 340 + 36.53 */
    out->temp_c = (float)t_raw / 340.0f + 36.53f;

    return ESP_OK;
}

这段代码里有三处细节值得单独拎出来说，都是我们 C 语言专栏讲过的东西在这里的具体落地：

① 字节序：为什么先拼 uint16_t 再转 int16_t

MPU6050 是高字节在前 （这一点手册的寄存器排布已经写死：ACCEL_XOUT_H 在 0x3B，ACCEL_XOUT_L 在 0x3C）。而 ESP32（Xtensa/RISC-V）和 STM32（ARM）都是小端，所以你不能把这 14 字节强转成 int16_t* 数组直接用------那样高低字节会颠倒，数据全是乱的。

c 复制代码

int16_t *p = (int16_t *)buf;   // ❌ 两个错误叠在一起

这行有两个问题：一是字节序反了 ；二是 buf 是 uint8_t 数组，只保证 1 字节对齐，强转成 int16_t* 再解引用属于未对齐访问，在有些平台上会直接 HardFault（对齐这事在《C语言_02_struct内存对齐详解》里展开讲过）。老老实实手动移位拼接，是唯一不会翻车的写法。

至于为什么先拼 uint16_t：buf[0] << 8 里 buf[0] 会先整型提升成 int，如果直接把结果丢给 int16_t，值超过 32767 时属于实现定义行为（GCC 明确按补码截断处理，所以能跑，但这属于"依赖编译器实现"）。先拼成 uint16_t 再转，逻辑上更清晰，也更容易在移植时不出事。字节序的完整讨论见《C语言_05_大端小端详解》。

② 为什么用 float 而不是 int

16250 / 16384 用整数除法算出来是 0 ，你就再也看不到 0.992 g 了。灵敏度常量必须带 .0f 后缀。

那 float 的精度够不够？float 有约 7 位十进制有效数字 ，而我们的原始数据本身只有 16 位（约 4.8 位十进制），换算后角度也就精确到小数点后一两位------float 绰绰有余，没必要用 double 。而且 ESP32 / ESP32-S3 带单精度 硬件 FPU，float 运算快；用 double 会退回软件仿真，慢好几倍。做姿态解算一律用 float 和 atan2f()、sqrtf() 这类 f 结尾的单精度函数。

⚠️ 平台差异 ：ESP32-C3 / C2 是 RISC-V 内核，没有 FPU ，所有浮点都靠软件模拟。在 C3 上跑 100 Hz 姿态解算问题不大，但如果你把频率拉到 1 kHz 又塞进一堆浮点，就要拿定时器实测一下耗时了。另外在 STM32 上用 GCC 编译时，用到 math.h 的函数记得链接 -lm（ESP-IDF 已经帮你链好了）。

③ 温度那个 36.53 是什么

手册的公式就是 温度(℃) = TEMP_OUT / 340 + 36.53（灵敏度 340 LSB/℃，35 ℃ 处偏移 -521 LSB，代进去正好对上）。但请注意：这是芯片自己的结温，不是环境温度 ，通常比室温高几度。它的实际用途不是测温，而是做零偏温度补偿------第四章会讲到，陀螺仪零偏会随温度漂。

3.5 跑起来看看

c 复制代码

static void imu_task(void *arg)
{
    mpu6050_data_t d;
    while (1) {
        esp_err_t err = mpu6050_read(&d);
        if (err != ESP_OK) {
            ESP_LOGW(TAG, "read failed: %s", esp_err_to_name(err));
            vTaskDelay(pdMS_TO_TICKS(100));   // 失败了别死循环狂刷，退一步再来
            continue;
        }
        ESP_LOGI(TAG, "a=[%6.3f %6.3f %6.3f]g  g=[%7.2f %7.2f %7.2f]dps  T=%.1fC",
                 d.ax, d.ay, d.az, d.gx, d.gy, d.gz, d.temp_c);
        vTaskDelay(pdMS_TO_TICKS(100));
    }
}

板子平放在桌上不动，串口应该看到类似这样的输出：

text 复制代码

a=[-0.017  0.008  0.992]g  g=[  -0.28    0.09   -0.04]dps  T=32.4C
a=[-0.018  0.008  0.991]g  g=[  -0.31    0.07   -0.02]dps  T=32.4C

现在这串数字终于有意义了：Z 轴 ≈ +0.99 g （重力，方向朝上的那个轴），X/Y ≈ 0，陀螺三轴 ≈ 0。能读到这个画面，说明你的驱动层已经通了，可以进入真正有意思的部分了。

💡 顺手做个自检：把模块任意角度 摆放，三轴加速度的模长 sqrt(ax²+ay²+az²) 应该始终 ≈ 1.0 g（静止时）。如果明显不是 1.0，那要么量程/灵敏度配错了，要么你读到的数据根本不对。这个"模长必为 1 g"的性质非常好用，后面判断数据可不可信也靠它。

四、从原始值到姿态：角度是算出来的

现在进入最有意思、也是新手最容易被"抄代码"糊弄过去的部分。我保证不讲卡尔曼滤波（那需要一整篇的篇幅，而且不理解互补滤波直接上卡尔曼纯属自虐），只讲两个基础方法和把它们缝起来的那一步。

4.1 加速度计算静态倾角：反三角函数登场

加速度计静止时测到的就是重力。重力方向是恒定的（永远指向地心），所以只要知道重力在三个轴上怎么分配，就能反推出板子相对水平面歪了多少。

先看最简单的情况------板子绕 X 轴滚转（roll，横滚角）：

text 复制代码

    平放（roll = 0°）              绕 X 轴倾斜 roll 角
  Z↑                              Z'↖      ↑ 真正的"上"
   │                                 ＼    │
   ├────► Y                            ＼  │
   板子水平                          板子倾斜 roll
                                        ＼
   az = 1 g   (重力全落在 Z 轴)      ay = g·sin(roll)   ← Y 轴分到一部分
   ay = 0                            az = g·cos(roll)   ← Z 轴剩下一部分
                                     两式相除：ay/az = tan(roll)

两式一除，g 被约掉了（这一点很妙：你不需要知道当地重力加速度到底是 9.79 还是 9.81），于是：

text 复制代码

  roll  = atan2(ay, az)                      ← 绕 X 轴的横滚角
  pitch = atan2(-ax, sqrt(ay² + az²))        ← 绕 Y 轴的俯仰角

为什么必须用 atan2 而不是 atan(ay/az)？ 两个理由，都是实打实会炸的：

	`atan(ay/az)`	`atan2(ay, az)`
输出范围	−90° ~ +90°	−180° ~ +180°
板子倒过来（az 为负）	分不出来，结果和正着一样	能正确给出大于 90° 的角
az = 0（板子竖直 90°）	除零，直接 inf/NaN	正常返回 ±90°

第二行就是我做云台时的翻车点：板子一翻过去，角度突然从 89° 跳到 -89°。atan2 拿两个参数分别看符号，能定位到正确的象限，这就是它存在的意义。

代码（ESP32 上一律用 atan2f/sqrtf 这些单精度版本）：

c 复制代码

#define RAD_TO_DEG   57.29577951f    // 180 / π

/* 由加速度计算静态倾角（单位：度）。前提：设备基本静止 */
static void accel_to_angle(const mpu6050_data_t *d, float *roll, float *pitch)
{
    *roll  = atan2f(d->ay, d->az) * RAD_TO_DEG;
    *pitch = atan2f(-d->ax, sqrtf(d->ay * d->ay + d->az * d->az)) * RAD_TO_DEG;
}

⚠️ 符号方向要自己实测确认一次。 各家模块丝印的 XYZ 方向、你把板子怎么装在设备上，都会影响正负号。做法很简单：把板子往一个方向倾斜，看 roll 是变正还是变负，不符合你的期望就在公式前加个负号。别对着别人的代码猜，动手倾一下 5 秒钟就能确定的事。

这套方法的三个硬伤，必须先说清楚，否则你会在后面莫名其妙：

只在静止（或匀速）时可信。 加速度计测的是比力 = 重力 + 自身运动加速度。设备一加速，公式就把"运动加速度"也当成重力算进去了。举个能想象的场景：把手机平放在正在起步的车上，加速度计会以为车"仰起来了"------这不是 bug，是原理决定的。
抖。加速度计对振动极其敏感，电机一转、桌子一敲，角度就跳几度。
算不出偏航角（yaw）。 板子绕重力轴 （Z 轴）水平旋转时，重力在三轴上的投影完全不变 ，加速度计根本感知不到。这是原理性的缺陷，不是算法问题。想要绝对的 yaw，六轴 IMU 做不到，必须加磁力计（九轴）或 GPS。 这句话请记住，面试被问到"六轴能不能测航向"时，答"不能，缺绝对参考"就是满分。

4.2 陀螺仪积分：跟手，但一定会漂

陀螺仪输出的是角速度（°/s），不是角度。想要角度，就得对时间积分。连续域是积分，到了单片机里就是一句累加：

text 复制代码

  θ(t) = θ(0) + ∫ ω dt        →  离散化  →   angle += gyro_dps * dt;

dt 是两次读取之间的实际间隔（秒）。请注意"实际"两个字：

c 复制代码

static float s_gyro_angle = 0.0f;    // 纯陀螺积分出来的角度（演示用）

static void gyro_integrate_demo(void)
{
    static int64_t last_us = 0;
    mpu6050_data_t d;

    if (mpu6050_read(&d) != ESP_OK) {
        return;   // 这次没读到就跳过，别拿旧数据硬积
    }

    int64_t now_us = esp_timer_get_time();   // 64 位微秒计数，不会几十天就回绕
    if (last_us == 0) {                      // 第一次进来只记时间，不积分
        last_us = now_us;
        return;
    }
    float dt = (float)(now_us - last_us) / 1000000.0f;   // µs → s
    last_us = now_us;

    s_gyro_angle += d.gx * dt;               // 绕 X 轴积分 → roll
}

⚠️ dt 必须实测，不能假设。 我见过太多代码直接写 angle += gyro * 0.01f;，理由是"我 vTaskDelay(pdMS_TO_TICKS(10)) 了呀"。但实际间隔 = 10 ms + I2C 传输耗时 + 任务被抢占的时间 + tick 量化误差，真实值可能是 11 ms 甚至 15 ms。这个误差会按比例 放进角度里：dt 少算 10%，角度就永远少 10%，而且随时间累加。用 esp_timer_get_time() 实测差值，这是一行代码就能消掉的系统性误差。

现在来看漂移。 把这段代码跑起来，板子放桌上一动不动，你会看到 s_gyro_angle 缓慢而坚定地往一个方向爬。原因就藏在第三章那串输出里：

text 复制代码

   静止时读到的陀螺值：  gx = -0.28 °/s   （理论上应该是 0.00）

这个"静止时的非零输出"叫零偏（Zero-Rate Output, ZRO） 。它是 MEMS 器件的固有特性，手册电气特性表里明明白白列着：Initial ZRO Tolerance, 25°C, ±20 º/s。也就是说，出厂就允许有零偏，这不是坏件。

零偏被积分之后有多可怕，算一下就知道：

text 复制代码

   零偏 0.28 °/s  ×  60 s   =  16.8°      （一分钟漂近 17 度）
   零偏 0.28 °/s  ×  600 s  =  168°       （十分钟漂出去 168 度）

对上了吧？这就是我开篇说的"板子不动，角度跑到一百多度"的全部真相------传感器没坏，是我把一个恒定的小误差积了十分钟。

第一步补救：开机零偏校准。 静止时采一批数据求平均，这个平均值就是零偏，之后每次读数都减掉它：

c 复制代码

typedef struct { float gx, gy, gz; } gyro_bias_t;

/* 开机校准：设备必须静止！采 N 次求平均作为零偏 */
static esp_err_t mpu6050_calib_gyro(gyro_bias_t *bias, int samples)
{
    if (bias == NULL || samples <= 0) {
        return ESP_ERR_INVALID_ARG;
    }

    float sx = 0.0f, sy = 0.0f, sz = 0.0f;
    int got = 0;

    for (int i = 0; i < samples; i++) {
        mpu6050_data_t d;
        if (mpu6050_read(&d) == ESP_OK) {
            sx += d.gx;  sy += d.gy;  sz += d.gz;
            got++;
        }
        vTaskDelay(pdMS_TO_TICKS(5));
    }

    /* 采样成功率太低说明总线有问题，别拿几个点算出来的假零偏去用 */
    if (got < samples / 2) {
        ESP_LOGE(TAG, "校准失败：%d/%d 次读取成功", got, samples);
        return ESP_FAIL;
    }

    bias->gx = sx / (float)got;
    bias->gy = sy / (float)got;
    bias->gz = sz / (float)got;
    ESP_LOGI(TAG, "gyro bias: %.3f %.3f %.3f dps (n=%d)", bias->gx, bias->gy, bias->gz, got);
    return ESP_OK;
}

用的时候：float wx = d.gx - bias.gx; 再拿 wx 去积分。校准 200 次约 1 秒，开机做一次，性价比极高。

但是------校准完还是会漂，只是慢一点。 原因有三层，一层比一层难对付：

漂移来源	说明	能不能治
初始零偏	出厂固有，静止时输出不为 0	✅ 开机校准可消掉大部分
零偏温漂	芯片发热后零偏跟着变（手册：ZRO 在 −40~+85 ℃ 内变化 ±20 °/s）	⚠️ 只能靠温度补偿缓解------这就是那个内部温度传感器的真实用途
随机游走 / 噪声	每次采样的随机噪声被积分后做随机游走	❌ 数学上无法根除

根本原因一句话：积分是"只进不出"的。 陀螺仪只知道"我转了多快"，它没有任何绝对参考能告诉它"我现在到底歪着多少度" 。任何误差进去了就永远留在结果里，时间越长错得越离谱。而加速度计恰恰有一个免费的绝对参考------重力。

这两句话摆在一起，答案就自己跳出来了。

4.3 一阶互补滤波：20 行让角度既不抖也不漂

把 4.1 和 4.2 的结论并排放：

text 复制代码

   加速度计算的角度：短期不可信（抖、受运动干扰），长期可信（重力永远在那）
   陀螺仪积分的角度：短期可信（平滑、跟手），      长期不可信（漂）
                     ↓
   短期听陀螺的，长期听加速度的 ------ 这就是"互补"

实现出来只有一行：

text 复制代码

  angle = α × (angle_prev + ω·dt)  +  (1 − α) × angle_acc

  其中：α 接近 1（典型 0.95 ~ 0.99）
        (angle_prev + ω·dt)  是陀螺积分给出的新角度   ← 占大头
        angle_acc            是加速度算出的角度        ← 占一点点，负责"拉回来"

为什么这一行就能同时解决两个问题？ 拆开看：

加速度那一项每次只掺进去 1−α（比如 2%），所以某一帧的抖动最多只能影响结果的 2%，高频噪声被大幅衰减 ，相当于给加速度做了个低通；
陀螺那一项每次都被乘以 α（0.98），累积的漂移每一轮都被"衰减"一点点，低频的漂移分量被削掉 ，相当于给陀螺做了个高通；
两个滤波器的权重加起来正好是 1，各管一段频率、互不重叠------这就是"互补"（Complementary）这个名字的由来。

α 到底怎么定？ 别再抄"0.98"了，它背后是一个有物理意义的量------时间常数 τ：

text 复制代码

   α = τ / (τ + dt)          反过来      τ = α · dt / (1 − α)

   例：α = 0.98，dt = 5 ms  →  τ = 0.98 × 0.005 / 0.02 = 0.245 s

τ 的含义：比 τ 短的时间尺度上，系统主要听陀螺仪的；比 τ 长的时间尺度上，逐渐交给加速度计接管。所以调参逻辑变得非常清晰：

想要的效果	τ 怎么调	对应的 α（以 dt = 5 ms 为例）	代价
角度更平滑、更抗振动	τ 调大（如 1 s）	α ≈ 0.995	漂移拉回得慢，长时间静止会残留误差
漂移拉回更快、更信重力	τ 调小（如 0.1 s）	α ≈ 0.95	抖动和运动加速度更容易穿透进来

我的起手参数：τ = 0.3 ~ 0.5 s，落在 100 Hz（dt = 10 ms）时 α ≈ 0.97 ~ 0.98。

💡 注意 α 和 dt 是绑定的。 同样一个 α = 0.98，在 dt = 5 ms 时 τ = 0.245 s，在 dt = 20 ms 时 τ = 0.98 s------行为差了 4 倍 。所以更稳妥的写法是：存 τ，每轮用实测 dt 现算 α。这样即使某次任务被抢占导致 dt 变长，滤波器的物理行为也不会跑偏。下面的代码就是这么做的。

完整实现：

c 复制代码

typedef struct {
    float   tau;        // 时间常数（秒），决定"信陀螺"和"信重力"的分界
    float   angle;      // 融合后的角度（度）
    int64_t last_us;    // 上次更新时刻
    bool    inited;     // 是否已用加速度做过初始对齐
} comp_filter_t;

static void comp_filter_init(comp_filter_t *f, float tau_s)
{
    f->tau     = tau_s;
    f->angle   = 0.0f;
    f->last_us = 0;
    f->inited  = false;
}

/* 每读到一帧数据调一次。gyro_dps 是已扣除零偏的角速度，acc_angle 是 4.1 算的静态倾角 */
static float comp_filter_update(comp_filter_t *f, float gyro_dps, float acc_angle)
{
    int64_t now_us = esp_timer_get_time();

    /* 第一次：直接用加速度的角度做初值，别从 0 开始慢慢收敛 */
    if (!f->inited) {
        f->angle   = acc_angle;
        f->last_us = now_us;
        f->inited  = true;
        return f->angle;
    }

    float dt = (float)(now_us - f->last_us) / 1000000.0f;
    f->last_us = now_us;

    /* 防御：任务被长时间挂起（比如刚做完 OTA）导致 dt 异常大时，不要让它一次积飞 */
    if (dt <= 0.0f || dt > 0.5f) {
        f->angle = acc_angle;    // 时间断层，直接用重力重新对齐
        return f->angle;
    }

    float alpha = f->tau / (f->tau + dt);        // 用实测 dt 现算 α
    f->angle = alpha * (f->angle + gyro_dps * dt) + (1.0f - alpha) * acc_angle;
    return f->angle;
}

把整条链路串起来的任务：

c 复制代码

static void attitude_task(void *arg)
{
    gyro_bias_t bias = {0};
    comp_filter_t roll_filter;

    /* 开机校准零偏：这 1 秒钟设备必须静止 */
    ESP_LOGI(TAG, "校准中，请保持静止 1 秒...");
    if (mpu6050_calib_gyro(&bias, 200) != ESP_OK) {
        ESP_LOGE(TAG, "零偏校准失败，姿态角不可信");
        vTaskDelete(NULL);
        return;                 // vTaskDelete(NULL) 不会返回，这行只是让静态分析闭嘴
    }
    comp_filter_init(&roll_filter, 0.4f);        // τ = 0.4 s

    while (1) {
        mpu6050_data_t d;
        if (mpu6050_read(&d) != ESP_OK) {
            vTaskDelay(pdMS_TO_TICKS(10));
            continue;
        }

        float acc_roll, acc_pitch;
        accel_to_angle(&d, &acc_roll, &acc_pitch);

        float wx = d.gx - bias.gx;               // ★ 别忘了减零偏
        float roll = comp_filter_update(&roll_filter, wx, acc_roll);

        ESP_LOGI(TAG, "acc=%6.2f  fused=%6.2f  (deg)", acc_roll, roll);
        vTaskDelay(pdMS_TO_TICKS(10));           // 约 100 Hz
    }
}

跑起来你会看到很爽的对比：用手指弹一下桌面，acc 那一列会跳好几度，fused 几乎纹丝不动；把板子慢慢倾斜 30° 停住，fused 会平滑跟过去并稳稳停在 30°，放一整天也不会漂走。

💡 和卡尔曼滤波什么关系？ 卡尔曼做的是同一件事------加权融合两个都不完美的信息源------区别在于卡尔曼会根据估计误差动态调整这个权重 ，而互补滤波的权重是你手工定死的。对入门项目（自平衡车、云台、翻倒检测），互补滤波的效果已经足够，而且它只有一个参数（τ），你调得明白它为什么变好变坏。先把互补滤波用透，再去看卡尔曼，会顺很多。

一个 20 行的进阶技巧：当 sqrt(ax²+ay²+az²) 明显偏离 1 g（说明设备正在剧烈运动，加速度不可信）时，把 α 临时调到接近 1（几乎只信陀螺），等模长回到 1 g 附近再放开。这就已经有点"自适应"的味道了。

五、新手必踩的 9 个坑

这张表里每一行我都亲自踩过，其中第 1、2 行各让我损失了一整个晚上。

#	坑	后果（现象长什么样）	正确做法
1	不校验 WHO_AM_I 就往下写	数据全 0 或全 0xFF，然后你在换算公式和算法里查一整晚，其实是线松了	初始化必读 `0x75`，应为 `0x68`；再配一次 `i2c_master_probe()`，把"接线问题"和"代码问题"彻底分开
2	量程改了，灵敏度没跟着改	不报任何错，读数稳定地差 2/4/8 倍。最阴的是"看起来挺像那么回事"，只是数值不对	量程宏和灵敏度宏写在相邻两行并加醒目注释；初始化后回读 `GYRO_CONFIG`/`ACCEL_CONFIG` 打印确认
3	忘了写 PWR_MGMT_1，芯片还在睡	WHO_AM_I 能读对，但所有数据寄存器恒为 0------最容易被误判成"传感器坏了"	复位（`0x80`）→ 等 100 ms → 写 `0x01`（SLEEP=0 且 CLKSEL=1，手册推荐用陀螺 PLL 做时钟）
4	把角速度当角度直接用	板子静止时"角度"是 0，一转动数值猛跳，松手又回 0------因为你显示的根本是"转多快"不是"转多少"	角速度必须积分才是角度；而积分一定要配零偏校准和融合，否则不可用
5	积分漂移没处理（不校准零偏、不融合）	设备一动不动，角度匀速爬升，十分钟漂出一百多度	开机静止采 200 次求平均扣零偏；再用互补滤波让重力持续把它拉回来
6	`dt` 用"我以为的"值	角度总是偏小或偏大一个固定比例，怎么调参都对不准	用 `esp_timer_get_time()` 实测两次采样的时间差；`vTaskDelay(10ms)` ≠ 实际 10 ms
7	I2C 时钟拉太快 / 线太长	偶发读到 `0xFF`、偶发 `ESP_ERR_TIMEOUT`，越晃动越容易出错	MPU6050 上限 400 kHz （手册明确值），杜邦线连接时先用 100 kHz 跑通；线尽量短、必须共地、确认有 4.7 kΩ 上拉
8	分 6 次单字节读，而不是一次 burst	静止时看不出问题，一运动算出来的角度莫名跳变	一次读 `0x3B` 起的 14 字节。手册保证 burst read 内的数据来自同一采样时刻
9	原始 LSB 值直接送云 / 存数据库	云端拿到 `16250` 这种数，脱离量程配置后完全无法解释；哪天你把量程从 ±2 g 改成 ±4 g，历史数据全部作废	在设备端就换算成物理单位（g、°/s、°）再上传；实在要传原始值，就必须把量程配置一起传上去

有三个我想再展开说两句，因为它们的"错误现象"特别有迷惑性。

坑 2 展开：为什么量程错配这么难发现

假设你把陀螺量程设成了 ±2000 °/s（FS_SEL=3），但换算还在除 131：

text 复制代码

   真实角速度 100 °/s
   → 芯片按 16.4 LSB/(°/s) 输出原始值 1640
   → 你按 131 换算：1640 / 131 = 12.5 °/s
   → 屏幕上显示 12.5，而真实值是 100，差 8 倍

关键在于：它不会报错，数据也不跳变，看上去平滑又"合理" 。你只有在拿量角器比对时才会发现不对。我当时的表现是拼命调互补滤波的 α，以为是滤波参数不好------完全找错了方向。换算系数错的时候，任何滤波调参都是白费力气。

坑 4 展开：角速度和角度到底差在哪

这个概念混淆值得画一下：

text 复制代码

   把板子从 0° 匀速转到 90°，用了 2 秒：

   角度  θ:   0° ────────────── 90°      转完就停在 90°，不回去
                  慢慢爬升

   角速度 ω:   0 ─┐         ┌─ 0         只在"转的过程中"非零
                  └─ 45°/s ─┘            转完立刻回 0

所以如果你屏幕上的"角度"在你停手之后回到了 0，那它 100% 是角速度不是角度。这个自检 3 秒钟就能做完。

坑 9 展开：原始值上云为什么是个坑

这是我在做课设时真实吃过的亏。我图省事把 ax_raw 直接 MQTT 发到服务器，想着"反正后面再算"。两周后我把加速度量程从 ±2 g 改成了 ±4 g（因为要测更剧烈的运动），结果数据库里前两周的数据和后面的数据用的是不同刻度，而记录里没有任何字段能区分它们------那批数据基本废了。

⚠️ 一条通用的教训，不止适用于 IMU：跨越"设备边界"的数据，必须是自解释的。 要么带单位（换算成 g / °/s），要么把量纲信息（量程配置）一起带上。设备内部你怎么用 LSB 都行，一旦要出网关、进数据库、给别的模块用，就得是人和程序都能无歧义理解的物理量。

六、动手练一练

前两个是常规验证，第三、四个是故意把代码写错------我强烈建议你做这两个，因为"见过错误现象长什么样"比"知道正确写法"更能救你。等哪天你在别人的代码里看到同样的现象，5 秒钟就能定位。

练习 1｜跑通并做模长自检（15 分钟）

照第三章把驱动跑起来，打印 ax, ay, az 和模长 sqrtf(ax*ax + ay*ay + az*az)。然后：

平放 → az ≈ ±1.0，其余 ≈ 0；
立起来（侧放、竖放各试一次）→ 1 g 会"转移"到另一个轴上；
无论怎么摆，静止时模长都应该 ≈ 1.00 g。

如果模长不是 1.0（比如 0.5 或 8.0），先别往下做，回去查量程和灵敏度是不是配错了------那个数字通常就是 2 的整数次幂倍。

练习 2｜零偏到底有多大（10 分钟）

跑一次 mpu6050_calib_gyro(&bias, 200)，把打印出来的三个零偏值记下来。然后：

用手捂住芯片 30 秒（模拟发热），再校准一次，对比两次的零偏差多少 → 你会亲眼看到温漂；
用你测到的零偏值，手算"如果不校准，积分 10 分钟会漂多少度"（零偏 × 600）。这个数字会让你对第四章的结论印象深刻得多。

练习 3｜【故意试错】不换算、不校准，直接积分（20 分钟）

分两小步，观察两种完全不同的"乱飞"：

c 复制代码

/* 步骤 a：连灵敏度都不除，直接拿原始 LSB 积分 ------ 错得最离谱的版本 */
angle_bad += (float)gx_raw * dt;        // ❌ 单位是 LSB·s，不是度

/* 步骤 b：换算了，但没减零偏 ------ 错得最隐蔽的版本 */
angle_drift += d.gx * dt;               // ❌ 少了 - bias.gx

预期现象（先自己预测，再上板验证，这一步别跳过）：

版本	静止时的现象	为什么
a：原始 LSB 直接积分	角度以每秒几十的速度狂涨，几秒就冲到几千、上万	数值被放大了 131 倍（±250 °/s 量程），一个 0.28 °/s 的零偏变成了每秒 37 个单位
b：换算了但没扣零偏	角度缓慢而平稳地单向爬升，一分钟十几度	只剩零偏在积分，这才是真正的"漂移"长相
c：校准 + 互补滤波（正确版）	静止时稳在一个值不动，倾斜后跟过去也稳住	重力在持续把积分误差拉回来

记住 b 的样子 ：不抖、不跳、匀速单向爬升。以后你在任何项目里看到一个物理量呈现这个形态，第一反应就该是"某个地方在积分一个没扣掉的常数偏置"。

练习 4｜【故意试错】把灵敏度系数改错（10 分钟）

保持 GYRO_CONFIG 不变（还是 ±250 °/s），只把换算常量改掉：

c 复制代码

// #define GYRO_SENS_LSB_PER_DPS   131.0f     // 正确：对应 FS_SEL=0
#define GYRO_SENS_LSB_PER_DPS      16.4f      // ❌ 故意用 ±2000 °/s 的系数

拿手匀速转动板子，对比修改前后的读数：同一个动作，数值会差整整 8 倍 （131 ÷ 16.4 ≈ 8）。加速度那边同理，把 16384.0f 改成 2048.0f，平放时的 az 会从 0.99 g 变成 7.9 g------一个静止放在桌上的板子测出 8 个 g，这在物理上是荒谬的，但代码不会给你任何警告。

这个练习想让你记住的是：换算系数是"沉默的错误"，它不会崩溃、不会报错，只会让你的所有后续计算都错得很平静。所以第五章那条"量程宏和灵敏度宏写在相邻两行"才不是强迫症。

练习 5｜调 τ，感受权衡（20 分钟）

把 comp_filter_init(&roll_filter, 0.4f) 里的 τ 依次改成 0.05f、0.4f、3.0f，每次都做两个动作：

用手指弹一下桌面（制造振动）→ 观察角度抖不抖；
把板子倾斜到 30° 保持住 → 观察多久能稳定到 30°，以及静置 2 分钟后会不会慢慢跑偏。

你会得到一张自己实测的表：τ 小的抗漂但抗不了抖，τ 大的平滑但拉不回来。这个权衡没有标准答案，取决于你的设备振不振动、动得快不快------这正是调参这件事真正的样子。

练习 6｜小挑战：做一个翻倒检测（选做）

用融合后的 roll/pitch 实现：任意一个角的绝对值 持续超过 45° 达 1 秒，就点亮 LED（或串口报警）。要点有两个：

必须"持续 1 秒"而不是"瞬间超过"，否则手一晃就误报------这个"去抖"思路和按键消抖是一回事；
想想为什么这个功能不能只用加速度计做（提示：设备翻滚过程中加速度计算出来的角是不可信的）。

做完这个，你就有了一个能写进简历、能在面试里讲清楚原理的小功能了。

小结

回到开头那三个困惑，现在应该都有答案了：

开头的困惑	答案
6 个数分别是什么？	前 3 个是加速度（÷16384 → g），后 3 个是角速度（÷131 → °/s）。量程和灵敏度必须成对
为什么没有一个数是"角度"？	角度不是读出来的，是算出来的。加速度用 `atan2` 反推倾角，陀螺靠积分
为什么一积分就漂？	零偏被积分。积分只进不出，陀螺没有绝对参考；而加速度计有------重力

三句话带走：

加速度计稳但只在静态可信，陀螺仪动态好但一定漂------它们的缺点正好错开，所以要融合。
互补滤波 angle = α(angle + ω·dt) + (1−α)·angle_acc ，α 由时间常数 τ 决定（α = τ/(τ+dt)），τ 以内信陀螺、τ 以上信重力。
六轴 IMU 测不了绝对偏航角，因为绕重力轴转动时重力投影不变。要 yaw 得上磁力计或 GPS。

还有一条不只属于 IMU 的经验：遇到"读数不对"，先按 芯片在不在 → 配置对不对 → 换算对不对 → 算法对不对 的顺序往下查。 我早期所有浪费掉的通宵，无一例外都是跳过了前两步，直接从第四步开始怀疑人生。

文中的寄存器地址、灵敏度表和 ESP-IDF I2C 用法都来自 MPU6050 的 Register Map / Datasheet 和 ESP-IDF 官方文档，具体数值以你手上的型号为准。

下一篇：传感器_03_电机驱动步进舵机直流与驱动电