File content as of revision 0:f463e270d211:

/* MPU9250 Basic Example Code
 by: Kris Winer
 date: April 1, 2014
 license: Beerware - Use this code however you'd like. If you 
 find it useful you can buy me a beer some time.
 
 Demonstrate basic MPU-9250 functionality including parameterizing the register addresses, initializing the sensor, 
 getting properly scaled accelerometer, gyroscope, and magnetometer data out. Added display functions to 
 allow display to on breadboard monitor. Addition of 9 DoF sensor fusion using open source Madgwick and 
 Mahony filter algorithms. Sketch runs on the 3.3 V 8 MHz Pro Mini and the Teensy 3.1.
 
 SDA and SCL should have external pull-up resistors (to 3.3V).
 10k resistors are on the EMSENSR-9250 breakout board.
 
 Hardware setup:
 MPU9250 Breakout --------- Arduino
 VDD ---------------------- 3.3V
 VDDI --------------------- 3.3V
 SDA ----------------------- A4
 SCL ----------------------- A5
 GND ---------------------- GND
 
 Note: The MPU9250 is an I2C sensor and uses the Arduino Wire library. 
 Because the sensor is not 5V tolerant, we are using a 3.3 V 8 MHz Pro Mini or a 3.3 V Teensy 3.1.
 We have disabled the internal pull-ups used by the Wire library in the Wire.h/twi.c utility file.
 We are also using the 400 kHz fast I2C mode by setting the TWI_FREQ  to 400000L /twi.h utility file.
 */
 
//#include "ST_F401_84MHZ.h" 
//F401_init84 myinit(0);
#include "mbed.h"
#include "MPU9250.h"
//#include "N5110.h"

// Using NOKIA 5110 monochrome 84 x 48 pixel display
// pin 9 - Serial clock out (SCLK)
// pin 8 - Serial data out (DIN)
// pin 7 - Data/Command select (D/C)
// pin 5 - LCD chip select (CS)
// pin 6 - LCD reset (RST)
//Adafruit_PCD8544 display = Adafruit_PCD8544(9, 8, 7, 5, 6);

float sum = 0;
uint32_t sumCount = 0;
int count = 0;
float yaw = 0;
float drift;
MPU9250 imu(PTE25, PTE24);         // SDA, SCL 
   
   Timer t;

   Serial pc(USBTX, USBRX); // tx, rx

   //        VCC,   SCE,  RST,  D/C,  MOSI,S CLK, LED
  // N5110 lcd(PA_8, PB_10, PA_9, PA_6, PA_7, PA_5, PC_7);
   

        
int main()
{
  pc.baud(9600);  
  pc.printf("CPU SystemCoreClock is %d Hz\r\n", SystemCoreClock);   
    
  // Read the WHO_AM_I register, this is a good test of communication
  uint8_t whoami = imu.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250);  // Read WHO_AM_I register for MPU-9250
  pc.printf("I AM 0x%x\n\r", whoami); pc.printf("I SHOULD BE 0x71\n\r");
  
  if (whoami == 0x71) // WHO_AM_I should always be 0x68
  {  
    pc.printf("MPU9250 is online...\n\r");
    
    wait(1);
    
    imu.resetMPU9250(); // Reset registers to default in preparation for device calibration
    imu.calibrateMPU9250(imu.gyroBias, imu.accelBias); // Calibrate gyro and accelerometers, load biases in bias registers  
    imu.initMPU9250(); 
    imu.initAK8963(imu.magCalibration);
    wait(2);
   }
   else
   {
    pc.printf("Could not connect to MPU9250: \n\r");
    pc.printf("%#x \n",  whoami);
 
    while(1) ; // Loop forever if communication doesn't happen
    }

    imu.getAres(); // Get accelerometer sensitivity
    imu.getGres(); // Get gyro sensitivity
    imu.getMres(); // Get magnetometer sensitivity
    pc.printf("Accelerometer sensitivity is %f LSB/g \n\r", 1.0f/imu.aRes);
    pc.printf("Gyroscope sensitivity is %f LSB/deg/s \n\r", 1.0f/imu.gRes);
    pc.printf("Magnetometer sensitivity is %f LSB/G \n\r", 1.0f/imu.mRes);
    imu.magbias[0] = +470.;  // User environmental x-axis correction in milliGauss, should be automatically calculated
    imu.magbias[1] = +120.;  // User environmental x-axis correction in milliGauss
    imu.magbias[2] = +125.;  // User environmental x-axis correction in milliGauss
    t.start();        

 while(1) {
  
  // If intPin goes high, all data registers have new data
  if(imu.readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) {  // On interrupt, check if data ready interrupt

    imu.readAccelData(imu.accelCount);  // Read the x/y/z adc values   
    // Now we'll calculate the accleration value into actual g's
    imu.ax = (float)imu.accelCount[0]*imu.aRes - imu.accelBias[0];  // get actual g value, this depends on scale being set
    imu.ay = (float)imu.accelCount[1]*imu.aRes - imu.accelBias[1];   
    imu.az = (float)imu.accelCount[2]*imu.aRes - imu.accelBias[2];  
   
    imu.readGyroData(imu.gyroCount);  // Read the x/y/z adc values
    // Calculate the gyro value into actual degrees per second
    imu.gx = (float)imu.gyroCount[0]*imu.gRes - imu.gyroBias[0];  // get actual gyro value, this depends on scale being set
    imu.gy = (float)imu.gyroCount[1]*imu.gRes - imu.gyroBias[1];  
    imu.gz = (float)imu.gyroCount[2]*imu.gRes - imu.gyroBias[2];   
  
    imu.readMagData(imu.magCount);  // Read the x/y/z adc values   
    // Calculate the magnetometer values in milliGauss
    // Include factory calibration per data sheet and user environmental corrections
    imu.mx = (float)imu.magCount[0]*imu.mRes*imu.magCalibration[0] - imu.magbias[0];  // get actual magnetometer value, this depends on scale being set
    imu.my = (float)imu.magCount[1]*imu.mRes*imu.magCalibration[1] - imu.magbias[1];  
    imu.mz = (float)imu.magCount[2]*imu.mRes*imu.magCalibration[2] - imu.magbias[2];   
  }
   
  if(imu.gz>.3 || imu.gz < -.3){
     yaw = (yaw - t.read()*imu.gz+drift);
    t.reset();        
    if(yaw > 360)
        yaw -= 360;
    if(yaw < 0)
        yaw += 360;
    pc.printf("Yaw: %f \n\r", yaw);
    }

}
}