File content as of revision 1:e542337d8586:

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

// Serial comms
Serial pc(USBTX, USBRX);

// Sensor board library
MPU9250 mpu = MPU9250(p26, p27);


// Configuration
bool test_comms = true;
bool do_sensor_init = false;
bool do_sensor_self_test = true;
bool print_accel = true;
bool print_gyro = true;

int main() {
    float test_result[6] = {0.0,0.0,0.0,0.0,0.0,0.0};
    int16_t accel[3] = {0,0,0};
    int16_t gyro[3] = {0,0,0};
    int16_t temp = 0;
    
    // Test MPU connection
    if (test_comms) {
        uint8_t whoami = mpu.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");
        wait(1);   
    }

    // Initialise sensor board
    if (do_sensor_init) {
        pc.printf("starting Init\n\r");
        mpu.initMPU9250();
        pc.printf("returned from init\n\r");
        wait(1);
    }
    
    // Run sensor board self-test
    if (do_sensor_self_test) {
        mpu.MPU9250SelfTest(test_result);
        for (int i = 0; i<6; i++)
            pc.printf("Self test result %i: %f\n", i, test_result[i]);
            
        wait(1);
    }
            
    // Stream sensor values (accel or gyro)
    while(1) {
        if (print_accel) {
            mpu.readAccelData(accel);
            float ax = accel[0] * 2.0 / 32768.0;
            float ay = accel[1] * 2.0 / 32768.0;
            float az = accel[2] * 2.0 / 32768.0;
            pc.printf("accel: (%f, %f, %f)\n", ax,ay,az);
        }
        
        if (print_gyro) {
            mpu.readGyroData(gyro);
            float gx = gyro[0] * 250.0 / 32768.0;
            float gy = gyro[1] * 250.0 / 32768.0;
            float gz = gyro[2] * 250.0 / 32768.0;
            pc.printf("gyro: (%f, %f, %f)\n", gx,gy,gz);
        }
        wait(0.1);
    }
}