File content as of revision 17:9d70e41ec8bc:

/* this program acquires the acceleration and the angular speed  on the 3 axes
the angular speed acquired in mdps( milli degree por second) is converrted in angular position 
using a simple algoritm of  discret integration.
To have clean and right position it's necessary to calibrate the giroscope.
To calibrate we read row position( comment the correctionoperation  of off and slope at line 97 and 106)
at specific and known angles and using a linear fit algoritm it's possible to know the 
slope and the offset that have to be correc.
The angles used are 90°,180°270°,360° because they are the easiest to reach whitout specific tools.
after the calibration it's possible uncomment line 97 and 106
WARNING:offset and slope depends by the dt of integration !!
*/

/* 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);


#define  sens 70 // sensibility for the range that  we are using  

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;
  
  for(int i=0;i<3;i++)
{parziale[i]=0;}

 for(int i=0;i<3;i++)
{angolo [i]=0;}
  /* Enable all sensors */
 
  acc_gyro->Enable_X();
  acc_gyro->Enable_G();
  
  printf("\r\n--- Starting new run ---\r\n");
  acc_gyro->ReadID(&id);
  printf("LSM6DSL accelerometer & gyroscope = 0x%X\r\n", id);
  // wait for inizialization 
  wait(1.5); 
     
  //first acquisition to know the offset of the angular speeds
 acc_gyro->Get_G_Axes(mdps);   
 printf("LSM6DSL [gyro/mdps]:   %6ld, %6ld, %6ld\r\n", mdps[0], mdps[1], mdps[2]);
 
 //print the offset
 for(int i=0;i<3;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(acc);// acquire the acceleration 
  
    printf("LSM6DSL [acc/mg]:      %6ld, %6ld, %6ld\r\n", acc[0], acc[1], acc[2]); //print the acceleration
    // stop the timer used to estimate the dt(=time between  2 acquisitions) used in the integration
 t.stop();
   dt= t.read(); 
    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 
        
        // acquire the angular speeds in mdps
    acc_gyro->Get_G_Axes(mdps);
    // reset the timer and restart it
    t.reset();
    t.start();
    
    printf("LSM6DSLrow [gyro/mdps]:   %6ld, %6ld, %6ld\r\n", mdps[0], mdps[1], mdps[2]);// print the angular speeds before the offset correction  
    
    // ssubtract the offset at the angulas speeds
for(int i=0;i<3;i++)
{mdps[i]=mdps[i]-off[i];}
       printf("LSM6DSLfine [gyro/mdps]:   %6ld, %6ld, %6ld\r\n", mdps[0], mdps[1], mdps[2]);// print the angular speeds after the offset correction 
    
    

    
    for(int i=0;i<3;i++)
{
// convert the angular speeds from  mdps to dpLSB

    parziale[i]=(mdps[i]*sens)/1000;
    
// delete the coerrection of offset and slope to integrate beacuse they are correction on the angle not on  the angular speed 
angolo[i]=(angolo[i]-(-3.19211))/0.0140888;
// multiply for dt 
    parziale[i]*=(dt);
    
   if (mdps[i]>70 ||mdps[i]<-70)// 
    angolo[i] += parziale[i];     // summ to ultimate the integration 
    
// correction of offset and  slope obtained aftera a "taratura" ( best fit line  )
  
   angolo[i]=(angolo[i]*0.0140888)+(-3.19211);
}


    printf("angolo  [gyro/d]:   %6f, %6f, %6f\r\n", angolo[0], angolo[1], angolo[2]);//print angoles
  }
}