基于STM8读取MPU6050的dmp中的数据问题

BPT_YCC

想要获得MPU6050的姿态角数据，从mpu的数据手册上看到有dwp功能可以快速获得四元数及欧拉角。
但是从mpu6050的fifo中读出的数据不对。
我是参考http://bbs.cmnxt.com/thread-13197-1-1.html写的代码（不是用的库函数）。
有没有大虾能都指导一下不用dwp的库函数如何正确读出fifo中的内容