Added support for LSM6DS0, improved hal_accelerometer and hal_gyroscope, added a new demo

git-svn-id: svn://svn.code.sf.net/p/chibios/svn/trunk@9349 35acf78f-673a-0410-8e92-d51de3d6d3f4

4 files changed, 400 insertions, 79 deletions

diff --git a/os/ex/ST/lsm6ds0.c b/os/ex/ST/lsm6ds0.c
index 72e79b558..0a494d362 100644
--- a/os/ex/ST/lsm6ds0.c
+++ b/os/ex/ST/lsm6ds0.c
@@ -44,6 +44,8 @@
 #define  LSM6DS0_GYRO_SENS_500DPS           ((float)0.01750f)
 #define  LSM6DS0_GYRO_SENS_2000DPS          ((float)0.07000f)
 
+#define  LSM6DS0_TEMP_SENS                  ((float)0.0625f)
+
 #define  LSM6DS0_DI                         ((uint8_t)0xFF)
 #define  LSM6DS0_DI_0                       ((uint8_t)0x01)
 #define  LSM6DS0_DI_1                       ((uint8_t)0x02)
@@ -265,12 +267,28 @@ msg_t lsm6ds0I2CWriteRegister(I2CDriver *i2cp, lsm6ds0_sad_t sad, uint8_t reg,
  */
 static size_t acc_get_axes_number(void *ip) {
 
-  osalDbgCheck((ip != NULL) &&
-              (((LSM6DS0Driver *)ip)->config->acccfg != NULL));
+  osalDbgCheck(ip != NULL);
   return LSM6DS0_ACC_NUMBER_OF_AXES;
 }
 
-static msg_t acc_read_raw(void *ip, int32_t axes[LSM6DS0_ACC_NUMBER_OF_AXES]) {
+static size_t gyro_get_axes_number(void *ip) {
+
+  osalDbgCheck(ip != NULL);
+  return LSM6DS0_GYRO_NUMBER_OF_AXES;
+}
+
+static size_t sens_get_axes_number(void *ip) {
+  size_t size = 0;
+  
+  osalDbgCheck(ip != NULL);
+  if (((LSM6DS0Driver *)ip)->config->acccfg != NULL)
+	size += acc_get_axes_number(ip);
+  if (((LSM6DS0Driver *)ip)->config->gyrocfg != NULL)
+    size += gyro_get_axes_number(ip);  
+  return size;
+}
+
+static msg_t acc_read_raw(void *ip, int32_t axes[]) {
 
   osalDbgCheck(((ip != NULL) && (axes != NULL)) &&
               (((LSM6DS0Driver *)ip)->config->acccfg != NULL));
@@ -292,6 +310,7 @@ static msg_t acc_read_raw(void *ip, int32_t axes[LSM6DS0_ACC_NUMBER_OF_AXES]) {
     axes[0] += (int16_t)(lsm6ds0I2CReadRegister(((LSM6DS0Driver *)ip)->config->i2cp,
                                                 ((LSM6DS0Driver *)ip)->config->slaveaddress,
                                                 LSM6DS0_AD_OUT_X_H_XL, NULL) << 8);
+	axes[0] -= ((LSM6DS0Driver *)ip)->accbias[0];
   }
   if(((LSM6DS0Driver *)ip)->config->acccfg->axesenabling & LSM6DS0_ACC_AE_Y){
     axes[1] = (int16_t)(lsm6ds0I2CReadRegister(((LSM6DS0Driver *)ip)->config->i2cp,
@@ -300,6 +319,7 @@ static msg_t acc_read_raw(void *ip, int32_t axes[LSM6DS0_ACC_NUMBER_OF_AXES]) {
     axes[1] += (int16_t)(lsm6ds0I2CReadRegister(((LSM6DS0Driver *)ip)->config->i2cp,
                                                 ((LSM6DS0Driver *)ip)->config->slaveaddress,
                                                 LSM6DS0_AD_OUT_Y_H_XL, NULL) << 8);
+	axes[1] -= ((LSM6DS0Driver *)ip)->accbias[1];
   }
   if(((LSM6DS0Driver *)ip)->config->acccfg->axesenabling & LSM6DS0_ACC_AE_Z){
     axes[2] = (int16_t)(lsm6ds0I2CReadRegister(((LSM6DS0Driver *)ip)->config->i2cp,
@@ -308,46 +328,16 @@ static msg_t acc_read_raw(void *ip, int32_t axes[LSM6DS0_ACC_NUMBER_OF_AXES]) {
     axes[2] += (int16_t)(lsm6ds0I2CReadRegister(((LSM6DS0Driver *)ip)->config->i2cp,
                                                 ((LSM6DS0Driver *)ip)->config->slaveaddress,
                                                 LSM6DS0_AD_OUT_Z_H_XL, NULL) << 8);
+	axes[2] -= ((LSM6DS0Driver *)ip)->accbias[2];
   }
 #if LSM6DS0_SHARED_I2C
   i2cReleaseBus(((LSM6DS0Driver *)ip)->config->i2cp);
 #endif /* LSM6DS0_SHARED_I2C */
-#endif
+#endif /* LSM6DS0_USE_I2C */
   return MSG_OK;
 }
 
-static msg_t acc_read_cooked(void *ip, float axes[]) {
-  uint32_t i;
-  int32_t raw[LSM6DS0_ACC_NUMBER_OF_AXES];
-  msg_t msg;
-
-  osalDbgCheck(((ip != NULL) && (axes != NULL)) &&
-              (((LSM6DS0Driver *)ip)->config->acccfg != NULL));
-
-  osalDbgAssert((((LSM6DS0Driver *)ip)->state == LSM6DS0_READY),
-              "acc_read_cooked(), invalid state");
-
-  msg = acc_read_raw(ip, raw);
-  for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES ; i++){
-    axes[i] = raw[i] * ((LSM6DS0Driver *)ip)->accsens;
-    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;
-    }
-  }
-  return msg;
-}
-
-static size_t gyro_get_axes_number(void *ip) {
-
-  osalDbgCheck((ip != NULL) &&
-              (((LSM6DS0Driver *)ip)->config->gyrocfg != NULL));
-  return LSM6DS0_GYRO_NUMBER_OF_AXES;
-}
-
-static msg_t gyro_read_raw(void *ip, int32_t axes[LSM6DS0_GYRO_NUMBER_OF_AXES]) {
+static msg_t gyro_read_raw(void *ip, int32_t axes[]) {
 
   osalDbgCheck(((ip != NULL) && (axes != NULL)) &&
               (((LSM6DS0Driver *)ip)->config->gyrocfg != NULL));
@@ -369,74 +359,120 @@ static msg_t gyro_read_raw(void *ip, int32_t axes[LSM6DS0_GYRO_NUMBER_OF_AXES])
     axes[0] += (int16_t)(lsm6ds0I2CReadRegister(((LSM6DS0Driver *)ip)->config->i2cp,
                                                 ((LSM6DS0Driver *)ip)->config->slaveaddress,
                                                 LSM6DS0_AD_OUT_X_H_G, NULL) << 8);
+	axes[0] -= ((LSM6DS0Driver *)ip)->gyrobias[0];
   }
-  if(((LSM6DS0Driver *)ip)->config->gyrocfg->axesenabling & LSM6DS0_GYRO_AE_Y){
+  if(((LSM6DS0Driver *)ip)->config->acccfg->axesenabling & LSM6DS0_GYRO_AE_Y){
     axes[1] = (int16_t)(lsm6ds0I2CReadRegister(((LSM6DS0Driver *)ip)->config->i2cp,
                                                ((LSM6DS0Driver *)ip)->config->slaveaddress,
                                                LSM6DS0_AD_OUT_Y_L_G, NULL));
     axes[1] += (int16_t)(lsm6ds0I2CReadRegister(((LSM6DS0Driver *)ip)->config->i2cp,
                                                 ((LSM6DS0Driver *)ip)->config->slaveaddress,
                                                 LSM6DS0_AD_OUT_Y_H_G, NULL) << 8);
+	axes[1] -= ((LSM6DS0Driver *)ip)->gyrobias[1];
   }
-  if(((LSM6DS0Driver *)ip)->config->gyrocfg->axesenabling & LSM6DS0_GYRO_AE_Z){
+  if(((LSM6DS0Driver *)ip)->config->acccfg->axesenabling & LSM6DS0_GYRO_AE_Z){
     axes[2] = (int16_t)(lsm6ds0I2CReadRegister(((LSM6DS0Driver *)ip)->config->i2cp,
                                                ((LSM6DS0Driver *)ip)->config->slaveaddress,
                                                LSM6DS0_AD_OUT_Z_L_G, NULL));
     axes[2] += (int16_t)(lsm6ds0I2CReadRegister(((LSM6DS0Driver *)ip)->config->i2cp,
                                                 ((LSM6DS0Driver *)ip)->config->slaveaddress,
                                                 LSM6DS0_AD_OUT_Z_H_G, NULL) << 8);
+	axes[2] -= ((LSM6DS0Driver *)ip)->gyrobias[2];
   }
 #if LSM6DS0_SHARED_I2C
   i2cReleaseBus(((LSM6DS0Driver *)ip)->config->i2cp);
 #endif /* LSM6DS0_SHARED_I2C */
-#endif
+#endif /* LSM6DS0_USE_I2C */ 
   return MSG_OK;
 }
 
-static msg_t gyro_read_cooked(void *ip, float axes[]) {
+static msg_t sens_read_raw(void *ip, int32_t axes[]) {
+  int32_t* bp = axes;
+  msg_t msg;
+  if (((LSM6DS0Driver *)ip)->config->acccfg != NULL) {
+	msg = acc_read_raw(ip, bp);
+	if(msg != MSG_OK)
+	  return msg;
+	bp += LSM6DS0_ACC_NUMBER_OF_AXES;
+  }
+  if (((LSM6DS0Driver *)ip)->config->gyrocfg != NULL) {
+    msg = gyro_read_raw(ip, bp);
+  }
+  return msg;
+}
+	
+static msg_t acc_read_cooked(void *ip, float axes[]) {
   uint32_t i;
-  int32_t raw[LSM6DS0_GYRO_NUMBER_OF_AXES];
+  int32_t raw[LSM6DS0_ACC_NUMBER_OF_AXES];
   msg_t msg;
 
   osalDbgCheck(((ip != NULL) && (axes != NULL)) &&
-              (((LSM6DS0Driver *)ip)->config->gyrocfg != NULL));
+              (((LSM6DS0Driver *)ip)->config->acccfg != NULL));
 
   osalDbgAssert((((LSM6DS0Driver *)ip)->state == LSM6DS0_READY),
-              "gyro_read_cooked(), invalid state");
+              "acc_read_cooked(), invalid state");
 
   msg = acc_read_raw(ip, raw);
-  for(i = 0; i < LSM6DS0_GYRO_NUMBER_OF_AXES ; i++){
-    axes[i] = raw[i] * ((LSM6DS0Driver *)ip)->gyrosens;
+  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;
+    }
   }
   return msg;
 }
 
-static msg_t gyro_reset_calibration(void *ip) {
+static msg_t gyro_read_cooked(void *ip, float axes[]) {
   uint32_t i;
+  int32_t raw[LSM6DS0_GYRO_NUMBER_OF_AXES];
+  msg_t msg;
 
-  osalDbgCheck(ip != NULL);
+  osalDbgCheck(((ip != NULL) && (axes != NULL)) &&
+              (((LSM6DS0Driver *)ip)->config->gyrocfg != NULL));
 
-  osalDbgAssert((((LSM6DS0Driver *)ip)->state == LSM6DS0_READY) ||
-                (((LSM6DS0Driver *)ip)->state == LSM6DS0_STOP),
-              "reset_calibration(), invalid state");
+  osalDbgAssert((((LSM6DS0Driver *)ip)->state == LSM6DS0_READY),
+              "gyro_read_cooked(), invalid state");
 
-  for(i = 0; i < LSM6DS0_GYRO_NUMBER_OF_AXES; i++)
-    ((LSM6DS0Driver *)ip)->gyrobias[i] = 0;
-  return MSG_OK;
+  msg = gyro_read_raw(ip, raw);
+  for(i = 0; i < LSM6DS0_GYRO_NUMBER_OF_AXES ; i++){
+    axes[i] = raw[i] * ((LSM6DS0Driver *)ip)->gyrosensitivity[i];
+  }
+  return msg;
 }
 
-static msg_t gyro_calibrate(void *ip) {
+static msg_t sens_read_cooked(void *ip, float axes[]) {
+  float* bp = axes;
+  msg_t msg;
+  if (((LSM6DS0Driver *)ip)->config->acccfg != NULL) {
+	msg = acc_read_cooked(ip, bp);
+	if(msg != MSG_OK)
+	  return msg;
+	bp += LSM6DS0_ACC_NUMBER_OF_AXES;
+  }
+  if (((LSM6DS0Driver *)ip)->config->gyrocfg != NULL) {
+    msg = gyro_read_cooked(ip, bp);
+  }
+  return msg;
+}
+
+static msg_t gyro_sample_bias(void *ip) {
   uint32_t i, j;
   int32_t raw[LSM6DS0_GYRO_NUMBER_OF_AXES];
   int32_t buff[LSM6DS0_GYRO_NUMBER_OF_AXES] = {0, 0, 0};
+  msg_t msg;
 
   osalDbgCheck(ip != NULL);
 
   osalDbgAssert((((LSM6DS0Driver *)ip)->state == LSM6DS0_READY),
-              "gyro_calibrate(), invalid state");
+                "sample_bias(), invalid state");
 
   for(i = 0; i < LSM6DS0_GYRO_BIAS_ACQ_TIMES; i++){
-    gyro_read_raw(ip, raw);
+    msg = gyro_read_raw(ip, raw);
+	if(msg != MSG_OK)
+	  return msg;
     for(j = 0; j < LSM6DS0_GYRO_NUMBER_OF_AXES; j++){
       buff[j] += raw[j];
     }
@@ -446,17 +482,188 @@ static msg_t gyro_calibrate(void *ip) {
   for(i = 0; i < LSM6DS0_GYRO_NUMBER_OF_AXES; i++){
     ((LSM6DS0Driver *)ip)->gyrobias[i] = buff[i] / LSM6DS0_GYRO_BIAS_ACQ_TIMES;
   }
+  return msg;
+}
+
+static msg_t acc_set_bias(void *ip, int32_t *bp) {
+  uint32_t i;
+  
+  osalDbgCheck((ip != NULL) && (bp !=NULL));
+
+  osalDbgAssert((((LSM6DS0Driver *)ip)->state == LSM6DS0_READY) ||
+                (((LSM6DS0Driver *)ip)->state == LSM6DS0_STOP),
+                "acc_set_bias(), invalid state");
+  
+  for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES; i++) {
+    ((LSM6DS0Driver *)ip)->accbias[i] = bp[i];
+  }
+  return MSG_OK;
+}
+
+static msg_t gyro_set_bias(void *ip, int32_t *bp) {
+  uint32_t i;
+  
+  osalDbgCheck((ip != NULL) && (bp !=NULL));
+
+  osalDbgAssert((((LSM6DS0Driver *)ip)->state == LSM6DS0_READY) ||
+                (((LSM6DS0Driver *)ip)->state == LSM6DS0_STOP),
+                "gyro_set_bias(), invalid state");
+  
+  for(i = 0; i < LSM6DS0_GYRO_NUMBER_OF_AXES; i++) {
+    ((LSM6DS0Driver *)ip)->gyrobias[i] = bp[i];
+  }
+  return MSG_OK;
+}
+
+static msg_t acc_reset_bias(void *ip) {
+  uint32_t i;
+
+  osalDbgCheck(ip != NULL);
+
+  osalDbgAssert((((LSM6DS0Driver *)ip)->state == LSM6DS0_READY) ||
+                (((LSM6DS0Driver *)ip)->state == LSM6DS0_STOP),
+              "acc_reset_bias(), invalid state");
+
+  for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES; i++)
+    ((LSM6DS0Driver *)ip)->accbias[i] = 0;
+  return MSG_OK;
+}
+
+static msg_t gyro_reset_bias(void *ip) {
+  uint32_t i;
+
+  osalDbgCheck(ip != NULL);
+
+  osalDbgAssert((((LSM6DS0Driver *)ip)->state == LSM6DS0_READY) ||
+                (((LSM6DS0Driver *)ip)->state == LSM6DS0_STOP),
+              "gyro_reset_bias(), invalid state");
+
+  for(i = 0; i < LSM6DS0_GYRO_NUMBER_OF_AXES; i++)
+    ((LSM6DS0Driver *)ip)->gyrobias[i] = 0;
+  return MSG_OK;
+}
+
+static msg_t acc_set_sensivity(void *ip, float *sp) {
+  uint32_t i;
+  
+  osalDbgCheck((ip != NULL) && (sp !=NULL));
+
+  osalDbgAssert((((LSM6DS0Driver *)ip)->state == LSM6DS0_READY),
+                "acc_set_sensivity(), invalid state");
+  
+  for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES; i++) {
+    ((LSM6DS0Driver *)ip)->accsensitivity[i] = sp[i];
+  }
+  return MSG_OK;
+}
+
+static msg_t gyro_set_sensivity(void *ip, float *sp) {
+  uint32_t i;
+  
+  osalDbgCheck((ip != NULL) && (sp !=NULL));
+
+  osalDbgAssert((((LSM6DS0Driver *)ip)->state == LSM6DS0_READY),
+                "gyro_set_sensivity(), invalid state");
+  
+  for(i = 0; i < LSM6DS0_GYRO_NUMBER_OF_AXES; i++) {
+    ((LSM6DS0Driver *)ip)->gyrosensitivity[i] = sp[i];
+  }
+  return MSG_OK;
+}
+
+static msg_t acc_reset_sensivity(void *ip) {
+  uint32_t i;
+
+  osalDbgCheck(ip != NULL);
+
+  osalDbgAssert((((LSM6DS0Driver *)ip)->state == LSM6DS0_READY),
+                "acc_reset_sensivity(), invalid state");
+
+  if(((LSM6DS0Driver *)ip)->config->acccfg->fullscale == LSM6DS0_ACC_FS_2G)
+    for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES; i++)
+      ((LSM6DS0Driver *)ip)->accsensitivity[i] = LSM6DS0_ACC_SENS_2G;
+  else if(((LSM6DS0Driver *)ip)->config->acccfg->fullscale == LSM6DS0_ACC_FS_4G)
+	for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES; i++)
+      ((LSM6DS0Driver *)ip)->accsensitivity[i] = LSM6DS0_ACC_SENS_4G;
+  else if(((LSM6DS0Driver *)ip)->config->acccfg->fullscale == LSM6DS0_ACC_FS_8G)
+	for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES; i++)
+      ((LSM6DS0Driver *)ip)->accsensitivity[i] = LSM6DS0_ACC_SENS_8G;
+  else if(((LSM6DS0Driver *)ip)->config->acccfg->fullscale == LSM6DS0_ACC_FS_16G)
+	for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES; i++)
+      ((LSM6DS0Driver *)ip)->accsensitivity[i] = LSM6DS0_ACC_SENS_16G;
+  return MSG_OK;
+}
+
+static msg_t gyro_reset_sensivity(void *ip) {
+  uint32_t i;
+
+  osalDbgCheck(ip != NULL);
+
+  osalDbgAssert((((LSM6DS0Driver *)ip)->state == LSM6DS0_READY),
+                "gyro_reset_sensivity(), invalid state");
+
+  if(((LSM6DS0Driver *)ip)->config->gyrocfg->fullscale == LSM6DS0_GYRO_FS_245DSP)
+    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)
+	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)
+	for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES; i++)
+      ((LSM6DS0Driver *)ip)->gyrosensitivity[i] = LSM6DS0_GYRO_SENS_2000DPS;
+  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 = (int16_t)(lsm6ds0I2CReadRegister(((LSM6DS0Driver *)ip)->config->i2cp,
+                                          ((LSM6DS0Driver *)ip)->config->slaveaddress,
+                                            LSM6DS0_AD_OUT_TEMP_L, NULL));
+  temp += (int16_t)(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;
   return MSG_OK;
 }
 
-static const struct LSM6DS0ACCVMT vmt_baseaccelerometer = {
-  acc_get_axes_number, acc_read_raw, acc_read_cooked
+static const struct BaseSensorVMT vmt_basesensor = {
+  sens_get_axes_number, sens_read_raw, sens_read_cooked
 };
 
-static const struct LSM6DS0GYROVMT vmt_basegyroscope = {
+static const struct BaseGyroscopeVMT vmt_basegyroscope = {
   gyro_get_axes_number, gyro_read_raw, gyro_read_cooked,
-  gyro_reset_calibration, gyro_calibrate
+  gyro_sample_bias, gyro_set_bias, gyro_reset_bias,
+  gyro_set_sensivity, gyro_reset_sensivity
+};
+
+static const struct BaseAccelerometerVMT vmt_baseaccelerometer = {
+  acc_get_axes_number, acc_read_raw, acc_read_cooked,
+  acc_set_bias, acc_reset_bias, acc_set_sensivity, acc_reset_sensivity
 };
+
+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
+};
+
+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
+};
+
 /*===========================================================================*/
 /* Driver exported functions.                                                */
 /*===========================================================================*/
@@ -469,11 +676,18 @@ static const struct LSM6DS0GYROVMT vmt_basegyroscope = {
  * @init
  */
 void lsm6ds0ObjectInit(LSM6DS0Driver *devp) {
-
+  uint32_t i;
+  devp->vmt_basesensor = &vmt_basesensor;
   devp->vmt_baseaccelerometer = &vmt_baseaccelerometer;
   devp->vmt_basegyroscope = &vmt_basegyroscope;
-  devp->state = LSM6DS0_STOP;
+  devp->vmt_lsm6ds0acc = &vmt_lsm6ds0acc;
+  devp->vmt_lsm6ds0gyro = &vmt_lsm6ds0gyro;
   devp->config = NULL;
+  for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES; i++)
+    devp->accbias[i] = 0;
+  for(i = 0; i < LSM6DS0_GYRO_NUMBER_OF_AXES; i++)
+    devp->gyrobias[i] = 0;
+  devp->state  = LSM6DS0_STOP;
 }
 
 /**
@@ -485,6 +699,7 @@ void lsm6ds0ObjectInit(LSM6DS0Driver *devp) {
  * @api
  */
 void lsm6ds0Start(LSM6DS0Driver *devp, const LSM6DS0Config *config) {
+  uint32_t i;
   osalDbgCheck((devp != NULL) && (config != NULL));
 
   osalDbgAssert((devp->state == LSM6DS0_STOP) || (devp->state == LSM6DS0_READY),
@@ -547,23 +762,30 @@ 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)
-      devp->accsens = LSM6DS0_ACC_SENS_2G;
+      for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES; i++)
+        devp->accsensitivity[i] = LSM6DS0_ACC_SENS_2G;
     else if(devp->config->acccfg->fullscale == LSM6DS0_ACC_FS_4G)
-      devp->accsens = LSM6DS0_ACC_SENS_4G;
+      for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES; i++)
+        devp->accsensitivity[i] = LSM6DS0_ACC_SENS_4G;
     else if(devp->config->acccfg->fullscale == LSM6DS0_ACC_FS_8G)
-      devp->accsens = LSM6DS0_ACC_SENS_8G;
+      for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES; i++)
+        devp->accsensitivity[i] = LSM6DS0_ACC_SENS_8G;
     else if(devp->config->acccfg->fullscale == LSM6DS0_ACC_FS_16G)
-      devp->accsens = LSM6DS0_ACC_SENS_16G;
+      for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES; i++)
+        devp->accsensitivity[i] = LSM6DS0_ACC_SENS_16G;
     else
       osalDbgAssert(FALSE, "lsm6ds0Start(), accelerometer full scale issue");
   }
   if((devp)->config->gyrocfg != NULL) {
     if(devp->config->gyrocfg->fullscale == LSM6DS0_GYRO_FS_245DSP)
-      devp->gyrosens = LSM6DS0_GYRO_SENS_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)
-      devp->gyrosens = LSM6DS0_GYRO_SENS_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)
-      devp->gyrosens = LSM6DS0_GYRO_SENS_2000DPS;
+      for(i = 0; i < LSM6DS0_GYRO_NUMBER_OF_AXES; i++)
+        devp->gyrosensitivity[i] = LSM6DS0_GYRO_SENS_2000DPS;
     else
       osalDbgAssert(FALSE, "lsm6ds0Start(), gyroscope full scale issue");
   }
diff --git a/os/ex/ST/lsm6ds0.h b/os/ex/ST/lsm6ds0.h
index 06f41419c..2315fd53b 100644
--- a/os/ex/ST/lsm6ds0.h
+++ b/os/ex/ST/lsm6ds0.h
@@ -89,7 +89,7 @@
  * @details This is the time between each bias acquisition.
  */
 #if !defined(LSM6DS0_GYRO_BIAS_SETTLING_uS) || defined(__DOXYGEN__)
-#define LSM6DS0_GYRO_BIAS_SETTLING_uS             5000
+#define LSM6DS0_GYRO_BIAS_SETTLING_uS       5000
 #endif
 /** @} */
 
@@ -502,7 +502,9 @@ typedef struct LSM6DS0Driver LSM6DS0Driver;
  * @brief   @p LSM6DS0 gyroscope subsystem specific methods.
  */
 #define _lsm6ds0_gyro_methods                                               \
-  _base_gyroscope_methods
+  _base_gyroscope_methods                                                   \
+  /* Retrieve the temperature of LSM6DS0 chip.*/                            \
+  msg_t (*get_temperature)(void *instance, float* temperature);
   
 /**
  * @extends BaseAccelerometerVMT
@@ -533,9 +535,11 @@ struct LSM6DS0GYROVMT {
   /* Current configuration data.*/                                          \
   const LSM6DS0Config       *config;                                        \
   /* Current accelerometer sensitivity.*/                                   \
-  float                     accsens;                                        \
+  float                     accsensitivity[LSM6DS0_ACC_NUMBER_OF_AXES];     \
+  /* Accelerometer bias data.*/                                             \
+  int32_t                   accbias[LSM6DS0_ACC_NUMBER_OF_AXES];            \
   /* Current gyroscope sensitivity.*/                                       \
-  float                     gyrosens;                                       \
+  float                     gyrosensitivity[LSM6DS0_GYRO_NUMBER_OF_AXES];   \
   /* Bias data.*/                                                           \
   int32_t                   gyrobias[LSM6DS0_GYRO_NUMBER_OF_AXES];
   
@@ -543,10 +547,16 @@ struct LSM6DS0GYROVMT {
  * @brief   LSM6DS0 6-axis accelerometer/gyroscope class.
  */
 struct LSM6DS0Driver {
-  /** @brief Accelerometer Virtual Methods Table.*/
-  const struct LSM6DS0ACCVMT  *vmt_baseaccelerometer;
-  /** @brief Gyroscope Virtual Methods Table.*/
-  const struct LSM6DS0GYROVMT *vmt_basegyroscope;
+  /** @brief BaseSensor Virtual Methods Table. */
+  const struct BaseSensorVMT *vmt_basesensor;
+  /** @brief BaseAccelerometer Virtual Methods Table. */
+  const struct BaseAccelerometerVMT *vmt_baseaccelerometer;
+  /** @brief BaseGyroscope Virtual Methods Table. */
+  const struct BaseGyroscopeVMT *vmt_basegyroscope;
+  /** @brief LSM6DS0 Accelerometer Virtual Methods Table. */
+  const struct LSM6DS0ACCVMT *vmt_lsm6ds0acc;
+  /** @brief LSM6DS0 Gyroscope Virtual Methods Table. */
+  const struct LSM6DS0GYROVMT *vmt_lsm6ds0gyro;
   _lsm6ds0_data
 };
 /** @} */
@@ -555,6 +565,21 @@ struct LSM6DS0Driver {
 /* Driver macros.                                                            */
 /*===========================================================================*/
 
+/**
+ * @brief   Get current MEMS temperature.
+ * @detail  This information is very useful especially for high accuracy IMU
+ *
+ * @param[in] ip        pointer to a @p BaseGyroscope class.
+ * @param[out] temp     the MEMS temperature as single precision floating.
+ *
+ * @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)
+	
 /*===========================================================================*/
 /* External declarations.                                                    */
 /*===========================================================================*/
diff --git a/os/hal/lib/peripherals/sensors/hal_accelerometer.h b/os/hal/lib/peripherals/sensors/hal_accelerometer.h
index e2f05d7fb..da6a58b4b 100644
--- a/os/hal/lib/peripherals/sensors/hal_accelerometer.h
+++ b/os/hal/lib/peripherals/sensors/hal_accelerometer.h
@@ -46,7 +46,15 @@
 /**

  * @brief   BaseAccelerometer specific methods.

  */

-#define _base_accelerometer_methods_alone

+#define _base_accelerometer_methods_alone                                   \

+  /* Invoke the set bias procedure.*/                                       \

+  msg_t (*set_bias)(void *instance, int32_t biases[]);                      \

+  /* Remove bias stored data.*/                                             \

+  msg_t (*reset_bias)(void *instance);                                      \

+  /* Invoke the set sensitivity procedure.*/                                \

+  msg_t (*set_sensitivity)(void *instance, float sensitivities[]);          \

+  /* Restore sensitivity stored data to default.*/                          \

+  msg_t (*reset_sensitivity)(void *instance);

 

 /**

  * @brief   BaseAccelerometer specific methods with inherited ones.

@@ -126,6 +134,71 @@ typedef struct {
  */

 #define accelerometerReadCooked(ip, dp)                                     \

         (ip)->vmt_baseaccelerometer->read_cooked(ip, dp)

+

+/**

+ * @brief   Updates accelerometer bias data from received buffer.

+ * @note    The bias buffer must have the same length of the

+ *          the accelerometer axes number. Bias must be computed on

+ *          raw data and is a signed integer.

+ *

+ *

+ * @param[in] ip        pointer to a @p BaseAccelerometer class.

+ * @param[in] bp        pointer to a buffer of bias values.

+ *

+ * @return              The operation status.

+ * @retval MSG_OK       if the function succeeded.

+ * @retval MSG_RESET    if one or more errors occurred.

+ *

+ * @api

+ */

+#define accelerometerSetBias(ip, bp)                                        \

+        (ip)->vmt_baseaccelerometer->set_bias(ip, bp)

+

+/**

+ * @brief   Reset accelerometer bias data restoring it to zero.

+ *

+ * @param[in] ip        pointer to a @p BaseAccelerometer class.

+ *

+ * @return              The operation status.

+ * @retval MSG_OK       if the function succeeded.

+ * @retval MSG_RESET    if one or more errors occurred.

+ *

+ * @api

+ */

+#define accelerometerResetBias(ip)                                          \

+        (ip)->vmt_baseaccelerometer->reset_bias(ip)

+

+/**

+ * @brief   Updates accelerometer sensitivity data from received buffer.

+ * @note    The sensitivity buffer must have the same length of the

+ *          the accelerometer axes number.

+ *

+ * @param[in] ip        pointer to a @p BaseAccelerometer class.

+ * @param[in] sp        pointer to a buffer of sensitivity values.

+ *

+ * @return              The operation status.

+ * @retval MSG_OK       if the function succeeded.

+ * @retval MSG_RESET    if one or more errors occurred.

+ *

+ * @api

+ */

+#define accelerometerSetSensitivity(ip, sp)                                 \

+        (ip)->vmt_baseaccelerometer->set_sensitivity(ip, sp)

+

+/**

+ * @brief   Reset accelerometer sensitivity data restoring it to its typical

+ *          value.

+ *

+ * @param[in] ip        pointer to a @p BaseAccelerometer class.

+ *

+ * @return              The operation status.

+ * @retval MSG_OK       if the function succeeded.

+ * @retval MSG_RESET    if one or more errors occurred.

+ *

+ * @api

+ */

+#define accelerometerResetSensitivity(ip)                                   \

+        (ip)->vmt_baseaccelerometer->reset_sensitivity(ip)

 /** @} */

 

 /*===========================================================================*/

diff --git a/os/hal/lib/peripherals/sensors/hal_gyroscope.h b/os/hal/lib/peripherals/sensors/hal_gyroscope.h
index 1fb36e8d8..6e43929d4 100644
--- a/os/hal/lib/peripherals/sensors/hal_gyroscope.h
+++ b/os/hal/lib/peripherals/sensors/hal_gyroscope.h
@@ -158,7 +158,8 @@ typedef struct {
 /**

  * @brief   Updates gyroscope bias data from received buffer.

  * @note    The bias buffer must have the same length of the

- *          the gyroscope axes number.

+ *          the gyroscope axes number. Bias must be computed on

+ *          raw data and is a signed integer.

  *

  * @param[in] ip        pointer to a @p BaseGyroscope class.

  * @param[in] bp        pointer to a buffer of bias values.