File content as of revision 1:8f6591373cfd:

#include "mbed.h"
#include "MPU6050.h"

Serial PC(SERIAL_TX, SERIAL_RX);
Serial BT(PA_10, PA_9);

MPU6050 mpu6050;
Ticker t;

DigitalOut myled(LED1);

float alpha, beta, gamma, R11, R12, R13, R21, R22, R23, R31, R32, R33, poidx, poidy, poidz, periode;

void recup_MPU(void){
        
        if(mpu6050.readByte(MPU6050_ADDRESS, INT_STATUS) & 0x01) {  // check if data ready interrupt
    mpu6050.readAccelData(accelCount);  // Read the x/y/z adc values
    mpu6050.getAres();
    
    // Now we'll calculate the accleration value into actual g's
    ax = (float)accelCount[0]*aRes - accelBias[0];  // get actual g value, this depends on scale being set
    ay = (float)accelCount[1]*aRes - accelBias[1];   
    az = (float)accelCount[2]*aRes - accelBias[2];  
   
    mpu6050.readGyroData(gyroCount);  // Read the x/y/z adc values
    mpu6050.getGres();
 
    // 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];   

    tempCount = mpu6050.readTempData();  // Read the adc values
    temperature = (tempCount) / 340. + 36.53; // Temperature in degrees Centigrade
   }
    
    Now = t.read_us();
    deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
    lastUpdate = Now;
    
    sum += deltat;
    sumCount++;
    
    if(lastUpdate - firstUpdate > 10000000.0f) {
     beta = 0.04;  // decrease filter gain after stabilized
     zeta = 0.015; // increasey bias drift gain after stabilized
    }
    
   // Pass gyro rate as rad/s
    mpu6050.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f);
    
    
            //sensorX <= accelCount[0];
            //sensorY <= accelCount[1];
            //sensorZ <= accelCount[2];
            
            PC.printf("acceleration in X = %u, or %f g\n", (unsigned int)accelCount[0], ax);
            PC.printf("acceleration in Y = %u, or %f g\n", (unsigned int)accelCount[1], ay);
            PC.printf("acceleration in Z = %u, or %f g\n", (unsigned int)accelCount[2], az);
            
            PC.printf("gyroscope in X = %u, or %f dps\n", (unsigned int)gyroCount[0], gx);
            PC.printf("gyroscope in Y = %u, or %f dps\n", (unsigned int)gyroCount[1], gy);
            PC.printf("gyroscope in Z = %u, or %f dps\n", (unsigned int)gyroCount[2], gz);
            
            PC.printf("temperature = %u, or %f C\n", (unsigned int)tempCount, temperature);
}

void boucle(void){
    recup_MPU();
    alpha+=gx*periode;
    beta+=gy*periode;
    gamma+=gz*periode;
    
    
}

int main()
{
    PC.baud(9600);  
    PC.printf("Hello World !\n");
    BT.baud(9600);  
    BT.printf("Connection BT\n");
    
    periode=0.1;
    
    alpha=0.0;
    beta=0.0;
    gamma=0.0;        
    
    recup_MPU();
    poidx=ax;
    poidy=ay;
    poidz=az;
    
    
    t.attach(&boucle, periode);
    
    
    
    while(1) {

    
    }
}