main.cpp

00001 
00002 /* Includes */
00003 #include "mbed.h"
00004 #include "x_nucleo_iks01a2.h"
00005 
00006 /* Instantiate the expansion board */
00007 static X_NUCLEO_IKS01A2 *mems_expansion_board = X_NUCLEO_IKS01A2::Instance(D14, D15, D4, D5);
00008 
00009 
00010 #define  sens 70 // sensibilità presa dal datasheet per il fondo scala utilizzato 
00011 
00012 static LSM6DSLSensor *acc_gyro = mems_expansion_board->acc_gyro;
00013 
00014 
00015 /* Simple main function */
00016 int main() {
00017   uint8_t id;
00018   int32_t mdps[3];
00019   int32_t acc[3];
00020   int32_t off[3];
00021   float parziale[3];
00022   float angolo[3];
00023   
00024   
00025   for(int i=0;i<3;i++)
00026 {parziale[i]=0;}
00027 
00028  for(int i=0;i<3;i++)
00029 {angolo [i]=0;}
00030   /* Enable all sensors */
00031  
00032   acc_gyro->Enable_X();
00033   acc_gyro->Enable_G();
00034   
00035   printf("\r\n--- Starting new run ---\r\n");
00036   acc_gyro->ReadID(&id);
00037   printf("LSM6DSL accelerometer & gyroscope = 0x%X\r\n", id);
00038   // attendo l' inzializzazione
00039   wait(1.5);    
00040   
00041   // effettuo una prima acquisizione del giroscopio per ottenere l'offset delle velocità angolari
00042  acc_gyro->Get_G_Axes(mdps);   
00043  printf("LSM6DSL [gyro/mdps]:   %6ld, %6ld, %6ld\r\n", mdps[0], mdps[1], mdps[2]);
00044  
00045  for(int i=0;i<3;i++){ 
00046 off[i]=mdps[i];}
00047 
00048   printf("off [gyro/mdps]:   %6ld, %6ld, %6ld\r\n", off[0], off[1], off[2]);
00049   
00050   while(1) {
00051     printf("\r\n");
00052 
00053     acc_gyro->Get_X_Axes(acc);
00054   
00055     printf("LSM6DSL [acc/mg]:      %6ld, %6ld, %6ld\r\n", acc[0], acc[1], acc[2]);
00056 
00057     acc_gyro->Get_G_Axes(mdps);
00058     printf("LSM6DSLrow [gyro/mdps]:   %6ld, %6ld, %6ld\r\n", mdps[0], mdps[1], mdps[2]);
00059     
00060     // sottraggo l'offset
00061 for(int i=0;i<3;i++)
00062 {mdps[i]=mdps[i]-off[i];}
00063        printf("LSM6DSLfine [gyro/mdps]:   %6ld, %6ld, %6ld\r\n", mdps[0], mdps[1], mdps[2]);
00064 
00065     wait_ms(1);
00066     
00067     // ricavo il parziale dalla velocità angolare
00068     
00069     for(int i=0;i<3;i++)
00070 {
00071 // passo da mdps a dpLSB
00072 
00073     parziale[i]=(mdps[i]*sens)/1000;
00074 // levo la correzione per poter sommare i dati parziali off 19 coeff 2.84
00075 angolo[i]=(angolo[i]- 19)/2.84;
00076 // moltiplico per il dt (1ms)
00077     parziale[i]/= 1000;
00078     
00079    if (mdps[i]>140 ||mdps[i]<-140)// ci si puo mettere anche 70 come soglia non cambia molto
00080     angolo[i] += parziale[i];     // integro 
00081     
00082 // correggo offset e guadagno che ho ricavato da una "taratura" grezza (ricavo la retta )
00083   
00084    angolo[i]=(angolo[i]*2.84)+19;
00085 }
00086 
00087 
00088     printf("angolo  [gyro/d]:   %6f, %6f, %6f\r\n", angolo[0], angolo[1], angolo[2]);//angolo
00089   }
00090 }