sensori
Dependencies: X_NUCLEO_IKS01A2 mbed
Fork of HelloWorld_IKS01A2 by
main.cpp@16:f50bbf3ae57c, 2017-02-16 (annotated)
- Committer:
- vidica94
- Date:
- Thu Feb 16 11:25:20 2017 +0000
- Revision:
- 16:f50bbf3ae57c
- Parent:
- 15:25999e71b22d
no timer
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
cparata | 0:69566eea0fba | 1 | |
cparata | 0:69566eea0fba | 2 | /* Includes */ |
cparata | 0:69566eea0fba | 3 | #include "mbed.h" |
cparata | 0:69566eea0fba | 4 | #include "x_nucleo_iks01a2.h" |
cparata | 0:69566eea0fba | 5 | |
cparata | 0:69566eea0fba | 6 | /* Instantiate the expansion board */ |
cparata | 8:8f495e604424 | 7 | static X_NUCLEO_IKS01A2 *mems_expansion_board = X_NUCLEO_IKS01A2::Instance(D14, D15, D4, D5); |
cparata | 0:69566eea0fba | 8 | |
cparata | 0:69566eea0fba | 9 | |
vidica94 | 16:f50bbf3ae57c | 10 | #define sens 70 // sensibilità presa dal datasheet per il fondo scala utilizzato |
cparata | 0:69566eea0fba | 11 | |
vidica94 | 13:2e809b3e6ea9 | 12 | static LSM6DSLSensor *acc_gyro = mems_expansion_board->acc_gyro; |
cparata | 0:69566eea0fba | 13 | |
cparata | 0:69566eea0fba | 14 | |
cparata | 0:69566eea0fba | 15 | /* Simple main function */ |
cparata | 0:69566eea0fba | 16 | int main() { |
cparata | 0:69566eea0fba | 17 | uint8_t id; |
vidica94 | 16:f50bbf3ae57c | 18 | int32_t mdps[3]; |
vidica94 | 16:f50bbf3ae57c | 19 | int32_t acc[3]; |
vidica94 | 13:2e809b3e6ea9 | 20 | int32_t off[3]; |
vidica94 | 13:2e809b3e6ea9 | 21 | float parziale[3]; |
vidica94 | 16:f50bbf3ae57c | 22 | float angolo[3]; |
vidica94 | 16:f50bbf3ae57c | 23 | |
vidica94 | 16:f50bbf3ae57c | 24 | |
vidica94 | 13:2e809b3e6ea9 | 25 | for(int i=0;i<3;i++) |
vidica94 | 13:2e809b3e6ea9 | 26 | {parziale[i]=0;} |
vidica94 | 13:2e809b3e6ea9 | 27 | |
vidica94 | 13:2e809b3e6ea9 | 28 | for(int i=0;i<3;i++) |
vidica94 | 16:f50bbf3ae57c | 29 | {angolo [i]=0;} |
cparata | 0:69566eea0fba | 30 | /* Enable all sensors */ |
vidica94 | 13:2e809b3e6ea9 | 31 | |
cparata | 0:69566eea0fba | 32 | acc_gyro->Enable_X(); |
cparata | 0:69566eea0fba | 33 | acc_gyro->Enable_G(); |
cparata | 0:69566eea0fba | 34 | |
cparata | 0:69566eea0fba | 35 | printf("\r\n--- Starting new run ---\r\n"); |
cparata | 0:69566eea0fba | 36 | acc_gyro->ReadID(&id); |
cparata | 0:69566eea0fba | 37 | printf("LSM6DSL accelerometer & gyroscope = 0x%X\r\n", id); |
vidica94 | 16:f50bbf3ae57c | 38 | // attendo l' inzializzazione |
vidica94 | 16:f50bbf3ae57c | 39 | wait(1.5); |
vidica94 | 16:f50bbf3ae57c | 40 | |
vidica94 | 16:f50bbf3ae57c | 41 | // effettuo una prima acquisizione del giroscopio per ottenere l'offset delle velocità angolari |
vidica94 | 16:f50bbf3ae57c | 42 | acc_gyro->Get_G_Axes(mdps); |
vidica94 | 16:f50bbf3ae57c | 43 | printf("LSM6DSL [gyro/mdps]: %6ld, %6ld, %6ld\r\n", mdps[0], mdps[1], mdps[2]); |
vidica94 | 16:f50bbf3ae57c | 44 | |
vidica94 | 15:25999e71b22d | 45 | for(int i=0;i<3;i++){ |
vidica94 | 16:f50bbf3ae57c | 46 | off[i]=mdps[i];} |
vidica94 | 16:f50bbf3ae57c | 47 | |
vidica94 | 13:2e809b3e6ea9 | 48 | printf("off [gyro/mdps]: %6ld, %6ld, %6ld\r\n", off[0], off[1], off[2]); |
vidica94 | 16:f50bbf3ae57c | 49 | |
cparata | 0:69566eea0fba | 50 | while(1) { |
cparata | 0:69566eea0fba | 51 | printf("\r\n"); |
cparata | 0:69566eea0fba | 52 | |
vidica94 | 16:f50bbf3ae57c | 53 | acc_gyro->Get_X_Axes(acc); |
vidica94 | 15:25999e71b22d | 54 | |
vidica94 | 16:f50bbf3ae57c | 55 | printf("LSM6DSL [acc/mg]: %6ld, %6ld, %6ld\r\n", acc[0], acc[1], acc[2]); |
cparata | 0:69566eea0fba | 56 | |
vidica94 | 16:f50bbf3ae57c | 57 | acc_gyro->Get_G_Axes(mdps); |
vidica94 | 16:f50bbf3ae57c | 58 | printf("LSM6DSLrow [gyro/mdps]: %6ld, %6ld, %6ld\r\n", mdps[0], mdps[1], mdps[2]); |
vidica94 | 16:f50bbf3ae57c | 59 | |
vidica94 | 16:f50bbf3ae57c | 60 | // sottraggo l'offset |
vidica94 | 13:2e809b3e6ea9 | 61 | for(int i=0;i<3;i++) |
vidica94 | 16:f50bbf3ae57c | 62 | {mdps[i]=mdps[i]-off[i];} |
vidica94 | 16:f50bbf3ae57c | 63 | printf("LSM6DSLfine [gyro/mdps]: %6ld, %6ld, %6ld\r\n", mdps[0], mdps[1], mdps[2]); |
vidica94 | 16:f50bbf3ae57c | 64 | |
vidica94 | 15:25999e71b22d | 65 | wait_ms(1); |
vidica94 | 13:2e809b3e6ea9 | 66 | |
vidica94 | 15:25999e71b22d | 67 | // ricavo il parziale dalla velocità angolare |
vidica94 | 13:2e809b3e6ea9 | 68 | |
vidica94 | 13:2e809b3e6ea9 | 69 | for(int i=0;i<3;i++) |
vidica94 | 14:de30b189c5ec | 70 | { |
vidica94 | 16:f50bbf3ae57c | 71 | // passo da mdps a dpLSB |
vidica94 | 15:25999e71b22d | 72 | |
vidica94 | 16:f50bbf3ae57c | 73 | parziale[i]=(mdps[i]*sens)/1000; |
vidica94 | 16:f50bbf3ae57c | 74 | // levo la correzione per poter sommare i dati parziali off 19 coeff 2.84 |
vidica94 | 16:f50bbf3ae57c | 75 | angolo[i]=(angolo[i]- 19)/2.84; |
vidica94 | 16:f50bbf3ae57c | 76 | // moltiplico per il dt (1ms) |
vidica94 | 16:f50bbf3ae57c | 77 | parziale[i]/= 1000; |
vidica94 | 15:25999e71b22d | 78 | |
vidica94 | 16:f50bbf3ae57c | 79 | if (mdps[i]>140 ||mdps[i]<-140)// ci si puo mettere anche 70 come soglia non cambia molto |
vidica94 | 16:f50bbf3ae57c | 80 | angolo[i] += parziale[i]; // integro |
vidica94 | 16:f50bbf3ae57c | 81 | |
vidica94 | 16:f50bbf3ae57c | 82 | // correggo offset e guadagno che ho ricavato da una "taratura" grezza (ricavo la retta ) |
vidica94 | 15:25999e71b22d | 83 | |
vidica94 | 16:f50bbf3ae57c | 84 | angolo[i]=(angolo[i]*2.84)+19; |
vidica94 | 15:25999e71b22d | 85 | } |
cparata | 0:69566eea0fba | 86 | |
vidica94 | 14:de30b189c5ec | 87 | |
vidica94 | 16:f50bbf3ae57c | 88 | printf("angolo [gyro/d]: %6f, %6f, %6f\r\n", angolo[0], angolo[1], angolo[2]);//angolo |
cparata | 0:69566eea0fba | 89 | } |
cparata | 0:69566eea0fba | 90 | } |