main.cpp

00001 /* this program acquires the acceleration and the angular speed  on the 3 axes
00002 the angular speed acquired in mdps( milli degree por second) is converrted in angular position 
00003 using a simple algoritm of  discret integration.
00004 To have clean and right position it's necessary to calibrate the giroscope.
00005 To calibrate we read row position( comment the correctionoperation  of off and slope at line 97 and 106)
00006 at specific and known angles and using a linear fit algoritm it's possible to know the 
00007 slope and the offset that have to be correc.
00008 The angles used are 90°,180°270°,360° because they are the easiest to reach whitout specific tools.
00009 after the calibration it's possible uncomment line 97 and 106
00010 WARNING:offset and slope depends by the dt of integration !!
00011 */
00012 
00013 /* Includes */
00014 #include "mbed.h"
00015 #include "x_nucleo_iks01a2.h"
00016 
00017 /* Instantiate the expansion board */
00018 static X_NUCLEO_IKS01A2 *mems_expansion_board = X_NUCLEO_IKS01A2::Instance(D14, D15, D4, D5);
00019 
00020 
00021 #define  sens 70 // sensibility for the range that  we are using  
00022 
00023 static LSM6DSLSensor *acc_gyro = mems_expansion_board->acc_gyro;
00024 Timer t;
00025 
00026 /* Simple main function */
00027 int main() {
00028   uint8_t id;
00029   int32_t mdps[3];
00030   int32_t acc[3];
00031   int32_t off[3];
00032   float parziale[3];
00033   float angolo[3];
00034   float dt=0;
00035   
00036   for(int i=0;i<3;i++)
00037 {parziale[i]=0;}
00038 
00039  for(int i=0;i<3;i++)
00040 {angolo [i]=0;}
00041   /* Enable all sensors */
00042  
00043   acc_gyro->Enable_X();
00044   acc_gyro->Enable_G();
00045   
00046   printf("\r\n--- Starting new run ---\r\n");
00047   acc_gyro->ReadID(&id);
00048   printf("LSM6DSL accelerometer & gyroscope = 0x%X\r\n", id);
00049   // wait for inizialization 
00050   wait(1.5); 
00051      
00052   //first acquisition to know the offset of the angular speeds
00053  acc_gyro->Get_G_Axes(mdps);   
00054  printf("LSM6DSL [gyro/mdps]:   %6ld, %6ld, %6ld\r\n", mdps[0], mdps[1], mdps[2]);
00055  
00056  //print the offset
00057  for(int i=0;i<3;i++){ 
00058 off[i]=mdps[i];}
00059 
00060   printf("off [gyro/mdps]:   %6ld, %6ld, %6ld\r\n", off[0], off[1], off[2]);
00061   
00062   while(1) {
00063     
00064     printf("\r\n");
00065 
00066     acc_gyro->Get_X_Axes(acc);// acquire the acceleration 
00067   
00068     printf("LSM6DSL [acc/mg]:      %6ld, %6ld, %6ld\r\n", acc[0], acc[1], acc[2]); //print the acceleration
00069     // stop the timer used to estimate the dt(=time between  2 acquisitions) used in the integration
00070  t.stop();
00071    dt= t.read(); 
00072     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 
00073         
00074         // acquire the angular speeds in mdps
00075     acc_gyro->Get_G_Axes(mdps);
00076     // reset the timer and restart it
00077     t.reset();
00078     t.start();
00079     
00080     printf("LSM6DSLrow [gyro/mdps]:   %6ld, %6ld, %6ld\r\n", mdps[0], mdps[1], mdps[2]);// print the angular speeds before the offset correction  
00081     
00082     // ssubtract the offset at the angulas speeds
00083 for(int i=0;i<3;i++)
00084 {mdps[i]=mdps[i]-off[i];}
00085        printf("LSM6DSLfine [gyro/mdps]:   %6ld, %6ld, %6ld\r\n", mdps[0], mdps[1], mdps[2]);// print the angular speeds after the offset correction 
00086     
00087     
00088 
00089     
00090     for(int i=0;i<3;i++)
00091 {
00092 // convert the angular speeds from  mdps to dpLSB
00093 
00094     parziale[i]=(mdps[i]*sens)/1000;
00095     
00096 // delete the coerrection of offset and slope to integrate beacuse they are correction on the angle not on  the angular speed 
00097 angolo[i]=(angolo[i]-(-3.19211))/0.0140888;
00098 // multiply for dt 
00099     parziale[i]*=(dt);
00100     
00101    if (mdps[i]>70 ||mdps[i]<-70)// 
00102     angolo[i] += parziale[i];     // summ to ultimate the integration 
00103     
00104 // correction of offset and  slope obtained aftera a "taratura" ( best fit line  )
00105   
00106    angolo[i]=(angolo[i]*0.0140888)+(-3.19211);
00107 }
00108 
00109 
00110     printf("angolo  [gyro/d]:   %6f, %6f, %6f\r\n", angolo[0], angolo[1], angolo[2]);//print angoles
00111   }
00112 }