File content as of revision 0:6e5bca5b4c06:

#include "mbed.h"
#include "MPU6050.h"
#include "I2Cdev.h"
#include <math.h>

#define NB_MEASURE 5
#define KP 10
#define T_MEASURE 2000
#define TE T_MEASURE*NB_MEASURE*1.0e-6

#ifndef M_PI
#define M_PI           3.14159265358979323846
#endif
 
  

Ticker T_measure;
DigitalOut led1(D2);
DigitalOut led2(D3);
Serial pc(USBTX, USBRX);
MPU6050 mpu;


int gOmega,gaccY,gAccZ;
bool gEndMeasure=false;

 
void measure() {
    static int cpt=0;
    static int accY,accZ,omegaM; //retour capteurs
    //mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
    //mesure angle et vitesse ang
    led2=1;
    if(cpt<NB_MEASURE){

        accY+= mpu.getAccelerationY();
        accZ+= mpu.getAccelerationZ();
        omegaM += mpu.getRotationY();
        cpt++;
        led2=0;
    }else
    {
        cpt=0;
        gaccY =accY/NB_MEASURE;
        gAccZ=accZ/NB_MEASURE; 
        gOmega=omegaM/NB_MEASURE; 
        accY=accZ=omegaM=0;
        gEndMeasure=true;
    }
    led2=0;

}
 
int main() {
    float tetaM,teta,tetaAv=0;
    mpu.initialize();
    pc.baud(115200);
    pc.printf("MPU6050 testConnection \n\r");
 
    bool mpu6050TestResult = mpu.testConnection();
    if(mpu6050TestResult) {
        pc.printf("\r\nMPU6050 test passed Te=%.4f\n\r",TE);
    } else {
        pc.printf("\r\nMPU6050 test failed \n\r");
    }
    led2 = 1;
    led1 = 1;
    T_measure.attach_us(&measure, T_MEASURE); // the address of the function to be attached (flip) and the interval (2 seconds)
   // temporisation.attach_us(&tempo, 10000); // spin in a main loop. flipper will interrupt it to call flip
    while(1) {
        if(gEndMeasure){
            led1=1;
            gEndMeasure=false;
            tetaM=atan2(static_cast<double>(gaccY),static_cast<double>(gAccZ)); //calcul de l'angle
            float tetaM_deg = tetaM * 180 / M_PI;
            float K=exp(static_cast<double>(-KP*TE));
            pc.printf("K = %6.4f  ",K);
            teta=K*tetaAv +(1-K)/KP*(KP*tetaM+gOmega); //angle estimé
            pc.printf("gaccY = %6d  gAccZ= %6d   gyro=%6d  angleM=%6.2f  angle=%.2f\r\n",gaccY, gAccZ, gOmega,tetaM_deg,teta);
            tetaAv=teta;
            led1=0;

           /* if(tetaM < 0) 
                tetaM += 2*PI;
            if(tetaM > 2*PI) 
                tetaM -= 2*PI;
            tetaM = tetaM * 180 / PI;*/
        }
    }
}