solo z veloce
Dependencies: X_NUCLEO_IKS01A2 mbed
Fork of Giroscopio_timer_fast by
main.cpp
- Committer:
- vidica94
- Date:
- 2017-03-07
- Revision:
- 19:ba121767de70
- Parent:
- 18:da0744a1b128
File content as of revision 19:ba121767de70:
/* Includes */ #include "mbed.h" #include "x_nucleo_iks01a2.h" /* Instantiate the expansion board */ static X_NUCLEO_IKS01A2 *mems_expansion_board = X_NUCLEO_IKS01A2::Instance(D14, D15, D4, D5); Serial pc(SERIAL_TX, SERIAL_RX); #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 mdps[3]; int32_t acc[3]; int32_t off[3]; float parziale[3]; float angolo[3]; float dt=0; parziale[2]=0; angolo [2]=0; /* Enable sensors */ acc_gyro->Enable_G(); pc.printf("\r\n--- Starting new run ---\r\n"); acc_gyro->ReadID(&id); pc.printf("LSM6DSL accelerometer & gyroscope = 0x%X\r\n", id); // 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); pc.printf("LSM6DSL [gyro/mdps]: %6ld\r\n", mdps[2]); off[2]=mdps[2]; pc.printf("off [gyro/mdps]: %6ld\r\n", off[2]); while(1) { pc.printf("\r\n"); t.stop(); dt= t.read(); pc.printf("The time taken was %d milliseconds\n", t.read_ms()); t.reset(); acc_gyro->Get_G_Axes(mdps); t.start(); //pc.printf("LSM6DSLrow [gyro/mdps]: %6ld\r\n", mdps[2]); // sottraggo l'offset mdps[2]=mdps[2]-off[2]; pc.printf("LSM6DSLfine [gyro/mdps]: %6ld\r\n",mdps[2]); //wait_ms(1); // ricavo il parziale dalla velocità angolare // passo da mdps a dpLSB parziale[2]=(mdps[2]*sens)/1000; // levo la correzione per poter sommare i dati parziali off 19 coeff 2.84 angolo[2]=(angolo[2]-(0.724005))/0.0226195; // moltiplico per il dt parziale[2]*=(dt); if (mdps[2]>70 ||mdps[2]<-70)// ci si puo mettere anche 70 come soglia non cambia molto angolo[2] += parziale[2]; // integro // correggo offset e guadagno che ho ricavato da una "taratura" (ricavo la best fit line ) angolo[2]=(angolo[2]*0.0226195)+(0.724005); pc.printf("angolo [gyro/d]: %6f\r\n", angolo[2]);//angolo } }