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 Timer t;
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   float dt=0;
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     
00052     printf("\r\n");
00053 
00054     acc_gyro->Get_X_Axes(acc);
00055   
00056     printf("LSM6DSL [acc/mg]:      %6ld, %6ld, %6ld\r\n", acc[0], acc[1], acc[2]);
00057     
00058  t.stop();
00059    dt= t.read();
00060        //printf("The time taken was %d milliseconds\n", t.read_ms());
00061         
00062         
00063     acc_gyro->Get_G_Axes(mdps);
00064     
00065     t.reset();
00066     t.start();
00067     
00068     printf("LSM6DSLrow [gyro/mdps]:   %6ld, %6ld, %6ld\r\n", mdps[0], mdps[1], mdps[2]);
00069     
00070     // sottraggo l'offset
00071 for(int i=0;i<3;i++)
00072 {mdps[i]=mdps[i]-off[i];}
00073        printf("LSM6DSLfine [gyro/mdps]:   %6ld, %6ld, %6ld\r\n", mdps[0], mdps[1], mdps[2]);
00074 
00075     //wait_ms(1);
00076     
00077     
00078     // ricavo il parziale dalla velocità angolare
00079     
00080     for(int i=0;i<3;i++)
00081 {
00082 // passo da mdps a dpLSB
00083 
00084     parziale[i]=(mdps[i]*sens)/1000;
00085     
00086 // levo la correzione per poter sommare i dati parziali off 19 coeff 2.84
00087 angolo[i]=(angolo[i]-(-3.19211))/0.0140888;
00088 // moltiplico per il dt 
00089     parziale[i]*=(dt);
00090     
00091    if (mdps[i]>140 ||mdps[i]<-140)// ci si puo mettere anche 70 come soglia non cambia molto
00092     angolo[i] += parziale[i];     // integro 
00093     
00094 // correggo offset e guadagno che ho ricavato da una "taratura" (ricavo la best fit line  )
00095   
00096    angolo[i]=(angolo[i]*0.0140888)+(-3.19211);
00097 }
00098 
00099 
00100     printf("angolo  [gyro/d]:   %6f, %6f, %6f\r\n", angolo[0], angolo[1], angolo[2]);//angolo
00101   }
00102 }