IMU3000 FIFO Rate

All,

We are working on getting quaternion data from an IMU3000. We are unable to fit the API into our PIC processor, so we are using the API as a guide to write minimal routines to configure the device. We have been able to initially configure the device and confirm that we can get accelerometer data by reading sensor data registers 0x23-0x28. We turned on the FIFO writes for quaternion data by emulating MLSetDataMode(ML_DATA_FIFO). We can see fifo data being turned on or off by calling the equivalent of the set data mode routine. However, the data is placed into the FIFO too quickly. We obtain the following data rate.

fifoRate = 20
226 clear events * 512 entires per FIFO per second / (4 * 4) entries per quaternion
= 7232 quaternions per second

If we stop writing the 1kB dmpMemory segment from dmpDefault.c, we obtain the following data rate. However, the behavior is intermittent. We get data in the FIFOs for about half of our runs.

fifoRate = 20
2 clear events * 512 entries per FIFO per second / (4 * 4) entries per quaternion
= 64 quaternions per second

Our initialization process is as follows. Does anyone have experience with a similar problem?

////////////////////////////////////////////////////////////////////////////
// 1) Set up a Kionix Accel -> AccelSetupKionix()
////////////////////////////////////////////////////////////////////////////
// This step initializes a structure in the API. The initialization
// process occurs in the MLDLDmpAccelInit and AccelInit functions called
// from IMUopen()->MLDLDMPInit();
////////////////////////////////////////////////////////////////////////////


////////////////////////////////////////////////////////////////////////////
// 2) Set up IMU -> IMUopen()
////////////////////////////////////////////////////////////////////////////
// dmpDefault Functions:
// setGetAddress( dmpDefaultGetAddress );
// MLDLClockSource( CLK_PLLGYROZ );
// MLDLCfgSamplingMPU( MPUFILTER_42HZ, MPUFS_2000DPS, 4 );
// loadDMP( dmpMemory, 1024, sStartAddress );
//
// MLDLDMPInit Functions:
// MLDLDmpAccelInit();
// MLSLSerialWriteSingle( mldlData.mpuSlaveAddr, MPUREG_ACCEL_BURST_ADDR, 0x86 );
// // Kionix Accel
// regs[0] = 0;
// regs[1] = 64;
// regs[2] = 0;
// regs[3] = 0;
// result += MLDLSetMemoryMPU(KEY_D_1_236, 4, regs);
//
// result += MLDLSetI2CBypass( 1 ); // By-pass on
// result += MLSLSerialWriteSingle( mldlData.auxSlaveAddr, 0x1d, 0xcd ); // RAM reset
// MLOSSleep(10);
// result += MLSLSerialWriteSingle( mldlData.auxSlaveAddr, 0x1b, 0x42 ); // Wake up
// result += MLSLSerialWriteSingle( mldlData.auxSlaveAddr, 0x1b, 0xc2 ); // Normal operation
// MLOSSleep(50);
// result += MLSLSerialWriteSingle( mldlData.auxSlaveAddr, 0x1e, 0x14 ); // INT_CTRL_REG1: Configure non-latching wake-up
// result += MLSLSerialWriteSingle( mldlData.auxSlaveAddr, 0x5a, 0x00 ); // WUF_THRESH: wake-up threshold (on motion)
// result += MLSLSerialWriteSingle( mldlData.auxSlaveAddr, 0x21, 0x06 ); // DATA_CTRL_REG: output data rate
// result += MLSLSerialWriteSingle( mldlData.auxSlaveAddr, 0x29, 0x02 ); // WUF_TIMER: wake-up timer
// result += MLDLSetI2CBypass( 0 ); // By-pass off
//
// /*---- set length of AUX burst read to cover status registers ----*/
// MLDLSetRegisterMPU( MPUREG_USER_CTRL,
// MPUGetRegisterShadow(MPUREG_USER_CTRL) | BIT_AUX_RD_LENG );
//
// AccelInit Functions:
// MLDLSetRegisterMPU( MPUREG_AUX_SLV_ADDR, accelDescr.addr );
// MLDLSetRegisterMPU( MPUREG_ACCEL_BURST_ADDR, accelBurstAddrMask|accelDescr.reg );
// MLDLSetI2CBypass(1);
// accelDescr.init(); // Set in Step 1
// MLDLSetI2CBypass(0);
//
// if(accelDescr.useDmpFeatures) {
// AccelSetupDmpByteSwap(accelDescr.byteswap);
// AccelApplyOrientation();
// }
//
// Miscellaneous Functions:
// // Send data out of the FIFO
// mlxData.mlDataMode = ML_DATA_FIFO;
// regs = DINADD;
// if ( MLDLSetMemoryMPU(KEY_CFG_17, 1, &regs) != ML_SUCCESS )
// return ML_ERROR;
////////////////////////////////////////////////////////////////////////////
/*---- power management operation ----*/
ByteWriteIMU(IMUREG_PWR_MGM, 0x00);

/*---- reset the MPU-3000 to make sure all settings are known ----*/
ByteWriteIMU(IMUREG_PWR_MGM, 0x80);

/*---- set AUX slave address so that MPU can access device ----*/
ByteWriteIMU(IMUREG_AUX_SLV_ADDR, I2C_ACCEL_SLAVE_ADDR);

/*---- reset AUX interface to make slave address take effect ----*/
ByteWriteIMU(IMUREG_USER_CTRL, ByteReadIMU(IMUREG_USER_CTRL, &error) | BIT_AUX_IF_RST);

/*---- disable pass through ----*/
ByteWriteIMU(IMUREG_USER_CTRL, ByteReadIMU(IMUREG_USER_CTRL, &error) | BIT_AUX_IF_EN);

/*---- reset AUX interface to pass through change take effect ----*/
ByteWriteIMU(IMUREG_USER_CTRL, ByteReadIMU(IMUREG_USER_CTRL, &error) | BIT_AUX_IF_RST);


// Power Management Register
// PLL with external 19.2MHz reference
ByteWriteIMU(IMUREG_PWR_MGM, 0x05);

ByteWriteIMU(IMUREG_SMPL_RATE_DIV, 0x07);

ByteWriteIMU(IMUREG_DLPF_FS, 0x05);

////////////////////////////////////////////////////////////////////////////
// loadDMP( dmpMemory, 1024, sStartAddress );
////////////////////////////////////////////////////////////////////////////
// Turn off DMP and FIFO
ByteWriteIMU(IMUREG_USER_CTRL, ByteReadIMU(IMUREG_USER_CTRL, &error) & ~(BIT_FIFO_EN | BIT_DMP_EN));

for(i = 0; i < 1024; i++) {
ByteWriteMemIMU(sStartAddress + i, dmpMemory);
} /* for */
////////////////////////////////////////////////////////////////////////////

// User Control Register
// Enable and reset DMP and FIFO. Reset Gyro
ByteWriteIMU(IMUREG_USER_CTRL, 0x6F);

ConfigAccelerometer();

// FIFO Enable Register
// Temp out, gyro x out, gyro y out, gyro z out,
// aux x out, aux y out, aux z out, FIFO footer
ByteWriteIMU(IMUREG_FIFO_EN, 0x0);
////////////////////////////////////////////////////////////////////////////


////////////////////////////////////////////////////////////////////////////
// 3) Set Bias Values -> IMUsetBiasUpdateFunc( ML_ALL );
////////////////////////////////////////////////////////////////////////////
// Leave bias corrections turned off initially.
////////////////////////////////////////////////////////////////////////////

////////////////////////////////////////////////////////////////////////////
// 4) Set up Motion Detection
// // no motion detection timer, unit is second
// MLSetNoMotionTime(3.0f);
// fusionGain( 0.0038f );
////////////////////////////////////////////////////////////////////////////
// These are set using the key table and writes to reserved register 0x39.
float motionVal = 3.0;
long tmp;
unsigned short mlMotionDuration;

tmp = (long)(motionVal*200);
mlMotionDuration = (unsigned short) tmp;

ByteWriteMemIMU(D_1_106, (mlMotionDuration >> 8) & 0xff);
ByteWriteMemIMU(D_1_106, mlMotionDuration & 0xff);
////////////////////////////////////////////////////////////////////////////

////////////////////////////////////////////////////////////////////////////
// 5) Register callback function to detect "motion" or "no motion"
// IMUsetMotionCallback(onMotion);
////////////////////////////////////////////////////////////////////////////
// This function registers a callback that is not needed in our
// implementation.
////////////////////////////////////////////////////////////////////////////

////////////////////////////////////////////////////////////////////////////
// 6) Setup Quaternion Data Output
// MLSetProcessedFIFOCallback(onQuaternion);
////////////////////////////////////////////////////////////////////////////
// This function registers a callback that is not needed in our
// implementation.
////////////////////////////////////////////////////////////////////////////


////////////////////////////////////////////////////////////////////////////
// 7) Setup FIFO Inputs
// IMUsendQuaternionToFIFO(ML_32_BIT)
////////////////////////////////////////////////////////////////////////////
//
// IMUsendQuaternionToFIFO
ByteWriteMemIMU(CFG_8, DINAF8 + 1);
ByteWriteMemIMU(CFG_8 + 1, DINA20);
ByteWriteMemIMU(CFG_8 + 2, DINA28);
ByteWriteMemIMU(CFG_8 + 3, DINA30);
ByteWriteMemIMU(CFG_8 + 4, DINA38);

// ByteWriteMemIMU(CFG_15, DINAF8+3);
// ByteWriteMemIMU(CFG_15 + 1, DINAF8+3);
// ByteWriteMemIMU(CFG_15 + 2, DINAA0+3);
// ByteWriteMemIMU(CFG_15 + 3, DINAF8+2);
// ByteWriteMemIMU(CFG_15 + 4, DINAA0+3);
// ByteWriteMemIMU(CFG_15 + 5, DINAF8+3);
// ByteWriteMemIMU(CFG_15 + 6, DINAA0+3);
// ByteWriteMemIMU(CFG_15 + 7, DINAF8+2);
// ByteWriteMemIMU(CFG_15 + 8, DINAA0+3);
// ByteWriteMemIMU(CFG_15 + 9, DINAF8+3);
// ByteWriteMemIMU(CFG_15 + 10, DINAA0+3);
// ByteWriteMemIMU(CFG_15 + 11, DINAF8+2);
// ByteWriteMemIMU(CFG_15 + 12, DINAA0+3);

// IMUsetFIFOFooter()
ByteWriteMemIMU(CFG_16, DINA30);
///////////////////////////////////////////////////////////////////////////


////////////////////////////////////////////////////////////////////////////
// 8) Setup FIFO Rate
// IMUsetFIFORate(1)
//
// The DMP will add fifo entries every fifoRate + 1 IMU cycles (200 Hz).
// The following values apply:
//
// fifoRate DMP Sample Rate FIFO update frequency
// 0 200Hz 200Hz
// 1 200Hz 100Hz
// 3 200Hz 50Hz
// 4 200Hz 40Hz
// 9 200Hz 20Hz
// 19 200Hz 10Hz
//
// fifoRate: Divider value - 1. Output rate is
// (DMP Sample Rate) / (fifoRate + 1).
////////////////////////////////////////////////////////////////////////////
fifoRate = 20;
ByteWriteMemIMU(D_0_22, fifoRate / 256);
ByteWriteMemIMU(D_0_22 + 1, fifoRate % 256);
////////////////////////////////////////////////////////////////////////////

////////////////////////////////////////////////////////////////////////////
// 9) Setup FIFO Data mode
// MLSetDataMode(ML_DATA_FIFO);
////////////////////////////////////////////////////////////////////////////
ByteWriteMemIMU(CFG_19, DINA18);
ByteWriteMemIMU(CFG_17, DINADD);
////////////////////////////////////////////////////////////////////////////

////////////////////////////////////////////////////////////////////////////
// 10) Start IMU
// IMUstart()
////////////////////////////////////////////////////////////////////////////
// Setup DMP
// MLDLResetDmp();
unsigned char userCtrlReg = ByteReadIMU(IMUREG_USER_CTRL, &error);

// MLSetDataMode(0); Set mode to none.
ByteWriteMemIMU(CFG_19, DINA08);
ByteWriteMemIMU(CFG_17, DINAA0+3);

// Force clear on FIFO. Read fifo level, force reset, and continue
// until the length is zero.
len = (ByteReadIMU(0x3A, &error) & 0x3) << 8;
len |= ByteReadIMU(0x3B, &error);

while(len != 0 && tries < 6) {
ByteWriteIMU(IMUREG_USER_CTRL,
(ByteReadIMU(IMUREG_USER_CTRL, &error) &
~BIT_FIFO_EN) | BIT_FIFO_RST);

len = (ByteReadIMU(0x3A, &error) & 0x3) << 8;
len |= ByteReadIMU(0x3B, &error);

tries++;
} /* while */

ByteWriteIMU(IMUREG_USER_CTRL, userCtrlReg);

// Setup previous mode. MLSetDataMode(ML_DATA_FIFO);
ByteWriteMemIMU(CFG_19, DINA18);
ByteWriteMemIMU(CFG_17, DINADD);

// Disable and Reset DMP
ByteWriteIMU(IMUREG_USER_CTRL, (userCtrlReg & ~0x80) | 0x04);

// Save Start Address
startAddress = ByteReadIMU(0x35, &error) << 8;
startAddress |= ByteReadIMU(0x36, &error);

ByteWriteIMU(0x35, startAddress >> 8);
ByteWriteIMU(0x36, startAddress & 0xFF);

// Enable and Reset DMP
ByteWriteIMU(IMUREG_USER_CTRL, userCtrlReg | 0x84);
////////////////////////////////////////////////////////////////////////////

// Setup Interrupt Configuration Register as Follows
// - LATCH_INT_EN Latch mode – 1=latch until interrupt is cleared
//
// - INT_ANYRD_2CLEAR Interrupt status register clear method –
// 0=clear by reading interrupt status register (26) only
//
// - I2C_MST_ERR_EN Enable interrupt when accelerometer on secondary
// I2C bus does not acknowledge IMU-3000
//
// - IMU_RDY_EN Enable interrupt when device is ready (PLL ready after
// changing clock source)
//
// - DMP_DONE_EN Enable interrupt when DMP is done (programmable
// functionality)
//
// - RAW_RDY_EN Enable interrupt when data is available
ByteWriteIMU(MPUREG_INT_CFG, ByteReadIMU(MPUREG_INT_CFG, &error) | 0x0F