Revision 5:a3a1c87ad660, committed 2021-06-10

Comitter:

iinewbieii

Date:

Thu Jun 10 12:34:54 2021 +0000

Parent:

4:25b2a1150cd0

Commit message:

gzidaogh

Changed in this revision

diff -r 25b2a1150cd0 -r a3a1c87ad660 COMPASS_DISCO_L476VG.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/COMPASS_DISCO_L476VG.lib	Thu Jun 10 12:34:54 2021 +0000
@@ -0,0 +1,1 @@
+https://developer.mbed.org/teams/ST/code/COMPASS_DISCO_L476VG/#6508fa5521e0

diff -r 25b2a1150cd0 -r a3a1c87ad660 main.cpp
--- a/main.cpp	Wed Nov 20 15:13:34 2019 +0100
+++ b/main.cpp	Thu Jun 10 12:34:54 2021 +0000
@@ -1,25 +1,98 @@
 #include "mbed.h"
 #include "GYRO_DISCO_L476VG.h"
+#include "COMPASS_DISCO_L476VG.h"
 
-GYRO_DISCO_L476VG gyro;
+#define Te 0.01
+#define r2d 57.2957795131
+#define PI 3.14159265359
+
+GYRO_DISCO_L476VG gyro; 
+COMPASS_DISCO_L476VG compass;
+
+Serial pc(USBTX, USBRX);
+Ticker t1;
+
+volatile bool flag = 0 ;
+
+double coef_a[2];
+double coef_g[2];
 
-DigitalOut led1(LED1);
+double xn[2];
+double yn[2];
+
+double sortie_a[2];
+double sortie_g[2];
+
+double angle_y_acc[2];
+double angle_y_gyro[2];
+
+float x,z;
+float derive;
+float offset;
+
+void coef_filtre(double,double ,double*);
+void filtre_PB(double *, double *, double *);
+
+void mesure();
+int u;
+
 
 int main()
 {
     float GyroBuffer[3];
-  
-    printf("Gyroscope started\n");
-  
-    while(1) {
-        // Read Gyroscope values
+    int16_t AccBuffer[3];
+    pc.baud(115200);
+    t1.attach(&mesure,Te);
+
+    coef_filtre(1.00,0.1,coef_a);
+    coef_filtre(1/(2*PI*0.1),0.1,coef_g);
+    
+    for(int a = 0;a<=10000;a++){
         gyro.GetXYZ(GyroBuffer);
-        // Display values      
-        printf("X = %f\n", GyroBuffer[0]);
-        printf("Y = %f\n", GyroBuffer[1]);
-        printf("Z = %f\n", GyroBuffer[2]);
-        printf("\033[3A"); // Moves cursor up x lines (x value is between [ and A)
-        led1 = !led1;
-        wait(1);
+        derive += GyroBuffer[1]/10000;
+        }
+    for(int a = 0;a<=1000;a++){
+        compass.AccGetXYZ(AccBuffer);
+        x = AccBuffer[0]*0.061/1000;
+        z = AccBuffer[2]*0.061/1000;
+        offset += ((atan2((double)x,(double)z))*r2d)/1000;
+        }
+
+    while(1) {
+
+        if(flag) {
+            flag = 0;
+            compass.AccGetXYZ(AccBuffer);
+            gyro.GetXYZ(GyroBuffer);
+            x = AccBuffer[0]*0.061/1000;
+            z = AccBuffer[2]*0.061/1000;
+            angle_y_gyro[0] = (GyroBuffer[1]-derive)/1000;
+            angle_y_acc[0] = (atan2((double)x,(double)z))*r2d-offset;
+
+            filtre_PB(angle_y_acc,sortie_a,coef_a);
+            filtre_PB(angle_y_gyro,sortie_g,coef_g);
+    
+            if((u%10)==0) {
+                pc.printf("$%lf %lf %lf;\n",sortie_g[0],sortie_a[0],sortie_a[0]+sortie_g[0]);
+            }
+        }
     }
 }
+
+void mesure()
+{
+    flag = 1;
+    u++;
+}
+
+void coef_filtre(double H0,double F0,double *coef)
+{
+    coef[0] = (H0*Te)/( Te + (1/(PI*F0)));
+    coef[1] = (Te -(1/(PI*F0)))/( Te + (1/(PI*F0)));
+}
+void filtre_PB(double *xn, double *yn, double *coef)
+{
+    yn[0] = coef[0]*(xn[1]+xn[0])-coef[1]*yn[1];
+    yn[1] = yn[0];
+    xn[1] = xn[0];
+}
\ No newline at end of file

diff -r 25b2a1150cd0 -r a3a1c87ad660 mbed-os.lib
--- a/mbed-os.lib	Wed Nov 20 15:13:34 2019 +0100
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,1 +0,0 @@
-https://github.com/ARMmbed/mbed-os/#b81aeff1a3e171c6421984faa2cc18d0e35746c0

diff -r 25b2a1150cd0 -r a3a1c87ad660 mbed.bld
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Thu Jun 10 12:34:54 2021 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/mbed_official/code/mbed/builds/65be27845400
\ No newline at end of file