File content as of revision 9:187f56fa6db5:

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

double H0 = 1;//    params filtrage
double f0_lo = 5;
double f0_hi = 1;
double Te = 0.01;
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 = -b*yn-1 + a*(xn + xn-1)
    yn[1] = yn[0];
    xn[1] = xn[0];
}

int main()
{
    float GyroBuffer[3], offset, GyrY;
    offset = 0;
    wait(0.5);
    for(int i=0; i<100; i++){//     séquence de mise à 0
        gyro.GetXYZ(GyroBuffer);
        offset = offset + GyroBuffer[1];
    }
    offset = offset/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];
    
    
  
    while(1) {  
        if(flag_mesure){//  mesure Gyro
            gyro.GetXYZ(GyroBuffer);
            GyrY = GyroBuffer[1]-offset;
            Gyr_angle = Gyr_angle + GyrY/100;
            Acc_angle = atan2((float)AccBuffer[0], (float)AccBuffer[2])*180.0/PI;
            
            if(count > 9){
                compass.AccGetXYZ(AccBuffer);       
                //pc.printf("$%f %f;", Acc_angle, Gyr_angle/1000);
                coef_filtre_PB((double)1,(double)5,(double)0.01,coef);
                pc.printf("%f %f\n\r", coef[0], coef[1]);
                count = 0;
            }
            else
                count++;
            flag_mesure = 0;
        }
        //wait(0.1);
    }
}