File content as of revision 3:2cd58c219b89:

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

//state
#define FinAcquisition  1
#define PretPourAcquisition  0

Serial pc(USBTX, USBRX);     // USB initialization
MPU6050 accelero(D4, D5);    // MPU6050 library initialization

// Variables globales
float AngleTan,AngleFiltre=0,AngleNonFiltre=0,A,B;
float Te_ms = 5, Te, Tau = 0.1;   // Valeurs periode d'enchantillonage et tau

char Flag=PretPourAcquisition;

void calculAngle(void)
{
    int gx, gz, OmegaY;
    
    // acq de gx et gz et calcul de AngleTan
    gx = accelero.getAcceleroRawX();
    gz = accelero.getAcceleroRawZ();
    AngleTan = atan2((double)gx,(double)gz);
    
    // acq de OmegaY
    OmegaY = accelero.getGyroRawY()*Tau/1000;

    // calcul AngleNonFiltre
    AngleNonFiltre = OmegaY + AngleTan;

    // filtre passe bas numérique
    AngleFiltre = A*AngleNonFiltre+B*AngleFiltre;
    
    Flag = FinAcquisition;
}

RtosTimer timer(mbed::callback(calculAngle),osTimerPeriodic); // definition of rtos timer 


int main (void)
{
    float AngleDeg;
    // calcul des coefficients du filtre de l'angle
    Te = Te_ms/1000;
    A = Te/(Te+Tau);
    B = Tau/(Te+Tau);
    
    // Usb init
    pc.baud(115200);
    pc.printf("\r\n Debut prog\r\n");

    // verification of connection
    if (accelero.testConnection()) {
        pc.printf("connected to MPU 6050\r\n");
    } else {
        pc.printf("No device  \r\n");
        while(1); // infinite pause
    }
    wait(0.2);
    
    // Scale init
    accelero.setGyroRange(MPU6050_GYRO_RANGE_2000);
    accelero.setAcceleroRange(MPU6050_ACCELERO_RANGE_2G);
    
    timer.start(Te_ms);
    
    while(1) {
        //display of angles calculated
        if (Flag == FinAcquisition) {
            pc.printf("%f %f \r\n",AngleFiltre, AngleTan);
            Flag = PretPourAcquisition ;
        }
       
    }
}
/*