File content as of revision 6:6df9f66ca6bb:

#include "mbed.h"
#include "GYRO_DISCO_L476VG.h"
#include "COMPASS_DISCO_L476VG.h"
//
GYRO_DISCO_L476VG gyro;
COMPASS_DISCO_L476VG compass;
Serial pc(SERIAL_TX, SERIAL_RX,115200);
Ticker ticker;
DigitalOut led1(LED1);
volatile bool flag=0;
float psig=0,psia;
void coef_filtre_PB(double H0,double f0,double Te,double* coef)
{
    double PI=4*atan(1.0);
    double tau=1/(2*PI*f0);
    coef[0]=H0/(1+(2*tau/Te));
    coef[1]=(Te-2*tau)/(Te+2*tau);
}
//
void filtre_PB(double* xn,double* yn, double* coef)
{
    yn[0]=coef[0]*(xn[0]+xn[1])-coef[1]*yn[1];
    xn[1]=xn[0];
    yn[1]=yn[0];
}
//
float gyro_zero(void)
{
    const int NN=100000;
    float GyroBuffer[3];
    float gy_off=0;
    for(int i=0; i<NN; i++) {
        gyro.GetXYZ(GyroBuffer);
        gy_off=gy_off+GyroBuffer[1]/NN;
    }
    return(gy_off);
}
//
float angle_zero(void)
{
    const int NN=100000;
    int16_t AccBuffer[3];
    double PI=4*atan(1.0);
    float ang_off=0;
    for(int i=0; i<NN; i++) {
        compass.AccGetXYZ(AccBuffer);
        double ang=(180/PI)*atan2((double)AccBuffer[0],(double)AccBuffer[2]);
        ang_off=ang_off+ang/NN;
    }
    return ang_off;
}
//
void mesure(void)
{
    flag=1;
}
//
int main()
{
    float GyroBuffer[3];
    int16_t AccBuffer[3];
    double coef_acc[2];
    double coef_gyr[2];
    double Acc[2];
    double AccF[2];
    double Gyr[2];
    double GyrF[2];
    wait(1);
    pc.printf("Super Inclinometre \n");
    coef_filtre_PB(1,0.05,0.01,coef_acc);   // H0 f0 Te
    coef_filtre_PB(1/(2*3.1415926*0.05),0.05,0.01,coef_gyr);   // H0 f0 Te
    //pc.printf("a= %f   b=%f \n\r",coef_acc[0],coef_acc[1]);
    float gyoff=gyro_zero();
    float anoff=angle_zero();
    ticker.attach(&mesure,0.01);
    unsigned char cpt=0;
    //
    while(1) {
        if(flag) {
            compass.AccGetXYZ(AccBuffer);
            psia=(180.0/3.1415926)*atan2((float)AccBuffer[0],(float)AccBuffer[2])-anoff;
            gyro.GetXYZ(GyroBuffer);
            psig=psig+(GyroBuffer[1]-gyoff)*0.01/1000;
            Acc[0]=psia;
            Gyr[0]=(GyroBuffer[1]-gyoff)/1000;
            filtre_PB(Acc,AccF,coef_acc);
            filtre_PB(Gyr,GyrF,coef_gyr);
            double fusion=AccF[0]+GyrF[0];
            if(cpt==9) {
                cpt=0;
                led1 = !led1;
                // angle accelero, angne gyro, angle acc. filtré, gyro filtré, angle fusion
                //pc.printf("$%f %f %f %f %f;\n",psia,psig,AccF[0],GyrF[0],fusion);  
                pc.printf("$%f %f %f;\n",AccF[0],GyrF[0],fusion);  
                
            }
            cpt++;
            flag=0;
        }
    }
}