File content as of revision 4:9d706f783b5a:

#include "SparkFunMPU9250-DMP.h"
#include "mdcompat.h"

Serial pc(USBTX,USBRX);
DigitalOut led(LED1);
MPU9250_DMP imu;
unsigned char whoami[1]={0};
unsigned char fifo_count[2]={32,32};
unsigned char temp[1]={32};
unsigned char outbuff[4];
long DEBUG[]={2,2,2,2};
unsigned long tamper;
unsigned char regadd;
char registeradress[20];
unsigned char registerdata[]={33};
void printIMUData(void);
int main() 
{
  pc.baud(115200);
  pc.printf("Hello World\n");
  imu_init();
  stamper_init();
#if 1
  // Call imu.begin() to verify communication and initialize
  if (imu.begin() != INV_SUCCESS){
    while (1){
      pc.printf("Unable to communicate with MPU-9250");
      pc.printf("Check connections, and try again.\n");
      wait_ms(5000);
    }
  }
  pc.printf("imu.begin() suceeded\n");
  
    if(imu.dmpBegin(DMP_FEATURE_6X_LP_QUAT | // Enable 6-axis quat
               DMP_FEATURE_GYRO_CAL, // Use gyro calibration
              20)==INV_ERROR){ // Set DMP FIFO rate to 20 Hz
        pc.printf("imu.dmpBegin have failed\n");//dmpLoad function under it fails which is caused by memcmp(firmware+ii, cur, this_write) (it is located at row 2871 of inv_mpu.c)
              }
    else{
        pc.printf("imu.dmpBegin() suceeded\n");
    }
#endif
#if 0//processes here is not important if imu.dmpBegin is not working -> 0 if dmpBegin fails
    mbed_i2c_read(0x68,0x75,1,whoami);
    pc.printf("whoami=%d\n",whoami[0]);
    mbed_i2c_read(0x68,0x23,1,outbuff);
    pc.printf("out buffer=%d\n",outbuff[0]);
    outbuff[0]|=1;
    pc.printf("updated out buffer=%d\n",outbuff[0]);
    mbed_i2c_write(0x68,0x23,2,outbuff);
    mbed_i2c_read(0x68,0x23,1,outbuff);
    pc.printf("new out buffer=%d(if zero, write function still needs to be fixed)\n",outbuff[0]);
#endif
while(1){
    if(pc.readable()){
        pc.scanf("%s",&registeradress);
        regadd=(registeradress[0]-48)*100+(registeradress[1]-48)*10+(registeradress[2]-48);
        mbed_i2c_read(0x68,(unsigned)regadd,1,registerdata);
        pc.printf("%d is gotten from serial port, data at that register is %d\n",regadd,registerdata[0]);
    }
    /*unsigned char buff[2]={0x38,0x39};
    pc.printf("trial-%s-%s\n",buff,(const char*)buff);*/
  /*imu.updateTemperature();
  temp[0]=imu.temperature;
  get_ms(&tamper);
  pc.printf("%d-%d\n",temp[0],tamper);*/
  // Check for new data in the FIFO
  if (imu.fifoAvailable()){//fifo is not being available
    led=0;
    wait_ms(500);
    led=1;
    wait_ms(500);
    // Use dmpUpdateFifo to update the ax, gx, mx, etc. values
    if ( imu.dmpUpdateFifo(DEBUG) == INV_SUCCESS){
      // computeEulerAngles can be used -- after updating the
      // quaternion values -- to estimate roll, pitch, and yaw
      imu.computeEulerAngles();
      //printIMUData();
        pc.printf("dmpUpdateFifo sucesfully occured\n");
     #if 1
      pc.printf("%lf",DEBUG[0]);
      pc.printf("\t");
      pc.printf("%lf",DEBUG[1]);
      pc.printf("\t");
      pc.printf("%lf",DEBUG[2]);
      pc.printf("\t");
      pc.printf("%lf\n ",DEBUG[3]);
     #endif
    }
  }
  else{
    led=1;
    wait_ms(100);
    led=0;
    wait_ms(100);      
  }
}
  return 0;
}

void printIMUData(void)
{  
  // After calling dmpUpdateFifo() the ax, gx, mx, etc. values
  // are all updated.
  // Quaternion values are, by default, stored in Q30 long
  // format. calcQuat turns them into a float between -1 and 1
  float q0 = imu.calcQuat(imu.qw);
  float q1 = imu.calcQuat(imu.qx);
  float q2 = imu.calcQuat(imu.qy);
  float q3 = imu.calcQuat(imu.qz);
  
  pc.printf("R:%lf  P:%lf   Y:%lf \n", imu.roll,imu.pitch,imu.yaw);
}