sensori
Dependencies: X_NUCLEO_IKS01A2 mbed
Fork of HelloWorld_IKS01A2 by
Revision 16:f50bbf3ae57c, committed 2017-02-16
- Comitter:
- vidica94
- Date:
- Thu Feb 16 11:25:20 2017 +0000
- Parent:
- 15:25999e71b22d
- Commit message:
- no timer
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Sat Feb 11 15:18:54 2017 +0000 +++ b/main.cpp Thu Feb 16 11:25:20 2017 +0000 @@ -7,7 +7,7 @@ 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; @@ -15,16 +15,18 @@ /* 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]; + + 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,56 @@ 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]); - acc_gyro->Get_G_Axes(axes); - printf("LSM6DSLrow [gyro/mdps]: %6ld, %6ld, %6ld\r\n", axes[0], axes[1], axes[2]); + acc_gyro->Get_G_Axes(mdps); + 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++) -{axes[i]=axes[i]-off[i];} - printf("LSM6DSLfine [gyro/mdps]: %6ld, %6ld, %6ld\r\n", axes[0], axes[1], axes[2]); - k=k+1; +{mdps[i]=mdps[i]-off[i];} + printf("LSM6DSLfine [gyro/mdps]: %6ld, %6ld, %6ld\r\n", mdps[0], mdps[1], mdps[2]); + 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]- 19)/2.84; +// moltiplico per il dt (1ms) + parziale[i]/= 1000; - 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" grezza (ricavo la retta ) + angolo[i]=(angolo[i]*2.84)+19; } - 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 } }