Revision 10:9a563a22a5e7, committed 2020-06-16

Comitter:

Leonnn

Date:

Tue Jun 16 11:23:59 2020 +0000

Parent:

9:187f56fa6db5

Commit message:

lissage acc

Changed in this revision

--- a/main.cpp	Tue Jun 16 09:30:13 2020 +0000
+++ b/main.cpp	Tue Jun 16 11:23:59 2020 +0000
@@ -3,10 +3,6 @@
 #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];
@@ -19,7 +15,7 @@
 
 volatile bool flag_mesure = 0;
 
-void mesure(void){flag_mesure = 1;}//   ISR mesure
+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)));
@@ -27,21 +23,21 @@
 }
 
 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[0] = -coef[1]*yn[1] + coef[0]*(xn[0]+xn[1]);  
     yn[1] = yn[0];
     xn[1] = xn[0];
 }
 
-int main()
+int main()///////////////////////////////////////////////////////////////////////////////////////////
 {
-    float GyroBuffer[3], offset, GyrY;
-    offset = 0;
+    float GyroBuffer[3], offsetGyr, GyrY;
+    offsetGyr = 0;
     wait(0.5);
-    for(int i=0; i<100; i++){//     séquence de mise à 0
+    for(int i=0; i<100; i++){//     --------------------------------------------séquence de mise à 0
         gyro.GetXYZ(GyroBuffer);
-        offset = offset + GyroBuffer[1];
+        offsetGyr = offsetGyr + GyroBuffer[1];
     }
-    offset = offset/100;//-----------------fin mise à 0
+    offsetGyr = offsetGyr/100;//------------------------------------------------------fin mise à 0
     
     tick_mesure.attach(&mesure, 0.01);
     pc.baud(115200);
@@ -51,20 +47,21 @@
     float Acc_angle;
     int16_t AccBuffer[3];
     
+    coef_filtre_PB((double)1,(double)0.5,(double)0.01,coef);//------------------calcul coeff acc
     
-  
-    while(1) {  
+    while(1) { //////////////////////////////////////////////////////////////////////////////////////
         if(flag_mesure){//  mesure Gyro
-            gyro.GetXYZ(GyroBuffer);
-            GyrY = GyroBuffer[1]-offset;
-            Gyr_angle = Gyr_angle + GyrY/100;
-            Acc_angle = atan2((float)AccBuffer[0], (float)AccBuffer[2])*180.0/PI;
+            gyro.GetXYZ(GyroBuffer);//------------------------------------------get gyro
+            GyrY = GyroBuffer[1]-offsetGyr;//--------------------------------------compensation gyro
+            Gyr_angle = Gyr_angle + GyrY/100000;//------------------------------clacul gyro (intégration)
             
-            if(count > 9){
-                compass.AccGetXYZ(AccBuffer);       
-                //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]);
+            compass.AccGetXYZ(AccBuffer); //------------------------------------get acc
+            Acc_angle = atan2((float)AccBuffer[0], (float)AccBuffer[2])*180.0/PI-3.32;//calcul acc + compensation
+            xn[0] = Acc_angle;//------------------------------------------------début filtrage acc
+            filtre_PB(xn,yn,coef);
+            Acc_angle = yn[0];//------------------------------------------------fin filtrage acc
+            if(count > 9){     
+                pc.printf("$%f %f;", Acc_angle, Gyr_angle);
                 count = 0;
             }
             else