企业官网建设流程全解析

简介：一份基于MPU6050内置DMP功能的姿态解算工程资源，面向无人机、机器人、智能穿戴等需要实时获取设备姿态角度的开发者，解决传统九轴融合算法复杂、易受万向锁困扰的痛点。包内共77个文件，以h头文件、c源码、o/axf/hex编译输出、uvprojx/uvoptx工程配置、sct分散加载脚本及txt说明文档为主，整体仅545KB，压缩包轻量，便于快速移植到Keil等常见嵌入式环境，且目录结构清晰，按模块组织易于检索。已有4899人浏览学习，适合刚接触MPU6050或希望缩短姿态解算开发周期的工程师。工程采用库函数版DMP方案，可直接读取四元数并输出pitch、roll、yaw，省去自行推导滤波与坐标转换矩阵的步骤；同时提供完整初始化流程与数据读取示例，涵盖固件加载、I2C通信、欧拉角输出等关键环节，可帮助读者理解DMP工作机制，并直接套用到实际硬件上验证使用。

1. DMP读取MPU6050姿态：让四元数自己从FIFO里长出来

第一次拿起MPU6050时，我的第一反应是先补数学：陀螺仪积分怎么消除漂移，加速度计怎么去噪，互补滤波怎么调系数。直到把DMP跑起来，才发现芯片内部的运动处理器已经把这一整条流水线算完了，主控只要读FIFO缓冲区，拿到的就是四元数和pitch、roll、yaw。这个项目的核心就是这一条最短路径：初始化MPU6050的DMP，读取它算好的姿态数据，全程不碰姿态解算算法。适合刚入门想做平衡车、云台、体感遥控器的人，也适合被滤波参数调到头大的老手——先把DMP用起来，再回头决定哪些东西值得自己算。

2. 硬件与协议：I2C接线、寄存器映射和DMP固件加载的完整链路

2.1 I2C接线和供电：别让AD0决定你的人生

MPU6050的管脚不多，但第一次接错电源或地址，容易在初始化阶段卡半天。我的常用接法是按下面这张表来：

注意AD0这件事。同一个I2C总线上如果有两个MPU6050，就要把其中一个AD0拉高，另一个拉低，否则地址冲突后一个也读不出来。我一般习惯把AD0接到GND，让地址固定为0x68，这样在I2C扫描时看到的设备地址是0x68，和寄存器WHO_AM_I读回的值保持一致，排查问题少一个变量。

电压是另一个容易翻车的地方。如果你的主控是3.3V逻辑，而传感器模块上有5V稳压电路，供电用5V没问题，但I2C上拉电阻可能被拉到5V，直接灌进3.3V主控的SCL/SDA引脚，轻则读数异常，重则烧引脚。判断方法是看模块上有没有电平转换芯片，没有的话就统一用3.3V供电。顺着这条思路，接线确认完，下一步就是寄存器层面的握手。

2.2 寄存器速查：DMP藏在FIFO后面的原因

MPU6050的I2C接口背后是一组寄存器，写入配置、读取数据都靠它们。DMP并不是一个独立I2C从设备，它就在芯片内部，主控通过寄存器把固件喂给它，然后DMP把姿态结果写进FIFO缓冲区，主控再从FIFO把这个结果读回来。所以整个链路的主角其实有两个：配置寄存器和FIFO。

常用寄存器就这几个，值得先背下来：

为什么说"不需计算"？因为当DMP使能之后，主控读FIFO拿到的不是原始加速度和角速度，而是DMP已经融合好的四元数。你可以把DMP理解成一个内置的算法协处理器，它把加速度计和陀螺仪的数据在一个很小的处理器上做积分、滤波、融合，最终输出可直接使用的姿态信息。主控要做的事，只是把FIFO内存搬到变量里。

验证I2C通没通，第一件事先读WHO_AM_I。我习惯写一段最简单的寄存器读取代码，确认设备ID再往下走：

int mpu_read_byte(uint8_t reg, uint8_t *val) { i2c_start(); i2c_write_byte(0x68 << 1 | 0); // 写地址，0x68左移一位并置写位 i2c_write_byte(reg); // 目标寄存器 i2c_start(); // 重启总线 i2c_write_byte(0x68 << 1 | 1); // 读地址，置读位 *val = i2c_read_byte(0); // 读回一个字节，0表示读完发NACK i2c_stop(); return 0; }

这段代码里，0x68 << 1 | 0是设备地址左移一位后拼上R/W位，I2C协议要求7位地址后跟读写位，所以0x68变成0xD0写地址和0xD1读地址。i2c_read_byte(0)最后一个参数表示读完这字节后发NACK，告诉从设备后面不用继续输出。很多初学者直接拿0x68当写地址，发现总线上没响应，原因就在这里。

读WHO_AM_I返回值如果不是0x68，先不要怀疑固件，回头查接线、查供电、查I2C地址。这个是整个项目里最基础的自检点。

2.3 DMP固件加载：把黑匣子点亮的过程

MPU6050芯片内部的DMP是一段可编程处理器，但上电后它里面没有可用的运动算法，需要主控把一段固件二进制块通过寄存器写入DMP的RAM，写入成功后DMP才开始干活。这个固件体积不小，通常由驱动库以常量数组的方式内置，我们不需要自己写。

DMP初始化流程在常见驱动库里一般可以缩成下面这个函数：

uint8_t dmp_init(void) { mpu_reset(); // 写PWR_MGMT_1，整颗芯片复位 delay(50); // 等复位完成 mpu_set_sleep(0); // 清除睡眠位，让芯片进入工作状态 mpu_set_clock_source(1); // 时钟源选内部PLL，陀螺仪时钟 mpu_set_sample_rate(200); // 内部采样率200Hz mpu_set_gyro_fsr(2000); // 陀螺仪量程 ±2000 dps mpu_set_accel_fsr(2); // 加速度计量程 ±2g dmp_load_firmware(); // 写入DMP固件并校验 dmp_set_output_frequency(50);// DMP输出频率50Hz dmp_enable(); // 使能DMP，输出进FIFO return 0; }

代码里的几个参数值得解释一下。mpu_set_clock_source(1)把时钟切到陀螺仪内部的PLL，DMP需要稳定的高频时钟源，用内部8MHz晶振容易在运行中产生频率偏差，进而让姿态漂移。mpu_set_sample_rate(200)是内部采样率，注意DMP输出频率和采样率是两回事，这里我先让传感器内部按200Hz采集，再让DMP按50Hz输出四元数，输出频率降一半，主控读FIFO的负载也小。mpu_set_gyro_fsr(2000)设的量程越大，刻度分辨率越粗，但能应付快速转动；普通手持设备设±1000已经够用，如果你只是做姿态显示，建议用±2000，少踩量程饱和的坑。

dmp_load_firmware()是耗时最多的一步，它要把固件数组一块块写入DMP。某些驱动库在这一步里会先算校验和，再逐块写入，每个块写完读回校验。如果你发现函数长时间卡在这里，常见原因是I2C速率太高，DMP写入对时序要求严格，把I2C时钟从400kHz降到100kHz再试，成功率会明显提升。另一个隐蔽问题是主控在写入DMP固件时被中断打断，造成写入超时，所以在调用dmp_load_firmware()之前最好把无关中断关掉，或者至少等一段时间保证没有高优先级中断来抢。

dmp_set_output_frequency(50)算的是DMP内部定时器分频，它与内部采样率、FIFO缓冲区大小都有关系。DMP输出频率太高，而主控读取不及时，FIFO就会溢出，溢出后新数据会覆盖旧数据，姿态看起来就像在乱跳。如果读到FIFO_COUNT一直等于0或设备ID正常但数据不更新，我第一步就是去查这个分频配置有没有生效。

到这里，DMP被点亮了，但代码刚走通一半。下一章我们把初始化到读取的完整代码拆细，看四元数到底是怎么从FIFO里变成小数的。

3. 核心代码拆解：一条从初始化到读取姿态的函数链

3.1 初始化DMP：从mpu_init到dmp_enable做了什么

前面已经看到了dmp_init的骨架，实际操作中还有一个容易被忽略的动作：清空FIFO并确认DMP的状态。我习惯把初始化分成两步，第一步是传感器基础配置，第二步是DMP专属配置：

void mpu6050_setup(void) { uint8_t dev_id = 0; mpu_read_byte(0x75, &dev_id); // 读WHO_AM_I if (dev_id != 0x68) return; // 设备不在线，直接报错 mpu_write_byte(0x6B, 0x80); // 写PWR_MGMT_1，整机复位 delay(100); mpu_write_byte(0x6B, 0x01); // 清除睡眠，时钟源选择PLL mpu_write_byte(0x19, 0x04); // SMPLRT_DIV，采样率1kHz/(1+4)=200Hz mpu_write_byte(0x1B, 0x18); // GYRO_CONFIG，量程±2000 mpu_write_byte(0x1C, 0x00); // ACCEL_CONFIG，量程±2g dmp_load_firmware(); // 写固件，失败返回非0 dmp_set_output_frequency(50); // DMP输出50Hz mpu_write_byte(0x6A, 0x04); // 使能FIFO，并允许DMP输出写入 dmp_enable(); // DMP正式启动 }

这里mpu_write_byte(0x6A, 0x04)值得单独说。0x6A是FIFO_EN寄存器，bit2对应DMP输出写入FIFO的开关，置1后DMP算好的数据才会进FIFO。很多人在初始化里漏了这一步，直接读FIFO永远读到0，而且不报错。这属于最典型的静默失败：所有函数返回正常，但数据就是不来。

dmp_load_firmware()和dmp_set_output_frequency(50)这两个函数依赖具体驱动库的实现，我不建议自己从寄存器层面去拼这些函数，因为DMP固件写入协议涉及专门的批量写入命令，手写容易出错。可以把它当成一个黑匣子，只要确认返回值为0就继续。如果真的不放心，可以在加载后读一下DMP相关状态寄存器，确认固件校验通过。

初始化完成后，最好主动读一次FIFO_COUNT，记下启动后有多少字节。如果它开始增长，说明DMP真的在写数据。这一步不费什么时间，却能让后续调试从“怀疑一切”变成“只怀疑解析逻辑”。

3.2 读取四元数：FIFO里的字节怎么变成小数

DMP输出四元数时，每个四元数分量是32位整数，按大端字节序排列在FIFO里。只输出四元数时，一包数据是16字节，顺序是q0、q1、q2、q3。实际驱动库中包里可能还带陀螺仪或加速度计数据，包长就不是16，而是更大，所以我习惯用一个动态包长变量，而不是写死。

读取并解析的代码长这样：

#define QUAT_RAW_SIZE 16 typedef struct { int32_t w, x, y, z; } quat_raw_t; int dmp_read_quaternion(quat_raw_t *q) { uint16_t fifo_count = 0; uint8_t buf[QUAT_RAW_SIZE]; mpu_read_byte(0x72, (uint8_t *)&fifo_count + 1); // FIFO_COUNT高字节 mpu_read_byte(0x73, (uint8_t *)&fifo_count); // FIFO_COUNT低字节 if (fifo_count < QUAT_RAW_SIZE) return -1; // 数据不够，等待下一轮 mpu_fifo_read(buf, QUAT_RAW_SIZE); // 从FIFO_R_W连续读 // 大端序转主机序 q->w = (int32_t)((buf[0] << 24) | (buf[1] << 16) | (buf[2] << 8) | buf[3]); q->x = (int32_t)((buf[4] << 24) | (buf[5] << 16) | (buf[6] << 8) | buf[7]); q->y = (int32_t)((buf[8] << 24) | (buf[9] << 16) | (buf[10] << 8) | buf[11]); q->z = (int32_t)((buf[12] << 24) | (buf[13] << 16) | (buf[14] << 8) | buf[15]); return 0; }

fifo_count是16位无符号整数，由两个寄存器拼出来，所以我在读的时候手动处理了大小端。这里有个常见的愚蠢错误：先读高字节还是先读低字节都无所谓，只要最后拼回原数就行，但如果你直接mpu_read_byte(0x72, &fifo_count)去读，会把低字节读进高位置，得到的结果就会差256倍，数据量看起来永远不对。

读到字节后，左移24位是取出最高字节。这里要注意C语言对有符号整数的移位行为，buf[0]如果是0xFF，它会被当作int的255，左移24位后变成负数，结果是正确的，因为DMP四元数本来就是有符号定点数，数值就可能是负的。所以不要把它换成无符号类型后再转，否则负数会变成异常大的正数，pitch和roll在静止时也会出现不可理喻的角度。

3.3 欧拉角转换：为什么这里依然“不需计算”

DMP输出的四元数是Q30格式定点数，也就是实际浮点值等于整数值除以2的30次方。要得到pitch、roll、yaw，还需要从这个四元数还原出欧拉角。这个还原只用三个三角函数，不需要做滤波，也不需要积分，所以项目的“不需计算”指的是不写姿态解算算法，而不是完全不做数学运算。

代码示例：

typedef struct { float q0, q1, q2, q3; } quat_float_t; typedef struct { float roll; float pitch; float yaw; } euler_t; euler_t quaternion_to_euler(quat_float_t q) { euler_t e; float q0 = q.q0, q1 = q.q1, q2 = q.q2, q3 = q.q3; e.roll = atan2f(2.0f * (q0 * q1 + q2 * q3), 1.0f - 2.0f * (q1 * q1 + q2 * q2)); e.pitch = asinf(2.0f * (q0 * q2 - q3 * q1)); e.yaw = atan2f(2.0f * (q0 * q3 + q1 * q2), 1.0f - 2.0f * (q2 * q2 + q3 * q3)); e.roll *= 57.29578f; e.pitch *= 57.29578f; e.yaw *= 57.29578f; return e; }

参数说明：atan2f返回弧度，所以乘上180/pi转成角度。roll的计算里分母是1 - 2*(q1^2+q2^2)，这个分母在pitch接近±90度时会趋向于0，最终让roll变得不稳定，这是欧拉角表示本身的万向锁问题，不是DMP算错了。如果你的应用需要在极限姿态下保持稳定，最好的做法是在整个控制链路里直接使用四元数，只在最后显示层转欧拉角。

另外说一句题外话：DMP输出四元数之前，它已经在内部做了加速度计修正和陀螺仪积分，所以你拿到的四元数是滤波后的结果。如果你自己从原始数据去写姿态解算，还要不断调互补滤波系数，而DMP把这些参数都固化在固件里，等于帮你避开了最玄学的那部分调参过程。这也是为什么这个项目值得拿来即用得原因。

4. 数据链路与验证：从FIFO字节流到可用的pitch、roll、yaw

4.1 数据包结构：一次FIFO读回什么内容

在只用四元数输出的配置下，FIFO包结构很简单。每个包16字节，四个分量按大端序排布：

DMP同时输出陀螺仪和加速度计原始数据时，包结构会在四元数后面追加对应数据段，整个包变成28甚至34字节。判断包长最简单的方法是调用驱动库提供的get_fifo_packet_size()函数，它返回的数值和FIFO_COUNT的增量一致。我在调试时习惯连续读两次FIFO_COUNT，两次读到的差值应该刚好等于一个包长，如果不等于，说明读取没有对齐，需要重新调用FIFO复位。

FIFO读取要注意一个细节：主控必须一次性连续读完整个包，中间不能被打断，否则包边界会错位，之后的四元数全部无效。所以在读取循环里，我一般先关中断再读FIFO，读完再开中断：

uint32_t flags = disable_irq_save(); dmp_read_quaternion(&q_raw); restore_irq(flags);

有人问为什么不靠INT引脚来做标记，因为DMP输出频率才50Hz，一个包16字节，I2C在400kHz下传输时间不到1毫秒，远短于20毫秒的包间隔。关闭中断这段时间完全不影响实时性，反而能保证包边界不丢。这是我在几个项目上验证过的最稳妥做法。

4.2 静态校准：拿到稳定姿态前先对齐零偏

DMP能直接输出姿态，但不代表你拿到的手感很准。上电后如果板子静止，但pitch、roll、yaw都停在某个偏置上，或者轻微漂移，大概率是陀螺仪零偏没有被校准。

做静态校准之前，先把板子水平放好，建议用双面胶固定在桌面上，不要用手扶着，手部微小颤动会被传感器放大。然后采集一段数据：

void calibrate_gyro_zero(void) { const int samples = 500; float sum_x = 0, sum_y = 0, sum_z = 0; int16_t gx, gy, gz; for (int i = 0; i < samples; i++) { mpu_read_raw_gyro(&gx, &gy, &gz); // 从原始陀螺仪寄存器读 sum_x += gx; sum_y += gy; sum_z += gz; delay(2); } gx_offset = (int16_t)(sum_x / samples); gy_offset = (int16_t)(sum_y / samples); gz_offset = (int16_t)(sum_z / samples); mpu_set_gyro_offset(gx_offset, gy_offset, gz_offset); // 写入偏移寄存器 }

这里采集的是原始陀螺仪寄存器，不是FIFO里的DMP输出。原因是DMP在输出四元数时已经对陀螺仪零偏做了一部分处理，但我们校准的目标是传感器原始层面的零偏。samples=500和delay(2)加起来约1秒，足够把高频噪声平均掉。mpu_set_gyro_offset写入偏移后，DMP下一次输出会自动把对应角速度偏置减掉。

静态校准后，我习惯等5秒再开始读姿态，因为DMP内部的姿态收敛需要时间。上电瞬间就往控制环里灌数据，相当于让PID一开始就看到一个乱跳的反馈量，整个系统容易抖起来。这个“预热时间”不是玄学，而是DMP内部状态重置后重新积分所需要的收敛过程，特别是yaw，初始参考方向要等陀螺仪稳定了才能定住。

4.3 动态验证：用手转板子时怎么判断数据可信

代码跑通后，第一步验证永远是静态看数值。把板子水平放平，读出当前roll和pitch，理论上应该接近0，误差在±1度以内。yaw则会保持一个恒定值，甚至有点小漂移都是可接受的。如果静态时pitch不停地在±5度之间晃，先怀疑传感器固定有没有松，再怀疑DMP输出频率和低通滤波配置。

动态验证不要一上来就做大幅度翻转。我的习惯是先绕z轴慢慢匀速旋转板子，观察yaw是否跟着连续变化。重点看两个东西：一是yaw是否出现过跳变，比如转到90度附近突然显示-270度；二是转到360度时yaw是否平滑回到0。如果出现±180度的跳变，说明解析代码在边界处没有做角度归一化，而不是DMP的问题。这种跳动在小角度云台控制里影响不大，但如果你想用yaw做航向显示，就需要下面这段归一化处理：

if (e.yaw > 180.0f) e.yaw -= 360.0f; if (e.yaw < -180.0f) e.yaw += 360.0f;

之后再快速绕x轴和y轴转动，观察roll和pitch的变化方向是否和实际转动方向一致。这里容易犯的错是坐标方向反了，因为不同安装方式下芯片正方向不同。如果你发现板子向右翻转，roll反而显示向左，不用改代码，把传感器安装方向反过来的万向支架调整一下，或者在显示层把roll取负就行。

一个验证数据是否可信的土办法：开启串口绘图，把四元数q0到q3同时输出，然后在手里来回晃动板子。四元数四个分量都应该连续变化，任何一个分量出现类似阶跃的突变，都说明FIFO读错包或字节序没对齐。一旦看到这个现象，FIFO清空再重新读一遍，基本会恢复。

5. 避坑指南：DMP常见的三个坑和寄存器级排查手记

5.1 第一个坑：DMP初始化成功但FIFO永远为空

现象：dmp_init()返回0，dmp_enable()也返回0，但读FIFO_COUNT始终是0，姿态数据完全不来。

原因：这是初始化顺序的问题。最常见的是FIFO_EN寄存器没有把DMP输出写进FIFO对应的bit打开，或者PWR_MGMT_1里的睡眠位被某个配置函数又写回了1。另一类原因是DMP时钟源不对，固件加载完成后DMP需要高精度时钟驱动，如果用内部RC时钟，DMP运行一段时间后会自动停摆。

解决：我按“先复位、再唤醒、后配置”的顺序重新检查PWR_MGMT_1。读取寄存器里bit6，确认睡眠位是0，bit2~bit0指向内部PLL或外部晶振。然后检查0x6A寄存器，bit2必须为1，bit4~bit5是FIFO模式位，一般保持默认。在重新使能DMP之后，手动写一次0x74（FIFO_R_W），用来清空FIFO，再读FIFO_COUNT，如果数值开始增长，问题就解决了。

5.2 第二个坑：静止状态下角度不稳，甚至缓慢漂移

现象：板子完全静止，roll和pitch能稳定在0度附近，但yaw以恒定速度往外飘，每分钟飘个3到5度。

原因：陀螺仪零偏没有完全消除，尤其DMP内部如果对零偏估计不准确，那个漂移就会叠加在yaw上。加速度计能修正roll和pitch，但原理上修正不了绕重力轴的yaw，所以yaw漂移是所有IMU都存在的问题，DMP只是把它降低了，不会归零。

解决：按第4.2节做完整的静态校准，把陀螺仪零偏写入偏移寄存器。如果校准后还漂，可能是温度变化导致零偏发生改变，这种情况需要做“上电时自动校准”，也就是每次启动后强制静置1秒，重新采样零偏。我习惯写一个状态机，在开机后先让系统停在“校准中”界面，等所有轴零偏采样完成再释放姿态数据。如果你想彻底解决长时间漂移，就只能外接磁力计做航向融合了，那是另一个项目的事。

5.3 第三个坑：读取yaw时偶尔出现跳变

现象：正常转动板子，yaw大部分时间连续，但偶尔从179度直接跳到-180度，或者从-180跳到179，中间没有任何过渡。

原因：欧拉角表示本身在±180度边界存在不连续，这是数学表示的限制，不是芯片坏了，也不是DMP固件错了。很多人在这一条上浪费了大量时间，甚至怀疑固件版本有问题。

解决：在做完四元数转欧拉角之后，马上做一次归一化，把yaw限制在-180到180。另外，如果控制环内部必须使用角度连续量，就不要直接用yaw，而是用四元数作为反馈量。我在云台项目里把yaw跳变处理放在显示层，控制层只用相对角度增量，跳变就不影响PID的输出。

5.4 排查工具：从寄存器读回开始有步骤地找问题

遇到DMP数据异常，不要两眼一抹黑地改代码，按下面节奏排查：

先做I2C扫描，确认设备地址是0x68还是0x69。如果扫描不到设备，那DMP再牛也没用，检查电源和接线。然后读WHO_AM_I，确认返回0x68，这一步能排除地址误判和芯片损坏。再读PWR_MGMT_1，确认bit6睡眠位是0，bit0~bit2时钟选择不是0。接着读FIFO_COUNT，确认数值在增长，增长速率是否和DMP输出频率匹配。最后才是读FIFO_R_W，抓一包数据出来打印。

如果FIFO_COUNT一直在增长，但主控读完一包后没减少，说明FIFO没有被正确读取，检查读取代码里有没有从0x74连续读取足够字节。如果FIFO_COUNT直接溢出回0，说明主控读得太慢，要么降低DMP输出频率，要么把I2C速率提上来。这里我建议先用100kHz I2C调通，再考虑提速，因为大部分“偶尔抽风”的DMP问题，都是I2C时序余量不足造成的，速度降低后症状会立即消失。

根据自己的排查经历说一句，DMP项目里八成问题出在初始化顺序和FIFO边界上，而不是固件本身。固件是别人写好并且大规模验证过的，不要轻易怀疑它。调试时每次只改一个配置变量，然后观察FIFO_COUNT和姿态输出，比在代码里到处加打印更有用。

6. 二次开发：把DMP姿态接进你的控制环

6.1 移植到自己的板子：改三个地方就能跑

DMP代码移植到新主控上，需要动的地方其实只有三个。第一是I2C底层，把i2c_start、i2c_write_byte这些函数替换成你自己平台的驱动接口，常见嵌入式平台都自带I2C外设库，只需要把这些函数映射好。第二是延时函数，初始化流程里delay(50)这类调用要换成你系统里的阻塞延时，如果跑在实时操作系统中，还要注意任务调度会不会打断DMP固件写入。第三是中断，如果想用INT引脚方式通知DMP数据就绪，需要配置外部中断并在回调里置一个标志位；如果用轮询FIFO，则不用动中断部分。

6.2 示例：用pitch做平衡车控制

DMP输出的pitch直接可以作为平衡环的反馈，省掉了自己滤波的环节。假设你的控制频率是100Hz，DMP输出50Hz，那么在两帧DMP数据之间保持上一次值即可。一个最小可用的控制骨架是这样：

void balance_control_loop(void) { static float pitch_ref = 0.0f; float pitch_out = get_dmp_pitch(); // 从DMP得到的pitch float error = pitch_ref - pitch_out; float output = kp * error + kd * (error - last_error) / dt; last_error = error; motor_set_voltage(output); }

参数说明：pitch_ref是目标角度，平衡车一般设为0。kp控制恢复力，太大会振荡，太小会站不稳。kd用来抑制角速度，DMP内部已经做了滤波，所以这里的kd不用像自己解算时调得那么激进。这个骨架里没有积分项，因为平衡车通常只用PD环就够，积分环节反而会导致低速持续振荡。

从那以后，我每次给新板子写MPU6050驱动，都会先跑一遍FIFO字节流回环测试，确认数据包长度和字节序对得上，再往控制代码里接。这个习惯帮我避开了至少三次“姿态乱跳”的玄学问题。希望帮到你。

本文还有配套的精品资源，点击获取