Diff: main.cpp

Revision:

16:553a1c68d606

Parent:

15:25999e71b22d

--- a/main.cpp	Sat Feb 11 15:18:54 2017 +0000
+++ b/main.cpp	Thu Feb 16 11:23:33 2017 +0000
@@ -7,24 +7,26 @@
 static X_NUCLEO_IKS01A2 *mems_expansion_board = X_NUCLEO_IKS01A2::Instance(D14, D15, D4, D5);
 
 
-#define  sens 70
+#define  sens 70 // sensibilità presa dal datasheet per il fondo scala utilizzato 
 
 static LSM6DSLSensor *acc_gyro = mems_expansion_board->acc_gyro;
-
+Timer t;
 
 /* Simple main function */
 int main() {
   uint8_t id;
-  int32_t axes[3];
+  int32_t mdps[3];
+  int32_t acc[3];
   int32_t off[3];
   float parziale[3];
-  float finale[3];
-  int32_t k=0;
+  float angolo[3];
+  float dt=0;
+  
   for(int i=0;i<3;i++)
 {parziale[i]=0;}
 
  for(int i=0;i<3;i++)
-{finale [i]=0;}
+{angolo [i]=0;}
   /* Enable all sensors */
  
   acc_gyro->Enable_X();
@@ -33,43 +35,68 @@
   printf("\r\n--- Starting new run ---\r\n");
   acc_gyro->ReadID(&id);
   printf("LSM6DSL accelerometer & gyroscope = 0x%X\r\n", id);
-  wait(1.5);
- acc_gyro->Get_G_Axes(axes);
- printf("LSM6DSL [gyro/mdps]:   %6ld, %6ld, %6ld\r\n", axes[0], axes[1], axes[2]);
+  // attendo l' inzializzazione
+  wait(1.5);    
+  
+  // effettuo una prima acquisizione del giroscopio per ottenere l'offset delle velocità angolari
+ acc_gyro->Get_G_Axes(mdps);   
+ printf("LSM6DSL [gyro/mdps]:   %6ld, %6ld, %6ld\r\n", mdps[0], mdps[1], mdps[2]);
+ 
  for(int i=0;i<3;i++){ 
-off[i]=axes[i];}
+off[i]=mdps[i];}
+
   printf("off [gyro/mdps]:   %6ld, %6ld, %6ld\r\n", off[0], off[1], off[2]);
+  
   while(1) {
+    
     printf("\r\n");
 
-    acc_gyro->Get_X_Axes(axes);
+    acc_gyro->Get_X_Axes(acc);
   
-    printf("LSM6DSL [acc/mg]:      %6ld, %6ld, %6ld\r\n", axes[0], axes[1], axes[2]);
+    printf("LSM6DSL [acc/mg]:      %6ld, %6ld, %6ld\r\n", acc[0], acc[1], acc[2]);
+    
+ t.stop();
+   dt= t.read();
+       //printf("The time taken was %d milliseconds\n", t.read_ms());
+        
+        
+    acc_gyro->Get_G_Axes(mdps);
+    
+    t.reset();
+    t.start();
+    
+    printf("LSM6DSLrow [gyro/mdps]:   %6ld, %6ld, %6ld\r\n", mdps[0], mdps[1], mdps[2]);
+    
+    // sottraggo l'offset
+for(int i=0;i<3;i++)
+{mdps[i]=mdps[i]-off[i];}
+       printf("LSM6DSLfine [gyro/mdps]:   %6ld, %6ld, %6ld\r\n", mdps[0], mdps[1], mdps[2]);
 
-    acc_gyro->Get_G_Axes(axes);
-    printf("LSM6DSLrow [gyro/mdps]:   %6ld, %6ld, %6ld\r\n", axes[0], axes[1], axes[2]);
-for(int i=0;i<3;i++)
-{axes[i]=axes[i]-off[i];}
-       printf("LSM6DSLfine [gyro/mdps]:   %6ld, %6ld, %6ld\r\n", axes[0], axes[1], axes[2]);
-    k=k+1;
-    wait_ms(1);
+    //wait_ms(1);
+    
     
     // ricavo il parziale dalla velocità angolare
     
     for(int i=0;i<3;i++)
 {
-    parziale[i]=(axes[i]*sens)/1000;// passo da mdps a dpLSB
+// passo da mdps a dpLSB
 
-finale[i]=(finale[i]-19)/2.84;// levo la correzione per poter sommare i dati parziali
-    parziale[i]/= 1000;// moltiplico per il dt (1ms)
-   if (axes[i]>150 ||axes[i]<-150)
-    finale[i] += parziale[i];     // integro 
+    parziale[i]=(mdps[i]*sens)/1000;
+    
+// levo la correzione per poter sommare i dati parziali off 19 coeff 2.84
+angolo[i]=(angolo[i]-(-3.19211))/0.0140888;
+// moltiplico per il dt 
+    parziale[i]*=(dt);
     
-   finale[i]=(finale[i]*2.84)+19;// correggo offset e guadagno che ho ricavato da una "taratura" grezza (ricavo la retta )
+   if (mdps[i]>140 ||mdps[i]<-140)// ci si puo mettere anche 70 come soglia non cambia molto
+    angolo[i] += parziale[i];     // integro 
+    
+// correggo offset e guadagno che ho ricavato da una "taratura" (ricavo la best fit line  )
   
+   angolo[i]=(angolo[i]*0.0140888)+(-3.19211);
 }
 
 
-    printf("finale  [gyro/d]:   %6f, %6f, %6f\r\n", finale[0], finale[1], finale[2]);//angolo
+    printf("angolo  [gyro/d]:   %6f, %6f, %6f\r\n", angolo[0], angolo[1], angolo[2]);//angolo
   }
 }