File content as of revision 2:f63f0417ab5f:

#include "mbed.h"
#include <math.h>
#include "LSM9DS1.h"



//
#define TE 0.01
LSM9DS1 lol(PB_9,PB_8, 0xD4, 0x38);



Serial pc(USBTX, USBRX);

float accx, accz, acccx, acccz, gy,ggy;


float test, test2;
Ticker  calcul;
int i =0;

volatile bool flag=0;
double psig,psia;
double pacc_sortie[2];
double PI=4*atan(1.0);
double coef_a[2];
double coef_b[2];
double tau;
//les deux etat instant de l'accelerometre
double x_a[2];
double y_a[2];
//les deux etat instant de gyromètre
double x_b[2];
double y_b[2];

//affichage non filtré
double angle;
float addition ,moyenne;

//fonction de filtre passe bas
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];
    
}
//calculer les coef de filtre passe bas
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));
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////supr offset
//fonctions pour calculer gy_off
double gyro_zero(void)
{
    const int NN=10000;
   
    double gy_off=0;
    for(int i=0; i<NN; i++) {
        lol.readGyro();
        gy_off=gy_off+lol.gy;
       // gy_off/10000;     (fait après)
    }
    return(gy_off);
}
//fonction pour calculer ang_off
double angle_zero(void)
{
    const int NN=1000;
    
   
    double ang_off=0;
    for(int i=0; i<NN; i++) {
        lol.readAccel();
        accx = lol.ax;
        accz = lol.az;
        double ang=(180/PI)*atan2(accx,accz);
        ang_off=ang_off+ang;
        ang_off/1000;
    }
    return ang_off;
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////
//fonction isr pour lever le flag
void mesure(void)
{
    flag=1;
}

int main()
{
    

  


        lol.begin();
    if (!lol.begin()) {
        pc.printf("Failed to communicate with LSM9DS1.\n");
        }
   lol.calibrate();
    
    double gyr_off=gyro_zero();
    gyr_off = gyr_off/10000;
    double ang_off=angle_zero();
    ang_off= ang_off/1000;
    calcul.attach(&mesure,0.01);
    unsigned char cpt=0;
    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);
     pc.baud(115200);
     


while(1) {
        if(flag) {
            lol.readAccel();
            //calculer angle acceleration
       acccx = lol.ax;               //{
       acccz = lol.az;               //affichage de l'angle
       
            psia=((180.0/PI)*atan2(acccx,acccz))-ang_off;
            angle =((180.0/PI)*atan2(acccx,acccz))-ang_off;
            //filtre d'etat instant presente de l'accelerometre doit egale angle mesure
            x_a[0]=psia;
            //on filtre le signal
            filtre_PB(x_a,y_a,coef_a);
           lol.readGyro();
            //meme chose en gyrometre
           
         
            x_b[0]=(lol.gy-gyr_off)*TE ;           
            filtre_PB(x_b,y_b,coef_b);
            
             lol.readGyro();
        addition = addition + lol.gy;
        
            
            if(cpt==9) {
                cpt=0;
                moyenne = moyenne +(addition/10.0)/1000;
               
        
                //affichage en chaque cpt =9;
               
                pc.printf("$%f %f %f;\n",y_a[0],y_b[0],y_a[0]+y_b[0]);
              
               addition = 0;  
            }
            cpt++;
            flag=0;
        }
    }
    
}