diff --git a/os/ex/ST/lsm6ds0.c b/os/ex/ST/lsm6ds0.c
index 054cca679..707c7b90c 100644
--- a/os/ex/ST/lsm6ds0.c
+++ b/os/ex/ST/lsm6ds0.c
@@ -33,93 +33,6 @@
 /* Driver local definitions.                                                 */
 /*===========================================================================*/
 
-#define  LSM6DS0_ACC_SENS_2G                ((float)0.061f)
-#define  LSM6DS0_ACC_SENS_4G                ((float)0.122f)
-#define  LSM6DS0_ACC_SENS_8G                ((float)0.244f)
-#define  LSM6DS0_ACC_SENS_16G               ((float)0.732f)
-
-#define  LSM6DS0_GYRO_SENS_245DPS           ((float)0.00875f)
-#define  LSM6DS0_GYRO_SENS_500DPS           ((float)0.01750f)
-#define  LSM6DS0_GYRO_SENS_2000DPS          ((float)0.07000f)
-
-#define  LSM6DS0_TEMP_SENS                  ((float)16.0f)
-#define  LSM6DS0_TEMP_SENS_OFF              ((float)25.0f)
-
-#define  LSM6DS0_DI                         ((uint8_t)0xFF)
-#define  LSM6DS0_DI_0                       ((uint8_t)0x01)
-#define  LSM6DS0_DI_1                       ((uint8_t)0x02)
-#define  LSM6DS0_DI_2                       ((uint8_t)0x04)
-#define  LSM6DS0_DI_3                       ((uint8_t)0x08)
-#define  LSM6DS0_DI_4                       ((uint8_t)0x10)
-#define  LSM6DS0_DI_5                       ((uint8_t)0x20)
-#define  LSM6DS0_DI_6                       ((uint8_t)0x40)
-#define  LSM6DS0_DI_7                       ((uint8_t)0x80)
-
-#define  LSM6DS0_AD_0                       ((uint8_t)0x01)
-#define  LSM6DS0_AD_1                       ((uint8_t)0x02)
-#define  LSM6DS0_AD_2                       ((uint8_t)0x04)
-#define  LSM6DS0_AD_3                       ((uint8_t)0x08)
-#define  LSM6DS0_AD_4                       ((uint8_t)0x10)
-#define  LSM6DS0_AD_5                       ((uint8_t)0x20)
-#define  LSM6DS0_AD_6                       ((uint8_t)0x40)
-
-#define  LSM6DS0_RW                         ((uint8_t)0x80)
-
-#define  LSM6DS0_AD_ACT_THS                 ((uint8_t)0x04)
-#define  LSM6DS0_AD_ACT_DUR                 ((uint8_t)0x05)
-#define  LSM6DS0_AD_INT_GEN_CFG_XL          ((uint8_t)0x06)
-#define  LSM6DS0_AD_INT_GEN_THS_X_XL        ((uint8_t)0x07)
-#define  LSM6DS0_AD_INT_GEN_THS_Y_XL        ((uint8_t)0x08)
-#define  LSM6DS0_AD_INT_GEN_THS_Z_XL        ((uint8_t)0x09)
-#define  LSM6DS0_AD_INT_GEN_DUR_XL          ((uint8_t)0x0A)
-#define  LSM6DS0_AD_REFERENCE_G             ((uint8_t)0x0B)
-#define  LSM6DS0_AD_INT_CTRL                ((uint8_t)0x0C)
-#define  LSM6DS0_AD_WHO_AM_I                ((uint8_t)0x0F)
-#define  LSM6DS0_AD_CTRL_REG1_G             ((uint8_t)0x10)
-#define  LSM6DS0_AD_CTRL_REG2_G             ((uint8_t)0x11)
-#define  LSM6DS0_AD_CTRL_REG3_G             ((uint8_t)0x12)
-#define  LSM6DS0_AD_ORIENT_CFG_G            ((uint8_t)0x13)
-#define  LSM6DS0_AD_INT_GEN_SRC_G           ((uint8_t)0x14)
-#define  LSM6DS0_AD_OUT_TEMP_L              ((uint8_t)0x15)
-#define  LSM6DS0_AD_OUT_TEMP_H              ((uint8_t)0x16)
-#define  LSM6DS0_AD_STATUS_REG1             ((uint8_t)0x17)
-#define  LSM6DS0_AD_OUT_X_L_G               ((uint8_t)0x18)
-#define  LSM6DS0_AD_OUT_X_H_G               ((uint8_t)0x19)
-#define  LSM6DS0_AD_OUT_Y_L_G               ((uint8_t)0x1A)
-#define  LSM6DS0_AD_OUT_Y_H_G               ((uint8_t)0x1B)
-#define  LSM6DS0_AD_OUT_Z_L_G               ((uint8_t)0x1C)
-#define  LSM6DS0_AD_OUT_Z_H_G               ((uint8_t)0x1D)
-#define  LSM6DS0_AD_CTRL_REG4               ((uint8_t)0x1E)
-#define  LSM6DS0_AD_CTRL_REG5_XL            ((uint8_t)0x1F)
-#define  LSM6DS0_AD_CTRL_REG6_XL            ((uint8_t)0x20)
-#define  LSM6DS0_AD_CTRL_REG7_XL            ((uint8_t)0x21)
-#define  LSM6DS0_AD_CTRL_REG8               ((uint8_t)0x22)
-#define  LSM6DS0_AD_CTRL_REG9               ((uint8_t)0x23)
-#define  LSM6DS0_AD_CTRL_REG10              ((uint8_t)0x24)
-#define  LSM6DS0_AD_INT_GEN_SRC_XL          ((uint8_t)0x26)
-#define  LSM6DS0_AD_STATUS_REG2             ((uint8_t)0x27)
-#define  LSM6DS0_AD_OUT_X_L_XL              ((uint8_t)0x28)
-#define  LSM6DS0_AD_OUT_X_H_XL              ((uint8_t)0x29)
-#define  LSM6DS0_AD_OUT_Y_L_XL              ((uint8_t)0x2A)
-#define  LSM6DS0_AD_OUT_Y_H_XL              ((uint8_t)0x2B)
-#define  LSM6DS0_AD_OUT_Z_L_XL              ((uint8_t)0x2C)
-#define  LSM6DS0_AD_OUT_Z_H_XL              ((uint8_t)0x2D)
-#define  LSM6DS0_AD_FIFO_CTRL               ((uint8_t)0x2E)
-#define  LSM6DS0_AD_FIFO_SRC                ((uint8_t)0x2F)
-#define  LSM6DS0_AD_INT_GEN_CFG_G           ((uint8_t)0x30)
-#define  LSM6DS0_AD_INT_GEN_THS_XH_G        ((uint8_t)0x31)
-#define  LSM6DS0_AD_INT_GEN_THS_XL_G        ((uint8_t)0x32)
-#define  LSM6DS0_AD_INT_GEN_THS_YH_G        ((uint8_t)0x33)
-#define  LSM6DS0_AD_INT_GEN_THS_YL_G        ((uint8_t)0x34)
-#define  LSM6DS0_AD_INT_GEN_THS_ZH_G        ((uint8_t)0x35)
-#define  LSM6DS0_AD_INT_GEN_THS_ZL_G        ((uint8_t)0x36)
-#define  LSM6DS0_AD_INT_GEN_DUR_G           ((uint8_t)0x37)
-
-#define  LSM6DS0_CTRL_REG1_G_FS_MASK        ((uint8_t)0x18)
-#define  LSM6DS0_CTRL_REG6_XL_FS_MASK       ((uint8_t)0x18)
-
-#define  TO_G                               ((float)0.001f)
-#define  TO_SI                              ((float)0.00981f)
 /*===========================================================================*/
 /* Driver exported variables.                                                */
 /*===========================================================================*/
@@ -194,70 +107,10 @@ uint8_t lsm6ds0I2CReadRegister(I2CDriver *i2cp, lsm6ds0_sad_t sad, uint8_t reg,
  */
 msg_t lsm6ds0I2CWriteRegister(I2CDriver *i2cp, lsm6ds0_sad_t sad, uint8_t reg,
                               uint8_t value) {
-  uint8_t rxbuf;
   uint8_t txbuf[2];
-  switch (reg) {
-  default:
-    /* Reserved register must not be written, according to the datasheet
-     * this could permanently damage the device.
-     */
-    osalDbgAssert(FALSE, "lsm6ds0WriteRegisterI2C(), reserved register");
-  case LSM6DS0_AD_WHO_AM_I:
-  case LSM6DS0_AD_INT_GEN_SRC_G:
-  case LSM6DS0_AD_OUT_TEMP_L:
-  case LSM6DS0_AD_OUT_TEMP_H:
-  case LSM6DS0_AD_STATUS_REG1:
-  case LSM6DS0_AD_OUT_X_L_G:
-  case LSM6DS0_AD_OUT_X_H_G:
-  case LSM6DS0_AD_OUT_Y_L_G:
-  case LSM6DS0_AD_OUT_Y_H_G:
-  case LSM6DS0_AD_OUT_Z_L_G:
-  case LSM6DS0_AD_OUT_Z_H_G:
-  case LSM6DS0_AD_INT_GEN_SRC_XL:
-  case LSM6DS0_AD_STATUS_REG2:
-  case LSM6DS0_AD_OUT_X_L_XL:
-  case LSM6DS0_AD_OUT_X_H_XL:
-  case LSM6DS0_AD_OUT_Y_L_XL:
-  case LSM6DS0_AD_OUT_Y_H_XL:
-  case LSM6DS0_AD_OUT_Z_L_XL:
-  case LSM6DS0_AD_OUT_Z_H_XL:
-  case LSM6DS0_AD_FIFO_SRC:
-  /* Read only registers cannot be written, the command is ignored.*/
-    return MSG_RESET;
-  case LSM6DS0_AD_ACT_THS:
-  case LSM6DS0_AD_ACT_DUR:
-  case LSM6DS0_AD_INT_GEN_CFG_XL:
-  case LSM6DS0_AD_INT_GEN_THS_X_XL:
-  case LSM6DS0_AD_INT_GEN_THS_Y_XL:
-  case LSM6DS0_AD_INT_GEN_THS_Z_XL:
-  case LSM6DS0_AD_INT_GEN_DUR_XL:
-  case LSM6DS0_AD_REFERENCE_G:
-  case LSM6DS0_AD_INT_CTRL:
-  case LSM6DS0_AD_CTRL_REG1_G:
-  case LSM6DS0_AD_CTRL_REG2_G:
-  case LSM6DS0_AD_CTRL_REG3_G:
-  case LSM6DS0_AD_ORIENT_CFG_G:
-  case LSM6DS0_AD_CTRL_REG4:
-  case LSM6DS0_AD_CTRL_REG5_XL:
-  case LSM6DS0_AD_CTRL_REG6_XL:
-  case LSM6DS0_AD_CTRL_REG7_XL:
-  case LSM6DS0_AD_CTRL_REG8:
-  case LSM6DS0_AD_CTRL_REG9:
-  case LSM6DS0_AD_CTRL_REG10:
-  case LSM6DS0_AD_FIFO_CTRL:
-  case LSM6DS0_AD_INT_GEN_CFG_G:
-  case LSM6DS0_AD_INT_GEN_THS_XH_G:
-  case LSM6DS0_AD_INT_GEN_THS_XL_G:
-  case LSM6DS0_AD_INT_GEN_THS_YH_G:
-  case LSM6DS0_AD_INT_GEN_THS_YL_G:
-  case LSM6DS0_AD_INT_GEN_THS_ZH_G:
-  case LSM6DS0_AD_INT_GEN_THS_ZL_G:
-  case LSM6DS0_AD_INT_GEN_DUR_G:
-    txbuf[0] = reg;
-    txbuf[1] = value;
-    return i2cMasterTransmitTimeout(i2cp, sad, txbuf, 2, &rxbuf, 0, TIME_INFINITE);
-    break;
-  }
+  txbuf[0] = reg;
+  txbuf[1] = value;
+  return i2cMasterTransmitTimeout(i2cp, sad, txbuf, 2, NULL, 0, TIME_INFINITE);
 }
 #endif /* LSM6DS0_USE_I2C */
 
@@ -289,6 +142,7 @@ static size_t sens_get_axes_number(void *ip) {
 
 static msg_t acc_read_raw(void *ip, int32_t axes[]) {
   int16_t tmp;
+	msg_t msg = MSG_OK;
   osalDbgCheck(((ip != NULL) && (axes != NULL)) &&
               (((LSM6DS0Driver *)ip)->config->acccfg != NULL));
   osalDbgAssert((((LSM6DS0Driver *)ip)->state == LSM6DS0_READY),
@@ -302,33 +156,41 @@ static msg_t acc_read_raw(void *ip, int32_t axes[]) {
   i2cStart(((LSM6DS0Driver *)ip)->config->i2cp,
            ((LSM6DS0Driver *)ip)->config->i2ccfg);
 #endif /* LSM6DS0_SHARED_I2C */
-  if(((LSM6DS0Driver *)ip)->config->acccfg->axesenabling & LSM6DS0_ACC_AE_X){
-    tmp = lsm6ds0I2CReadRegister(((LSM6DS0Driver *)ip)->config->i2cp,
-                                 ((LSM6DS0Driver *)ip)->config->slaveaddress,
-                                 LSM6DS0_AD_OUT_X_L_XL, NULL);
-    tmp += lsm6ds0I2CReadRegister(((LSM6DS0Driver *)ip)->config->i2cp,
-                                  ((LSM6DS0Driver *)ip)->config->slaveaddress,
-                                  LSM6DS0_AD_OUT_X_H_XL, NULL) << 8;
-    axes[0] = (int32_t)tmp - ((LSM6DS0Driver *)ip)->accbias[0];
-  }
-  if(((LSM6DS0Driver *)ip)->config->acccfg->axesenabling & LSM6DS0_ACC_AE_Y){
-    tmp = lsm6ds0I2CReadRegister(((LSM6DS0Driver *)ip)->config->i2cp,
-                                 ((LSM6DS0Driver *)ip)->config->slaveaddress,
-                                 LSM6DS0_AD_OUT_Y_L_XL, NULL);
-    tmp += lsm6ds0I2CReadRegister(((LSM6DS0Driver *)ip)->config->i2cp,
-                                  ((LSM6DS0Driver *)ip)->config->slaveaddress,
-                                  LSM6DS0_AD_OUT_Y_H_XL, NULL) << 8;
-    axes[1] = (int32_t)tmp - ((LSM6DS0Driver *)ip)->accbias[1];
-  }
-  if(((LSM6DS0Driver *)ip)->config->acccfg->axesenabling & LSM6DS0_ACC_AE_Z){
-    tmp = lsm6ds0I2CReadRegister(((LSM6DS0Driver *)ip)->config->i2cp,
-                                 ((LSM6DS0Driver *)ip)->config->slaveaddress,
-                                 LSM6DS0_AD_OUT_Z_L_XL, NULL);
-    tmp += lsm6ds0I2CReadRegister(((LSM6DS0Driver *)ip)->config->i2cp,
-                                  ((LSM6DS0Driver *)ip)->config->slaveaddress,
-                                  LSM6DS0_AD_OUT_Z_H_XL, NULL) << 8;
-    axes[2] = (int32_t)tmp - ((LSM6DS0Driver *)ip)->accbias[2];
-  }
+	tmp = lsm6ds0I2CReadRegister(((LSM6DS0Driver *)ip)->config->i2cp,
+															 ((LSM6DS0Driver *)ip)->config->slaveaddress,
+															 LSM6DS0_AD_OUT_X_L_XL, NULL);
+	if (msg != MSG_OK)
+		return msg;
+	tmp += lsm6ds0I2CReadRegister(((LSM6DS0Driver *)ip)->config->i2cp,
+																((LSM6DS0Driver *)ip)->config->slaveaddress,
+																LSM6DS0_AD_OUT_X_H_XL, NULL) << 8;
+	if (msg != MSG_OK)
+		return msg;
+	axes[0] = (int32_t)tmp;
+
+	tmp = lsm6ds0I2CReadRegister(((LSM6DS0Driver *)ip)->config->i2cp,
+															 ((LSM6DS0Driver *)ip)->config->slaveaddress,
+															 LSM6DS0_AD_OUT_Y_L_XL, NULL);
+	if (msg != MSG_OK)
+		return msg;
+	tmp += lsm6ds0I2CReadRegister(((LSM6DS0Driver *)ip)->config->i2cp,
+																((LSM6DS0Driver *)ip)->config->slaveaddress,
+																LSM6DS0_AD_OUT_Y_H_XL, NULL) << 8;
+	if (msg != MSG_OK)
+		return msg;
+	axes[1] = (int32_t)tmp;
+	
+	tmp = lsm6ds0I2CReadRegister(((LSM6DS0Driver *)ip)->config->i2cp,
+															 ((LSM6DS0Driver *)ip)->config->slaveaddress,
+															 LSM6DS0_AD_OUT_Z_L_XL, NULL);
+	if (msg != MSG_OK)
+		return msg;
+	tmp += lsm6ds0I2CReadRegister(((LSM6DS0Driver *)ip)->config->i2cp,
+																((LSM6DS0Driver *)ip)->config->slaveaddress,
+																LSM6DS0_AD_OUT_Z_H_XL, NULL) << 8;
+	if (msg != MSG_OK)
+		return msg;
+	axes[2] = (int32_t)tmp;
 #if LSM6DS0_SHARED_I2C
   i2cReleaseBus(((LSM6DS0Driver *)ip)->config->i2cp);
 #endif /* LSM6DS0_SHARED_I2C */
@@ -338,6 +200,7 @@ static msg_t acc_read_raw(void *ip, int32_t axes[]) {
 
 static msg_t gyro_read_raw(void *ip, int32_t axes[]) {
   int16_t tmp;
+	msg_t msg = MSG_OK;
   osalDbgCheck(((ip != NULL) && (axes != NULL)) &&
               (((LSM6DS0Driver *)ip)->config->gyrocfg != NULL));
   osalDbgAssert((((LSM6DS0Driver *)ip)->state == LSM6DS0_READY),
@@ -351,33 +214,41 @@ static msg_t gyro_read_raw(void *ip, int32_t axes[]) {
   i2cStart(((LSM6DS0Driver *)ip)->config->i2cp,
            ((LSM6DS0Driver *)ip)->config->i2ccfg);
 #endif /* LSM6DS0_SHARED_I2C */
-  if(((LSM6DS0Driver *)ip)->config->gyrocfg->axesenabling & LSM6DS0_GYRO_AE_X){
-    tmp = lsm6ds0I2CReadRegister(((LSM6DS0Driver *)ip)->config->i2cp,
-                                 ((LSM6DS0Driver *)ip)->config->slaveaddress,
-                                   LSM6DS0_AD_OUT_X_L_G, NULL);
-    tmp += lsm6ds0I2CReadRegister(((LSM6DS0Driver *)ip)->config->i2cp,
-                                  ((LSM6DS0Driver *)ip)->config->slaveaddress,
+	tmp = lsm6ds0I2CReadRegister(((LSM6DS0Driver *)ip)->config->i2cp,
+															 ((LSM6DS0Driver *)ip)->config->slaveaddress,
+																 LSM6DS0_AD_OUT_X_L_G, NULL);
+	if (msg != MSG_OK)
+		return msg;
+	tmp += lsm6ds0I2CReadRegister(((LSM6DS0Driver *)ip)->config->i2cp,
+																((LSM6DS0Driver *)ip)->config->slaveaddress,
                                     LSM6DS0_AD_OUT_X_H_G, NULL) << 8;
-	axes[0] = (int32_t)tmp - ((LSM6DS0Driver *)ip)->gyrobias[0];
-  }
-  if(((LSM6DS0Driver *)ip)->config->acccfg->axesenabling & LSM6DS0_GYRO_AE_Y){
-    tmp = lsm6ds0I2CReadRegister(((LSM6DS0Driver *)ip)->config->i2cp,
-                                 ((LSM6DS0Driver *)ip)->config->slaveaddress,
-                                   LSM6DS0_AD_OUT_Y_L_G, NULL);
-    tmp += lsm6ds0I2CReadRegister(((LSM6DS0Driver *)ip)->config->i2cp,
-                                  ((LSM6DS0Driver *)ip)->config->slaveaddress,
-                                    LSM6DS0_AD_OUT_Y_H_G, NULL) << 8;
-    axes[1] = (int32_t)tmp - ((LSM6DS0Driver *)ip)->gyrobias[1];
-  }
-  if(((LSM6DS0Driver *)ip)->config->acccfg->axesenabling & LSM6DS0_GYRO_AE_Z){
-    tmp = lsm6ds0I2CReadRegister(((LSM6DS0Driver *)ip)->config->i2cp,
-                                 ((LSM6DS0Driver *)ip)->config->slaveaddress,
-                                   LSM6DS0_AD_OUT_Z_L_G, NULL);
-    tmp += lsm6ds0I2CReadRegister(((LSM6DS0Driver *)ip)->config->i2cp,
-                                  ((LSM6DS0Driver *)ip)->config->slaveaddress,
-                                    LSM6DS0_AD_OUT_Z_H_G, NULL) << 8;
-    axes[2] = (int32_t)tmp - ((LSM6DS0Driver *)ip)->gyrobias[2];
-  }
+	if (msg != MSG_OK)
+		return msg;
+	axes[0] = (int32_t)tmp;
+
+	tmp = lsm6ds0I2CReadRegister(((LSM6DS0Driver *)ip)->config->i2cp,
+															 ((LSM6DS0Driver *)ip)->config->slaveaddress,
+																 LSM6DS0_AD_OUT_Y_L_G, NULL);
+	if (msg != MSG_OK)
+		return msg;
+	tmp += lsm6ds0I2CReadRegister(((LSM6DS0Driver *)ip)->config->i2cp,
+																((LSM6DS0Driver *)ip)->config->slaveaddress,
+																	LSM6DS0_AD_OUT_Y_H_G, NULL) << 8;
+	if (msg != MSG_OK)
+		return msg;
+	axes[1] = (int32_t)tmp;
+
+	tmp = lsm6ds0I2CReadRegister(((LSM6DS0Driver *)ip)->config->i2cp,
+															 ((LSM6DS0Driver *)ip)->config->slaveaddress,
+																 LSM6DS0_AD_OUT_Z_L_G, NULL);
+	if (msg != MSG_OK)
+		return msg;
+	tmp += lsm6ds0I2CReadRegister(((LSM6DS0Driver *)ip)->config->i2cp,
+																((LSM6DS0Driver *)ip)->config->slaveaddress,
+																	LSM6DS0_AD_OUT_Z_H_G, NULL) << 8;
+	if (msg != MSG_OK)
+		return msg;
+	axes[2] = (int32_t)tmp;
 #if LSM6DS0_SHARED_I2C
   i2cReleaseBus(((LSM6DS0Driver *)ip)->config->i2cp);
 #endif /* LSM6DS0_SHARED_I2C */
@@ -414,12 +285,7 @@ static msg_t acc_read_cooked(void *ip, float axes[]) {
   msg = acc_read_raw(ip, raw);
   for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES ; i++){
     axes[i] = raw[i] * ((LSM6DS0Driver *)ip)->accsensitivity[i];
-    if(((LSM6DS0Driver *)ip)->config->acccfg->unit == LSM6DS0_ACC_UNIT_G){
-      axes[i] *= TO_G;
-    }
-    else if(((LSM6DS0Driver *)ip)->config->acccfg->unit == LSM6DS0_ACC_UNIT_SI){
-      axes[i] *= TO_SI;
-    }
+    axes[i] -= ((LSM6DS0Driver *)ip)->accbias[i];
   }
   return msg;
 }
@@ -438,6 +304,7 @@ static msg_t gyro_read_cooked(void *ip, float axes[]) {
   msg = gyro_read_raw(ip, raw);
   for(i = 0; i < LSM6DS0_GYRO_NUMBER_OF_AXES ; i++){
     axes[i] = raw[i] * ((LSM6DS0Driver *)ip)->gyrosensitivity[i];
+		axes[i] -= ((LSM6DS0Driver *)ip)->gyrobias[i];
   }
   return msg;
 }
@@ -470,8 +337,8 @@ static msg_t gyro_sample_bias(void *ip) {
 
   for(i = 0; i < LSM6DS0_GYRO_BIAS_ACQ_TIMES; i++){
     msg = gyro_read_raw(ip, raw);
-	if(msg != MSG_OK)
-	  return msg;
+		if(msg != MSG_OK)
+			return msg;
     for(j = 0; j < LSM6DS0_GYRO_NUMBER_OF_AXES; j++){
       buff[j] += raw[j];
     }
@@ -480,6 +347,7 @@ static msg_t gyro_sample_bias(void *ip) {
 
   for(i = 0; i < LSM6DS0_GYRO_NUMBER_OF_AXES; i++){
     ((LSM6DS0Driver *)ip)->gyrobias[i] = buff[i] / LSM6DS0_GYRO_BIAS_ACQ_TIMES;
+	((LSM6DS0Driver *)ip)->gyrobias[i] *= ((LSM6DS0Driver *)ip)->gyrosensitivity[i];
   }
   return msg;
 }
@@ -605,13 +473,13 @@ static msg_t gyro_reset_sensivity(void *ip) {
   osalDbgAssert((((LSM6DS0Driver *)ip)->state == LSM6DS0_READY),
                 "gyro_reset_sensivity(), invalid state");
 
-  if(((LSM6DS0Driver *)ip)->config->gyrocfg->fullscale == LSM6DS0_GYRO_FS_245DSP)
+  if(((LSM6DS0Driver *)ip)->config->gyrocfg->fullscale == LSM6DS0_GYRO_FS_245DPS)
     for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES; i++)
       ((LSM6DS0Driver *)ip)->gyrosensitivity[i] = LSM6DS0_GYRO_SENS_245DPS;
-  else if(((LSM6DS0Driver *)ip)->config->gyrocfg->fullscale == LSM6DS0_GYRO_FS_500DSP)
+  else if(((LSM6DS0Driver *)ip)->config->gyrocfg->fullscale == LSM6DS0_GYRO_FS_500DPS)
 	for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES; i++)
       ((LSM6DS0Driver *)ip)->gyrosensitivity[i] = LSM6DS0_GYRO_SENS_500DPS;
-  else if(((LSM6DS0Driver *)ip)->config->gyrocfg->fullscale == LSM6DS0_GYRO_FS_2000DSP)
+  else if(((LSM6DS0Driver *)ip)->config->gyrocfg->fullscale == LSM6DS0_GYRO_FS_2000DPS)
 	for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES; i++)
       ((LSM6DS0Driver *)ip)->gyrosensitivity[i] = LSM6DS0_GYRO_SENS_2000DPS;
   else {
@@ -621,27 +489,98 @@ static msg_t gyro_reset_sensivity(void *ip) {
   return MSG_OK;
 }
 
-static msg_t sens_get_temperature(void *ip, float* tempp) {
-  int16_t temp;
-#if LSM6DS0_USE_I2C
-  osalDbgAssert((((LSM6DS0Driver *)ip)->config->i2cp->state == I2C_READY),
-                "gyro_read_raw(), channel not ready");
-#if LSM6DS0_SHARED_I2C
-  i2cAcquireBus(((LSM6DS0Driver *)ip)->config->i2cp);
-  i2cStart(((LSM6DS0Driver *)ip)->config->i2cp,
-           ((LSM6DS0Driver *)ip)->config->i2ccfg);
-#endif /* LSM6DS0_SHARED_I2C */
-  temp = lsm6ds0I2CReadRegister(((LSM6DS0Driver *)ip)->config->i2cp,
+static msg_t acc_set_full_scale(void *ip, lsm6ds0_acc_fs_t fs) {
+  float newfs, scale;
+  uint8_t i, cr;
+  msg_t msg;
+
+  if(fs == LSM6DS0_ACC_FS_2G) {
+    newfs = LSM6DS0_ACC_2G;
+  }
+  else if(fs == LSM6DS0_ACC_FS_4G) {
+    newfs = LSM6DS0_ACC_4G;
+  }
+  else if(fs == LSM6DS0_ACC_FS_8G) {
+    newfs = LSM6DS0_ACC_8G;
+  }
+	else if(fs == LSM6DS0_ACC_FS_16G) {
+    newfs = LSM6DS0_ACC_16G;
+  }
+  else {
+    return MSG_RESET;
+  }
+
+  if(newfs != ((LSM6DS0Driver *)ip)->accfullscale) {
+    scale = newfs / ((LSM6DS0Driver *)ip)->accfullscale;
+    ((LSM6DS0Driver *)ip)->accfullscale = newfs;
+
+    /* Updating register.*/
+    cr = lsm6ds0I2CReadRegister(((LSM6DS0Driver *)ip)->config->i2cp,
                                 ((LSM6DS0Driver *)ip)->config->slaveaddress,
-                                  LSM6DS0_AD_OUT_TEMP_L, NULL);
-  temp += lsm6ds0I2CReadRegister(((LSM6DS0Driver *)ip)->config->i2cp,
-                                 ((LSM6DS0Driver *)ip)->config->slaveaddress,
-                                   LSM6DS0_AD_OUT_TEMP_H, NULL) << 8;
-#if LSM6DS0_SHARED_I2C
-  i2cReleaseBus(((LSM6DS0Driver *)ip)->config->i2cp);
-#endif /* LSM6DS0_SHARED_I2C */
-#endif /* LSM6DS0_USE_I2C */
-  *tempp = ((float)temp / LSM6DS0_TEMP_SENS) + LSM6DS0_TEMP_SENS_OFF;
+                                  LSM6DS0_AD_CTRL_REG6_XL, &msg);
+    if(msg != MSG_OK)
+      return msg;
+		
+    cr &= ~(LSM6DS0_CTRL_REG6_XL_FS_MASK);
+    cr |= fs;
+    msg = lsm6ds0I2CWriteRegister(((LSM6DS0Driver *)ip)->config->i2cp,
+                                  ((LSM6DS0Driver *)ip)->config->slaveaddress,
+                                    LSM6DS0_AD_CTRL_REG6_XL, cr);
+	if(msg != MSG_OK)
+      return msg;
+
+    /* Scaling sensitivity and bias. Re-calibration is suggested anyway. */
+    for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES; i++) {
+      ((LSM6DS0Driver *)ip)->accsensitivity[i] *= scale;
+      ((LSM6DS0Driver *)ip)->accbias[i] *= scale;
+    }
+  }
+  return MSG_OK;
+}
+
+static msg_t gyro_set_full_scale(void *ip, lsm6ds0_gyro_fs_t fs) {
+  float newfs, scale;
+  uint8_t i, cr;
+  msg_t msg;
+
+  if(fs == LSM6DS0_GYRO_FS_245DPS) {
+    newfs = LSM6DS0_GYRO_245DPS;
+  }
+  else if(fs == LSM6DS0_GYRO_FS_500DPS) {
+    newfs = LSM6DS0_GYRO_500DPS;
+  }
+  else if(fs == LSM6DS0_GYRO_FS_2000DPS) {
+    newfs = LSM6DS0_GYRO_2000DPS;
+  }
+  else {
+    return MSG_RESET;
+  }
+
+  if(newfs != ((LSM6DS0Driver *)ip)->gyrofullscale) {
+    scale = newfs / ((LSM6DS0Driver *)ip)->gyrofullscale;
+    ((LSM6DS0Driver *)ip)->gyrofullscale = newfs;
+
+    /* Updating register.*/
+    cr = lsm6ds0I2CReadRegister(((LSM6DS0Driver *)ip)->config->i2cp,
+                                ((LSM6DS0Driver *)ip)->config->slaveaddress,
+                                  LSM6DS0_AD_CTRL_REG1_G, &msg);
+		if(msg != MSG_OK)
+      return msg;
+		
+    cr &= ~(LSM6DS0_CTRL_REG1_G_FS_MASK);
+    cr |= fs;
+    msg = lsm6ds0I2CWriteRegister(((LSM6DS0Driver *)ip)->config->i2cp,
+                                  ((LSM6DS0Driver *)ip)->config->slaveaddress,
+                                    LSM6DS0_AD_CTRL_REG1_G, cr);
+		if(msg != MSG_OK)
+      return msg;
+
+    /* Scaling sensitivity and bias. Re-calibration is suggested anyway. */
+    for(i = 0; i < LSM6DS0_GYRO_NUMBER_OF_AXES; i++) {
+      ((LSM6DS0Driver *)ip)->gyrosensitivity[i] *= scale;
+      ((LSM6DS0Driver *)ip)->gyrobias[i] *= scale;
+    }
+  }
   return MSG_OK;
 }
 
@@ -662,13 +601,14 @@ static const struct BaseAccelerometerVMT vmt_baseaccelerometer = {
 
 static const struct LSM6DS0ACCVMT vmt_lsm6ds0acc = {
   acc_get_axes_number, acc_read_raw, acc_read_cooked,
-  acc_set_bias, acc_reset_bias, acc_set_sensivity, acc_reset_sensivity
+  acc_set_bias, acc_reset_bias, acc_set_sensivity, acc_reset_sensivity,
+  acc_set_full_scale
 };
 
 static const struct LSM6DS0GYROVMT vmt_lsm6ds0gyro = {
   gyro_get_axes_number, gyro_read_raw, gyro_read_cooked,
   gyro_sample_bias, gyro_set_bias, gyro_reset_bias,
-  gyro_set_sensivity, gyro_reset_sensivity, sens_get_temperature
+  gyro_set_sensivity, gyro_reset_sensivity, gyro_set_full_scale
 };
 
 /*===========================================================================*/
@@ -707,6 +647,7 @@ void lsm6ds0ObjectInit(LSM6DS0Driver *devp) {
  */
 void lsm6ds0Start(LSM6DS0Driver *devp, const LSM6DS0Config *config) {
   uint32_t i;
+  uint8_t cr;
   osalDbgCheck((devp != NULL) && (config != NULL));
 
   osalDbgAssert((devp->state == LSM6DS0_STOP) || (devp->state == LSM6DS0_READY),
@@ -721,47 +662,87 @@ void lsm6ds0Start(LSM6DS0Driver *devp, const LSM6DS0Config *config) {
   i2cStart((devp)->config->i2cp,
            (devp)->config->i2ccfg);
   if((devp)->config->acccfg != NULL) {
-    lsm6ds0I2CWriteRegister(devp->config->i2cp,
-                            devp->config->slaveaddress,
-                            LSM6DS0_AD_CTRL_REG5_XL,
-                            devp->config->acccfg->decmode |
-                            devp->config->acccfg->axesenabling);
-    lsm6ds0I2CWriteRegister(devp->config->i2cp,
-                            devp->config->slaveaddress,
-                            LSM6DS0_AD_CTRL_REG6_XL,
-                            devp->config->acccfg->outdatarate |
-                            devp->config->acccfg->fullscale );
+    /* Control register 5 configuration block.*/
+    {
+        cr = LSM6DS0_CTRL_REG5_XL_XEN_XL | LSM6DS0_CTRL_REG5_XL_YEN_XL |
+			 LSM6DS0_CTRL_REG5_XL_ZEN_XL;
+#if LSM6DS0_ACC_USE_ADVANCED || defined(__DOXYGEN__)
+		cr |= devp->config->acccfg->decmode;
+
+#endif
+        lsm6ds0I2CWriteRegister(devp->config->i2cp, devp->config->slaveaddress,
+                                LSM6DS0_AD_CTRL_REG5_XL, cr);
+    }
+    /* Control register 6 configuration block.*/
+    {
+        cr = devp->config->acccfg->outdatarate |
+             devp->config->acccfg->fullscale;
+        lsm6ds0I2CWriteRegister(devp->config->i2cp, devp->config->slaveaddress,
+                                LSM6DS0_AD_CTRL_REG6_XL, cr);
+    }
   }
   if((devp)->config->gyrocfg != NULL) {
-    lsm6ds0I2CWriteRegister(devp->config->i2cp,
-                            devp->config->slaveaddress,
-                            LSM6DS0_AD_CTRL_REG1_G,
-                            devp->config->gyrocfg->fullscale |
-                            devp->config->gyrocfg->outdatarate);
-    lsm6ds0I2CWriteRegister(devp->config->i2cp,
-                            devp->config->slaveaddress,
-                            LSM6DS0_AD_CTRL_REG2_G,
-                            devp->config->gyrocfg->outsel);
-    lsm6ds0I2CWriteRegister(devp->config->i2cp,
-                            devp->config->slaveaddress,
-                            LSM6DS0_AD_CTRL_REG3_G,
-                            devp->config->gyrocfg->hpfenable |
-                            devp->config->gyrocfg->lowmodecfg |
-                            devp->config->gyrocfg->hpcfg);
-    lsm6ds0I2CWriteRegister(devp->config->i2cp,
-                            devp->config->slaveaddress,
-                            LSM6DS0_AD_CTRL_REG4,
-                            devp->config->gyrocfg->axesenabling);
-    lsm6ds0I2CWriteRegister(devp->config->i2cp,
-                            devp->config->slaveaddress,
-                            LSM6DS0_AD_CTRL_REG9,
-                            LSM6DS0_GYRO_SLP_DISABLED);
+    /* Control register 1 configuration block.*/
+    {
+        cr = devp->config->gyrocfg->fullscale |
+            devp->config->gyrocfg->outdatarate;
+#if LSM6DS0_GYRO_USE_ADVANCED || defined(__DOXYGEN__)
+        cr |= devp->config->acccfg->decmode;
+
+#endif
+        lsm6ds0I2CWriteRegister(devp->config->i2cp, devp->config->slaveaddress,
+                                LSM6DS0_AD_CTRL_REG1_G, cr);
+    }
+
+    /* Control register 2 configuration block.*/
+    {
+        cr = 0;
+#if LSM6DS0_GYRO_USE_ADVANCED || defined(__DOXYGEN__)
+        cr |= devp->config->gyrocfg->outsel;
+
+#endif
+        lsm6ds0I2CWriteRegister(devp->config->i2cp, devp->config->slaveaddress,
+                                LSM6DS0_AD_CTRL_REG2_G, cr);
+    }
+
+    /* Control register 3 configuration block.*/
+    {
+        cr = 0;
+#if LSM6DS0_GYRO_USE_ADVANCED || defined(__DOXYGEN__)
+        cr |= devp->config->gyrocfg->hpfenable |
+              devp->config->gyrocfg->lowmodecfg |
+              devp->config->gyrocfg->hpcfg;
+
+#endif
+        lsm6ds0I2CWriteRegister(devp->config->i2cp, devp->config->slaveaddress,
+                                LSM6DS0_AD_CTRL_REG3_G, cr);
+    }
+
+    /* Control register 4 configuration block.*/
+    {
+        cr = LSM6DS0_CTRL_REG4_XEN_G | LSM6DS0_CTRL_REG4_YEN_G |
+             LSM6DS0_CTRL_REG4_ZEN_G;
+        lsm6ds0I2CWriteRegister(devp->config->i2cp, devp->config->slaveaddress,
+                                LSM6DS0_AD_CTRL_REG4, cr);
+    }
+
+    /* Control register 9 configuration block.*/
+    {
+        cr = LSM6DS0_GYRO_SLP_DISABLED;
+        lsm6ds0I2CWriteRegister(devp->config->i2cp, devp->config->slaveaddress,
+                                LSM6DS0_AD_CTRL_REG9, cr);
+    }
+  }
+
+  /* Control register 8 configuration block.*/
+  {
+    cr = 0;
+#if LSM6DS0_USE_ADVANCED || defined(__DOXYGEN__)
+    cr |= devp->config->endianness | devp->config->blockdataupdate;
+#endif
+    lsm6ds0I2CWriteRegister(devp->config->i2cp, devp->config->slaveaddress,
+                            LSM6DS0_AD_CTRL_REG8, cr);
   }
-  lsm6ds0I2CWriteRegister(devp->config->i2cp,
-                          devp->config->slaveaddress,
-                          LSM6DS0_AD_CTRL_REG8,
-                          devp->config->endianness |
-                          devp->config->blockdataupdate);
 #if	LSM6DS0_SHARED_I2C
   i2cReleaseBus((devp)->config->i2cp);
 #endif /* LSM6DS0_SHARED_I2C */  
@@ -769,30 +750,44 @@ void lsm6ds0Start(LSM6DS0Driver *devp, const LSM6DS0Config *config) {
   /* Storing sensitivity information according to full scale value */
   if((devp)->config->acccfg != NULL) {
     if(devp->config->acccfg->fullscale == LSM6DS0_ACC_FS_2G)
-      for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES; i++)
+      for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES; i++) {
         devp->accsensitivity[i] = LSM6DS0_ACC_SENS_2G;
+        devp->accfullscale = LSM6DS0_ACC_2G;
+      }
     else if(devp->config->acccfg->fullscale == LSM6DS0_ACC_FS_4G)
-      for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES; i++)
+      for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES; i++){
         devp->accsensitivity[i] = LSM6DS0_ACC_SENS_4G;
+        devp->accfullscale = LSM6DS0_ACC_4G;
+      }
     else if(devp->config->acccfg->fullscale == LSM6DS0_ACC_FS_8G)
-      for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES; i++)
+      for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES; i++){
         devp->accsensitivity[i] = LSM6DS0_ACC_SENS_8G;
+        devp->accfullscale = LSM6DS0_ACC_8G;
+      }
     else if(devp->config->acccfg->fullscale == LSM6DS0_ACC_FS_16G)
-      for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES; i++)
+      for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES; i++){
         devp->accsensitivity[i] = LSM6DS0_ACC_SENS_16G;
+        devp->accfullscale = LSM6DS0_ACC_16G;
+      }
     else
       osalDbgAssert(FALSE, "lsm6ds0Start(), accelerometer full scale issue");
   }
   if((devp)->config->gyrocfg != NULL) {
-    if(devp->config->gyrocfg->fullscale == LSM6DS0_GYRO_FS_245DSP)
-      for(i = 0; i < LSM6DS0_GYRO_NUMBER_OF_AXES; i++)
+    if(devp->config->gyrocfg->fullscale == LSM6DS0_GYRO_FS_245DPS)
+      for(i = 0; i < LSM6DS0_GYRO_NUMBER_OF_AXES; i++) {
         devp->gyrosensitivity[i] = LSM6DS0_GYRO_SENS_245DPS;
-    else if(devp->config->gyrocfg->fullscale == LSM6DS0_GYRO_FS_500DSP)
-      for(i = 0; i < LSM6DS0_GYRO_NUMBER_OF_AXES; i++)
+        devp->gyrofullscale = LSM6DS0_GYRO_245DPS;
+      }
+    else if(devp->config->gyrocfg->fullscale == LSM6DS0_GYRO_FS_500DPS)
+      for(i = 0; i < LSM6DS0_GYRO_NUMBER_OF_AXES; i++) {
         devp->gyrosensitivity[i] = LSM6DS0_GYRO_SENS_500DPS;
-    else if(devp->config->gyrocfg->fullscale == LSM6DS0_GYRO_FS_2000DSP)
-      for(i = 0; i < LSM6DS0_GYRO_NUMBER_OF_AXES; i++)
+        devp->gyrofullscale = LSM6DS0_GYRO_500DPS;
+      }
+    else if(devp->config->gyrocfg->fullscale == LSM6DS0_GYRO_FS_2000DPS)
+      for(i = 0; i < LSM6DS0_GYRO_NUMBER_OF_AXES; i++) {
         devp->gyrosensitivity[i] = LSM6DS0_GYRO_SENS_2000DPS;
+        devp->gyrofullscale = LSM6DS0_GYRO_2000DPS;
+      }
     else
       osalDbgAssert(FALSE, "lsm6ds0Start(), gyroscope full scale issue");
   }
diff --git a/os/ex/ST/lsm6ds0.h b/os/ex/ST/lsm6ds0.h
index a2cee28ec..34efdcec3 100644
--- a/os/ex/ST/lsm6ds0.h
+++ b/os/ex/ST/lsm6ds0.h
@@ -35,14 +35,260 @@
 /*===========================================================================*/
 
 /**
- * @brief   LSM6DS0 accelerometer subsystem number of axes.
+ * @name    Version identification
+ * @{
  */
-#define  LSM6DS0_ACC_NUMBER_OF_AXES         ((size_t)   3U)
+/**
+ * @brief   LSM6DS0 driver version string.
+ */
+#define EX_LSM6DS0_VERSION          "1.0.0"
 
 /**
- * @brief   LSM6DS0 gyroscope subsystem number of axes.
+ * @brief   LSM6DS0 driver version major number.
  */
-#define  LSM6DS0_GYRO_NUMBER_OF_AXES        ((size_t)   3U)
+#define EX_LSM6DS0_MAJOR            1
+
+/**
+ * @brief   LSM6DS0 driver version minor number.
+ */
+#define EX_LSM6DS0_MINOR            0
+
+/**
+ * @brief   LSM6DS0 driver version patch number.
+ */
+#define EX_LSM6DS0_PATCH            0
+/** @} */
+
+/**
+ * @brief   LSM6DS0 accelerometer subsystem characteristics.
+ *
+ * @{
+ */
+#define LSM6DS0_ACC_NUMBER_OF_AXES          3U
+
+#define LSM6DS0_ACC_2G                      2.0f
+#define LSM6DS0_ACC_4G                      4.0f
+#define LSM6DS0_ACC_8G                      8.0f
+#define LSM6DS0_ACC_16G                     16.0f
+
+#define LSM6DS0_ACC_SENS_2G                 0.061f
+#define LSM6DS0_ACC_SENS_4G                 0.122f
+#define LSM6DS0_ACC_SENS_8G                 0.244f
+#define LSM6DS0_ACC_SENS_16G                0.732f
+/** @} */
+
+/**
+ * @brief   LSM6DS0 gyroscope subsystem characteristics.
+ *
+ * @{
+ */
+#define LSM6DS0_GYRO_NUMBER_OF_AXES         3U
+
+#define LSM6DS0_GYRO_245DPS                 245.0f
+#define LSM6DS0_GYRO_500DPS                 500.0f
+#define LSM6DS0_GYRO_2000DPS                2000.0f
+
+#define LSM6DS0_GYRO_SENS_245DPS            0.00875f
+#define LSM6DS0_GYRO_SENS_500DPS            0.01750f
+#define LSM6DS0_GYRO_SENS_2000DPS           0.07000f
+/** @} */
+
+/**
+ * @name   LSM6DS0 communication interfaces related bit masks
+ * @{
+ */
+#define LSM6DS0_DI_MASK             0xFF        /**< Data In mask           */
+#define LSM6DS0_DI(n)               (1 << n)    /**< Data In bit n          */
+#define LSM6DS0_AD_MASK             0x7F        /**< Address Data mask      */
+#define LSM6DS0_AD(n)               (1 << n)    /**< Address Data bit n     */
+#define LSM6DS0_MS                  (1 << 7)    /**< Multiple read write    */
+/** @} */
+
+/**
+ * @name   LSM6DS0 register addresses
+ * @{
+ */
+#define LSM6DS0_AD_ACT_THS                  0x04
+#define LSM6DS0_AD_ACT_DUR                  0x05
+#define LSM6DS0_AD_INT_GEN_CFG_XL           0x06
+#define LSM6DS0_AD_INT_GEN_THS_X_XL         0x07
+#define LSM6DS0_AD_INT_GEN_THS_Y_XL         0x08
+#define LSM6DS0_AD_INT_GEN_THS_Z_XL         0x09
+#define LSM6DS0_AD_INT_GEN_DUR_XL           0x0A
+#define LSM6DS0_AD_REFERENCE_G              0x0B
+#define LSM6DS0_AD_INT_CTRL                 0x0C
+#define LSM6DS0_AD_WHO_AM_I                 0x0F
+#define LSM6DS0_AD_CTRL_REG1_G              0x10
+#define LSM6DS0_AD_CTRL_REG2_G              0x11
+#define LSM6DS0_AD_CTRL_REG3_G              0x12
+#define LSM6DS0_AD_ORIENT_CFG_G             0x13
+#define LSM6DS0_AD_INT_GEN_SRC_G            0x14
+#define LSM6DS0_AD_OUT_TEMP_L               0x15
+#define LSM6DS0_AD_OUT_TEMP_H               0x16
+#define LSM6DS0_AD_STATUS_REG1              0x17
+#define LSM6DS0_AD_OUT_X_L_G                0x18
+#define LSM6DS0_AD_OUT_X_H_G                0x19
+#define LSM6DS0_AD_OUT_Y_L_G                0x1A
+#define LSM6DS0_AD_OUT_Y_H_G                0x1B
+#define LSM6DS0_AD_OUT_Z_L_G                0x1C
+#define LSM6DS0_AD_OUT_Z_H_G                0x1D
+#define LSM6DS0_AD_CTRL_REG4                0x1E
+#define LSM6DS0_AD_CTRL_REG5_XL             0x1F
+#define LSM6DS0_AD_CTRL_REG6_XL             0x20
+#define LSM6DS0_AD_CTRL_REG7_XL             0x21
+#define LSM6DS0_AD_CTRL_REG8                0x22
+#define LSM6DS0_AD_CTRL_REG9                0x23
+#define LSM6DS0_AD_CTRL_REG10               0x24
+#define LSM6DS0_AD_INT_GEN_SRC_XL           0x26
+#define LSM6DS0_AD_STATUS_REG2              0x27
+#define LSM6DS0_AD_OUT_X_L_XL               0x28
+#define LSM6DS0_AD_OUT_X_H_XL               0x29
+#define LSM6DS0_AD_OUT_Y_L_XL               0x2A
+#define LSM6DS0_AD_OUT_Y_H_XL               0x2B
+#define LSM6DS0_AD_OUT_Z_L_XL               0x2C
+#define LSM6DS0_AD_OUT_Z_H_XL               0x2D
+#define LSM6DS0_AD_FIFO_CTRL                0x2E
+#define LSM6DS0_AD_FIFO_SRC                 0x2F
+#define LSM6DS0_AD_INT_GEN_CFG_G            0x30
+#define LSM6DS0_AD_INT_GEN_THS_XH_G         0x31
+#define LSM6DS0_AD_INT_GEN_THS_XL_G         0x32
+#define LSM6DS0_AD_INT_GEN_THS_YH_G         0x33
+#define LSM6DS0_AD_INT_GEN_THS_YL_G         0x34
+#define LSM6DS0_AD_INT_GEN_THS_ZH_G         0x35
+#define LSM6DS0_AD_INT_GEN_THS_ZL_G         0x36
+#define LSM6DS0_AD_INT_GEN_DUR_G            0x37
+/** @} */
+
+/**
+ * @name    LSM6DS0_AD_CTRL_REG1_G register bits definitions
+ * @{
+ */
+#define LSM6DS0_CTRL_REG1_G                 0xFA
+#define LSM6DS0_CTRL_REG1_G_BW_G0           (1 << 0)
+#define LSM6DS0_CTRL_REG1_G_BW_G1           (1 << 1)
+#define LSM6DS0_CTRL_REG1_G_FS_MASK         0x1F
+#define LSM6DS0_CTRL_REG1_G_FS_G0           (1 << 3)
+#define LSM6DS0_CTRL_REG1_G_FS_G1           (1 << 4)
+#define LSM6DS0_CTRL_REG1_G_ODR_G0          (1 << 5)
+#define LSM6DS0_CTRL_REG1_G_ODR_G1          (1 << 6)
+#define LSM6DS0_CTRL_REG1_G_ODR_G2          (1 << 7)
+/** @} */
+
+/**
+ * @name    LSM6DS0_AD_CTRL_REG2_G register bits definitions
+ * @{
+ */
+#define LSM6DS0_CTRL_REG2_G                 0x0F
+#define LSM6DS0_CTRL_REG2_G_OUT_SEL0        (1 << 0)
+#define LSM6DS0_CTRL_REG2_G_OUT_SEL1        (1 << 1)
+#define LSM6DS0_CTRL_REG2_G_INT_SEL0        (1 << 2)
+#define LSM6DS0_CTRL_REG2_G_INT_SEL1        (1 << 3)
+/** @} */
+
+/**
+ * @name    LSM6DS0_AD_CTRL_REG3_G register bits definitions
+ * @{
+ */
+#define LSM6DS0_CTRL_REG3_G                 0x64
+#define LSM6DS0_CTRL_REG3_G_HP_CF0_G        (1 << 0)
+#define LSM6DS0_CTRL_REG3_G_HP_CF1_G        (1 << 1)
+#define LSM6DS0_CTRL_REG3_G_HP_CF2_G        (1 << 2)
+#define LSM6DS0_CTRL_REG3_G_HP_CF3_G        (1 << 3)
+#define LSM6DS0_CTRL_REG3_G_HP_EN           (1 << 6)
+#define LSM6DS0_CTRL_REG3_G_LP_MODE         (1 << 7)
+/** @} */
+
+/**
+ * @name    LSM6DS0_AD_CTRL_REG4 register bits definitions
+ * @{
+ */
+#define LSM6DS0_CTRL_REG4                   0x3A
+#define LSM6DS0_CTRL_REG4_4D_XL1            (1 << 0)
+#define LSM6DS0_CTRL_REG4_LIR_XL1           (1 << 1)
+#define LSM6DS0_CTRL_REG4_XEN_G             (1 << 3)
+#define LSM6DS0_CTRL_REG4_YEN_G             (1 << 4)
+#define LSM6DS0_CTRL_REG4_ZEN_G             (1 << 5)
+/** @} */
+
+/**
+ * @name    LSM6DS0_AD_CTRL_REG5_XL register bits definitions
+ * @{
+ */
+#define LSM6DS0_CTRL_REG5_XL                0xF8
+#define LSM6DS0_CTRL_REG5_XL_XEN_XL         (1 << 3)
+#define LSM6DS0_CTRL_REG5_XL_YEN_XL         (1 << 4)
+#define LSM6DS0_CTRL_REG5_XL_ZEN_XL         (1 << 5)
+#define LSM6DS0_CTRL_REG5_XL_DEC0           (1 << 6)
+#define LSM6DS0_CTRL_REG5_XL_DEC1           (1 << 7)
+/** @} */
+
+/**
+ * @name    LSM6DS0_AD_CTRL_REG6_XL register bits definitions
+ * @{
+ */
+#define LSM6DS0_CTRL_REG6_XL                0xFF
+#define LSM6DS0_CTRL_REG6_XL_BW_XL0         (1 << 0)
+#define LSM6DS0_CTRL_REG6_XL_BW_XL1         (1 << 1)
+#define LSM6DS0_CTRL_REG6_XL_BW_SCAL_ODR    (1 << 2)
+#define LSM6DS0_CTRL_REG6_XL_FS_MASK        0x1F
+#define LSM6DS0_CTRL_REG6_XL_FS0_XL         (1 << 3)
+#define LSM6DS0_CTRL_REG6_XL_FS1_XL         (1 << 4)
+#define LSM6DS0_CTRL_REG6_XL_ODR_XL0        (1 << 5)
+#define LSM6DS0_CTRL_REG6_XL_ODR_XL1        (1 << 6)
+#define LSM6DS0_CTRL_REG6_XL_ODR_XL2        (1 << 7)
+/** @} */
+
+/**
+ * @name    LSM6DS0_AD_CTRL_REG7_XL register bits definitions
+ * @{
+ */
+#define LSM6DS0_CTRL_REG7_XL                0xE5
+#define LSM6DS0_CTRL_REG7_XL_HPIS1          (1 << 0)
+#define LSM6DS0_CTRL_REG7_XL_FDS            (1 << 2)
+#define LSM6DS0_CTRL_REG7_XL_DCF0           (1 << 5)
+#define LSM6DS0_CTRL_REG7_XL_DCF1           (1 << 6)
+#define LSM6DS0_CTRL_REG7_XL_HR             (1 << 7)
+/** @} */
+
+/**
+ * @name    LSM6DS0_AD_CTRL_REG8 register bits definitions
+ * @{
+ */
+#define LSM6DS0_CTRL_REG8                   0xFF
+#define LSM6DS0_CTRL_REG8_SW_RESET          (1 << 0)
+#define LSM6DS0_CTRL_REG8_BLE               (1 << 1)
+#define LSM6DS0_CTRL_REG8_IF_ADD_INC        (1 << 2)
+#define LSM6DS0_CTRL_REG8_SIM               (1 << 3)
+#define LSM6DS0_CTRL_REG8_PP_OD             (1 << 4)
+#define LSM6DS0_CTRL_REG8_H_LACTIVE         (1 << 5)
+#define LSM6DS0_CTRL_REG8_BDU               (1 << 6)
+#define LSM6DS0_CTRL_REG8_BOOT              (1 << 7)
+/** @} */
+
+/**
+ * @name    LSM6DS0_AD_CTRL_REG9 register bits definitions
+ * @{
+ */
+#define LSM6DS0_CTRL_REG9                   0x5F
+#define LSM6DS0_CTRL_REG9_STOP_ON_FTH       (1 << 0)
+#define LSM6DS0_CTRL_REG9_FIFO_EN           (1 << 1)
+#define LSM6DS0_CTRL_REG9_I2C_DISABLE       (1 << 2)
+#define LSM6DS0_CTRL_REG9_DRDY_MASK_BIT     (1 << 3)
+#define LSM6DS0_CTRL_REG9_FIFO_TEMP_EN      (1 << 4)
+#define LSM6DS0_CTRL_REG9_SLEEP_G           (1 << 6)
+/** @} */
+
+/**
+ * @name    LSM6DS0_AD_CTRL_REG10 register bits definitions
+ * @{
+ */
+#define LSM6DS0_CTRL_REG10                  0x05
+#define LSM6DS0_CTRL_REG10_ST_XL            (1 << 0)
+#define LSM6DS0_CTRL_REG10_ST_G             (1 << 2)
+
+/** @} */
+
+//TODO: ADD more LSM6DS0 register bits definitions
 
 /*===========================================================================*/
 /* Driver pre-compile time settings.                                         */
@@ -80,6 +326,35 @@
 #define LSM6DS0_SHARED_I2C                  FALSE
 #endif
 
+/**
+ * @brief   LSM6DS0 subsystem advanced configurations switch.
+ * @details If set to @p TRUE more configurations are available.
+ * @note    The default is @p FALSE.
+ */
+#if !defined(LSM6DS0_USE_ADVANCED) || defined(__DOXYGEN__)
+#define LSM6DS0_USE_ADVANCED                FALSE
+#endif
+
+/**
+ * @brief   LSM6DS0 accelerometer subsystem advanced configurations 
+ *          switch.
+ * @details If set to @p TRUE more configurations are available.
+ * @note    The default is @p FALSE.
+ */
+#if !defined(LSM6DS0_ACC_USE_ADVANCED) || defined(__DOXYGEN__)
+#define LSM6DS0_ACC_USE_ADVANCED            FALSE
+#endif
+
+/**
+ * @brief   LSM6DS0 gyroscope subsystem advanced configurations 
+ *          switch.
+ * @details If set to @p TRUE more configurations are available.
+ * @note    The default is @p FALSE.
+ */
+#if !defined(LSM6DS0_GYRO_USE_ADVANCED) || defined(__DOXYGEN__)
+#define LSM6DS0_GYRO_USE_ADVANCED           FALSE
+#endif
+
 /**
  * @brief   Number of acquisitions for gyroscope bias removal.
  * @details This is the number of acquisitions performed to compute the
@@ -126,7 +401,6 @@
  * @name    LSM6DS0 accelerometer subsystem data structures and types.
  * @{
  */
- 
 /**
  * @brief   LSM6DS0 accelerometer subsystem full scale.
  */
@@ -150,65 +424,6 @@ typedef enum {
   LSM6DS0_ACC_ODR_952Hz = 0xC0      /**< ODR 952 Hz                         */
 } lsm6ds0_acc_odr_t;
 
-/**
- * @brief   LSM6DS0 accelerometer subsystem axes enabling.
- */
-typedef enum {
-  LSM6DS0_ACC_AE_DISABLED = 0x00,   /**< All axes disabled.                 */
-  LSM6DS0_ACC_AE_X = 0x08,          /**< Only X-axis enabled.               */
-  LSM6DS0_ACC_AE_Y = 0x10,          /**< Only Y-axis enabled.               */
-  LSM6DS0_ACC_AE_XY = 0x18,         /**< X and Y axes enabled.              */
-  LSM6DS0_ACC_AE_Z = 0x20,          /**< Only Z-axis enabled.               */
-  LSM6DS0_ACC_AE_XZ = 0x28,         /**< X and Z axes enabled.              */
-  LSM6DS0_ACC_AE_YZ = 0x30,         /**< Y and Z axes enabled.              */
-  LSM6DS0_ACC_AE_XYZ = 0x38         /**< All axes enabled.                  */
-} lsm6ds0_acc_ae_t;
-
-/**
- * @brief   LSM6DS0 accelerometer subsystem anti aliasing filter bandwidth.
- */
-typedef enum {
-  LSM6DS0_ACC_OBW_ODR = 0x00,      /**< Depending on ODR.                   */
-  LSM6DS0_ACC_OBW_AA = 0x04        /**< Same of the Anti aliasing.          */
-} lsm6ds0_acc_obw_t;
-
-/**
- * @brief   LSM6DS0 accelerometer subsystem high resolution mode.
- */
-typedef enum {
-  LSM6DS0_ACC_HR_DISABLED = 0x00,   /**< High resolution mode disabled.     */
-  LSM6DS0_ACC_HR_ENABLED = 0x80     /**< High resolution mode enabled.      */
-} lsm6ds0_acc_hr_t;
-
-/**
- * @brief   LSM6DS0 accelerometer subsystem filtered data selection.
- */
-typedef enum {
-  LSM6DS0_ACC_FDS_ENABLED = 0x00,   /**< Internal filter bypassed.          */
-  LSM6DS0_ACC_FDS_DISABLED = 0x04   /**< Internal filter not bypassed.      */
-} lsm6ds0_acc_fds_t;
-
-/**
- * @brief   LSM6DS0 accelerometer subsystem digital filter.
- */
-typedef enum {
-  LSM6DS0_ACC_DCF_400 = 0x00,       /**< Low pass cutoff freq. ODR/400 Hz.  */
-  LSM6DS0_ACC_DCF_100 = 0x20,       /**< Low pass cutoff freq. ODR/100 Hz.  */
-  LSM6DS0_ACC_DCF_50 = 0x60,        /**< Low pass cutoff freq. ODR/50 Hz.   */
-  LSM6DS0_ACC_DCF_9 = 0x40          /**< Low pass cutoff freq. ODR/9 Hz.    */
-} lsm6ds0_acc_dcf_t;
-
-/**
- * @brief   LSM6DS0 accelerometer subsystem anti aliasing filter bandwidth.
- */
-typedef enum {
-  LSM6DS0_ACC_AABW_408Hz = 0x00,    /**< Anti Aliasing filter BW 408Hz.     */
-  LSM6DS0_ACC_AABW_211Hz = 0x01,    /**< Anti Aliasing filter BW 211Hz.     */
-  LSM6DS0_ACC_AABW_105Hz = 0x02,    /**< Anti Aliasing filter BW 105Hz.     */
-  LSM6DS0_ACC_AABW_50Hz = 0x03,     /**< Anti Aliasing filter BW 50Hz.      */
-  LSM6DS0_ACC_AABW_AUTO = 0x04      /**< Anti Aliasing filter BW auto.      */
-} lsm6ds0_acc_aabw_t;
-
 /**
  * @brief   LSM6DS0 accelerometer subsystem decimation mode.
  */
@@ -219,77 +434,50 @@ typedef enum {
   LSM6DS0_ACC_DEC_X8 = 0xC0         /**< Output updated every 8 samples.    */
 } lsm6ds0_acc_dec_t;
 
-/**
- * @brief   LSM6DS0 accelerometer subsystem unit.
- */
-typedef enum {
-  LSM6DS0_ACC_UNIT_G = 0x00,        /**< Cooked data in g.                  */
-  LSM6DS0_ACC_UNIT_MG = 0x01,       /**< Cooked data in mg.                 */
-  LSM6DS0_ACC_UNIT_SI = 0x02,       /**< Cooked data in m/s^2.              */
-} lsm6ds0_acc_unit_t;
-
 /**
  * @brief   LSM6DS0 accelerometer subsystem configuration structure.
  */
 typedef struct {
   /**
-   * @brief   LSM6DS0 accelerometer subsystem full scale.
+   * @brief LSM6DS0 accelerometer initial sensitivity.
    */
-  lsm6ds0_acc_fs_t          fullscale;
+  float                        sensitivity[LSM6DS0_ACC_NUMBER_OF_AXES];
   /**
-   * @brief   LSM6DS0 accelerometer subsystem output data rate.
+   * @brief LSM6DS0 accelerometer initial bias.
    */
-  lsm6ds0_acc_odr_t         outdatarate;
+  float                        bias[LSM6DS0_ACC_NUMBER_OF_AXES];
   /**
-   * @brief   LSM6DS0 accelerometer subsystem axes enabling.
+   * @brief LSM6DS0 accelerometer subsystem full scale.
    */
-  lsm6ds0_acc_ae_t          axesenabling;
+  lsm6ds0_acc_fs_t             fullscale;
   /**
-   * @brief   LSM6DS0 accelerometer subsystem output bandwidth.
+   * @brief LSM6DS0 accelerometer subsystem output data rate.
    */
-  lsm6ds0_acc_obw_t         outbandwidth;
+  lsm6ds0_acc_odr_t            outdatarate;
+#if LSM6DS0_ACC_USE_ADVANCED || defined(__DOXYGEN__)
   /**
-   * @brief   LSM6DS0 accelerometer subsystem high resolution mode.
+   * @brief LSM6DS0 accelerometer subsystem decimation mode.
    */
-  lsm6ds0_acc_hr_t          highresmode;
-  /**
-   * @brief   LSM6DS0 accelerometer subsystem filtered data selection.
-   */
-  lsm6ds0_acc_fds_t         filtdatasel;
-  /**
-   * @brief   LSM6DS0 accelerometer subsystem digital filter.
-   */
-  lsm6ds0_acc_dcf_t         digifilter;
-  /**
-   * @brief   LSM6DS0 accelerometer subsystem anti aliasing filter bandwidth.
-   */
-  lsm6ds0_acc_aabw_t        aabandwidth;
-  /**
-   * @brief   LSM6DS0 accelerometer subsystem decimation mode.
-   */
-  lsm6ds0_acc_dec_t         decmode;
-  /**
-   * @brief   LSM6DS0 accelerometer subsystem unit.
-   */
-  lsm6ds0_acc_unit_t        unit;
+  lsm6ds0_acc_dec_t            decmode;
+#endif /* LSM6DS0_ACC_USE_ADVANCED */
 } LSM6DS0AccConfig;
 /** @} */
 
 /**
- * @name    LSM6DS0 gyroscope subsystem data structures and types.
+ * @name  LSM6DS0 gyroscope subsystem data structures and types.
  * @{
  */
 /**
- * @brief   LSM6DS0 gyroscope subsystem full scale.
+ * @brief LSM6DS0 gyroscope subsystem full scale.
  */
 typedef enum {
-  LSM6DS0_GYRO_FS_245DSP  = 0x00,   /**< Full scale ±245 degree per second  */
-  LSM6DS0_GYRO_FS_500DSP  = 0x08,   /**< Full scale ±500 degree per second  */
-  LSM6DS0_GYRO_FS_2000DSP = 0x18    /**< Full scale ±2000 degree per second */
+  LSM6DS0_GYRO_FS_245DPS  = 0x00,   /**< Full scale ±245 degree per second  */
+  LSM6DS0_GYRO_FS_500DPS  = 0x08,   /**< Full scale ±500 degree per second  */
+  LSM6DS0_GYRO_FS_2000DPS = 0x18    /**< Full scale ±2000 degree per second */
 } lsm6ds0_gyro_fs_t;
 
 /**
- * @brief   LSM6DS0 gyroscope subsystem output data rate.
+ * @brief LSM6DS0 gyroscope subsystem output data rate.
  */
 typedef enum {
   LSM6DS0_GYRO_ODR_PD = 0x00,
@@ -313,21 +501,7 @@ typedef enum {
 } lsm6ds0_gyro_odr_t;
 
 /**
- * @brief   LSM6DS0 gyroscope subsystem axes enabling.
- */
-typedef enum {
-  LSM6DS0_GYRO_AE_DISABLED = 0x00,  /**< All axes disabled.                 */
-  LSM6DS0_GYRO_AE_X = 0x08,         /**< Only X-axis enabled.               */
-  LSM6DS0_GYRO_AE_Y = 0x10,         /**< Only Y-axis enabled.               */
-  LSM6DS0_GYRO_AE_XY = 0x18,        /**< X and Y axes enabled.              */
-  LSM6DS0_GYRO_AE_Z = 0x20,         /**< Only Z-axis enabled.               */
-  LSM6DS0_GYRO_AE_XZ = 0x28,        /**< X and Z axes enabled.              */
-  LSM6DS0_GYRO_AE_YZ = 0x30,        /**< Y and Z axes enabled.              */
-  LSM6DS0_GYRO_AE_XYZ = 0x38        /**< All axes enabled.                  */
-} lsm6ds0_gyro_ae_t;
-
-/**
- * @brief   LSM6DS0 gyroscope subsystem low mode configuration.
+ * @brief LSM6DS0 gyroscope subsystem low mode configuration.
  */
 typedef enum {
   LSM6DS0_GYRO_LP_DISABLED = 0x00,  /**< Low power mode disabled.           */
@@ -368,42 +542,48 @@ typedef enum {
 } lsm6ds0_gyro_hpcf_t;
 
 /**
- * @brief   LSM6DS0 gyroscope subsystem configuration structure.
+ * @brief LSM6DS0 gyroscope subsystem configuration structure.
  */
 typedef struct {
   /**
-   * @brief   LSM6DS0 gyroscope subsystem full scale.
+   * @brief LSM6DS0 gyroscope initial sensitivity.
    */
-  lsm6ds0_gyro_fs_t         fullscale;
+  float                        sensitivity[LSM6DS0_ACC_NUMBER_OF_AXES];
   /**
-   * @brief   LSM6DS0 gyroscope subsystem output data rate.
+   * @brief LSM6DS0 gyroscope initial bias.
    */
-  lsm6ds0_gyro_odr_t        outdatarate;
+  float                        bias[LSM6DS0_ACC_NUMBER_OF_AXES];
   /**
-   * @brief   LSM6DS0 gyroscope subsystem axes enabling.
+   * @brief LSM6DS0 gyroscope subsystem full scale.
    */
-  lsm6ds0_gyro_ae_t         axesenabling;
+  lsm6ds0_gyro_fs_t            fullscale;
   /**
-   * @brief   LSM6DS0 gyroscope subsystem low mode configuration.
+   * @brief LSM6DS0 gyroscope subsystem output data rate.
    */
-  lsm6ds0_gyro_lp_t         lowmodecfg;
+  lsm6ds0_gyro_odr_t           outdatarate;
+#if LSM6DS0_GYRO_USE_ADVANCED || defined(__DOXYGEN__)
   /**
-   * @brief   LSM6DS0 gyroscope subsystem output selection.
+   * @brief LSM6DS0 gyroscope subsystem low mode configuration.
    */
-  lsm6ds0_gyro_out_sel_t    outsel;
+  lsm6ds0_gyro_lp_t            lowmodecfg;
   /**
-   * @brief   LSM6DS0 gyroscope subsystem high pass filter.
+   * @brief LSM6DS0 gyroscope subsystem output selection.
    */
-  lsm6ds0_gyro_hp_t         hpfenable;
+  lsm6ds0_gyro_out_sel_t       outsel;
   /**
-   * @brief   LSM6DS0 gyroscope subsystem high pass filter configuration.
+   * @brief LSM6DS0 gyroscope subsystem high pass filter.
    */
-  lsm6ds0_gyro_hpcf_t       hpcfg;
+  lsm6ds0_gyro_hp_t            hpfenable;
+  /**
+   * @brief LSM6DS0 gyroscope subsystem high pass filter configuration.
+   */
+  lsm6ds0_gyro_hpcf_t          hpcfg;
+	#endif /* LSM6DS0_GYRO_USE_ADVANCED */
 } LSM6DS0GyroConfig;
 /** @} */
 
 /**
- * @name    LSM6DS0 main system data structures and types.
+ * @name  LSM6DS0 main system data structures and types.
  * @{
  */
 /**
@@ -415,7 +595,7 @@ typedef enum {
 } lsm6ds0_sad_t;
 
 /**
- * @brief   LSM6DS0 block data update.
+ * @brief LSM6DS0 block data update.
  */
 typedef enum {
   LSM6DS0_BDU_CONTINUOUS = 0x00,    /**< Block data continuously updated.   */
@@ -423,7 +603,7 @@ typedef enum {
 } lsm6ds0_bdu_t;
 
 /**
- * @brief   LSM6DS0 endianness.
+ * @brief LSM6DS0 endianness.
  */
 typedef enum {
   LSM6DS0_END_LITTLE = 0x00,        /**< Little endian.                     */
@@ -440,7 +620,7 @@ typedef enum {
 } lsm6ds0_state_t;
 
 /**
- * @brief   LSM6DS0 configuration structure.
+ * @brief LSM6DS0 configuration structure.
  */
 typedef struct {
 #if (LSM6DS0_USE_SPI) || defined(__DOXYGEN__)
@@ -471,25 +651,27 @@ typedef struct {
   const I2CConfig           *i2ccfg;
 #endif /* LSM6DS0_USE_I2C */
   /**
-   * @brief   LSM6DS0 accelerometer subsystem configuration structure
+   * @brief LSM6DS0 accelerometer subsystem configuration structure
    */
   const LSM6DS0AccConfig    *acccfg;
   /**
-   * @brief   LSM6DS0 gyroscope subsystem configuration structure
+   * @brief LSM6DS0 gyroscope subsystem configuration structure
    */
   const LSM6DS0GyroConfig   *gyrocfg;
   /**
    * @brief  Accelerometer and Gyroscope Slave Address
    */
   lsm6ds0_sad_t             slaveaddress;
+#if (LSM6DS0_USE_ADVANCED) || defined(__DOXYGEN__)
   /**
-   * @brief   LSM6DS0 block data update
+   * @brief LSM6DS0 block data update
    */
   lsm6ds0_bdu_t             blockdataupdate;
   /**
-   * @brief   LSM6DS0  endianness
+   * @brief LSM6DS0  endianness
    */
   lsm6ds0_end_t             endianness;
+#endif /* LSM6DS0_USE_ADVANCED */
 } LSM6DS0Config;
 
 /**
@@ -501,15 +683,17 @@ typedef struct LSM6DS0Driver LSM6DS0Driver;
  * @brief   @p LSM6DS0 accelerometer subsystem specific methods.
  */
 #define _lsm6ds0_acc_methods                                                \
-  _base_accelerometer_methods
+  _base_accelerometer_methods                                               \
+  /* Change full scale value of LSM6DS0 accelerometer subsystem .*/         \
+  msg_t (*set_full_scale)(void *instance, lsm6ds0_acc_fs_t fs);
   
 /**
  * @brief   @p LSM6DS0 gyroscope subsystem specific methods.
  */
 #define _lsm6ds0_gyro_methods                                               \
   _base_gyroscope_methods                                                   \
-  /* Retrieve the temperature of LSM6DS0 chip.*/                            \
-  msg_t (*get_temperature)(void *instance, float* temperature);
+  /* Change full scale value of LSM6DS0 gyroscope subsystem .*/             \
+  msg_t (*set_full_scale)(void *instance, lsm6ds0_gyro_fs_t fs);
   
 /**
  * @extends BaseAccelerometerVMT
@@ -542,14 +726,18 @@ struct LSM6DS0GYROVMT {
   /* Current accelerometer sensitivity.*/                                   \
   float                     accsensitivity[LSM6DS0_ACC_NUMBER_OF_AXES];     \
   /* Accelerometer bias data.*/                                             \
-  int32_t                   accbias[LSM6DS0_ACC_NUMBER_OF_AXES];            \
+  float                     accbias[LSM6DS0_ACC_NUMBER_OF_AXES];            \
+  /* Current accelerometer full scale value.*/                              \
+  float                     accfullscale;                                   \
   /* Current gyroscope sensitivity.*/                                       \
   float                     gyrosensitivity[LSM6DS0_GYRO_NUMBER_OF_AXES];   \
   /* Bias data.*/                                                           \
-  int32_t                   gyrobias[LSM6DS0_GYRO_NUMBER_OF_AXES];
+  float                     gyrobias[LSM6DS0_GYRO_NUMBER_OF_AXES];          \
+  /* Current gyroscope full scale value.*/                                  \
+  float                     gyrofullscale;
   
 /**
- * @brief   LSM6DS0 6-axis accelerometer/gyroscope class.
+ * @brief LSM6DS0 6-axis accelerometer/gyroscope class.
  */
 struct LSM6DS0Driver {
   /** @brief BaseSensor Virtual Methods Table. */
@@ -571,19 +759,32 @@ struct LSM6DS0Driver {
 /*===========================================================================*/
 
 /**
- * @brief   Get current MEMS temperature.
- * @detail  This information is very useful especially for high accuracy IMU
+ * @brief   Change accelerometer fullscale value.
  *
- * @param[in] ip        pointer to a @p BaseGyroscope class.
- * @param[out] temp     the MEMS temperature as single precision floating.
+ * @param[in] ip        pointer to a @p BaseAccelerometer class.
+ * @param[in] fs        the new full scale value.
  *
  * @return              The operation status.
  * @retval MSG_OK       if the function succeeded.
  * @retval MSG_RESET    if one or more errors occurred.
  * @api
  */
-#define gyroscopeGetTemp(ip, tpp)                                           \
-        (ip)->vmt_lsm6ds0gyro->get_temperature(ip, tpp)
+#define accelerometerSetFullScale(ip, fs)                                   \
+        (ip)->vmt_lsm6ds0acc->set_full_scale(ip, fs)
+
+/**
+ * @brief   Change compass fullscale value.
+ *
+ * @param[in] ip        pointer to a @p BaseGyroscope class.
+ * @param[in] fs        the new full scale value.
+ *
+ * @return              The operation status.
+ * @retval MSG_OK       if the function succeeded.
+ * @retval MSG_RESET    if one or more errors occurred.
+ * @api
+ */
+#define gyroscopeSetFullScale(ip, fs)                                         \
+        (ip)->vmt_lsm6ds0gyro->set_full_scale(ip, fs)
 	
 /*===========================================================================*/
 /* External declarations.                                                    */