File content as of revision 5:a3a1c87ad660:

#include "mbed.h"
#include "GYRO_DISCO_L476VG.h"
#include "COMPASS_DISCO_L476VG.h"

#define Te 0.01
#define r2d 57.2957795131
#define PI 3.14159265359

GYRO_DISCO_L476VG gyro; 
COMPASS_DISCO_L476VG compass;

Serial pc(USBTX, USBRX);
Ticker t1;

volatile bool flag = 0 ;

double coef_a[2];
double coef_g[2];

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

double sortie_a[2];
double sortie_g[2];

double angle_y_acc[2];
double angle_y_gyro[2];

float x,z;
float derive;
float offset;

void coef_filtre(double,double ,double*);
void filtre_PB(double *, double *, double *);

void mesure();
int u;


int main()
{
    float GyroBuffer[3];
    int16_t AccBuffer[3];
    pc.baud(115200);
    t1.attach(&mesure,Te);

    coef_filtre(1.00,0.1,coef_a);
    coef_filtre(1/(2*PI*0.1),0.1,coef_g);
    
    for(int a = 0;a<=10000;a++){
        gyro.GetXYZ(GyroBuffer);
        derive += GyroBuffer[1]/10000;
        }
    for(int a = 0;a<=1000;a++){
        compass.AccGetXYZ(AccBuffer);
        x = AccBuffer[0]*0.061/1000;
        z = AccBuffer[2]*0.061/1000;
        offset += ((atan2((double)x,(double)z))*r2d)/1000;
        }

    while(1) {

        if(flag) {
            flag = 0;
            compass.AccGetXYZ(AccBuffer);
            gyro.GetXYZ(GyroBuffer);
            x = AccBuffer[0]*0.061/1000;
            z = AccBuffer[2]*0.061/1000;
            angle_y_gyro[0] = (GyroBuffer[1]-derive)/1000;
            angle_y_acc[0] = (atan2((double)x,(double)z))*r2d-offset;

            filtre_PB(angle_y_acc,sortie_a,coef_a);
            filtre_PB(angle_y_gyro,sortie_g,coef_g);
    
            if((u%10)==0) {
                pc.printf("$%lf %lf %lf;\n",sortie_g[0],sortie_a[0],sortie_a[0]+sortie_g[0]);
            }
        }
    }
}

void mesure()
{
    flag = 1;
    u++;
}

void coef_filtre(double H0,double F0,double *coef)
{
    coef[0] = (H0*Te)/( Te + (1/(PI*F0)));
    coef[1] = (Te -(1/(PI*F0)))/( Te + (1/(PI*F0)));
}
void filtre_PB(double *xn, double *yn, double *coef)
{
    yn[0] = coef[0]*(xn[1]+xn[0])-coef[1]*yn[1];
    yn[1] = yn[0];
    xn[1] = xn[0];
}