The ideal way to read data from the mpu is with an interrupt. I cannot do this as the interrupt pin is not connected on the crius board. So I’m polling the imu at 1Khz hoping the data is ready but as it turns out, the avr and the imu clocks don’t exactly match. For this reason I sometimes get the same sample twice in a row or skip a sample. This results in errors in the gyro integration – a 90 degree rotation in one axis is interpreted as a 89 or 91 degrees for example.
FIFO to the rescue. The mpu6000/6050 has a built in fifo that you can interrogate regularly. By polling the fifo counter I know if enough data has arrived and read it – or if not enough just wait for it.
The problem is that the FIFO – being a First In First Out buffer – can be read only from the back – the oldest data. And if the buffer overflows (it has 1024 bytes) the oldest data gets discarded.
For this reason I have to make sure that the data packet size is a divisor of 1024 so that when a new packet arrives and overflows the 1024 bytes buffer, it discards one entire packet and not a fragment only.
My packet has 14 bytes right now – 6 for the accelerometer, 6 for the gyro and 2 for temperature – so whenever there is an overflow I get corrupted data from the fifo as I’m not reading whole packets anymore – but some data from one and the rest from the next one..
My temporary solution is to throw away data from the fifo whenever there’s too much in it – say more than packet size * 10.
if (fifo_size > packet_size * 10)
i2c::discard(mpu_addr, fifo_rw_register, fifo_size - packet_size); //leave one packet intact
fifo_size = packet_size;
if (fifo_size >= packet_size)
i2c::read(mpu_addr, fifo_rw_register, packet);
This solved most of the too-much-rate problem but not entirely. I still need to debug it a bit more..