File content as of revision 2:f89067092cef:

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

Serial PC(SERIAL_TX, SERIAL_RX);
Serial BT(PA_9, PA_10);
MPU6050 mpu6050;
Ticker t;
Timer t1;

DigitalOut myled(LED1);

float alpha, betaa, gammaa, R11, R12, R13, R21, R22, R23, R31, R32, R33, poidx, poidy, poidz, periode, sumCount, sum;

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 = t1.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);
    
    (gx,gy,gz)=(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 rad/s\n", (unsigned int)gyroCount[0], gx);
            PC.printf("gyroscope in Y = %u, or %f rad/s\n", (unsigned int)gyroCount[1], gy);
            PC.printf("gyroscope in Z = %u, or %f rad/s\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;
    betaa+=gy*periode;
    gammaa+=gz*periode;
    
    BT.printf("acceleration in X = %u, or %f g\n", (unsigned int)accelCount[0], ax);
    
    myled!=myled;
    
}





int main()
{
    PC.baud(9600);  
    PC.printf("Hello World !\n");
    BT.baud(38400);  
    BT.printf("Connection BT\n");
    
    periode=1;
    
    alpha=0.0;
    betaa=0.0;
    gammaa=0.0;        
    




    /////////////////////////////////////////////////////////////////////////////////////////////////
  // Read the WHO_AM_I register, this is a good test of communication
  uint8_t whoami = mpu6050.readByte(MPU6050_ADDRESS, WHO_AM_I_MPU6050);  // Read WHO_AM_I register for MPU-6050
  PC.printf("I AM 0x%x\n\r", whoami);
   PC.printf("I SHOULD BE 0x68\n\r");
  
  if (whoami == 0x68) // WHO_AM_I should always be 0x68
  {  
    PC.printf("MPU6050 is online...");
    wait(1);
    
    mpu6050.MPU6050SelfTest(SelfTest); // Start by performing self test and reporting values
    PC.printf("x-axis self test: acceleration trim within : "); PC.printf("%f", SelfTest[0]); PC.printf("% of factory value \n\r");
    PC.printf("y-axis self test: acceleration trim within : "); PC.printf("%f", SelfTest[1]); PC.printf("% of factory value \n\r");
    PC.printf("z-axis self test: acceleration trim within : "); PC.printf("%f", SelfTest[2]); PC.printf("% of factory value \n\r");
    PC.printf("x-axis self test: gyration trim within : "); PC.printf("%f", SelfTest[3]); PC.printf("% of factory value \n\r");
    PC.printf("y-axis self test: gyration trim within : "); PC.printf("%f", SelfTest[4]); PC.printf("% of factory value \n\r");
    PC.printf("z-axis self test: gyration trim within : "); PC.printf("%f", SelfTest[5]); PC.printf("% of factory value \n\r");
    wait(1);

    if(SelfTest[0] < 1.0f && SelfTest[1] < 1.0f && SelfTest[2] < 1.0f && SelfTest[3] < 1.0f && SelfTest[4] < 1.0f && SelfTest[5] < 1.0f) 
    {
    mpu6050.resetMPU6050(); // Reset registers to default in preparation for device calibration
    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

    wait(2);
       }
    else
    {
    PC.printf("Device did not the pass self-test!\n\r");
     
      }
    }
    else
    {
    PC.printf("Could not connect to MPU6050: \n\r");
    PC.printf("%#x \n",  whoami);
    
    while(1) ; // Loop forever if communication doesn't happen
    }
    
    PC.printf("init sensor done\n");
    /////////////////////////////////////////////////////////////////////////////////////
    
    
    
    
  
    t1.start();  
    
    recup_MPU();
    poidx=ax;
    poidy=ay;
    poidz=az;
    
    
    t.attach(&boucle, periode);
    
    
    
    
    while(1) {

        char c = BT.getc();
        
        if(c == 'a') {
            BT.printf("\nOK\n");
        }
    
    }
}