Hello,
I find many libraries for Python but none seem to make use of the DMP to get the quaternion. The one I found that uses the FIFO only looks at Accel and Gyro values.
So I have ported the C code for the MPU6050 to Python, from v6.12 of the MotionApp code, but I'm running into issues and i have several questions:
1) FIFO structure:
following dmp_read_fifo code in inv_mpu_dmp_motion.c I expect to see:
16 Bytes for Quaternion (4x4)
6 Bytes for Accel (3x2)
6 Bytes for Gyro (3x2)
4 Bytes for Gesture (4)
So I'd expect FIFO structure to look like:
[QW,QW,QW,QW,QX,QX,QX,QX,QY,QY,QY,QY,QZ,QZ,QZ,QZ][AX,AX,AY,AY,AZ,AZ][GX,Gx,GY,GY,GZ,GZ][G1,G2,G3,G4]
I'm not using Gesture at this point so I expect 28 bytes total (as per the code dmp_enable_feature which defines packet_length)
When enabling DMP_FEATURE_SEND_RAW_ACCEL and DMP_FEATURE_SEND_RAW_GYRO only, all is well, but when I activate DMP_FEATURE_LP_QUAT or DMP_FEATURE_6X_LP_QUAT, the FIFO doesn't match anymore:
After much experimenting I determined that the Accel data start at 24 Bytes from the beginning. Gyro data follows.
The first 24 Bytes include the Quaternion as far as I can tell, but i do not know in what order, and what are the extra 8 Bytes for.
So what I read seems to look like:
[QW,QW,QW,QW,QX,QX,QX,QX,QY,QY,QY,QY,QZ,QZ,QZ,QZ][00,01,02,03,04,05,06,07][AX,AX,AY,AY,AZ,AZ][GX,Gx,GY,GY,GZ,GZ][G1,G2,G3,G4]
if I only read the 28 Bytes defined by the packet_length, the FIFO is misaligned.
I have to read the 36 bytes (28 + 8 mystery Bytes) in 2 passes since the MAX_PACKET_LENGTH=32
Can someone point me to documentation that describes the structure of the FIFO with various features enabled?
2) Then I have questions about the order of the values in the quaternion: is it W,X,Y,Z or X,Y,Z,W? I see some other code that switches the W from end to front. My quaternion data is definitely not right, whether I start at Byte 0 or Byte 8 in the FIFO packet.
3) I'm also confused about the DMP_FEATURE_GYRO_CAL:
I tried enabling and disabling the feature, along with using DMP_FEATURE_SEND_CAL_GYRO or DMP_FEATURE_SEND_RAW_GYRO but I do not see any difference in the gyro data. What should I expect there?
PS: My Gyro and Accel data from the FIFO match the data retrieved from registers 0x3B through 0x48, so that part I thin is right.