Revision 7:24a3e72617fa, committed 2021-06-14

Comitter:

houms

Date:

Mon Jun 14 21:52:39 2021 +0000

Parent:

6:81d8b03a9673

Commit message:

Programme inclinometre;

Changed in this revision

--- a/main.cpp	Mon Jun 15 14:48:08 2020 +0000
+++ b/main.cpp	Mon Jun 14 21:52:39 2021 +0000
@@ -3,13 +3,27 @@
 #include "COMPASS_DISCO_L476VG.h"
 //
 #define TE 0.01
+#define F0 0.1
+#define H 1
+
 GYRO_DISCO_L476VG gyro;
 COMPASS_DISCO_L476VG compass;
 Serial pc(SERIAL_TX, SERIAL_RX,115200);
 Ticker ticker;
 DigitalOut led1(LED1);
 volatile bool flag=0;
-double psig=0,psia;
+
+double psiafiltre[2];
+double psia[2]={0};
+double coef[2];
+
+double psig[2]={0};
+double psigfiltre[2];
+double coef2[2];
+
+double inclonometre ;
+double T;
+
 double gyro_zero(void)
 {
     const int NN=10000;
@@ -30,10 +44,28 @@
     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;
+        ang_off=ang_off
+                +ang/NN;
     }
     return ang_off;
 }
+void filtre_PB(double* xn,double* yn,double* coef1)
+{
+    yn[0] = (xn[0]+xn[1])*coef1[0]- yn[1]*coef1[1];
+    yn[1]= yn[0];
+    xn[1] = xn[0];
+
+}
+
+void coef_filtre_PB(double H10,double f10,double TeE,double* cof)
+{
+
+    float T2 ;
+    T2=1.00/(3.14*f10);
+    cof[0]=TeE*H10/(TeE+T2) ;
+    cof[1]=(TeE-T2)/(TeE+T2);
+}
+
 void mesure(void)
 {
     flag=1;
@@ -47,18 +79,27 @@
     double gyr_off=gyro_zero();
     double ang_off=angle_zero();
     ticker.attach(&mesure,TE);
+    coef_filtre_PB(H,F0,TE,coef);
+    T=(1.00/(2*3.14*F0));
+    coef_filtre_PB(T,F0,TE,coef2);
+
     unsigned char cpt=0;
-    
+
     while(1) {
         if(flag) {
             compass.AccGetXYZ(AccBuffer);
-            psia=((180.0/PI)*atan2((double)AccBuffer[0],(double)AccBuffer[2]))-ang_off;
+            psia[0]=((180.0/PI)*atan2((double)AccBuffer[0],(double)AccBuffer[2]))-ang_off;
+            filtre_PB(psia,psiafiltre,coef);
             gyro.GetXYZ(GyroBuffer);
-            psig=psig+(GyroBuffer[1]-gyr_off)*TE/1000;
+            psig[0]=(GyroBuffer[1]-gyr_off)/1000;
+            filtre_PB(psig,psigfiltre,coef2);
+
             if(cpt==9) {
                 cpt=0;
+                
                 led1 = !led1;
-                pc.printf("$%f %f;\n",psia,psig);
+                inclonometre= psiafiltre[0]+psigfiltre[0];
+                pc.printf("$%f %f %f;\n",psiafiltre[0],psigfiltre[0],inclonometre);
             }
             cpt++;
             flag=0;