main.cpp

00001 #include "mbed.h"
00002 #include <math.h>
00003 #include "LSM9DS1.h"
00004 
00005 
00006 
00007 //
00008 #define TE 0.01
00009 LSM9DS1 lol(PB_9,PB_8, 0xD4, 0x38);
00010 
00011 
00012 
00013 Serial pc(USBTX, USBRX);
00014 
00015 float accx, accz, acccx, acccz, gy,ggy;
00016 
00017 
00018 float test, test2;
00019 Ticker  calcul;
00020 int i =0;
00021 
00022 volatile bool flag=0;
00023 double psig,psia;
00024 double pacc_sortie[2];
00025 double PI=4*atan(1.0);
00026 double coef_a[2];
00027 double coef_b[2];
00028 double tau;
00029 //les deux etat instant de l'accelerometre
00030 double x_a[2];
00031 double y_a[2];
00032 //les deux etat instant de gyromètre
00033 double x_b[2];
00034 double y_b[2];
00035 
00036 //affichage non filtré
00037 double angle;
00038 float addition ,moyenne;
00039 
00040 //fonction de filtre passe bas
00041 void filtre_PB(double* xn,double* yn,double* coef)
00042 {
00043     yn[0]=coef[0]*xn[0]+coef[0]*xn[1]-coef[1]*yn[1];
00044     yn[1]=yn[0];
00045     xn[1]=xn[0];
00046     
00047 }
00048 //calculer les coef de filtre passe bas
00049 void coef_filtre_PB(double H0,double f0,double Te,double* coef)
00050 {
00051     coef[0]=(H0*Te)/(Te+(1/(PI*f0)));
00052     coef[1]=(Te-(1/(PI*f0)))/(Te+1/(PI*f0));
00053 }
00054 //////////////////////////////////////////////////////////////////////////////////////////////////////////////supr offset
00055 //fonctions pour calculer gy_off
00056 double gyro_zero(void)
00057 {
00058     const int NN=10000;
00059    
00060     double gy_off=0;
00061     for(int i=0; i<NN; i++) {
00062         lol.readGyro();
00063         gy_off=gy_off+lol.gy;
00064        // gy_off/10000;     (fait après)
00065     }
00066     return(gy_off);
00067 }
00068 //fonction pour calculer ang_off
00069 double angle_zero(void)
00070 {
00071     const int NN=1000;
00072     
00073    
00074     double ang_off=0;
00075     for(int i=0; i<NN; i++) {
00076         lol.readAccel();
00077         accx = lol.ax;
00078         accz = lol.az;
00079         double ang=(180/PI)*atan2(accx,accz);
00080         ang_off=ang_off+ang;
00081         ang_off/1000;
00082     }
00083     return ang_off;
00084 }
00085 //////////////////////////////////////////////////////////////////////////////////////////////////////////////
00086 //fonction isr pour lever le flag
00087 void mesure(void)
00088 {
00089     flag=1;
00090 }
00091 
00092 int main()
00093 {
00094     
00095 
00096   
00097 
00098 
00099         lol.begin();
00100     if (!lol.begin()) {
00101         pc.printf("Failed to communicate with LSM9DS1.\n");
00102         }
00103    lol.calibrate();
00104     
00105     double gyr_off=gyro_zero();
00106     gyr_off = gyr_off/10000;
00107     double ang_off=angle_zero();
00108     ang_off= ang_off/1000;
00109     calcul.attach(&mesure,0.01);
00110     unsigned char cpt=0;
00111     coef_filtre_PB(1,0.1,0.01,coef_a);
00112     tau=1/(2*PI*0.1);
00113     coef_filtre_PB(tau,0.1,0.01,coef_b);
00114      pc.baud(115200);
00115      
00116 
00117 
00118 while(1) {
00119         if(flag) {
00120             lol.readAccel();
00121             //calculer angle acceleration
00122        acccx = lol.ax;               //{
00123        acccz = lol.az;               //affichage de l'angle
00124        
00125             psia=((180.0/PI)*atan2(acccx,acccz))-ang_off;
00126             angle =((180.0/PI)*atan2(acccx,acccz))-ang_off;
00127             //filtre d'etat instant presente de l'accelerometre doit egale angle mesure
00128             x_a[0]=psia;
00129             //on filtre le signal
00130             filtre_PB(x_a,y_a,coef_a);
00131            lol.readGyro();
00132             //meme chose en gyrometre
00133            
00134          
00135             x_b[0]=(lol.gy-gyr_off)*TE ;           
00136             filtre_PB(x_b,y_b,coef_b);
00137             
00138              lol.readGyro();
00139         addition = addition + lol.gy;
00140         
00141             
00142             if(cpt==9) {
00143                 cpt=0;
00144                 moyenne = moyenne +(addition/10.0)/1000;
00145                
00146         
00147                 //affichage en chaque cpt =9;
00148                
00149                 pc.printf("$%f %f %f;\n",y_a[0],y_b[0],y_a[0]+y_b[0]);
00150               
00151                addition = 0;  
00152             }
00153             cpt++;
00154             flag=0;
00155         }
00156     }
00157     
00158 }
00159 
00160 
00161 
00162 
00163 
00164     
00165 
00166 
00167         
00168