【求助帖】imu的姿态解算问题

想请问一下大家是怎么对mpu6500的原始数据进行姿态解算的：
目前我用的是读取加速度计和磁力计数据转化成四元数的方法，直接通过算法写的代码
但是目前一加上磁力计进去解算的四元数和姿态角就会乱飘。。。
想问问有没有用自带的dmp直接读取的呢？有没有代码什么的？
请教一下经验~谢谢大家{:3_59:}