DuckieTownHS_UniNA / Mbed 2 deprecated AccpiuGiroscopio_timer

calib tutorial for giroscope

Dependencies:   X_NUCLEO_IKS01A2 mbed

Fork of AccpiuGiroscopio_timer by duckietownhsunina

main.cpp@17:9d70e41ec8bc, 2017-02-20 (annotated)

Committer:
vidica94
Date:
Mon Feb 20 09:40:43 2017 +0000
Revision:
17:9d70e41ec8bc
Parent:
16:553a1c68d606 
calibration tutorial

Who changed what in which revision?

UserRevisionLine numberNew contents of line
vidica94 17:9d70e41ec8bc 1 /* this program acquires the acceleration and the angular speed on the 3 axes
vidica94 17:9d70e41ec8bc 2 the angular speed acquired in mdps( milli degree por second) is converrted in angular position
vidica94 17:9d70e41ec8bc 3 using a simple algoritm of discret integration.
vidica94 17:9d70e41ec8bc 4 To have clean and right position it's necessary to calibrate the giroscope.
vidica94 17:9d70e41ec8bc 5 To calibrate we read row position( comment the correctionoperation of off and slope at line 97 and 106)
vidica94 17:9d70e41ec8bc 6 at specific and known angles and using a linear fit algoritm it's possible to know the
vidica94 17:9d70e41ec8bc 7 slope and the offset that have to be correc.
vidica94 17:9d70e41ec8bc 8 The angles used are 90°,180°270°,360° because they are the easiest to reach whitout specific tools.
vidica94 17:9d70e41ec8bc 9 after the calibration it's possible uncomment line 97 and 106
vidica94 17:9d70e41ec8bc 10 WARNING:offset and slope depends by the dt of integration !!
vidica94 17:9d70e41ec8bc 11 */
cparata 0:69566eea0fba 12
cparata 0:69566eea0fba 13 /* Includes */
cparata 0:69566eea0fba 14 #include "mbed.h"
cparata 0:69566eea0fba 15 #include "x_nucleo_iks01a2.h"
cparata 0:69566eea0fba 16
cparata 0:69566eea0fba 17 /* Instantiate the expansion board */
cparata 8:8f495e604424 18 static X_NUCLEO_IKS01A2 *mems_expansion_board = X_NUCLEO_IKS01A2::Instance(D14, D15, D4, D5);
cparata 0:69566eea0fba 19
cparata 0:69566eea0fba 20
vidica94 17:9d70e41ec8bc 21 #define sens 70 // sensibility for the range that we are using
cparata 0:69566eea0fba 22
vidica94 13:2e809b3e6ea9 23 static LSM6DSLSensor *acc_gyro = mems_expansion_board->acc_gyro;
vidica94 16:553a1c68d606 24 Timer t;
cparata 0:69566eea0fba 25
cparata 0:69566eea0fba 26 /* Simple main function */
cparata 0:69566eea0fba 27 int main() {
cparata 0:69566eea0fba 28 uint8_t id;
vidica94 16:553a1c68d606 29 int32_t mdps[3];
vidica94 16:553a1c68d606 30 int32_t acc[3];
vidica94 13:2e809b3e6ea9 31 int32_t off[3];
vidica94 13:2e809b3e6ea9 32 float parziale[3];
vidica94 16:553a1c68d606 33 float angolo[3];
vidica94 16:553a1c68d606 34 float dt=0;
vidica94 16:553a1c68d606 35
vidica94 13:2e809b3e6ea9 36 for(int i=0;i<3;i++)
vidica94 13:2e809b3e6ea9 37 {parziale[i]=0;}
vidica94 13:2e809b3e6ea9 38
vidica94 13:2e809b3e6ea9 39 for(int i=0;i<3;i++)
vidica94 16:553a1c68d606 40 {angolo [i]=0;}
cparata 0:69566eea0fba 41 /* Enable all sensors */
vidica94 13:2e809b3e6ea9 42
cparata 0:69566eea0fba 43 acc_gyro->Enable_X();
cparata 0:69566eea0fba 44 acc_gyro->Enable_G();
cparata 0:69566eea0fba 45
cparata 0:69566eea0fba 46 printf("\r\n--- Starting new run ---\r\n");
cparata 0:69566eea0fba 47 acc_gyro->ReadID(&id);
cparata 0:69566eea0fba 48 printf("LSM6DSL accelerometer & gyroscope = 0x%X\r\n", id);
vidica94 17:9d70e41ec8bc 49 // wait for inizialization
vidica94 17:9d70e41ec8bc 50 wait(1.5);
vidica94 17:9d70e41ec8bc 51
vidica94 17:9d70e41ec8bc 52 //first acquisition to know the offset of the angular speeds
vidica94 16:553a1c68d606 53 acc_gyro->Get_G_Axes(mdps);
vidica94 16:553a1c68d606 54 printf("LSM6DSL [gyro/mdps]: %6ld, %6ld, %6ld\r\n", mdps[0], mdps[1], mdps[2]);
vidica94 16:553a1c68d606 55
vidica94 17:9d70e41ec8bc 56 //print the offset
vidica94 15:25999e71b22d 57 for(int i=0;i<3;i++){
vidica94 16:553a1c68d606 58 off[i]=mdps[i];}
vidica94 16:553a1c68d606 59
vidica94 13:2e809b3e6ea9 60 printf("off [gyro/mdps]: %6ld, %6ld, %6ld\r\n", off[0], off[1], off[2]);
vidica94 16:553a1c68d606 61
cparata 0:69566eea0fba 62 while(1) {
vidica94 16:553a1c68d606 63
cparata 0:69566eea0fba 64 printf("\r\n");
cparata 0:69566eea0fba 65
vidica94 17:9d70e41ec8bc 66 acc_gyro->Get_X_Axes(acc);// acquire the acceleration
vidica94 15:25999e71b22d 67
vidica94 17:9d70e41ec8bc 68 printf("LSM6DSL [acc/mg]: %6ld, %6ld, %6ld\r\n", acc[0], acc[1], acc[2]); //print the acceleration
vidica94 17:9d70e41ec8bc 69 // stop the timer used to estimate the dt(=time between 2 acquisitions) used in the integration
vidica94 16:553a1c68d606 70 t.stop();
vidica94 17:9d70e41ec8bc 71 dt= t.read();
vidica94 17:9d70e41ec8bc 72 printf("The time taken was %d milliseconds\n", t.read_ms());// this prinft it's not necessary it's just to know the time of integration
vidica94 16:553a1c68d606 73
vidica94 17:9d70e41ec8bc 74 // acquire the angular speeds in mdps
vidica94 16:553a1c68d606 75 acc_gyro->Get_G_Axes(mdps);
vidica94 17:9d70e41ec8bc 76 // reset the timer and restart it
vidica94 16:553a1c68d606 77 t.reset();
vidica94 16:553a1c68d606 78 t.start();
vidica94 16:553a1c68d606 79
vidica94 17:9d70e41ec8bc 80 printf("LSM6DSLrow [gyro/mdps]: %6ld, %6ld, %6ld\r\n", mdps[0], mdps[1], mdps[2]);// print the angular speeds before the offset correction
vidica94 16:553a1c68d606 81
vidica94 17:9d70e41ec8bc 82 // ssubtract the offset at the angulas speeds
vidica94 16:553a1c68d606 83 for(int i=0;i<3;i++)
vidica94 16:553a1c68d606 84 {mdps[i]=mdps[i]-off[i];}
vidica94 17:9d70e41ec8bc 85 printf("LSM6DSLfine [gyro/mdps]: %6ld, %6ld, %6ld\r\n", mdps[0], mdps[1], mdps[2]);// print the angular speeds after the offset correction
vidica94 16:553a1c68d606 86
vidica94 13:2e809b3e6ea9 87
vidica94 17:9d70e41ec8bc 88
vidica94 13:2e809b3e6ea9 89
vidica94 13:2e809b3e6ea9 90 for(int i=0;i<3;i++)
vidica94 14:de30b189c5ec 91 {
vidica94 17:9d70e41ec8bc 92 // convert the angular speeds from mdps to dpLSB
vidica94 15:25999e71b22d 93
vidica94 16:553a1c68d606 94 parziale[i]=(mdps[i]*sens)/1000;
vidica94 16:553a1c68d606 95
vidica94 17:9d70e41ec8bc 96 // delete the coerrection of offset and slope to integrate beacuse they are correction on the angle not on the angular speed
vidica94 16:553a1c68d606 97 angolo[i]=(angolo[i]-(-3.19211))/0.0140888;
vidica94 17:9d70e41ec8bc 98 // multiply for dt
vidica94 16:553a1c68d606 99 parziale[i]*=(dt);
vidica94 15:25999e71b22d 100
vidica94 17:9d70e41ec8bc 101 if (mdps[i]>70 ||mdps[i]<-70)//
vidica94 17:9d70e41ec8bc 102 angolo[i] += parziale[i]; // summ to ultimate the integration
vidica94 16:553a1c68d606 103
vidica94 17:9d70e41ec8bc 104 // correction of offset and slope obtained aftera a "taratura" ( best fit line )
vidica94 15:25999e71b22d 105
vidica94 16:553a1c68d606 106 angolo[i]=(angolo[i]*0.0140888)+(-3.19211);
vidica94 15:25999e71b22d 107 }
cparata 0:69566eea0fba 108
vidica94 14:de30b189c5ec 109
vidica94 17:9d70e41ec8bc 110 printf("angolo [gyro/d]: %6f, %6f, %6f\r\n", angolo[0], angolo[1], angolo[2]);//print angoles
cparata 0:69566eea0fba 111 }
cparata 0:69566eea0fba 112 }