Diff: main.cpp

Revision:

6:6df9f66ca6bb

Parent:

5:f4a35a2a9085

Child:

7:763b230d3b66

--- a/main.cpp	Mon Jun 08 17:42:48 2020 +0000
+++ b/main.cpp	Sat Jun 13 16:28:37 2020 +0000
@@ -1,55 +1,98 @@
 #include "mbed.h"
 #include "GYRO_DISCO_L476VG.h"
-
+#include "COMPASS_DISCO_L476VG.h"
+//
 GYRO_DISCO_L476VG gyro;
+COMPASS_DISCO_L476VG compass;
 Serial pc(SERIAL_TX, SERIAL_RX,115200);
 Ticker ticker;
 DigitalOut led1(LED1);
 volatile bool flag=0;
-float psig=0;
+float psig=0,psia;
+void coef_filtre_PB(double H0,double f0,double Te,double* coef)
+{
+    double PI=4*atan(1.0);
+    double tau=1/(2*PI*f0);
+    coef[0]=H0/(1+(2*tau/Te));
+    coef[1]=(Te-2*tau)/(Te+2*tau);
+}
+//
+void filtre_PB(double* xn,double* yn, double* coef)
+{
+    yn[0]=coef[0]*(xn[0]+xn[1])-coef[1]*yn[1];
+    xn[1]=xn[0];
+    yn[1]=yn[0];
+}
+//
 float gyro_zero(void)
 {
     const int NN=100000;
     float GyroBuffer[3];
     float gy_off=0;
-    for(int i=0; i<NN; i++)
-      {
+    for(int i=0; i<NN; i++) {
         gyro.GetXYZ(GyroBuffer);
         gy_off=gy_off+GyroBuffer[1]/NN;
-        
     }
     return(gy_off);
 }
+//
+float angle_zero(void)
+{
+    const int NN=100000;
+    int16_t AccBuffer[3];
+    double PI=4*atan(1.0);
+    float ang_off=0;
+    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;
+    }
+    return ang_off;
+}
+//
 void mesure(void)
 {
     flag=1;
 }
-
+//
 int main()
 {
     float GyroBuffer[3];
-
-    printf("Gyroscope started\n");
+    int16_t AccBuffer[3];
+    double coef_acc[2];
+    double coef_gyr[2];
+    double Acc[2];
+    double AccF[2];
+    double Gyr[2];
+    double GyrF[2];
+    wait(1);
+    pc.printf("Super Inclinometre \n");
+    coef_filtre_PB(1,0.05,0.01,coef_acc);   // H0 f0 Te
+    coef_filtre_PB(1/(2*3.1415926*0.05),0.05,0.01,coef_gyr);   // H0 f0 Te
+    //pc.printf("a= %f   b=%f \n\r",coef_acc[0],coef_acc[1]);
+    float gyoff=gyro_zero();
+    float anoff=angle_zero();
     ticker.attach(&mesure,0.01);
     unsigned char cpt=0;
-    float gyoff=gyro_zero();
+    //
     while(1) {
         if(flag) {
-
-            // Read Gyroscope values
+            compass.AccGetXYZ(AccBuffer);
+            psia=(180.0/3.1415926)*atan2((float)AccBuffer[0],(float)AccBuffer[2])-anoff;
             gyro.GetXYZ(GyroBuffer);
             psig=psig+(GyroBuffer[1]-gyoff)*0.01/1000;
-            
-            // Display values
-            //printf("X = %8.1f  \n", GyroBuffer[0]);
-            // printf("Y = %8.1f   \n", GyroBuffer[1]);
-            //printf("Z = %8.1f  \n", GyroBuffer[2]);
-            // printf("\033[3A"); // Moves cursor up x lines (x value is between [ and A)
+            Acc[0]=psia;
+            Gyr[0]=(GyroBuffer[1]-gyoff)/1000;
+            filtre_PB(Acc,AccF,coef_acc);
+            filtre_PB(Gyr,GyrF,coef_gyr);
+            double fusion=AccF[0]+GyrF[0];
             if(cpt==9) {
                 cpt=0;
                 led1 = !led1;
-                //pc.printf("$%f;\n",GyroBuffer[1]);
-                pc.printf("$%f;\n",psig);
+                // angle accelero, angne gyro, angle acc. filtré, gyro filtré, angle fusion
+                //pc.printf("$%f %f %f %f %f;\n",psia,psig,AccF[0],GyrF[0],fusion);  
+                pc.printf("$%f %f %f;\n",AccF[0],GyrF[0],fusion);  
+                
             }
             cpt++;
             flag=0;