File content as of revision 17:7a9459ac7469:

/**
 * Masahiro FURUKAWA - m.furukawa@ist.osaka-u.ac.jp
 *
 * June 17, 2016
 *
 * MPU9250 9DoF Sensor (Extended to Ch1 ~ Ch2)
 *
 **/

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


/* MPU9250 Library
 *
 *  https://developer.mbed.org/users/kylongmu/code/MPU9250_SPI_Test/file/5839d1b118bc/main.cpp
 *  
 *   MOSI (Master Out Slave In)  p5
 *   MISO (Master In Slave Out   p6
 *   SCK  (Serial Clock)         p7
 *   ~CS  (Chip Select)          p8
 */

/* Madgwick AHRS Library 
 *
 * AHRS algorithm is one of hte sensor fusion algorism.
 * http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/AHRS algorithm is one of hte sensor fusion algorism.
 * http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/
 */
 
//define the mpu9250 object
mpu9250_spi     *imu[2];

// define AHRS filters
MadgwickAHRS    *ahrs[2];

// define serial objects
Serial          pc(USBTX, USBRX);

// define SPI object for imu objects
SPI             spi(p5, p6, p7);

Ticker          ticker;

void init(void)
{
    pc.baud(921600);

    imu[0] = new mpu9250_spi(spi, p8);
    imu[1] = new mpu9250_spi(spi, p9);

    ahrs[0] = new MadgwickAHRS();
    ahrs[1] = new MadgwickAHRS();

    for(int i=0; i<2; i++) {

        imu[0]->deselect();
        imu[1]->deselect();
        imu[i]->select();

        if(imu[i]->init(1,BITS_DLPF_CFG_188HZ)) { //INIT the mpu9250
            printf("\nCH %d\n\nCouldn't initialize MPU9250 via SPI!", i);
            wait(90);
        }
        printf("\nCH %d\nWHOAMI=0x%2x\n",i, imu[i]->whoami());                //output the I2C address to know if SPI is working, it should be 104
        printf("Gyro_scale=%u\n",imu[i]->set_gyro_scale(BITS_FS_500DPS));    //Set 500DPS scale range for gyros
        printf("Acc_scale=%u\n",imu[i]->set_acc_scale(BITS_FS_4G));           //Set 4G scale range for accs
        printf("AK8963 WHIAM=0x%2x\n",imu[i]->AK8963_whoami());
        imu[i]->AK8963_calib_Magnetometer();
        wait(0.1);
    }
}

void eventFunc(void)
{    
    for(int i=0; i<2; i++) {
        imu[0]->deselect();
        imu[1]->deselect();

        imu[i]->select();
        imu[i]->read_all();
    }
    
    // update filters
    for(int i=0; i<2; i++) 
       ahrs[i]->update(
        imu[i]->gyroscope_data[0]*DEGREE2RAD,
        imu[i]->gyroscope_data[1]*DEGREE2RAD,
        imu[i]->gyroscope_data[2]*DEGREE2RAD,
        imu[i]->accelerometer_data[0],
        imu[i]->accelerometer_data[1],
        imu[i]->accelerometer_data[2],
        imu[i]->Magnetometer[0],
        imu[i]->Magnetometer[1],
        imu[i]->Magnetometer[2]
        );
        
    for(int i=0; i<2; i++) 
        printf("%+0.3f,%+0.3f,%+0.3f,%+0.3f,%+0.3f,%+0.3f,%+0.3f,",
               imu[i]->gyroscope_data[0]*DEGREE2RAD,
               imu[i]->gyroscope_data[1]*DEGREE2RAD,
               imu[i]->gyroscope_data[2]*DEGREE2RAD,
               ahrs[i]->q0,
               ahrs[i]->q1,
               ahrs[i]->q2,
               ahrs[i]->q3);
    
    printf("\n");
}

int main()
{
    // make instances and check sensors
    init();

    // define callback event
    ticker.attach_us(eventFunc, 1000000.0f/sampleFreq);

    while(1) {

        if(pc.readable())
            if(pc.getc() == 'r') {
                ticker.detach();
                // write something event here
                ticker.attach_us(eventFunc, 1000000.0f/sampleFreq);
            }
        /*
        imu[i]->read_all();
        printf("%10.3f,%10.3f,%10.3f,%10.3f,%10.3f,%10.3f,%10.3f,%10.3f,%10.3f,%10.3f ",
               imu[i]->Temperature,
               imu[i]->gyroscope_data[0],
               imu[i]->gyroscope_data[1],
               imu[i]->gyroscope_data[2],
               imu[i]->accelerometer_data[0],
               imu[i]->accelerometer_data[1],
               imu[i]->accelerometer_data[2],
               imu[i]->Magnetometer[0],
               imu[i]->Magnetometer[1],
               imu[i]->Magnetometer[2]
              );*/
        //myled = 0;
        //wait(0.5);
    }
}