Revision 2:f63f0417ab5f, committed 2021-10-29

Comitter:

natvich

Date:

Fri Oct 29 22:33:46 2021 +0000

Parent:

1:87d535bf8c53

Commit message:

Projet d'inclinometre avec fusion de donnee

Changed in this revision

--- a/LSM9DS1.h	Mon Oct 26 16:14:04 2015 +0000
+++ b/LSM9DS1.h	Fri Oct 29 22:33:46 2021 +0000
@@ -312,7 +312,7 @@
     *    Any OR'd combination of ZIEN, YIEN, XIEN
     *  - activeLow = Interrupt active configuration
     *    Can be either INT_ACTIVE_HIGH or INT_ACTIVE_LOW
-    */  - latch: latch gyroscope interrupt request.
+      - latch: latch gyroscope interrupt request.   */
     void configMagInt(uint8_t generator, h_lactive activeLow, bool latch = true);
     
     /** configMagThs() -- Configure the threshold of a gyroscope axis

--- a/main.cpp	Mon Oct 26 16:14:04 2015 +0000
+++ b/main.cpp	Fri Oct 29 22:33:46 2021 +0000
@@ -1,29 +1,168 @@
+#include "mbed.h"
+#include <math.h>
 #include "LSM9DS1.h"
 
-DigitalOut myled(LED1);
+
+
+//
+#define TE 0.01
+LSM9DS1 lol(PB_9,PB_8, 0xD4, 0x38);
+
+
+
 Serial pc(USBTX, USBRX);
 
-int main() {
-    //LSM9DS1 lol(p9, p10, 0x6B, 0x1E);
-    LSM9DS1 lol(p9, p10, 0xD6, 0x3C);
-    lol.begin();
+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();
-    while(1) {
-        lol.readTemp();
-        lol.readMag();
-        lol.readGyro();
+        }
+   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;
         
-        //pc.printf("%d %d %d %d %d %d %d %d %d\n\r", lol.calcGyro(lol.gx), lol.calcGyro(lol.gy), lol.calcGyro(lol.gz), lol.ax, lol.ay, lol.az, lol.mx, lol.my, lol.mz);
-        //pc.printf("%d %d %d\n\r", lol.calcGyro(lol.gx), lol.calcGyro(lol.gy), lol.calcGyro(lol.gz));
-        pc.printf("gyro: %d %d %d\n\r", lol.gx, lol.gy, lol.gz);
-        pc.printf("accel: %d %d %d\n\r", lol.ax, lol.ay, lol.az);
-        pc.printf("mag: %d %d %d\n\n\r", lol.mx, lol.my, lol.mz);
-        myled = 1;
-        wait(2);
-        myled = 0;
-        wait(2);
+            
+            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;
+        }
     }
+    
 }
+
+
+
+
+
+    
+
+
+        
+  
\ No newline at end of file