Diff: main.cpp

Revision:

7:65ef53c3b7bc

Parent:

6:81d8b03a9673

--- a/main.cpp	Mon Jun 15 14:48:08 2020 +0000
+++ b/main.cpp	Sun Jun 13 10:18:17 2021 +0000
@@ -8,8 +8,33 @@
 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;
@@ -21,6 +46,7 @@
     }
     return(gy_off);
 }
+//fonction pour le calul angulaire
 double angle_zero(void)
 {
     const int NN=1000;
@@ -34,34 +60,47 @@
     }
     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];
-    printf("Super inclinometre\n\r");
+
     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;
-                led1 = !led1;
-                pc.printf("$%f %f;\n",psia,psig);
+                //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;
         }
     }
-}
+}
\ No newline at end of file