MPU9250 DMP数据读取速率为什么达不到200 Hz

用xilinx sdk 中 Xtime_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
  }