File content as of revision 7:65ef53c3b7bc:

#include "mbed.h"
#include "GYRO_DISCO_L476VG.h"
#include "COMPASS_DISCO_L476VG.h"
//
#define TE 0.01
GYRO_DISCO_L476VG gyro;
COMPASS_DISCO_L476VG compass;
Serial pc(SERIAL_TX, SERIAL_RX,115200);
Ticker ticker;
DigitalOut led1(LED1);
//variables
volatile bool flag=0;
double psig=0,psia;
double pacc_sortie[2];
double PI=4*atan(1.0);
double coef_a[2];
double coef_b[2];
double tau;
double x_a[2];
double y_a[2];
double x_b[2];
double y_b[2];
//Fonction qui va permettre le filtrage
void filtre_PB(double* xn,double* yn,double* coef)
{
    yn[0]=coef[0]*xn[0]+coef[0]*xn[1]-coef[1]*yn[1];
    yn[1]=yn[0];
    xn[1]=xn[0];
    
}
//fonction pour calculer les coefficient a et b de la fonction de transfert
void coef_filtre_PB(double H0,double f0,double Te,double* coef)
{
    coef[0]=(H0*Te)/(Te+(1/PI/f0));
    coef[1]=(Te-(1/PI/f0))/(Te+1/PI/f0);
}
//fonction pour le gyroscope
double gyro_zero(void)
{
    const int NN=10000;
    float GyroBuffer[3];
    double gy_off=0;
    for(int i=0; i<NN; i++) {
        gyro.GetXYZ(GyroBuffer);
        gy_off=gy_off+GyroBuffer[1]/NN;
    }
    return(gy_off);
}
//fonction pour le calul angulaire
double angle_zero(void)
{
    const int NN=1000;
    int16_t AccBuffer[3];
    double PI=4*atan(1.0);
    double 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;
}
//fonction qui leve un flag qui va etre utiliser dans le main
void mesure(void)
{
    flag=1;
}
int main()
{
    
    float GyroBuffer[3];
    int16_t AccBuffer[3];

    double PI=4*atan(1.0);
    double gyr_off=gyro_zero();
    double ang_off=angle_zero();
    //prise de mesure toute les 10 ms
    ticker.attach(&mesure,TE);
    unsigned char cpt=0;
    //definition des coef a et b
    coef_filtre_PB(1,0.1,0.01,coef_a);
    tau=1/(2*PI*0.1);
    coef_filtre_PB(tau,0.1,0.01,coef_b);
    
    while(1) {
        //calcul des angles pour l'accelerometre 
        if(flag) {
            compass.AccGetXYZ(AccBuffer);
            psia=((180.0/PI)*atan2((double)AccBuffer[0],(double)AccBuffer[2]))-ang_off;
            x_a[0]=psia;
            filtre_PB(x_a,y_a,coef_a);
            gyro.GetXYZ(GyroBuffer);
            psig=psig+(GyroBuffer[1]-gyr_off)*TE/1000;
            x_b[0]=(GyroBuffer[1]-gyr_off)/1000;
            filtre_PB(x_b,y_b,coef_b);
            
            if(cpt==9) {
                cpt=0;
                //affichage au bout de 10 mesure sur serial port plotter
                pc.printf("$%f %f %f;\n",y_a[0],y_b[0],y_a[0]+y_b[0]);
            }
            cpt++;
            flag=0;
        }
    }
}