File content as of revision 1:f0f34b17c4f0:

#include "PCA9547/PCA9547.h"
#include "PowerControl/PowerControl.h"
#include "PowerControl/EthernetPowerControl.h"
#define I2C_SDA p9 //28
#define I2C_SCL p10 //27

I2C i2c(I2C_SDA, I2C_SCL);

PCA9547 mux( i2c, 0xE0 );
int selectmux = 0;

Serial pc(USBTX, USBRX); // tx, rx
Timer t;
int delt_t = 0, count = 0;
int tpsend = 0;

int IMU_DATA[9][3];
int IMU_DATA_COMP[9][3];

#include "MPU9250.h"

MPU9250 IMU1(1), IMU2(2), IMU3(3), IMU4(4), IMU5(5), IMU6(6), IMU7(7), IMU8(8), IMU9(9);
MPU9250 NIMU[9];

void flushSerialBuffer(void) { 
    char char1 = 0; 
    while (pc.readable()) { 
        char1 = pc.getc(); 
    } 
    return; 
}

int main() { 
    NIMU[0] = IMU1;
    NIMU[1] = IMU2;
    NIMU[2] = IMU3;
    NIMU[3] = IMU4;
    NIMU[4] = IMU5;
    NIMU[5] = IMU6;
    NIMU[6] = IMU7;
    NIMU[7] = IMU8;
    NIMU[8] = IMU9;
    
    pc.baud(9600);  
    i2c.frequency(400000);
    PHY_PowerDown(); //Eteind le module Ethernet du Mbed afin d'économiser l'energie 
    t.start();
 
    for(int i = 0; i<8; i++){      
        mux.select( i );
        wait_us(5);  
        /*i2c.stop();
        wait_us(5); 
        i2c.start();*/
 
        uint8_t whoami_imu = 0;
        while(whoami_imu != 0x71){
            whoami_imu = NIMU[i].readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250);  
        } 
              
        NIMU[i].CalibIMU();    
        NIMU[i].BiasIMU();  
        
        pc.printf("Initilisation IMU numero : %d OK\n\r", i+1); 
        pc.printf("\n\r");
    }
      
    selectmux = 0;
    mux.select( selectmux ); 
    wait_us(10);  
    /*i2c.stop();
    wait_us(10); 
    i2c.start();*/
              
    while(1){      

        if(NIMU[selectmux].readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01){        
            NIMU[selectmux].GetQuaternion();
        }
               
        delt_t = t.read_ms() - count;
        
        if (delt_t > 20) {   
            NIMU[selectmux].MadgwickQuaternionUpdate();
            NIMU[selectmux].FinalQuaternion(); 
            
            IMU_DATA[selectmux][0] = NIMU[selectmux].GetPitch();
            IMU_DATA[selectmux][1] = NIMU[selectmux].GetRoll();
            IMU_DATA[selectmux][2] = NIMU[selectmux].GetYaw();
             
            selectmux++;
            if(selectmux == 8) selectmux = 0;            
            mux.select( selectmux );
            wait_us(10);   
            /*i2c.stop();
            wait_us(10); 
            i2c.start();*/
            
            tpsend+=delt_t;
            count = t.read_ms(); 
            
            flushSerialBuffer();
        }

        if(tpsend > 500){                  
            tpsend = 0;     

            for(int i=0;i<8;i++){
                //pc.printf("%d;%d;%d\n\r",IMU_DATA[i][0], IMU_DATA[i][1], IMU_DATA[i][2]);// 0 = Pitch, 1 = Roll et 2 = Yaw
                pc.printf("IMU n%d, Pitch = %d; Roll = %d; Yaw = %d\n\r",i, IMU_DATA[i][0], IMU_DATA[i][1], IMU_DATA[i][2]);// 0 = Pitch, 1 = Roll et 2 = Yaw
            }
            
            pc.printf("\n"); 
        }       
    }
}