Diff: main.cpp

Revision:

6:81d8b03a9673

Parent:

5:f4a35a2a9085

diff -r f4a35a2a9085 -r 81d8b03a9673 main.cpp
--- a/main.cpp	Mon Jun 08 17:42:48 2020 +0000
+++ b/main.cpp	Mon Jun 15 14:48:08 2020 +0000
@@ -1,55 +1,64 @@
 #include "mbed.h"
 #include "GYRO_DISCO_L476VG.h"
-
+#include "COMPASS_DISCO_L476VG.h"
+//
+#define TE 0.01
 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 gyro_zero(void)
+double psig=0,psia;
+double gyro_zero(void)
 {
-    const int NN=100000;
+    const int NN=10000;
     float GyroBuffer[3];
-    float gy_off=0;
-    for(int i=0; i<NN; i++)
-      {
+    double gy_off=0;
+    for(int i=0; i<NN; i++) {
         gyro.GetXYZ(GyroBuffer);
         gy_off=gy_off+GyroBuffer[1]/NN;
-        
     }
     return(gy_off);
 }
+double angle_zero(void)
+{
+    const int NN=1000;
+    int16_t AccBuffer[3];
+    double PI=4*atan(1.0);
+    double 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");
-    ticker.attach(&mesure,0.01);
+    int16_t AccBuffer[3];
+    printf("Super inclinometre\n\r");
+    double PI=4*atan(1.0);
+    double gyr_off=gyro_zero();
+    double ang_off=angle_zero();
+    ticker.attach(&mesure,TE);
     unsigned char cpt=0;
-    float gyoff=gyro_zero();
+    
     while(1) {
         if(flag) {
-
-            // Read Gyroscope values
+            compass.AccGetXYZ(AccBuffer);
+            psia=((180.0/PI)*atan2((double)AccBuffer[0],(double)AccBuffer[2]))-ang_off;
             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)
+            psig=psig+(GyroBuffer[1]-gyr_off)*TE/1000;
             if(cpt==9) {
                 cpt=0;
                 led1 = !led1;
-                //pc.printf("$%f;\n",GyroBuffer[1]);
-                pc.printf("$%f;\n",psig);
+                pc.printf("$%f %f;\n",psia,psig);
             }
             cpt++;
             flag=0;