Revision 3:4c1180a712e3, committed 2018-03-15

Comitter:

JohanBeverini

Date:

Thu Mar 15 15:02:02 2018 +0000

Parent:

2:e0381ca0edac

Commit message:

+ filtre + matrice euler

Changed in this revision

--- a/MPU6050.h	Sun Jun 29 21:53:23 2014 +0000
+++ b/MPU6050.h	Thu Mar 15 15:02:02 2018 +0000
@@ -153,14 +153,14 @@
 int Ascale = AFS_2G;
 
 //Set up I2C, (SDA,SCL)
-I2C i2c(I2C_SDA, I2C_SCL);
+I2C i2c(PB_7, PB_6);
 
 DigitalOut myled(LED1);
    
 float aRes, gRes; // scale resolutions per LSB for the sensors
   
 // Pin definitions
-int intPin = 12;  // These can be changed, 2 and 3 are the Arduinos ext int pins
+int intPin = 0;  // These can be changed, 2 and 3 are the Arduinos ext int pins
 
 int16_t accelCount[3];  // Stores the 16-bit signed accelerometer sensor output
 float ax, ay, az;       // Stores the real accel value in g's

--- a/N5110.lib	Sun Jun 29 21:53:23 2014 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,1 +0,0 @@
-http://mbed.org/users/onehorse/code/MPU60506-axisMotionSensor/#313c258ada8a

--- a/main.cpp	Sun Jun 29 21:53:23 2014 +0000
+++ b/main.cpp	Thu Mar 15 15:02:02 2018 +0000
@@ -1,66 +1,79 @@
-
-/* MPU6050 Basic Example Code
- by: Kris Winer
- date: May 1, 2014
- license: Beerware - Use this code however you'd like. If you 
- find it useful you can buy me a beer some time.
- 
- Demonstrate  MPU-6050 basic functionality including initialization, accelerometer trimming, sleep mode functionality as well as
- parameterizing the register addresses. Added display functions to allow display to on breadboard monitor. 
- No DMP use. We just want to get out the accelerations, temperature, and gyro readings.
- 
- SDA and SCL should have external pull-up resistors (to 3.3V).
- 10k resistors worked for me. They should be on the breakout
- board.
- 
- Hardware setup:
- MPU6050 Breakout --------- Arduino
- 3.3V --------------------- 3.3V
- SDA ----------------------- A4
- SCL ----------------------- A5
- GND ---------------------- GND
- 
-  Note: The MPU6050 is an I2C sensor and uses the Arduino Wire library. 
- Because the sensor is not 5V tolerant, we are using a 3.3 V 8 MHz Pro Mini or a 3.3 V Teensy 3.1.
- We have disabled the internal pull-ups used by the Wire library in the Wire.h/twi.c utility file.
- We are also using the 400 kHz fast I2C mode by setting the TWI_FREQ  to 400000L /twi.h utility file.
- */
- 
 #include "mbed.h"
 #include "MPU6050.h"
-#include "N5110.h"
+#include <math.h>
 
-// Using NOKIA 5110 monochrome 84 x 48 pixel display
-// pin 9 - Serial clock out (SCLK)
-// pin 8 - Serial data out (DIN)
-// pin 7 - Data/Command select (D/C)
-// pin 5 - LCD chip select (CS)
-// pin 6 - LCD reset (RST)
-//Adafruit_PCD8544 display = Adafruit_PCD8544(9, 8, 7, 5, 6);
 
 float sum = 0;
 uint32_t sumCount = 0;
 
    MPU6050 mpu6050;
    
+   AnalogOut ANA1(A3);
+   //AnalogOut ANA2(PA_5);
+   
+   Ticker ms;
+   
    Timer t;
 
-   Serial pc(USBTX, USBRX); // tx, rx
+   Serial pc(SERIAL_TX, SERIAL_RX); // tx, rx
+   
+   Serial BT(PA_9, PA_10); // tx, rx
+   
 
-   //        VCC,   SCE,  RST,  D/C,  MOSI,S CLK, LED
-   N5110 lcd(PA_8, PB_10, PA_9, PA_6, PA_7, PA_5, PC_7);
+   
+    float alpha, betaa, gammaa;
+    float axx, ayy, azz;
+    float poid[3];
+    float a, b, c, d, e, s;
+    int i;
+    float matrice[3][3], resultat[3];
+    
+    bool first = true;
+    
+    bool tick_mili;
+    
+    float x_x_filter[3]={0,0,0}, x_y_filter[3]={0,0,0};
+    float y_x_filter[3]={0,0,0}, y_y_filter[3]={0,0,0};
+    float z_x_filter[3]={0,0,0}, z_y_filter[3]={0,0,0};
+    float a_coef[3]={1.0000,   -1.5610,    0.6414};
+    float b_coef[3]={0.0201,    0.0402,    0.0201};
+    float x_x_filter_ph[3]={0,0,0}, x_y_filter_ph[3]={0,0,0};
+    float y_x_filter_ph[3]={0,0,0}, y_y_filter_ph[3]={0,0,0};
+    float z_x_filter_ph[3]={0,0,0}, z_y_filter_ph[3]={0,0,0};
+    float a_coef_ph[3]={1.0000,   -1.9956,    0.9956};
+    float b_coef_ph[3]={0.9978,   -1.9956,    0.9978};
+    float gx_filtre, gy_filtre, gz_filtre;
+    float gx_filtre2=0.0f, gy_filtre2=0.0f, gz_filtre2=0.0f;
+    float trapeze_x = 0.0f;
+    float trapeze_y = 0.0f;
+    float trapeze_z = 0.0f;
+ 
+void mili(void){
+    tick_mili=true;
+}
+ 
+ 
         
 int main()
 {
   pc.baud(9600);  
+  BT.baud(9600); 
+  
+  pc.printf("hello word\n");
+  BT.printf("connection...\n");
 
   //Set up I2C
   i2c.frequency(400000);  // use fast (400 kHz) I2C   
   
+    alpha=0;
+    betaa=0;
+    gammaa=0;  
+  
+  ms.attach(&mili, 0.001);
   t.start();        
   
-  lcd.init();
-  lcd.setBrightness(0.05);
+  //lcd.init();
+  //lcd.setBrightness(0.05);
   
     
   // Read the WHO_AM_I register, this is a good test of communication
@@ -71,8 +84,8 @@
   {  
     pc.printf("MPU6050 is online...");
     wait(1);
-    lcd.clear();
-    lcd.printString("MPU6050 OK", 0, 0);
+    //lcd.clear();
+    //lcd.printString("MPU6050 OK", 0, 0);
 
     
     mpu6050.MPU6050SelfTest(SelfTest); // Start by performing self test and reporting values
@@ -90,20 +103,20 @@
     mpu6050.calibrateMPU6050(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers  
     mpu6050.initMPU6050(); pc.printf("MPU6050 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature
 
-    lcd.clear();
-    lcd.printString("MPU6050", 0, 0);
-    lcd.printString("pass self test", 0, 1);
-    lcd.printString("initializing", 0, 2);  
+    //lcd.clear();
+    //lcd.printString("MPU6050", 0, 0);
+    //lcd.printString("pass self test", 0, 1);
+    //lcd.printString("initializing", 0, 2);  
     wait(2);
        }
     else
     {
     pc.printf("Device did not the pass self-test!\n\r");
  
-       lcd.clear();
-       lcd.printString("MPU6050", 0, 0);
-       lcd.printString("no pass", 0, 1);
-       lcd.printString("self test", 0, 2);      
+       //lcd.clear();
+       //lcd.printString("MPU6050", 0, 0);
+       //lcd.printString("no pass", 0, 1);
+       //lcd.printString("self test", 0, 2);      
       }
     }
     else
@@ -111,10 +124,10 @@
     pc.printf("Could not connect to MPU6050: \n\r");
     pc.printf("%#x \n",  whoami);
  
-    lcd.clear();
-    lcd.printString("MPU6050", 0, 0);
-    lcd.printString("no connection", 0, 1);
-    lcd.printString("0x", 0, 2);  lcd.setXYAddress(20, 2); lcd.printChar(whoami);
+    //lcd.clear();
+    //lcd.printString("MPU6050", 0, 0);
+    //lcd.printString("no connection", 0, 1);
+    //lcd.printString("0x", 0, 2);  lcd.setXYAddress(20, 2); lcd.printChar(whoami);
  
     while(1) ; // Loop forever if communication doesn't happen
   }
@@ -123,6 +136,10 @@
 
  while(1) {
   
+  if (tick_mili==true){
+      tick_mili=false;
+  
+  
   // If data ready bit set, all data registers have new data
   if(mpu6050.readByte(MPU6050_ADDRESS, INT_STATUS) & 0x01) {  // check if data ready interrupt
     mpu6050.readAccelData(accelCount);  // Read the x/y/z adc values
@@ -139,7 +156,8 @@
     // Calculate the gyro value into actual degrees per second
     gx = (float)gyroCount[0]*gRes; // - gyroBias[0];  // get actual gyro value, this depends on scale being set
     gy = (float)gyroCount[1]*gRes; // - gyroBias[1];  
-    gz = (float)gyroCount[2]*gRes; // - gyroBias[2];   
+    gz = (float)gyroCount[2]*gRes; // - gyroBias[2];
+ 
 
     tempCount = mpu6050.readTempData();  // Read the x/y/z adc values
     temperature = (tempCount) / 340. + 36.53; // Temperature in degrees Centigrade
@@ -158,11 +176,111 @@
     }
     
    // Pass gyro rate as rad/s
-    mpu6050.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f);
+    //mpu6050.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f);
+
+
+    //gx*=PI/180.0f;
+    //gy*=PI/180.0f;
+    //gz*=PI/180.0f;
+    //gx/=1000.0f;
+    //gy/=1000.0f;
+    //gz/=1000.0f;
+    
+    ////////filtre PB 100Hz / PH 1Hz
+    //x_x_filter[6]=x_x_filter[5]; x_x_filter[5]=x_x_filter[4]; x_x_filter[4]=x_x_filter[3]; 
+    //x_x_filter[3]=x_x_filter[2]; 
+    x_x_filter[2]=x_x_filter[1]; x_x_filter[1]=x_x_filter[0];
+    x_x_filter[0]=gx;
+    //x_y_filter[6]=x_y_filter[5]; x_y_filter[5]=x_y_filter[4]; x_y_filter[4]=x_y_filter[3];
+    //x_y_filter[3]=x_y_filter[2]; 
+    x_y_filter[2]=x_y_filter[1]; x_y_filter[1]=x_y_filter[0];
+    x_y_filter[0]=b_coef[0]*x_x_filter[0]+b_coef[1]*x_x_filter[1]+b_coef[2]*x_x_filter[2] //+b_coef[3]*x_x_filter[3] //+b_coef[4]*x_x_filter[4]+b_coef[5]*x_x_filter[5]+b_coef[6]*x_x_filter[6]
+                    -(a_coef[1]*x_y_filter[1]+a_coef[2]*x_y_filter[2]); //+a_coef[3]*x_y_filter[3]); //+a_coef[4]*x_y_filter[4]+a_coef[5]*x_y_filter[5]+a_coef[6]*x_y_filter[6]);
+    gx_filtre=x_y_filter[0];
+    
+    //y_x_filter[6]=y_x_filter[5]; y_x_filter[5]=y_x_filter[4]; y_x_filter[4]=y_x_filter[3];
+    //y_x_filter[3]=y_x_filter[2]; 
+    y_x_filter[2]=y_x_filter[1]; y_x_filter[1]=y_x_filter[0];
+    y_x_filter[0]=gy;
+    //y_y_filter[6]=y_y_filter[5]; y_y_filter[5]=y_y_filter[4]; y_y_filter[4]=y_y_filter[3];
+    //y_y_filter[3]=y_y_filter[2]; 
+    y_y_filter[2]=y_y_filter[1]; y_y_filter[1]=y_y_filter[0];
+    y_y_filter[0]=b_coef[0]*y_x_filter[0]+b_coef[1]*y_x_filter[1]+b_coef[2]*y_x_filter[2] //+b_coef[3]*y_x_filter[3] //+b_coef[4]*y_x_filter[4]+b_coef[5]*y_x_filter[5]+b_coef[6]*y_x_filter[6]
+                    -(a_coef[1]*y_y_filter[1]+a_coef[2]*y_y_filter[2]); //+a_coef[3]*y_y_filter[3]); //+a_coef[4]*y_y_filter[4]+a_coef[5]*y_y_filter[5]+a_coef[6]*y_y_filter[6]);
+    gy_filtre=y_y_filter[0];
+    
+    //z_x_filter[6]=z_x_filter[5]; z_x_filter[5]=z_x_filter[4]; z_x_filter[4]=z_x_filter[3]; 
+    //z_x_filter[3]=z_x_filter[2];
+    z_x_filter[2]=z_x_filter[1]; z_x_filter[1]=z_x_filter[0];
+    z_x_filter[0]=gz;
+    //z_y_filter[6]=z_y_filter[5]; z_y_filter[5]=z_y_filter[4]; z_y_filter[4]=z_y_filter[3]; 
+    //z_y_filter[3]=z_y_filter[2]; 
+    z_y_filter[2]=z_y_filter[1]; z_y_filter[1]=z_y_filter[0];
+    z_y_filter[0]=b_coef[0]*z_x_filter[0]+b_coef[1]*z_x_filter[1]+b_coef[2]*z_x_filter[2] //+b_coef[3]*z_x_filter[3] //+b_coef[4]*z_x_filter[4]+b_coef[5]*z_x_filter[5]+b_coef[6]*z_x_filter[6]
+                    -(a_coef[1]*z_y_filter[1]+a_coef[2]*z_y_filter[2]); //+a_coef[3]*z_y_filter[3]); //+a_coef[4]*z_y_filter[4]+a_coef[5]*z_y_filter[5]+a_coef[6]*z_y_filter[6]);
+    gz_filtre=z_y_filter[0];
+    
+     ////////filtre PB 100Hz / PH 1Hz
+    //x_x_filter[6]=x_x_filter[5]; x_x_filter[5]=x_x_filter[4]; x_x_filter[4]=x_x_filter[3]; 
+    //x_x_filter_ph[3]=x_x_filter_ph[2]; 
+    x_x_filter_ph[2]=x_x_filter_ph[1]; x_x_filter_ph[1]=x_x_filter_ph[0];
+   x_x_filter_ph[0]=gx_filtre;
+    //x_y_filter[6]=x_y_filter[5]; x_y_filter[5]=x_y_filter[4]; x_y_filter[4]=x_y_filter[3];
+    //x_y_filter_ph[3]=x_y_filter_ph[2]; 
+    x_y_filter_ph[2]=x_y_filter_ph[1]; x_y_filter_ph[1]=x_y_filter_ph[0];
+    x_y_filter_ph[0]=b_coef_ph[0]*x_x_filter_ph[0]+b_coef_ph[1]*x_x_filter_ph[1]+b_coef_ph[2]*x_x_filter_ph[2] //+b_coef_ph[3]*x_x_filter_ph[3] //+b_coef[4]*x_x_filter[4]+b_coef[5]*x_x_filter[5]+b_coef[6]*x_x_filter[6]
+                    -(a_coef_ph[1]*x_y_filter_ph[1]+a_coef_ph[2]*x_y_filter_ph[2]); //+a_coef_ph[3]*x_y_filter_ph[3]); //+a_coef[4]*x_y_filter[4]+a_coef[5]*x_y_filter[5]+a_coef[6]*x_y_filter[6]);
+    gx_filtre=x_y_filter_ph[0];
+    
+   //y_x_filter[6]=y_x_filter[5]; y_x_filter[5]=y_x_filter[4]; y_x_filter[4]=y_x_filter[3];
+    //y_x_filter_ph[3]=y_x_filter_ph[2]; 
+    y_x_filter_ph[2]=y_x_filter_ph[1]; y_x_filter_ph[1]=y_x_filter_ph[0];
+    y_x_filter_ph[0]=gy_filtre;
+    //y_y_filter[6]=y_y_filter[5]; y_y_filter[5]=y_y_filter[4]; y_y_filter[4]=y_y_filter[3];
+    //y_y_filter_ph[3]=y_y_filter_ph[2]; 
+    y_y_filter_ph[2]=y_y_filter_ph[1]; y_y_filter_ph[1]=y_y_filter_ph[0];
+    y_y_filter_ph[0]=b_coef_ph[0]*y_x_filter_ph[0]+b_coef_ph[1]*y_x_filter_ph[1]+b_coef_ph[2]*y_x_filter_ph[2] //+b_coef_ph[3]*y_x_filter_ph[3] //+b_coef[4]*y_x_filter[4]+b_coef[5]*y_x_filter[5]+b_coef[6]*y_x_filter[6]
+                    -(a_coef_ph[1]*y_y_filter_ph[1]+a_coef_ph[2]*y_y_filter_ph[2]); //+a_coef_ph[3]*y_y_filter_ph[3]); //+a_coef[4]*y_y_filter[4]+a_coef[5]*y_y_filter[5]+a_coef[6]*y_y_filter[6]);
+    gy_filtre=y_y_filter_ph[0];
+    
+   //z_x_filter[6]=z_x_filter[5]; z_x_filter[5]=z_x_filter[4]; z_x_filter[4]=z_x_filter[3]; 
+    //z_x_filter_ph[3]=z_x_filter_ph[2]; 
+    z_x_filter_ph[2]=z_x_filter_ph[1]; z_x_filter_ph[1]=z_x_filter_ph[0];
+    z_x_filter_ph[0]=gz_filtre;
+    //z_y_filter[6]=z_y_filter[5]; z_y_filter[5]=z_y_filter[4]; z_y_filter[4]=z_y_filter[3]; 
+    //z_y_filter_ph[3]=z_y_filter_ph[2]; 
+    z_y_filter_ph[2]=z_y_filter_ph[1]; z_y_filter_ph[1]=z_y_filter_ph[0];
+    z_y_filter_ph[0]=b_coef_ph[0]*z_x_filter_ph[0]+b_coef_ph[1]*z_x_filter_ph[1]+b_coef_ph[2]*z_x_filter_ph[2] //+b_coef_ph[3]*z_x_filter_ph[3] //+b_coef[4]*z_x_filter[4]+b_coef[5]*z_x_filter[5]+b_coef[6]*z_x_filter[6]
+                    -(a_coef_ph[1]*z_y_filter_ph[1]+a_coef_ph[2]*z_y_filter_ph[2]); //+a_coef_ph[3]*z_y_filter_ph[3]); //+a_coef[4]*z_y_filter[4]+a_coef[5]*z_y_filter[5]+a_coef[6]*z_y_filter[6]);
+    gz_filtre=z_y_filter_ph[0];
+
+
+    trapeze_x=deltat*((gx_filtre+gx_filtre2)/2.0f);
+    trapeze_y=deltat*((gy_filtre+gy_filtre2)/2.0f);
+    trapeze_z=deltat*((gz_filtre+gz_filtre2)/2.0f);
+    
+    gx_filtre2=gx_filtre;
+    gy_filtre2=gy_filtre;
+    gz_filtre2=gz_filtre;
+
+    //calcule angle
+    alpha+=trapeze_x;
+    betaa+=trapeze_y;
+    gammaa+=trapeze_z;  
+
+    if(alpha>=360.0f){alpha-=360.0f;}
+    if(alpha<=-360.0f){alpha+=360.0f;}
+    if(betaa>=360.0f){betaa-=360.0f;}
+    if(betaa<=-360.0f){betaa+=360.0f;}
+    if(gammaa>=360.0f){gammaa-=360.0f;}
+    if(gammaa<=-360.0f){gammaa+=360.0f;}
+
+    ANA1.write((alpha+500.0f)/1000.0f);
+    //ANA2.write(alpha/360.0f);
 
     // Serial print and/or display at 0.5 s rate independent of data rates
     delt_t = t.read_ms() - count;
-    if (delt_t > 500) { // update LCD once per half-second independent of read rate
+    if (delt_t > 100) { // update LCD once per half-second independent of read rate
 
     pc.printf("ax = %f", 1000*ax); 
     pc.printf(" ay = %f", 1000*ay); 
@@ -172,19 +290,23 @@
     pc.printf(" gy = %f", gy); 
     pc.printf(" gz = %f  deg/s\n\r", gz); 
     
+//    pc.printf("post filtre : gx = %f", gx_filtre2); 
+//    pc.printf(" gy = %f", gy_filtre2); 
+//    pc.printf(" gz = %f  deg/s\n\r", gz_filtre2);
+    
     pc.printf(" temperature = %f  C\n\r", temperature); 
     
-    pc.printf("q0 = %f\n\r", q[0]);
-    pc.printf("q1 = %f\n\r", q[1]);
-    pc.printf("q2 = %f\n\r", q[2]);
-    pc.printf("q3 = %f\n\r", q[3]);      
+//    pc.printf("q0 = %f\n\r", q[0]);
+//    pc.printf("q1 = %f\n\r", q[1]);
+//    pc.printf("q2 = %f\n\r", q[2]);
+//    pc.printf("q3 = %f\n\r", q[3]);      
     
-    lcd.clear();
-    lcd.printString("MPU6050", 0, 0);
-    lcd.printString("x   y   z", 0, 1);
-    lcd.setXYAddress(0, 2); lcd.printChar((char)(1000*ax));
-    lcd.setXYAddress(20, 2); lcd.printChar((char)(1000*ay));
-    lcd.setXYAddress(40, 2); lcd.printChar((char)(1000*az)); lcd.printString("mg", 66, 2);
+    //lcd.clear();
+    //lcd.printString("MPU6050", 0, 0);
+    //lcd.printString("x   y   z", 0, 1);
+    //lcd.setXYAddress(0, 2); lcd.printChar((char)(1000*ax));
+    //lcd.setXYAddress(20, 2); lcd.printChar((char)(1000*ay));
+    //lcd.setXYAddress(40, 2); lcd.printChar((char)(1000*az)); lcd.printString("mg", 66, 2);
     
     
   // Define output variables from updated quaternion---these are Tait-Bryan angles, commonly used in aircraft orientation.
@@ -196,12 +318,12 @@
   // Tait-Bryan angles as well as Euler angles are non-commutative; that is, the get the correct orientation the rotations must be
   // applied in the correct order which for this configuration is yaw, pitch, and then roll.
   // For more see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles which has additional links.
-    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; 
-    roll  *= 180.0f / PI;
+    //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; 
+    //roll  *= 180.0f / PI;
 
 //    pc.printf("Yaw, Pitch, Roll: \n\r");
 //    pc.printf("%f", yaw);
@@ -211,8 +333,62 @@
 //    pc.printf("%f\n\r", roll);
 //    pc.printf("average rate = "); pc.printf("%f", (sumCount/sum)); pc.printf(" Hz\n\r");
 
-     pc.printf("Yaw, Pitch, Roll: %f %f %f\n\r", yaw, pitch, roll);
-     pc.printf("average rate = %f\n\r", (float) sumCount/sum);
+     //pc.printf("Yaw, Pitch, Roll: %f %f %f\n\r", yaw, pitch, roll);
+     //pc.printf("average rate = %f\n\r", (float) sumCount/sum);
+     
+     //BT.printf("Yaw, Pitch, Roll: %f %f %f\n\r", yaw, pitch, roll);
+     //BT.printf("average rate = %f\n\r", (float) sumCount/sum); 
+     
+    //alpha=yaw;
+    //betaa=pitch;
+    //gammaa=roll;
+
+    pc.printf("delta = %f\n\r", (float) deltat);
+//    pc.printf("alpha, beta, gamma: %f %f %f\n\r", alpha, betaa, gammaa);
+    
+    axx=ax;
+    ayy=ay;
+    azz=az;
+    
+////////////////////////////////////////////////////////Matrice d'Euler();
+    c = cos(alpha*PI/180.0f); s = sin(alpha*PI/180.0f);
+    a = cos(betaa*PI/180.0f); b = sin(betaa*PI/180.0f);
+    d = cos(gammaa*PI/180.0f); e = sin(gammaa*PI/180.0f);
+
+    matrice[0][0] = e*a - e*c*b;
+    matrice[0][1] = (-d)*b - e*c*a;
+    matrice[0][2] = e*s;
+    matrice[1][0] = e*a + d*c*b;
+    matrice[1][1] = (-e)*b + d*c*a;
+    matrice[1][2] = (-d)*s;
+    matrice[2][0] = s*b;
+    matrice[2][1] = s*a;
+    matrice[2][2] = c;
+
+   for(i=0; i<3; i++)
+   {
+        float temp = 0;
+        temp = axx * matrice[i][0] + ayy * matrice[i][1] + azz * matrice[i][2];
+        resultat[i] = temp;
+   }
+//////////////////////////////////////////////////////////
+    
+//    if (first==true){
+//     poid[0]=resultat[0];
+//     poid[1]=resultat[1];
+//     poid[2]=resultat[2];
+//     first=false;
+//    } else {
+//     resultat[0]-=poid[0];
+//     resultat[1]-=poid[1];
+//     resultat[2]-=poid[2];
+//    }
+    
+//     pc.printf("acceleration sans Euler : %f ; %f ; %f\n\r", axx, ayy, azz);
+//     pc.printf("acceleration avec Euler : %f ; %f ; %f\n\r", resultat[0], resultat[1], resultat[2]);
+     BT.printf("acceleration sans Euler : %f ; %f ; %f\n\r", axx, ayy, azz);
+     BT.printf("acceleration avec Euler : %f ; %f ; %f\n\r", resultat[0], resultat[1], resultat[2]);
+     
  
     myled= !myled;
     count = t.read_ms(); 
@@ -220,5 +396,15 @@
     sumCount = 0; 
 }
 }
+    if (BT.readable()) {
+        char c = BT.getc();
+        if(c == 'a') {
+            BT.printf("\nOK\n");
+        }
+    }
+}
  
- }
\ No newline at end of file
+ }
+
+
+ 
\ No newline at end of file

--- a/mbed.bld	Sun Jun 29 21:53:23 2014 +0000
+++ b/mbed.bld	Thu Mar 15 15:02:02 2018 +0000
@@ -1,1 +1,1 @@
-http://mbed.org/users/mbed_official/code/mbed/builds/0b3ab51c8877
\ No newline at end of file
+https://os.mbed.com/users/mbed_official/code/mbed/builds/aa5281ff4a02
\ No newline at end of file