File content as of revision 0:f96b9b35ac4c:

#include "mbed.h"
#include "MPU9250.h"
#include "TextLCD.h"

struct UART_buf
{ 
    uint8_t STA;
    uint8_t MODE; 
    uint8_t CMD;
    uint8_t LEN;
    uint8_t DATA[32];
    uint8_t END; 
     
};

union Data_DB{
    int16_t data16;
    uint8_t data8[2];
}Data_Tr;

MPU9250 mpu9250;

Ticker Sensor_Timer;

Serial SerialUART(PA_2, PA_3); // tx, rx
  
// rs, rw, e, d0-d3
TextLCD lcd(PB_12, PB_13, PB_14, PB_15, PA_9, PA_10, PA_11); 

uint8_t Buffer[37];
volatile uint8_t Sensor_flag = 0;

UART_buf RX_BUF;

void SerialUARTRX_ISR(void);
void Timer_setting(uint8_t cmd, uint8_t value);
void Sensor_Read(void);

int main()
{
    SerialUART.baud(115200);  

    //Set up I2C
    i2c.frequency(400000);  // use fast (400 kHz) I2C  
    
    // Read the WHO_AM_I register, this is a good test of communication
    uint8_t whoami = mpu9250.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250);  // Read WHO_AM_I register for MPU-9250
    //SerialUART.printf("I AM 0x%x\n\r", whoami); SerialUART.printf("I SHOULD BE 0x71\n\r");
    
    if (whoami == 0x71) // WHO_AM_I should always be 0x68
    {  
        /*SerialUART.printf("MPU9250 WHO_AM_I is 0x%x\n\r", whoami);
        SerialUART.printf("MPU9250 is online...\n\r");*/
        lcd.printf("MPU9250 is 0x%x\n",whoami);
        lcd.printf("   Connected    ");

        wait(1);
        
        mpu9250.resetMPU9250(); // Reset registers to default in preparation for device calibration
        mpu9250.MPU9250SelfTest(SelfTest); // Start by performing self test and reporting values
        /*SerialUART.printf("x-axis self test: acceleration trim within : %f % of factory value\n\r", SelfTest[0]);  
        SerialUART.printf("y-axis self test: acceleration trim within : %f % of factory value\n\r", SelfTest[1]);  
        SerialUART.printf("z-axis self test: acceleration trim within : %f % of factory value\n\r", SelfTest[2]);  
        SerialUART.printf("x-axis self test: gyration trim within : %f % of factory value\n\r", SelfTest[3]);  
        SerialUART.printf("y-axis self test: gyration trim within : %f % of factory value\n\r", SelfTest[4]);  
        SerialUART.printf("z-axis self test: gyration trim within : %f % of factory value\n\r", SelfTest[5]);  */
        mpu9250.calibrateMPU9250(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers  
        /*SerialUART.printf("x gyro bias = %f\n\r", gyroBias[0]);
        SerialUART.printf("y gyro bias = %f\n\r", gyroBias[1]);
        SerialUART.printf("z gyro bias = %f\n\r", gyroBias[2]);
        SerialUART.printf("x accel bias = %f\n\r", accelBias[0]);
        SerialUART.printf("y accel bias = %f\n\r", accelBias[1]);
        SerialUART.printf("z accel bias = %f\n\r", accelBias[2]);*/
        wait(2);
        mpu9250.initMPU9250(); 
        //SerialUART.printf("MPU9250 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature
        mpu9250.initAK8963(magCalibration);
        /*SerialUART.printf("AK8963 initialized for active data mode....\n\r"); // Initialize device for active mode read of magnetometer
        SerialUART.printf("Accelerometer full-scale range = %f  g\n\r", 2.0f*(float)(1<<Ascale));
        pSerialUARTc.printf("Gyroscope full-scale range = %f  deg/s\n\r", 250.0f*(float)(1<<Gscale));
        if(Mscale == 0) SerialUART.printf("Magnetometer resolution = 14  bits\n\r");
        if(Mscale == 1) SerialUART.printf("Magnetometer resolution = 16  bits\n\r");
        if(Mmode == 2) SerialUART.printf("Magnetometer ODR = 8 Hz\n\r");
        if(Mmode == 6) SerialUART.printf("Magnetometer ODR = 100 Hz\n\r");*/
        wait(1);
    }
    else
    {
        //SerialUART.printf("Could not connect to MPU9250: \n\r");
        //SerialUART.printf("%#x \n",  whoami);
        
        lcd.printf("MPU9250 is 0x%x\n",whoami);
        lcd.printf(" No connection  ");
       
        while(1) ; // Loop forever if communication doesn't happen
    }

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

    SerialUART.attach(&SerialUARTRX_ISR);
    
    Timer_setting(0x06, 1);

    while(1)
    {
        if(mpu9250.readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) {  // On interrupt, check if data ready interrupt
        
            mpu9250.readAccelData(accelCount);  // Read the x/y/z adc values   
            mpu9250.readGyroData(gyroCount);  // Read the x/y/z adc values
            mpu9250.readMagData(magCount);  // Read the x/y/z adc values   
            // Now we'll calculate the accleration value into actual g's
            if(Sensor_flag)
            {
                Sensor_flag = 0;
                
                ax = (float)accelCount[0]*aRes - accelBias[0];  // get actual g value, this depends on scale being set
                ay = (float)accelCount[1]*aRes - accelBias[1];   
                az = (float)accelCount[2]*aRes - accelBias[2];  
                /*
                // Calculate the gyro value into actual degrees per second
                gx = (float)gyroCount[0]*gRes - gyroBias[0];  // get actual gyro value, this depends on scale being set
                gy = (float)gyroCount[1]*gRes - gyroBias[1];  
                gz = (float)gyroCount[2]*gRes - gyroBias[2];   
                
                // Calculate the magnetometer values in milliGauss
                // Include factory calibration per data sheet and user environmental corrections
                mx = (float)magCount[0]*mRes*magCalibration[0] - magbias[0];  // get actual magnetometer value, this depends on scale being set
                my = (float)magCount[1]*mRes*magCalibration[1] - magbias[1];  
                mz = (float)magCount[2]*mRes*magCalibration[2] - magbias[2];*/
                
                Buffer[0] = 0x76;
                Buffer[1] = 0x01;
                Buffer[2] = 0x02;
                Buffer[3] = 6;
                Data_Tr.data16 = (int16_t)(ax * 1000);
                Buffer[4] = Data_Tr.data8[1];
                Buffer[5] = Data_Tr.data8[0];
                Data_Tr.data16 = (int16_t)(ay * 1000);
                Buffer[6] = Data_Tr.data8[1];
                Buffer[7] = Data_Tr.data8[0];
                Data_Tr.data16 = (int16_t)(az * 1000);
                Buffer[8] = Data_Tr.data8[1];
                Buffer[9] = Data_Tr.data8[0];
                Buffer[10] = 0x3E;

                for(int i=0; i<11; i++)
                    SerialUART.putc(Buffer[i]);
            }
        }
    }
}

void SerialUARTRX_ISR(void)
{
    static uint8_t RX_count = 0, RX_Len = 32, RX_Status = 0;
    uint8_t rx_da = SerialUART.getc();
    switch(RX_Status)
    {
        case 0:
            if(rx_da == 0x76)
            {
                RX_BUF.STA = rx_da;
                RX_Status++;
            }
            break;
        case 1:
            RX_BUF.MODE = rx_da;
            RX_Status++;
            break;
        case 2:
            RX_BUF.CMD = rx_da;
            RX_Status++;
            break;
        case 3:
            RX_BUF.LEN = rx_da;
            RX_Len = RX_BUF.LEN;
            RX_Status++;
            if(RX_Len == 0)
                RX_Status++;
            break;
        case 4:
            RX_BUF.DATA[RX_count] = rx_da;
            RX_count++;
            if(RX_count == RX_Len)
            {
                RX_Status++;
                RX_count = 0;
                RX_Len = 32;
            }
            break;
        case 5:
            if(rx_da == 0x3E)
            {
                RX_BUF.END = rx_da;
                RX_Status = 0;
                switch(RX_BUF.MODE)
                {
                    case 0x04:
                        Timer_setting(RX_BUF.CMD, RX_BUF.DATA[0]);
                        break;
                }
            }
            break;
    }
}

void Timer_setting(uint8_t cmd, uint8_t value)
{
    double Time_value = 0;
    switch(cmd)
    {
        case 0x01:
            Time_value = 30;
            break;
        case 0x02:
            Time_value = 60;
            break;
        case 0x03:
            Time_value = 120;
            break;
        case 0x04:
            Time_value = 300;
            break;
        case 0x05:
            Time_value = 600;
            break;
        case 0x06:
            Time_value = value;
            Time_value = 1.0/Time_value;
            break;
    }
    Sensor_Timer.attach(&Sensor_Read, Time_value);
}

void Sensor_Read(void)
{
    Sensor_flag = 1;
}