Revision 0:80f939ca1f14, committed 2015-09-04

Comitter:

yenzo

Date:

Fri Sep 04 21:37:38 2015 +0000

Commit message:

Screen-Puppet

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/DefineMPU.h	Fri Sep 04 21:37:38 2015 +0000
@@ -0,0 +1,169 @@
+#ifndef DEFINEMPU_H
+#define DEFINEMPU_H
+
+#include <stdint.h>
+
+#define MPU9250_ADDRESS 0x68<<1
+#define AK8963_ADDRESS 0x0C<<1
+
+#define WHO_AM_I_AK8963  0x00 // should return 0x48
+#define INFO             0x01
+#define AK8963_ST1       0x02  // data ready status bit 0
+#define AK8963_XOUT_L    0x03  // data
+#define AK8963_XOUT_H    0x04
+#define AK8963_YOUT_L    0x05
+#define AK8963_YOUT_H    0x06
+#define AK8963_ZOUT_L    0x07
+#define AK8963_ZOUT_H    0x08
+#define AK8963_ST2       0x09  // Data overflow bit 3 and data read error status bit 2
+#define AK8963_CNTL      0x0A  // Power down (0000), single-measurement (0001), self-test (1000) and Fuse ROM (1111) modes on bits 3:0
+#define AK8963_ASTC      0x0C  // Self test control
+#define AK8963_I2CDIS    0x0F  // I2C disable
+#define AK8963_ASAX      0x10  // Fuse ROM x-axis sensitivity adjustment value
+#define AK8963_ASAY      0x11  // Fuse ROM y-axis sensitivity adjustment value
+#define AK8963_ASAZ      0x12  // Fuse ROM z-axis sensitivity adjustment value
+
+#define SELF_TEST_X_GYRO 0x00                  
+#define SELF_TEST_Y_GYRO 0x01                                                                          
+#define SELF_TEST_Z_GYRO 0x02
+
+#define SELF_TEST_X_ACCEL 0x0D
+#define SELF_TEST_Y_ACCEL 0x0E    
+#define SELF_TEST_Z_ACCEL 0x0F
+
+#define SELF_TEST_A      0x10
+
+#define XG_OFFSET_H      0x13  // User-defined trim values for gyroscope
+#define XG_OFFSET_L      0x14
+#define YG_OFFSET_H      0x15
+#define YG_OFFSET_L      0x16
+#define ZG_OFFSET_H      0x17
+#define ZG_OFFSET_L      0x18
+#define SMPLRT_DIV       0x19
+#define CONFIG           0x1A
+#define GYRO_CONFIG      0x1B
+#define ACCEL_CONFIG     0x1C
+#define ACCEL_CONFIG2    0x1D
+#define LP_ACCEL_ODR     0x1E   
+#define WOM_THR          0x1F   
+
+#define MOT_DUR          0x20  // Duration counter threshold for motion interrupt generation, 1 kHz rate, LSB = 1 ms
+#define ZMOT_THR         0x21  // Zero-motion detection threshold bits [7:0]
+#define ZRMOT_DUR        0x22  // Duration counter threshold for zero motion interrupt generation, 16 Hz rate, LSB = 64 ms
+
+#define FIFO_EN          0x23
+#define I2C_MST_CTRL     0x24   
+#define I2C_SLV0_ADDR    0x25
+#define I2C_SLV0_REG     0x26
+#define I2C_SLV0_CTRL    0x27
+#define I2C_SLV1_ADDR    0x28
+#define I2C_SLV1_REG     0x29
+#define I2C_SLV1_CTRL    0x2A
+#define I2C_SLV2_ADDR    0x2B
+#define I2C_SLV2_REG     0x2C
+#define I2C_SLV2_CTRL    0x2D
+#define I2C_SLV3_ADDR    0x2E
+#define I2C_SLV3_REG     0x2F
+#define I2C_SLV3_CTRL    0x30
+#define I2C_SLV4_ADDR    0x31
+#define I2C_SLV4_REG     0x32
+#define I2C_SLV4_DO      0x33
+#define I2C_SLV4_CTRL    0x34
+#define I2C_SLV4_DI      0x35
+#define I2C_MST_STATUS   0x36
+#define INT_PIN_CFG      0x37
+#define INT_ENABLE       0x38
+#define DMP_INT_STATUS   0x39  // Check DMP interrupt
+#define INT_STATUS       0x3A
+#define ACCEL_XOUT_H     0x3B
+#define ACCEL_XOUT_L     0x3C
+#define ACCEL_YOUT_H     0x3D
+#define ACCEL_YOUT_L     0x3E
+#define ACCEL_ZOUT_H     0x3F
+#define ACCEL_ZOUT_L     0x40
+#define TEMP_OUT_H       0x41
+#define TEMP_OUT_L       0x42
+#define GYRO_XOUT_H      0x43
+#define GYRO_XOUT_L      0x44
+#define GYRO_YOUT_H      0x45
+#define GYRO_YOUT_L      0x46
+#define GYRO_ZOUT_H      0x47
+#define GYRO_ZOUT_L      0x48
+#define EXT_SENS_DATA_00 0x49
+#define EXT_SENS_DATA_01 0x4A
+#define EXT_SENS_DATA_02 0x4B
+#define EXT_SENS_DATA_03 0x4C
+#define EXT_SENS_DATA_04 0x4D
+#define EXT_SENS_DATA_05 0x4E
+#define EXT_SENS_DATA_06 0x4F
+#define EXT_SENS_DATA_07 0x50
+#define EXT_SENS_DATA_08 0x51
+#define EXT_SENS_DATA_09 0x52
+#define EXT_SENS_DATA_10 0x53
+#define EXT_SENS_DATA_11 0x54
+#define EXT_SENS_DATA_12 0x55
+#define EXT_SENS_DATA_13 0x56
+#define EXT_SENS_DATA_14 0x57
+#define EXT_SENS_DATA_15 0x58
+#define EXT_SENS_DATA_16 0x59
+#define EXT_SENS_DATA_17 0x5A
+#define EXT_SENS_DATA_18 0x5B
+#define EXT_SENS_DATA_19 0x5C
+#define EXT_SENS_DATA_20 0x5D
+#define EXT_SENS_DATA_21 0x5E
+#define EXT_SENS_DATA_22 0x5F
+#define EXT_SENS_DATA_23 0x60
+#define MOT_DETECT_STATUS 0x61
+#define I2C_SLV0_DO      0x63
+#define I2C_SLV1_DO      0x64
+#define I2C_SLV2_DO      0x65
+#define I2C_SLV3_DO      0x66
+#define I2C_MST_DELAY_CTRL 0x67
+#define SIGNAL_PATH_RESET  0x68
+#define MOT_DETECT_CTRL  0x69
+#define USER_CTRL        0x6A  // Bit 7 enable DMP, bit 3 reset DMP
+#define PWR_MGMT_1       0x6B // Device defaults to the SLEEP mode
+#define PWR_MGMT_2       0x6C
+#define DMP_BANK         0x6D  // Activates a specific bank in the DMP
+#define DMP_RW_PNT       0x6E  // Set read/write pointer to a specific start address in specified DMP bank
+#define DMP_REG          0x6F  // Register in DMP from which to read or to which to write
+#define DMP_REG_1        0x70
+#define DMP_REG_2        0x71 
+#define FIFO_COUNTH      0x72
+#define FIFO_COUNTL      0x73
+#define FIFO_R_W         0x74
+#define WHO_AM_I_MPU9250 0x75 // Should return 0x71
+#define XA_OFFSET_H      0x77
+#define XA_OFFSET_L      0x78
+#define YA_OFFSET_H      0x7A
+#define YA_OFFSET_L      0x7B
+#define ZA_OFFSET_H      0x7D
+#define ZA_OFFSET_L      0x7E
+
+enum Ascale {
+  AFS_2G = 0,
+  AFS_4G,
+  AFS_8G,
+  AFS_16G
+};
+
+enum Gscale {
+  GFS_250DPS = 0,
+  GFS_500DPS,
+  GFS_1000DPS,
+  GFS_2000DPS
+};
+
+enum Mscale {
+  MFS_14BITS = 0, // 0.6 mG per LSB
+  MFS_16BITS      // 0.15 mG per LSB
+};
+
+float PI = 3.14159265358979323846f;
+
+uint8_t Ascale = AFS_2G;     // AFS_2G, AFS_4G, AFS_8G, AFS_16G
+uint8_t Gscale = GFS_250DPS; // GFS_250DPS, GFS_500DPS, GFS_1000DPS, GFS_2000DPS
+uint8_t Mscale = MFS_16BITS; // MFS_14BITS or MFS_16BITS, 14-bit or 16-bit magnetometer resolution
+uint8_t Mmode = 0x06;        // Either 8 Hz 0x02) or 100 Hz (0x06) magnetometer data ODR  
+
+#endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MPU9250.h	Fri Sep 04 21:37:38 2015 +0000
@@ -0,0 +1,493 @@
+#include "DefineMPU.h"
+#include "mbed.h"
+#include "math.h"
+
+#define Kp 3.0f * 5.0f // 2 - 5 these are the free parameters in the Mahony filter and fusion scheme, Kp for proportional feedback, Ki for integral
+#define Ki 1.0f
+
+class MPU9250 {
+    public :     
+        int numero;                
+   
+    private:   
+        
+        static float Filter;
+                  
+        static float magCalibration[3]; // Factory mag calibration and mag bias
+        static float magbias[3];        // Factory mag calibration and mag bias
+        
+        static float gyroBias[3];  // Bias corrections for gyro and accelerometer
+        static float accelBias[3]; // Bias corrections for gyro and accelerometer
+        
+        int16_t accelCount[3];  // Stores the 16-bit signed accelerometer sensor output
+        int16_t gyroCount[3];   // Stores the 16-bit signed gyro sensor output
+        int16_t magCount[3];    // Stores the 16-bit signed magnetometer sensor output
+        
+        int16_t tempCount;   // Stores the real internal chip temperature in degrees Celsius
+        float temperature;
+        float SelfTest[6];
+        
+        static float GyroMeasError;   // gyroscope measurement error in rads/s (start at 60 deg/s), then reduce after ~10 s to 3
+        static float beta;            // compute beta
+        static float GyroMeasDrift;   // gyroscope measurement drift in rad/s/s (start at 0.0 deg/s/s)
+        static float zeta;  // compute zeta, the other free parameter in the Madgwick scheme usually set to a small or zero value
+    
+        float pitch, yaw, roll;
+        
+        static float q[4];        // vector to hold quaternion
+        static float eInt[3];     // vector to hold integral error for Mahony method 
+         
+        float aRes, gRes, mRes;      // scale resolutions per LSB for the sensors
+        float ax, ay, az, gx, gy, gz, mx, my, mz; // variables to hold latest sensor data values 
+                 
+    public:  
+      
+//===================================================================================================================
+//====== Set of useful function to access acceleratio, gyroscope, and temperature data
+//===================================================================================================================
+
+    MPU9250() {}
+
+    MPU9250(int n){ numero = n; }
+    
+    int GetPitch (void){ return (int)pitch; }
+    int GetRoll (void){ return (int)roll; }      
+    int GetYaw (void){ return (int)yaw; }     
+       
+    void writeByte(uint8_t address, uint8_t subAddress, uint8_t data){
+       char data_write[2];
+       data_write[0] = subAddress;
+       data_write[1] = data;       
+       i2c.write(address, data_write, 2, 0);
+   }
+
+    char readByte(uint8_t address, uint8_t subAddress){
+        char data[1]; 
+        char data_write[1];
+        data_write[0] = subAddress;
+        i2c.write(address, data_write, 1, 1); 
+        i2c.read(address, data, 1, 0); 
+        return data[0]; 
+    }
+
+    void readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t * dest){     
+        char data[14];
+        char data_write[1];
+        data_write[0] = subAddress;
+        i2c.write(address, data_write, 1, 1); 
+        i2c.read(address, data, count, 0); 
+        for(int ii = 0; ii < count; ii++) {
+            dest[ii] = data[ii];
+        }
+    } 
+ 
+    void getMres() {
+        switch (Mscale){
+            case MFS_14BITS: mRes = 10.0*4219.0/8190.0; break;
+            case MFS_16BITS: mRes = 10.0*4219.0/32760.0; break;
+        }
+    }
+
+    void getGres() {
+        switch (Gscale){
+            case GFS_250DPS: gRes = 250.0/32768.0; break;
+            case GFS_500DPS: gRes = 500.0/32768.0; break;
+            case GFS_1000DPS: gRes = 1000.0/32768.0; break;
+            case GFS_2000DPS: gRes = 2000.0/32768.0; break;
+        }
+    }
+
+    void getAres() {
+        switch (Ascale){
+            case AFS_2G: aRes = 2.0/32768.0; break;
+            case AFS_4G: aRes = 4.0/32768.0; break;
+            case AFS_8G: aRes = 8.0/32768.0; break;
+            case AFS_16G: aRes = 16.0/32768.0; break;
+        }
+    }
+
+    void readAccelData(int16_t * destination){
+        uint8_t rawData[6];  // x/y/z accel register data stored here
+        readBytes(MPU9250_ADDRESS, ACCEL_XOUT_H, 6, &rawData[0]);  // Read the six raw data registers into data array
+        destination[0] = (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ;  // Turn the MSB and LSB into a signed 16-bit value
+        destination[1] = (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ;  
+        destination[2] = (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ; 
+    }
+    
+    void readGyroData(int16_t * destination){
+        uint8_t rawData[6];  // x/y/z gyro register data stored here
+        readBytes(MPU9250_ADDRESS, GYRO_XOUT_H, 6, &rawData[0]);  // Read the six raw data registers sequentially into data array
+        destination[0] = (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ;  // Turn the MSB and LSB into a signed 16-bit value
+        destination[1] = (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ;  
+        destination[2] = (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ; 
+    }
+
+    void readMagData(int16_t * destination){
+        uint8_t rawData[7];  // x/y/z gyro register data, ST2 register stored here, must read ST2 at end of data acquisition
+        if(readByte(AK8963_ADDRESS, AK8963_ST1) & 0x01) { // wait for magnetometer data ready bit to be set
+        readBytes(AK8963_ADDRESS, AK8963_XOUT_L, 7, &rawData[0]);  // Read the six raw data and ST2 registers sequentially into data array
+        uint8_t c = rawData[6]; // End data read by reading ST2 register
+        if(!(c & 0x08)) { // Check if magnetic sensor overflow set, if not then report data
+            destination[0] = (int16_t)(((int16_t)rawData[1] << 8) | rawData[0]);  // Turn the MSB and LSB into a signed 16-bit value
+            destination[1] = (int16_t)(((int16_t)rawData[3] << 8) | rawData[2]) ;  // Data stored as little Endian
+            destination[2] = (int16_t)(((int16_t)rawData[5] << 8) | rawData[4]) ; 
+            }
+        }
+    }    
+    
+    void resetMPU9250() { // reset device
+        writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x80); // Write a one to bit 7 reset bit; toggle reset device
+        wait(0.1);
+    }
+      
+    void initAK8963(float * destination){ // First extract the factory calibration for each magnetometer axis
+        uint8_t rawData[3];  // x/y/z gyro calibration data stored here
+        writeByte(AK8963_ADDRESS, AK8963_CNTL, 0x00); // Power down magnetometer  
+        wait(0.01);
+        writeByte(AK8963_ADDRESS, AK8963_CNTL, 0x0F); // Enter Fuse ROM access mode
+        wait(0.01);
+        readBytes(AK8963_ADDRESS, AK8963_ASAX, 3, &rawData[0]);  // Read the x-, y-, and z-axis calibration values
+        destination[0] =  (float)(rawData[0] - 128)/256.0f + 1.0f;   // Return x-axis sensitivity adjustment values, etc.
+        destination[1] =  (float)(rawData[1] - 128)/256.0f + 1.0f;  
+        destination[2] =  (float)(rawData[2] - 128)/256.0f + 1.0f; 
+        writeByte(AK8963_ADDRESS, AK8963_CNTL, 0x00); // Power down magnetometer  
+        wait(0.01);
+        writeByte(AK8963_ADDRESS, AK8963_CNTL, Mscale << 4 | Mmode); // Set magnetometer data resolution and sample ODR
+        wait(0.01);
+    }
+
+    void initMPU9250(){  
+        writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x00); // Clear sleep mode bit (6), enable all sensors 
+        wait(0.1); // Delay 100 ms for PLL to get established on x-axis gyro; should check for PLL ready interrupt  
+        
+        writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x01);  // Set clock source to be PLL with x-axis gyroscope reference, bits 2:0 = 001
+        writeByte(MPU9250_ADDRESS, CONFIG, 0x03);  
+     
+        writeByte(MPU9250_ADDRESS, SMPLRT_DIV, 0x04);  // Use a 200 Hz rate; the same rate set in CONFIG above
+        
+        uint8_t c =  readByte(MPU9250_ADDRESS, GYRO_CONFIG);
+        writeByte(MPU9250_ADDRESS, GYRO_CONFIG, c & ~0xE0); // Clear self-test bits [7:5] 
+        writeByte(MPU9250_ADDRESS, GYRO_CONFIG, c & ~0x18); // Clear AFS bits [4:3]
+        writeByte(MPU9250_ADDRESS, GYRO_CONFIG, c | Gscale << 3); // Set full scale range for the gyro
+    
+        c =  readByte(MPU9250_ADDRESS, ACCEL_CONFIG);
+        writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, c & ~0xE0); // Clear self-test bits [7:5] 
+        writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, c & ~0x18); // Clear AFS bits [4:3]
+        writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, c | Ascale << 3); // Set full scale range for the accelerometer 
+    
+        c = readByte(MPU9250_ADDRESS, ACCEL_CONFIG2);
+        writeByte(MPU9250_ADDRESS, ACCEL_CONFIG2, c & ~0x0F); // Clear accel_fchoice_b (bit 3) and A_DLPFG (bits [2:0])  
+        writeByte(MPU9250_ADDRESS, ACCEL_CONFIG2, c | 0x03); // Set accelerometer rate to 1 kHz and bandwidth to 41 Hz
+    
+        writeByte(MPU9250_ADDRESS, INT_PIN_CFG, 0x22);    
+        writeByte(MPU9250_ADDRESS, INT_ENABLE, 0x01);  // Enable data ready (bit 0) interrupt
+    }
+
+    void calibrateMPU9250(float * dest1, float * dest2){  
+        uint8_t data[12]; // data array to hold accelerometer and gyro x, y, z, data
+        uint16_t ii, packet_count, fifo_count;
+        int32_t gyro_bias[3] = {0, 0, 0}, accel_bias[3] = {0, 0, 0};
+
+        writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x80); // Write a one to bit 7 reset bit; toggle reset device
+        wait(0.1);  
+
+        writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x01);  
+        writeByte(MPU9250_ADDRESS, PWR_MGMT_2, 0x00); 
+        wait(0.2);
+
+        writeByte(MPU9250_ADDRESS, INT_ENABLE, 0x00);   // Disable all interrupts
+        writeByte(MPU9250_ADDRESS, FIFO_EN, 0x00);      // Disable FIFO
+        writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x00);   // Turn on internal clock source
+        writeByte(MPU9250_ADDRESS, I2C_MST_CTRL, 0x00); // Disable I2C master
+        writeByte(MPU9250_ADDRESS, USER_CTRL, 0x00);    // Disable FIFO and I2C master modes
+        writeByte(MPU9250_ADDRESS, USER_CTRL, 0x0C);    // Reset FIFO and DMP
+        wait(0.015);
+
+        writeByte(MPU9250_ADDRESS, CONFIG, 0x01);      // Set low-pass filter to 188 Hz
+        writeByte(MPU9250_ADDRESS, SMPLRT_DIV, 0x00);  // Set sample rate to 1 kHz
+        writeByte(MPU9250_ADDRESS, GYRO_CONFIG, 0x00);  // Set gyro full-scale to 250 degrees per second, maximum sensitivity
+        writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, 0x00); // Set accelerometer full-scale to 2 g, maximum sensitivity
+        
+        uint16_t  gyrosensitivity  = 131;   // = 131 LSB/degrees/sec
+        uint16_t  accelsensitivity = 16384;  // = 16384 LSB/g
+
+        writeByte(MPU9250_ADDRESS, USER_CTRL, 0x40);   // Enable FIFO  
+        writeByte(MPU9250_ADDRESS, FIFO_EN, 0x78);     // Enable gyro and accelerometer sensors for FIFO (max size 512 bytes in MPU-9250)
+        wait(0.04); // accumulate 40 samples in 80 milliseconds = 480 bytes
+
+        writeByte(MPU9250_ADDRESS, FIFO_EN, 0x00);        // Disable gyro and accelerometer sensors for FIFO
+        readBytes(MPU9250_ADDRESS, FIFO_COUNTH, 2, &data[0]); // read FIFO sample count
+        fifo_count = ((uint16_t)data[0] << 8) | data[1];
+        packet_count = fifo_count/12;// How many sets of full gyro and accelerometer data for averaging
+    
+        for (ii = 0; ii < packet_count; ii++) {
+            int16_t accel_temp[3] = {0, 0, 0}, gyro_temp[3] = {0, 0, 0};
+            readBytes(MPU9250_ADDRESS, FIFO_R_W, 12, &data[0]); // read data for averaging
+            accel_temp[0] = (int16_t) (((int16_t)data[0] << 8) | data[1]  ) ;  // Form signed 16-bit integer for each sample in FIFO
+            accel_temp[1] = (int16_t) (((int16_t)data[2] << 8) | data[3]  ) ;
+            accel_temp[2] = (int16_t) (((int16_t)data[4] << 8) | data[5]  ) ;    
+            gyro_temp[0]  = (int16_t) (((int16_t)data[6] << 8) | data[7]  ) ;
+            gyro_temp[1]  = (int16_t) (((int16_t)data[8] << 8) | data[9]  ) ;
+            gyro_temp[2]  = (int16_t) (((int16_t)data[10] << 8) | data[11]) ;
+            
+            accel_bias[0] += (int32_t) accel_temp[0]; // Sum individual signed 16-bit biases to get accumulated signed 32-bit biases
+            accel_bias[1] += (int32_t) accel_temp[1];
+            accel_bias[2] += (int32_t) accel_temp[2];
+            gyro_bias[0]  += (int32_t) gyro_temp[0];
+            gyro_bias[1]  += (int32_t) gyro_temp[1];
+            gyro_bias[2]  += (int32_t) gyro_temp[2];       
+        }
+        
+        accel_bias[0] /= (int32_t) packet_count; // Normalize sums to get average count biases
+        accel_bias[1] /= (int32_t) packet_count;
+        accel_bias[2] /= (int32_t) packet_count;
+        gyro_bias[0]  /= (int32_t) packet_count;
+        gyro_bias[1]  /= (int32_t) packet_count;
+        gyro_bias[2]  /= (int32_t) packet_count;
+        
+        if(accel_bias[2] > 0L) {accel_bias[2] -= (int32_t) accelsensitivity;}  // Remove gravity from the z-axis accelerometer bias calculation
+        else {accel_bias[2] += (int32_t) accelsensitivity;}
+     
+        data[0] = (-gyro_bias[0]/4  >> 8) & 0xFF; // Divide by 4 to get 32.9 LSB per deg/s to conform to expected bias input format
+        data[1] = (-gyro_bias[0]/4)       & 0xFF; // Biases are additive, so change sign on calculated average gyro biases
+        data[2] = (-gyro_bias[1]/4  >> 8) & 0xFF;
+        data[3] = (-gyro_bias[1]/4)       & 0xFF;
+        data[4] = (-gyro_bias[2]/4  >> 8) & 0xFF;
+        data[5] = (-gyro_bias[2]/4)       & 0xFF;
+    
+        dest1[0] = (float) gyro_bias[0]/(float) gyrosensitivity; // construct gyro bias in deg/s for later manual subtraction
+        dest1[1] = (float) gyro_bias[1]/(float) gyrosensitivity;
+        dest1[2] = (float) gyro_bias[2]/(float) gyrosensitivity;
+    
+        int32_t accel_bias_reg[3] = {0, 0, 0}; // A place to hold the factory accelerometer trim biases
+        readBytes(MPU9250_ADDRESS, XA_OFFSET_H, 2, &data[0]); // Read factory accelerometer trim values
+        accel_bias_reg[0] = (int16_t) ((int16_t)data[0] << 8) | data[1];
+        readBytes(MPU9250_ADDRESS, YA_OFFSET_H, 2, &data[0]);
+        accel_bias_reg[1] = (int16_t) ((int16_t)data[0] << 8) | data[1];
+        readBytes(MPU9250_ADDRESS, ZA_OFFSET_H, 2, &data[0]);
+        accel_bias_reg[2] = (int16_t) ((int16_t)data[0] << 8) | data[1];
+    
+        uint32_t mask = 1uL; // Define mask for temperature compensation bit 0 of lower byte of accelerometer bias registers
+        uint8_t mask_bit[3] = {0, 0, 0}; // Define array to hold mask bit for each accelerometer bias axis
+    
+        for(ii = 0; ii < 3; ii++) {
+            if(accel_bias_reg[ii] & mask) mask_bit[ii] = 0x01; // If temperature compensation bit is set, record that fact in mask_bit
+        }
+    
+        accel_bias_reg[0] -= (accel_bias[0]/8); // Subtract calculated averaged accelerometer bias scaled to 2048 LSB/g (16 g full scale)
+        accel_bias_reg[1] -= (accel_bias[1]/8);
+        accel_bias_reg[2] -= (accel_bias[2]/8);
+        
+        data[0] = (accel_bias_reg[0] >> 8) & 0xFF;
+        data[1] = (accel_bias_reg[0])      & 0xFF;
+        data[1] = data[1] | mask_bit[0]; // preserve temperature compensation bit when writing back to accelerometer bias registers
+        data[2] = (accel_bias_reg[1] >> 8) & 0xFF;
+        data[3] = (accel_bias_reg[1])      & 0xFF;
+        data[3] = data[3] | mask_bit[1]; // preserve temperature compensation bit when writing back to accelerometer bias registers
+        data[4] = (accel_bias_reg[2] >> 8) & 0xFF;
+        data[5] = (accel_bias_reg[2])      & 0xFF;
+        data[5] = data[5] | mask_bit[2]; // preserve temperature compensation bit when writing back to accelerometer bias registers
+    
+        dest2[0] = (float)accel_bias[0]/(float)accelsensitivity; 
+        dest2[1] = (float)accel_bias[1]/(float)accelsensitivity;
+        dest2[2] = (float)accel_bias[2]/(float)accelsensitivity;
+    }   
+
+    void MadgwickQuaternionUpdate(void){
+    //void MadgwickQuaternionUpdate(float deltat){
+        
+        /*float tmp = mx;
+        mx = my;
+        my = tmp;         
+        az = -az;*/
+        
+        gx = gx*PI/180.0f;
+        gy = gy*PI/180.0f;
+        gz = gz*PI/180.0f;
+        
+        float q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3];   // short name local variable for readability
+        float norm;
+        float hx, hy, _2bx, _2bz;
+        float s1, s2, s3, s4;
+        float qDot1, qDot2, qDot3, qDot4;
+
+        float _2q1mx;
+        float _2q1my;
+        float _2q1mz;
+        float _2q2mx;
+        float _4bx;
+        float _4bz;
+        float _2q1 = 2.0f * q1;
+        float _2q2 = 2.0f * q2;
+        float _2q3 = 2.0f * q3;
+        float _2q4 = 2.0f * q4;
+        float _2q1q3 = 2.0f * q1 * q3;
+        float _2q3q4 = 2.0f * q3 * q4;
+        float q1q1 = q1 * q1;
+        float q1q2 = q1 * q2;
+        float q1q3 = q1 * q3;
+        float q1q4 = q1 * q4;
+        float q2q2 = q2 * q2;
+        float q2q3 = q2 * q3;
+        float q2q4 = q2 * q4;
+        float q3q3 = q3 * q3;
+        float q3q4 = q3 * q4;
+        float q4q4 = q4 * q4;
+
+        norm = sqrt(ax * ax + ay * ay + az * az);
+        if (norm == 0.0f) return; 
+        norm = 1.0f/norm;
+        ax *= norm;
+        ay *= norm;
+        az *= norm;
+
+        norm = sqrt(mx * mx + my * my + mz * mz);
+        if (norm == 0.0f) return; 
+        norm = 1.0f/norm;
+        mx *= norm;
+        my *= norm;
+        mz *= norm;
+
+        _2q1mx = 2.0f * q1 * mx;
+        _2q1my = 2.0f * q1 * my;
+        _2q1mz = 2.0f * q1 * mz;
+        _2q2mx = 2.0f * q2 * mx;
+        
+        hx = mx * q1q1 - _2q1my * q4 + _2q1mz * q3 + mx * q2q2 + _2q2 * my * q3 + _2q2 * mz * q4 - mx * q3q3 - mx * q4q4;
+        hy = _2q1mx * q4 + my * q1q1 - _2q1mz * q2 + _2q2mx * q3 - my * q2q2 + my * q3q3 + _2q3 * mz * q4 - my * q4q4;
+        _2bx = sqrt(hx * hx + hy * hy);
+        _2bz = -_2q1mx * q3 + _2q1my * q2 + mz * q1q1 + _2q2mx * q4 - mz * q2q2 + _2q3 * my * q4 - mz * q3q3 + mz * q4q4;
+        _4bx = 2.0f * _2bx;
+        _4bz = 2.0f * _2bz;
+
+        s1 = -_2q3 * (2.0f * q2q4 - _2q1q3 - ax) + _2q2 * (2.0f * q1q2 + _2q3q4 - ay) - _2bz * q3 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q4 + _2bz * q2) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + _2bx * q3 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
+        s2 = _2q4 * (2.0f * q2q4 - _2q1q3 - ax) + _2q1 * (2.0f * q1q2 + _2q3q4 - ay) - 4.0f * q2 * (1.0f - 2.0f * q2q2 - 2.0f * q3q3 - az) + _2bz * q4 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (_2bx * q3 + _2bz * q1) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + (_2bx * q4 - _4bz * q2) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
+        s3 = -_2q1 * (2.0f * q2q4 - _2q1q3 - ax) + _2q4 * (2.0f * q1q2 + _2q3q4 - ay) - 4.0f * q3 * (1.0f - 2.0f * q2q2 - 2.0f * q3q3 - az) + (-_4bx * q3 - _2bz * q1) * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (_2bx * q2 + _2bz * q4) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + (_2bx * q1 - _4bz * q3) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
+        s4 = _2q2 * (2.0f * q2q4 - _2q1q3 - ax) + _2q3 * (2.0f * q1q2 + _2q3q4 - ay) + (-_4bx * q4 + _2bz * q2) * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q1 + _2bz * q3) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + _2bx * q2 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
+        
+        norm = sqrt(s1 * s1 + s2 * s2 + s3 * s3 + s4 * s4);    
+        norm = 1.0f/norm;
+        s1 *= norm;
+        s2 *= norm;
+        s3 *= norm;
+        s4 *= norm;
+
+        qDot1 = 0.5f * (-q2 * gx - q3 * gy - q4 * gz) - beta * s1;
+        qDot2 = 0.5f * (q1 * gx + q3 * gz - q4 * gy) - beta * s2;
+        qDot3 = 0.5f * (q1 * gy - q2 * gz + q4 * gx) - beta * s3;
+        qDot4 = 0.5f * (q1 * gz + q2 * gy - q3 * gx) - beta * s4;
+
+        /*q1 += qDot1 * deltat;
+        q2 += qDot2 * deltat;
+        q3 += qDot3 * deltat;
+        q4 += qDot4 * deltat;*/
+        
+        q1 += qDot1 * Filter;
+        q2 += qDot2 * Filter;
+        q3 += qDot3 * Filter;
+        q4 += qDot4 * Filter;      
+        
+        norm = sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4); 
+        norm = 1.0f/norm;
+        q[0] = q1 * norm;
+        q[1] = q2 * norm;
+        q[2] = q3 * norm;
+        q[3] = q4 * norm;
+    }
+    
+    int Float_to_Int(float numb){
+        float n = numb -(int)numb;
+        if(n >= 0 && n < 0.5) return (int)numb;
+        if(n >= 0.5 && n <= 1) return (int)numb + 1;
+        if(n <= 0 && n > -0.5) return (int)numb;
+        if(n <= -0.5 && n >= -1) return (int)numb - 1;
+        else return 0;
+    }  
+    
+    void CalibIMU(void){
+        wait(1);          
+        resetMPU9250();  
+        calibrateMPU9250(gyroBias, accelBias);   
+        wait(2);  
+        initMPU9250();    
+        initAK8963(magCalibration);  
+        wait(2);       
+    }
+
+    void BiasIMU(void){
+        getAres(); 
+        getGres(); 
+        getMres();  
+        magbias[0] = +470.;  
+        magbias[1] = +120.;  
+        magbias[2] = +125.;  
+    } 
+    
+    void GetQuaternion(void){                
+        readAccelData(accelCount);  
+        ax = (float)accelCount[0]*aRes - accelBias[0];  
+        ay = (float)accelCount[1]*aRes - accelBias[1];   
+        az = (float)accelCount[2]*aRes - accelBias[2];  
+        
+        readGyroData(gyroCount);  
+        gx = (float)gyroCount[0]*gRes - gyroBias[0]; 
+        gy = (float)gyroCount[1]*gRes - gyroBias[1];  
+        gz = (float)gyroCount[2]*gRes - gyroBias[2];   
+        
+        readMagData(magCount); 
+        mx = (float)magCount[0]*mRes*magCalibration[0] - magbias[0];  
+        my = (float)magCount[1]*mRes*magCalibration[1] - magbias[1];  
+        mz = (float)magCount[2]*mRes*magCalibration[2] - magbias[2];                                       
+    } 
+    
+    void FinalQuaternion(void){
+        yaw   = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]);   
+        pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
+        roll  = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);
+        pitch *= 180.0f / PI;
+        yaw   *= 180.0f / PI; 
+        yaw   -= 0.9f;
+        roll  *= 180.0f / PI; 
+        
+        /*if((q[0]*q[1] + q[2]*q[3]) == 0.5){    //north pole
+            pitch = atan2(2*q[1]*q[3]-2*q[0]*q[2], 1 - 2*(q[1] * q[1]) - 2*(q[2] * q[2]));
+            yaw = atan2(2*q[0]*q[3]-2*q[1]*q[2], 1 - 2*(q[0] * q[0]) - 2*(q[2] * q[2]));
+        }   
+        else{
+            pitch = 2.0 * atan2(q[0],q[4]);
+            yaw = 0;           
+        }
+        
+        if((q[0]*q[1] + q[2]*q[3]) == -0.5){   //south pole
+            pitch = atan2(2*q[1]*q[3]-2*q[0]*q[2], 1 - 2*(q[1] * q[1]) - 2*(q[2] * q[2]));
+            yaw = atan2(2*q[0]*q[3]-2*q[1]*q[2], 1 - 2*(q[0] * q[0]) - 2*(q[2] * q[2]));
+        }   
+        else {
+            pitch = -2.0 * atan2(q[0],q[4]);
+            yaw = 0;        
+        }                     
+        roll = asin(2*q[0]*q[1] + 2*q[2]*q[3]);*/ 
+
+        
+        
+        //if(roll < 180 & roll > 0) roll -= 180; 
+        //else if(roll > -180 & roll < 0) roll += 180; 
+        
+        pitch = Float_to_Int(pitch);
+        roll = Float_to_Int(roll);
+        yaw = Float_to_Int(yaw);    
+    }         
+};
+  
+//Definition des variables dans la Class MPU9250, car la definition ne peut pas être faite à l'intérieur  
+float MPU9250::gyroBias[3] = {0, 0, 0};
+float MPU9250::accelBias[3] = {0, 0, 0}; // Bias corrections for gyro and accelerometer 
+float MPU9250::magCalibration[3] = {0, 0, 0};
+float MPU9250::magbias[3] = {0, 0, 0};
+float MPU9250::GyroMeasError = PI * (60.0f / 180.0f);     // gyroscope measurement error in rads/s (start at 60 deg/s), then reduce after ~10 s to 3
+float MPU9250::beta = sqrt(3.0f / 4.0f) * GyroMeasError;  // compute beta
+float MPU9250::GyroMeasDrift = PI * (1.0f / 180.0f);      // gyroscope measurement drift in rad/s/s (start at 0.0 deg/s/s)
+float MPU9250::zeta = sqrt(3.0f / 4.0f) * GyroMeasDrift;  // compute zeta, the other free parameter in the Madgwick scheme usually set to a small or zero value
+float MPU9250::q[4] = {1.0f, 0.0f, 0.0f, 0.0f};           // vector to hold quaternion
+float MPU9250::eInt[3] = {0.0f, 0.0f, 0.0f};              // vector to hold integral error for Mahony method
+float MPU9250::Filter = 0.05;
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PCA9547.lib	Fri Sep 04 21:37:38 2015 +0000
@@ -0,0 +1,1 @@
+https://developer.mbed.org/users/yenzo/code/PCA9547/#d0c2d2b24941

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PowerControl.lib	Fri Sep 04 21:37:38 2015 +0000
@@ -0,0 +1,1 @@
+https://developer.mbed.org/users/yenzo/code/PowerControl/#dedd847106ba

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Sep 04 21:37:38 2015 +0000
@@ -0,0 +1,116 @@
+#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"); 
+        }       
+    }
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Fri Sep 04 21:37:38 2015 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/7cff1c4259d7
\ No newline at end of file