Diff: main.cpp

Revision:

6:65c511f6f1fc

Parent:

5:d51ba8e93d10

Child:

7:f5c5cf499311

--- a/main.cpp	Sat May 23 10:02:24 2020 +0000
+++ b/main.cpp	Sat May 23 10:30:02 2020 +0000
@@ -1,5 +1,5 @@
 // test avec RTOS
-#define viti 1    // 1 montage viti ;  0 montage sur table
+#define viti 0    // 1 montage viti ;  0 montage sur table
 #include "mbed.h"
 #include "LSM9DS1.h"
 
@@ -55,39 +55,40 @@
 //
 double dt=dt0 ; // pas reel
 double angle_final;
+//
 void calcul(void)
 {
     while(1) {
+        ana=0.3;
         Fc=0.05;
-RC=1./(Fc*2*pi);            //calcul de RC
-a0=1./(1+(2*RC/dt));   //calcul du coefficient a du filtre
-b0=(1-(2.*RC/dt))*a0;    //calcul du coefficient b du filtre
-a1=a0*RC*1.0;           //calcul du coefficient a du filtre
+        RC=1./(Fc*2*pi);            //calcul de RC
+        a0=1./(1+(2*RC/dt));   //calcul du coefficient a du filtre
+        b0=(1-(2.*RC/dt))*a0;    //calcul du coefficient b du filtre
+        a1=a0*RC*1.0;           //calcul du coefficient a du filtre
         dt=tt.read_us()/1000000.0;
         tt.reset();
-        tt.start();
-        ana=0.6;
+        //tt.start();
         DOF.readAccel();
         DOF.readGyro();
-        #if viti
+#if viti
         ang=((180/pi)*atan2((double)DOF.ax,-(double)DOF.ay)-90-ang_off);  // sur site
-        #else
+#else
         ang=((180/pi)*atan2((double)DOF.ay,(double)DOF.az)-ang_off);       // sur table
-        #endif
+#endif
 
         // filtres complémentaires
         angle_acce_pred = angle_acce;
         angle_acce=ang;
         angle_acce_f_pred = angle_acce_f;
         angle_acce_f=a0*angle_acce+a0*angle_acce_pred-b0*angle_acce_f_pred;      //filtrage accéléromètre
-        
+
         gyroy_pred = gyroy;
-        #if viti
+#if viti
         gyroy=DOF.gz-gz_off;  //sur site
-        #else
+#else
         gyroy=-DOF.gx-gx_off;  //sur table
-        #endif       
-        
+#endif
+
         angle_gyroy_f_pred = angle_gyroy_f;
         angle_gyroy_f=a1*gyroy+a1*gyroy_pred-b0*angle_gyroy_f_pred;
         //
@@ -104,7 +105,8 @@
 int main()
 {
 
-dt=dt0;
+    dt=dt0;
+    tt.start();
     wait(1);
     //DOF.calibration();
     DOF.begin();
@@ -112,11 +114,12 @@
     DOF.calibration();
     wait(1);
     thread.start(calcul);
+    
 
     while(1) {
 
 
-        ana=0.3;
+        ana=0.6;
         //moteur
         float x=verin.read();
         float s=0.0;