I managed to get the mpu6000 fifo done and I now have pretty good gyro readings. It seems like I was getting 20-30 duplicate samples per second.
Here’s how the Ground Station sensor page looks like:
The bottom 3 sensors also have an average graphed (the blue line).
With the gyro fixed, I tested the dead reckoning and god pretty bad results.
Top graph is linear acceleration, middle one is velocity and bottom one position. They are reset every 10 seconds. So the bottom graph shows 2-3m of drift when the quad is perfectly still.
This indicated a mismatch between the quad rotation I’m calculating and the real rotation. So the gravity cancellation doesn’t work correctly.
This needs more work.