简介一份基于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的管脚不多但第一次接错电源或地址容易在初始化阶段卡半天。我的常用接法是按下面这张表来MPU6050引脚主控侧说明VCC3.3V或5V大部分模块板载稳压芯片供电范围更宽裸芯片建议3.3VGNDGND主控和传感器必须共地否则I2C时序会出现诡异偶发错误SCLI2C时钟线接主控SCL模块板通常已有上拉电阻裸片需外接4.7kΩ上拉SDAI2C数据线接主控SDA同上AD0GND或3V3决定I2C地址接GND为0x68接3V3为0x69XCL / XDA不接连接外部磁力计用的辅助I2C本项目不涉及INT主控外部中断可选DMP数据就绪中断脚轮询FIFO时可以不接但接了能省CPU注意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。常用寄存器就这几个值得先背下来寄存器名地址作用WHO_AM_I0x75设备IDMPU6050固定返回0x68PWR_MGMT_10x6B复位、睡眠、时钟源选择DMP初始化第一步动它SMPLRT_DIV0x19采样率分频决定内部采样频率CONFIG0x1A数字低通滤波配置影响输出延迟和噪声GYRO_CONFIG0x1B陀螺仪量程和自检ACCEL_CONFIG0x1C加速度计量程和自检FIFO_EN0x6A往FIFO里喂数据的开关DMP的输出也走这里FIFO_COUNT0x72~0x73FIFO内尚未读取的字节数FIFO_R_W0x74FIFO数据读取口为什么说不需计算因为当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)把时钟切到陀螺仪内部的PLLDMP需要稳定的高频时钟源用内部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/(14)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^2q2^2)这个分母在pitch接近±90度时会趋向于0最终让roll变得不稳定这是欧拉角表示本身的万向锁问题不是DMP算错了。如果你的应用需要在极限姿态下保持稳定最好的做法是在整个控制链路里直接使用四元数只在最后显示层转欧拉角。另外说一句题外话DMP输出四元数之前它已经在内部做了加速度计修正和陀螺仪积分所以你拿到的四元数是滤波后的结果。如果你自己从原始数据去写姿态解算还要不断调互补滤波系数而DMP把这些参数都固化在固件里等于帮你避开了最玄学的那部分调参过程。这也是为什么这个项目值得拿来即用得原因。4. 数据链路与验证从FIFO字节流到可用的pitch、roll、yaw4.1 数据包结构一次FIFO读回什么内容在只用四元数输出的配置下FIFO包结构很简单。每个包16字节四个分量按大端序排布偏移内容类型说明0~3q0uint32/int32四元数实部4~7q1uint32/int32四元数x轴虚部8~11q2uint32/int32四元数y轴虚部12~15q3uint32/int32四元数z轴虚部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在输出四元数时已经对陀螺仪零偏做了一部分处理但我们校准的目标是传感器原始层面的零偏。samples500和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()返回0dmp_enable()也返回0但读FIFO_COUNT始终是0姿态数据完全不来。原因这是初始化顺序的问题。最常见的是FIFO_EN寄存器没有把DMP输出写进FIFO对应的bit打开或者PWR_MGMT_1里的睡眠位被某个配置函数又写回了1。另一类原因是DMP时钟源不对固件加载完成后DMP需要高精度时钟驱动如果用内部RC时钟DMP运行一段时间后会自动停摆。解决我按“先复位、再唤醒、后配置”的顺序重新检查PWR_MGMT_1。读取寄存器里bit6确认睡眠位是0bit2~bit0指向内部PLL或外部晶振。然后检查0x6A寄存器bit2必须为1bit4~bit5是FIFO模式位一般保持默认。在重新使能DMP之后手动写一次0x74FIFO_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睡眠位是0bit0~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直接可以作为平衡环的反馈省掉了自己滤波的环节。假设你的控制频率是100HzDMP输出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字节流回环测试确认数据包长度和字节序对得上再往控制代码里接。这个习惯帮我避开了至少三次“姿态乱跳”的玄学问题。希望帮到你。本文还有配套的精品资源点击获取
