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 Serial pc(SERIAL_TX, SERIAL_RX);
00010 
00011 #define  sens 70 // sensibilità presa dal datasheet per il fondo scala utilizzato 
00012 
00013 static LSM6DSLSensor *acc_gyro = mems_expansion_board->acc_gyro;
00014 Timer t;
00015 
00016 /* Simple main function */
00017 int main()
00018 {
00019     uint8_t id;
00020     int32_t mdps[3];
00021     int32_t acc[3];
00022     int32_t off[3];
00023     float parziale[3];
00024     float angolo[3];
00025     float dt=0;
00026 
00027 
00028     parziale[2]=0;
00029 
00030 
00031     angolo [2]=0;
00032     /* Enable  sensors */
00033 
00034 
00035     acc_gyro->Enable_G();
00036 
00037     pc.printf("\r\n--- Starting new run ---\r\n");
00038     acc_gyro->ReadID(&id);
00039     pc.printf("LSM6DSL accelerometer & gyroscope = 0x%X\r\n", id);
00040     // attendo l' inzializzazione
00041     wait(1.5);
00042 
00043     // effettuo una prima acquisizione del giroscopio per ottenere l'offset delle velocità angolari
00044     acc_gyro->Get_G_Axes(mdps);
00045     pc.printf("LSM6DSL [gyro/mdps]:    %6ld\r\n",  mdps[2]);
00046 
00047 
00048     off[2]=mdps[2];
00049 
00050     pc.printf("off [gyro/mdps]:  %6ld\r\n",  off[2]);
00051 
00052     while(1) {
00053 
00054         pc.printf("\r\n");
00055 
00056 
00057         t.stop();
00058         dt= t.read();
00059         pc.printf("The time taken was %d milliseconds\n", t.read_ms());
00060         t.reset();
00061         acc_gyro->Get_G_Axes(mdps);
00062 
00063         
00064         t.start();
00065 
00066         //pc.printf("LSM6DSLrow [gyro/mdps]:    %6ld\r\n", mdps[2]);
00067 
00068         // sottraggo l'offset
00069 
00070         mdps[2]=mdps[2]-off[2];
00071         pc.printf("LSM6DSLfine [gyro/mdps]:   %6ld\r\n",mdps[2]);
00072 
00073         //wait_ms(1);
00074 
00075 
00076         // ricavo il parziale dalla velocità angolare
00077 
00078 
00079 // passo da mdps a dpLSB
00080 
00081         parziale[2]=(mdps[2]*sens)/1000;
00082 
00083 // levo la correzione per poter sommare i dati parziali off 19 coeff 2.84
00084         angolo[2]=(angolo[2]-(0.724005))/0.0226195;
00085 // moltiplico per il dt
00086         parziale[2]*=(dt);
00087 
00088         if (mdps[2]>70 ||mdps[2]<-70)// ci si puo mettere anche 70 come soglia non cambia molto
00089             angolo[2] += parziale[2];     // integro
00090 
00091 // correggo offset e guadagno che ho ricavato da una "taratura" (ricavo la best fit line  )
00092 
00093         angolo[2]=(angolo[2]*0.0226195)+(0.724005);
00094 
00095 
00096 
00097         pc.printf("angolo  [gyro/d]:  %6f\r\n", angolo[2]);//angolo
00098     }
00099 }