File content as of revision 28:76e2ba7a1ecd:

/**
 * 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"
#include "MahonyAHRS.h"
#define DEBUG_putc  //Wada

/* 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
MahonyAHRS    *ahrs[2];

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

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

Ticker          ticker;

float putg[3] = {0};
float putq[4] = {0};
int waittime = 80;

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

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

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

    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_2000DPS));    //Set 500DPS scale range for gyros    //0706 wada 500to2000
        printf("Acc_scale=%u\n",imu[i]->set_acc_scale(BITS_FS_16G));           //Set 4G scale range for accs        //0706 wada 4to16
        printf("AK8963 WHIAM=0x%2x\n",imu[i]->AK8963_whoami());
        imu[i]->AK8963_calib_Magnetometer();
        wait(0.1);
    }
}

// Wada June 17, 2016
void float2byte(float gq, float k)//k = gyro_divider 32.8 or q_divider 32800
{
    int tmp;
    float omegaF = gq * k;
    int16_t omegaI = (int16_t)omegaF;
    if(omegaI < 0){//1の補数
        omegaI = 0xFFFF + omegaI;
        }
    for(int i=0;i<4;i++){
        tmp = (0xF & (omegaI >> (i*4)));
        #ifdef DEBUG_putc
        pc.putc(tmp);
        #endif      
        wait_us(waittime);
    }        
}//float2byte


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]->MahonyAHRSupdate(
        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]
        );
    #ifdef DEBUG_putc
    pc.putc(0x34);  //STX
    #endif
    wait_us(waittime);
    for(int i=0; i<2; i++) {
            putg[0] = imu[i]->gyroscope_data[0];
            putg[1] = imu[i]->gyroscope_data[1];
            putg[2] = imu[i]->gyroscope_data[2];
            putq[0] = ahrs[i]->q0;
            putq[1] = ahrs[i]->q1;
            putq[2] = ahrs[i]->q2;
            putq[3] = ahrs[i]->q3;
            
            /*
            printf("%+0.3f,%+0.3f,%+0.3f,%+0.3f,%+0.3f,%+0.3f,%+0.3f,",
                putg[0]*DEGREE2RAD,
                putg[1]*DEGREE2RAD,
                putg[2]*DEGREE2RAD,
                putq[0],
                putq[1],
                putq[2],
                putq[3]);
            */
            
            float2byte(putg[0],16.4);
            float2byte(putg[1],16.4);
            float2byte(putg[2],16.4);
            float2byte(putq[0],32800);
            float2byte(putq[1],32800);
            float2byte(putq[2],32800);
            float2byte(putq[3],32800);      
            
            /*
            //test signal 29,June wada
            float2byte(-200,32.8);
            float2byte(   0,32.8);
            float2byte( 200,32.8);
            float2byte(-0.2,32800);
            float2byte(   0,32800);
            float2byte( 0.4,32800);
            float2byte( 0.8,32800);
            */
                           
    }
    #ifdef DEBUG_putc
    pc.putc(0x12);  //ETX
    #endif    
    //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);
    }
}