File content as of revision 10:9a563a22a5e7:

#include "mbed.h"
#include "GYRO_DISCO_L476VG.h"
#include "COMPASS_DISCO_L476VG.h"
#define PI 3.14159265358979323846

double coef[2];
double xn[2];
double yn[2];

COMPASS_DISCO_L476VG compass;
GYRO_DISCO_L476VG gyro;

Serial pc(SERIAL_TX, SERIAL_RX);
Ticker tick_mesure;

volatile bool flag_mesure = 0;

void mesure(void){flag_mesure = 1;}//-------------------------------------------ISR mesure

void coef_filtre_PB(double H0,double f0,double Te,double* coef){
    coef[0] = (H0*Te)/(Te+2*(1/(2*PI*f0)));
    coef[1] = (Te-2*(1/(2*PI*f0)))/(Te+2*(1/(2*PI*f0)));
}

void filtre_PB(double* xn,double* yn,double* coef){
    yn[0] = -coef[1]*yn[1] + coef[0]*(xn[0]+xn[1]);  
    yn[1] = yn[0];
    xn[1] = xn[0];
}

int main()///////////////////////////////////////////////////////////////////////////////////////////
{
    float GyroBuffer[3], offsetGyr, GyrY;
    offsetGyr = 0;
    wait(0.5);
    for(int i=0; i<100; i++){//     --------------------------------------------séquence de mise à 0
        gyro.GetXYZ(GyroBuffer);
        offsetGyr = offsetGyr + GyroBuffer[1];
    }
    offsetGyr = offsetGyr/100;//------------------------------------------------------fin mise à 0
    
    tick_mesure.attach(&mesure, 0.01);
    pc.baud(115200);
    
    unsigned int count = 0;   
    float Gyr_angle = 0;
    float Acc_angle;
    int16_t AccBuffer[3];
    
    coef_filtre_PB((double)1,(double)0.5,(double)0.01,coef);//------------------calcul coeff acc
    
    while(1) { //////////////////////////////////////////////////////////////////////////////////////
        if(flag_mesure){//  mesure Gyro
            gyro.GetXYZ(GyroBuffer);//------------------------------------------get gyro
            GyrY = GyroBuffer[1]-offsetGyr;//--------------------------------------compensation gyro
            Gyr_angle = Gyr_angle + GyrY/100000;//------------------------------clacul gyro (intégration)
            
            compass.AccGetXYZ(AccBuffer); //------------------------------------get acc
            Acc_angle = atan2((float)AccBuffer[0], (float)AccBuffer[2])*180.0/PI-3.32;//calcul acc + compensation
            xn[0] = Acc_angle;//------------------------------------------------début filtrage acc
            filtre_PB(xn,yn,coef);
            Acc_angle = yn[0];//------------------------------------------------fin filtrage acc
            if(count > 9){     
                pc.printf("$%f %f;", Acc_angle, Gyr_angle);
                count = 0;
            }
            else
                count++;
            flag_mesure = 0;
        }
        //wait(0.1);
    }
}