MPU-6500 FIFO is not filling up

By greatshoji , 28 December 2014

I would like to read MPU-6500 3axis accelerometer values from FIFO, using SPI, at 4kHz ODR, lowpass filters disabled, but it looks like buffer starts with ~72 samples and does not get filled up. Can anyone look at the snippets of my code?

function init_MPU6500()      //make stetting to MPU-6500
{
spi_write_reg(PWR_MGMT_1,0x80) // Rst internal regs and restores the default settings
imp.sleep(0.1) // recommended wait time by datasheet
spi_write_reg(SIGNAL_PATH_RST,0x07)// recommended by datasheet
imp.sleep(0.1) // recommended wait time by datasheet
spi_write_reg(ACCEL_CONFIG2,0x08) // set ODR to 4kHz and bypass digital low pass filter (BW=1.13kHz)
spi_write_reg(FIFO_EN,0x08) // write accelerometer data to FIFO
spi_write_reg(USER_CTRL,0x50) // enable FIFO operation, SPI only mode
spi_write_reg(PWR_MGMT_2,0x07) // enabel accels, disable gyros
imp.sleep(0.1) // recommended wait time by datasheet
}

function loop_MPU6500()
{ //to display the g's on the screen
spi_read_reg(FIFO_COUNT_H,2) // these registers contain the number of bytes in fifo
local bytes_in_fifo=data[0]<<8|data[1] // OR the high and low byte
if(bytes_in_fifo>5){ // if there are 6 bytes in fifo we can display them
spi_read_reg(FIFO_R_W,6) // read 6 bytes from fifo
local x = data[0]<<8|data[1] // left shift high byte and OR with low bye
local y = data[2]<<8|data[3] // left shift high byte and OR with low bye
local z = data[4]<<8|data[5] // left shift high byte and OR with low bye
if(x&0x8000){x=-((~x&0x7fff)+1)} // to convert two's complement to a float
if(y&0x8000){y=-((~y&0x7fff)+1)} // if the bit15 is set
if(z&0x8000){z=-((~z&0x7fff)+1)} // invert the bits and add 1
x=x*61e-6 // each digit is 4g/2^16=61ug
y=y*61e-6 // each digit is 4g/2^16=61ug
z=z*61e-6 // each digit is 4g/2^16=61ug
server.log(format("%i, X:%0.6fg, Y: %0.6fg, Z: %0.6fg",bytes_in_fifo,x,y,z))
}
imp.wakeup(0.3, loop_MPU6500) // this loop repeats every x second
}

init_MPU6500() // Set up accelerometer
loop_MPU6500()

Below is what is printed to the screen
2014-12-28 13:59:17 UTC-8	[Device]	66, X:0.080520g, Y: -0.023424g, Z: 0.992104g
2014-12-28 13:59:16 UTC-8 [Device] 72, X:0.080520g, Y: -0.018056g, Z: 0.995032g
2014-12-28 13:59:17 UTC-8 [Device] 60, X:0.080520g, Y: -0.023424g, Z: 0.992104g
2014-12-28 13:59:17 UTC-8 [Device] 54, X:0.081984g, Y: -0.017568g, Z: 1.000888g
2014-12-28 13:59:18 UTC-8 [Device] 48, X:0.081984g, Y: -0.017568g, Z: 1.000888g
2014-12-28 13:59:18 UTC-8 [Device] 42, X:0.075152g, Y: -0.020008g, Z: 0.997472g
2014-12-28 13:59:18 UTC-8 [Device] 36, X:0.075152g, Y: -0.020008g, Z: 0.997472g
2014-12-28 13:59:19 UTC-8 [Device] 30, X:0.072224g, Y: -0.019520g, Z: 0.995520g
2014-12-28 13:59:19 UTC-8 [Device] 24, X:0.072224g, Y: -0.019520g, Z: 0.995520g
2014-12-28 13:59:19 UTC-8 [Device] 18, X:0.073688g, Y: -0.023424g, Z: 1.007720g
2014-12-28 13:59:20 UTC-8 [Device] 12, X:0.073688g, Y: -0.023424g, Z: 1.007720g
2014-12-28 13:59:20 UTC-8 [Device] 6, X:0.081984g, Y: -0.030744g, Z: 1.003816g
phpbb Topic ID
16848