用xilinx sdk 中 X
time_GetTime 函数测得读取DMP速率仅为 67 Hz ，但是 在Processing中跑的正常，所以不知道为什么达不到200 Hz ，DMP 初始化代码如下，求告知问题所在，感激涕零 ！
int Mpu9250_DMP_Init(void)
{
uint8_t dmpUpdate[16], j;
uint16_t pos = 0;
uint16_t fifoCount;
uint8_t fifoBuffer[128];
uint8_t mpuIntStatus;
// Reset MPU9250
Reset();
usleep(3000); // wait 3ms after reset
// disable sleep mode
Set_Sleep_Enabled(false);
//Selecting user bank 16
Set_MemoryBank(0x10, true, true);
//Selecting memory byte 6...
Set_MemoryStartAddress(0x06);
hwRevision = Read_MemoryByte();
printf("hwRevision is %dn",hwRevision);
//Resetting memory bank selection to 0...
Set_MemoryBank(0, false, false);
// check OTP bank valid
otpValid = Get_OTPBankValid(); //the reg 0x00 is different 9250 & 9250
printf("otpValid is %dn",hwRevision);
xgOffsetTC = Get_XGyroOffsetTC();
ygOffsetTC = Get_YGyroOffsetTC();
zgOffsetTC = Get_ZGyroOffsetTC();
printf("xgOffsetTC is %d.n",xgOffsetTC);
printf("ygOffsetTC is %d.n",ygOffsetTC);
printf("zgOffsetTC is %d.n",zgOffsetTC);
//Setting slave 0 address to 0x7F...
Set_SlaveAddress(0,0x7f);
//Disabling I2C Master mode...
Set_I2CMasterModeEnabled(false);
//Setting slave 0 address to 0x68 (self)...
Set_SlaveAddress(0,0x68);
//Resetting I2C Master control...
Reset_I2CMaster();
usleep(200);
// load DMP code into memory banks
if(Write_ProgMemoryBlock(dmpMemory, MPU9250_DMP_CODE_SIZE,0,0,1))
{
//Success! DMP code written and verified.
//write DMP configuration
if (Write_ProgDMPConfigurationSet(dmpConfig, MPU9250_DMP_CONFIG_SIZE))
{
//Success! DMP configuration written and verified.
//Setting clock source to Z Gyro...
Set_Clock_Source(MPU9250_CLOCK_PLL_ZGYRO);
//Setting DMP and FIFO_OFLOW interrupts enabled...
Set_IntEnabled(0x12);
//Setting sample rate to 200Hz...
Set_Rate(4); // 1khz / (1 + 4) = 200 Hz
//Setting external frame sync to TEMP_OUT_L[0]...;
Set_ExternalFrameSync(MPU9250_EXT_SYNC_TEMP_OUT_L);
//Setting DLPF bandwidth to 42Hz...
Set_DLPFMode(MPU9250_DLPF_BW_42);
//Setting gyro sensitivity to +/- 2000 deg/sec...
Set_FullScaleGyroRange(MPU9250_GYRO_FS_2000);
//Setting DMP configuration bytes (function unknown)...
Set_DMPConfig1(0x03);
Set_DMPConfig2(0x00);
//Clearing OTP Bank flag...
Set_OTPBankValid(false);
//Setting X/Y/Z gyro offset TCs to previous values...
Set_XGyroOffsetTC(xgOffsetTC);
Set_YGyroOffsetTC(ygOffsetTC);
Set_ZGyroOffsetTC(zgOffsetTC);
//Writing final memory update 1/7 (function unknown)...
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++)
dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
Write_MemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1],true,false);
//Writing final memory update 2/7 (function unknown)...
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++)
dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
Write_MemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1],true,false);
//Resetting FIFO...
Reset_FIFO();
//Reading FIFO count...
fifoCount = Get_FIFOCount();
//Reading FIFO data...
ReadFIFO_S(fifoBuffer, fifoCount);
//Setting motion detection threshold to 2...
Set_MotionDetectionThreshold(2);
//Setting zero-motion detection threshold to 156...
Set_ZeroMotionDetectionThreshold(156);
//Setting motion detection duration to 80...
Set_MotionDetectionDuration(80);
//Setting zero-motion detection duration to 0...
Set_ZeroMotionDetectionDuration(0);
//Resetting FIFO...
Reset_FIFO();
//Enabling FIFO...
Set_FIFOEnabled(true);
//Enabling DMP...
Set_DMPEnabled(true);
//Resetting DMP...
Reset_DMP();
//Writing final memory update 3/7 (function unknown)...
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++)
dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
Write_MemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1],true,false);
//Writing final memory update 4/7 (function unknown)...
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++)
dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
Write_MemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1],true,false);
//Writing final memory update 5/7 (function unknown)...
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++)
dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
Write_MemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1],true,false);
//Waiting for FIFO count > 2...
while((fifoCount = Get_FIFOCount()) < 3)
;
//Reading FIFO data...
ReadFIFO_S(fifoBuffer, fifoCount);
//Reading interrupt status...
mpuIntStatus = Get_IntStatus();
//Reading final memory update 6/7 (function unknown)...
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++)
dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
Read_MemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
//Waiting for FIFO count > 2...
while ((fifoCount = Get_FIFOCount()) < 3)
;
//Reading FIFO data...
ReadFIFO_S(fifoBuffer, fifoCount);
//Reading interrupt status...
mpuIntStatus = Get_IntStatus();
//Writing final memory update 7/7 (function unknown)...
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++)
dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
Write_MemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1],true,false);
//Mpu9250_WriteReg_R(MPU9250_RA_FIFO_EN,0x00); //default 0x00
//DMP is good to go! Finally.
//Disabling DMP (you turn it on later)...
Set_DMPEnabled(false);
//Setting up internal 42-byte (default) DMP packet buffer...
dmpPacketSize = 42;
/*if ((dmpPacketBuffer = (uint8_t *)malloc(42)) == 0) {
return 3; // TODO: proper error code for no memory
}*/
//Resetting FIFO and clearing INT status one last time...
Reset_FIFO();
Get_IntStatus();
}
else
{
//ERROR! DMP configuration verification failed.
return 2; // configuration block loading failed
}
}
else
{
//ERROR! DMP code verification failed.
return 1; // main binary block loading failed
}
return 0; // success
}
举报
