Diff: main.cpp

Revision:

9:187f56fa6db5

Parent:

8:97589b322a4f

Child:

10:9a563a22a5e7

--- a/main.cpp	Fri Jun 12 13:48:55 2020 +0000
+++ b/main.cpp	Tue Jun 16 09:30:13 2020 +0000
@@ -3,6 +3,14 @@
 #include "COMPASS_DISCO_L476VG.h"
 #define PI 3.14159265358979323846
 
+double H0 = 1;//    params filtrage
+double f0_lo = 5;
+double f0_hi = 1;
+double Te = 0.01;
+double coef[2];
+double xn[2];
+double yn[2];
+
 COMPASS_DISCO_L476VG compass;
 GYRO_DISCO_L476VG gyro;
 
@@ -13,6 +21,17 @@
 
 void mesure(void){flag_mesure = 1;}//   ISR mesure
 
+void coef_filtre_PB(double H0,double f0,double Te,double* coef){
+    coef[0] = (H0*Te)/(Te+2*(1/(2*PI*f0)));
+    coef[1] = (Te-2*(1/(2*PI*f0)))/(Te+2*(1/(2*PI*f0)));
+}
+
+void filtre_PB(double* xn,double* yn,double* coef){
+    yn[0] = -coef[1]*yn[1] + coef[0]*(xn[0]+xn[1]);  //yn = -b*yn-1 + a*(xn + xn-1)
+    yn[1] = yn[0];
+    xn[1] = xn[0];
+}
+
 int main()
 {
     float GyroBuffer[3], offset, GyrY;
@@ -21,7 +40,6 @@
     for(int i=0; i<100; i++){//     séquence de mise à 0
         gyro.GetXYZ(GyroBuffer);
         offset = offset + GyroBuffer[1];
-        wait(0.01);
     }
     offset = offset/100;//-----------------fin mise à 0
     
@@ -32,6 +50,8 @@
     float Gyr_angle = 0;
     float Acc_angle;
     int16_t AccBuffer[3];
+    
+    
   
     while(1) {  
         if(flag_mesure){//  mesure Gyro
@@ -42,7 +62,9 @@
             
             if(count > 9){
                 compass.AccGetXYZ(AccBuffer);       
-                pc.printf("$%f %f;", Acc_angle, Gyr_angle/1000);
+                //pc.printf("$%f %f;", Acc_angle, Gyr_angle/1000);
+                coef_filtre_PB((double)1,(double)5,(double)0.01,coef);
+                pc.printf("%f %f\n\r", coef[0], coef[1]);
                 count = 0;
             }
             else