Fixed l3gd20 and lsm6ds0 makefiles. Resumed development on lsm6ds0 gyro subsystem

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

4 files changed, 143 insertions, 8 deletions

diff --git a/os/ex/ST/l3gd20.mk b/os/ex/ST/l3gd20.mk
index cd2b79684..eb547a739 100644
--- a/os/ex/ST/l3gd20.mk
+++ b/os/ex/ST/l3gd20.mk
@@ -2,5 +2,5 @@
 L3GD20SRC := $(CHIBIOS)/os/ex/ST/l3gd20.c

 

 # Required include directories

-L3GD20INC := $(CHIBIOS)/os/hal/lib/peripherals/sensors/include \

+L3GD20INC := $(CHIBIOS)/os/hal/lib/peripherals/sensors \

              $(CHIBIOS)/os/ex/ST
\ No newline at end of file
diff --git a/os/ex/ST/lsm6ds0.c b/os/ex/ST/lsm6ds0.c
index 4aa7a7eb0..72e79b558 100644
--- a/os/ex/ST/lsm6ds0.c
+++ b/os/ex/ST/lsm6ds0.c
@@ -265,14 +265,15 @@ msg_t lsm6ds0I2CWriteRegister(I2CDriver *i2cp, lsm6ds0_sad_t sad, uint8_t reg,
  */
 static size_t acc_get_axes_number(void *ip) {
 
-  osalDbgCheck(ip != NULL);
+  osalDbgCheck((ip != NULL) &&
+              (((LSM6DS0Driver *)ip)->config->acccfg != NULL));
   return LSM6DS0_ACC_NUMBER_OF_AXES;
 }
 
 static msg_t acc_read_raw(void *ip, int32_t axes[LSM6DS0_ACC_NUMBER_OF_AXES]) {
 
-  osalDbgCheck((ip != NULL) && (axes != NULL));
-
+  osalDbgCheck(((ip != NULL) && (axes != NULL)) &&
+              (((LSM6DS0Driver *)ip)->config->acccfg != NULL));
   osalDbgAssert((((LSM6DS0Driver *)ip)->state == LSM6DS0_READY),
               "acc_read_raw(), invalid state");
 
@@ -320,10 +321,11 @@ static msg_t acc_read_cooked(void *ip, float axes[]) {
   int32_t raw[LSM6DS0_ACC_NUMBER_OF_AXES];
   msg_t msg;
 
-  osalDbgCheck((ip != NULL) && (axes != NULL));
+  osalDbgCheck(((ip != NULL) && (axes != NULL)) &&
+              (((LSM6DS0Driver *)ip)->config->acccfg != NULL));
 
   osalDbgAssert((((LSM6DS0Driver *)ip)->state == LSM6DS0_READY),
-              "read_cooked(), invalid state");
+              "acc_read_cooked(), invalid state");
 
   msg = acc_read_raw(ip, raw);
   for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES ; i++){
@@ -338,10 +340,123 @@ static msg_t acc_read_cooked(void *ip, float axes[]) {
   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]) {
+
+  osalDbgCheck(((ip != NULL) && (axes != NULL)) &&
+              (((LSM6DS0Driver *)ip)->config->gyrocfg != NULL));
+  osalDbgAssert((((LSM6DS0Driver *)ip)->state == LSM6DS0_READY),
+              "gyro_read_raw(), invalid state");
+
+#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 */
+  if(((LSM6DS0Driver *)ip)->config->gyrocfg->axesenabling & LSM6DS0_GYRO_AE_X){
+    axes[0] = (int16_t)(lsm6ds0I2CReadRegister(((LSM6DS0Driver *)ip)->config->i2cp,
+                                               ((LSM6DS0Driver *)ip)->config->slaveaddress,
+                                               LSM6DS0_AD_OUT_X_L_G, NULL));
+    axes[0] += (int16_t)(lsm6ds0I2CReadRegister(((LSM6DS0Driver *)ip)->config->i2cp,
+                                                ((LSM6DS0Driver *)ip)->config->slaveaddress,
+                                                LSM6DS0_AD_OUT_X_H_G, NULL) << 8);
+  }
+  if(((LSM6DS0Driver *)ip)->config->gyrocfg->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);
+  }
+  if(((LSM6DS0Driver *)ip)->config->gyrocfg->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);
+  }
+#if LSM6DS0_SHARED_I2C
+  i2cReleaseBus(((LSM6DS0Driver *)ip)->config->i2cp);
+#endif /* LSM6DS0_SHARED_I2C */
+#endif
+  return MSG_OK;
+}
+
+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) && (axes != NULL)) &&
+              (((LSM6DS0Driver *)ip)->config->gyrocfg != NULL));
+
+  osalDbgAssert((((LSM6DS0Driver *)ip)->state == LSM6DS0_READY),
+              "gyro_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;
+  }
+  return msg;
+}
+
+static msg_t gyro_reset_calibration(void *ip) {
+  uint32_t i;
+
+  osalDbgCheck(ip != NULL);
+
+  osalDbgAssert((((LSM6DS0Driver *)ip)->state == LSM6DS0_READY) ||
+                (((LSM6DS0Driver *)ip)->state == LSM6DS0_STOP),
+              "reset_calibration(), invalid state");
+
+  for(i = 0; i < LSM6DS0_GYRO_NUMBER_OF_AXES; i++)
+    ((LSM6DS0Driver *)ip)->gyrobias[i] = 0;
+  return MSG_OK;
+}
+
+static msg_t gyro_calibrate(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};
+
+  osalDbgCheck(ip != NULL);
+
+  osalDbgAssert((((LSM6DS0Driver *)ip)->state == LSM6DS0_READY),
+              "gyro_calibrate(), invalid state");
+
+  for(i = 0; i < LSM6DS0_GYRO_BIAS_ACQ_TIMES; i++){
+    gyro_read_raw(ip, raw);
+    for(j = 0; j < LSM6DS0_GYRO_NUMBER_OF_AXES; j++){
+      buff[j] += raw[j];
+    }
+    osalThreadSleepMicroseconds(LSM6DS0_GYRO_BIAS_SETTLING_uS);
+  }
+
+  for(i = 0; i < LSM6DS0_GYRO_NUMBER_OF_AXES; i++){
+    ((LSM6DS0Driver *)ip)->gyrobias[i] = buff[i] / LSM6DS0_GYRO_BIAS_ACQ_TIMES;
+  }
+  return MSG_OK;
+}
+
 static const struct LSM6DS0ACCVMT vmt_baseaccelerometer = {
   acc_get_axes_number, acc_read_raw, acc_read_cooked
 };
 
+static const struct LSM6DS0GYROVMT vmt_basegyroscope = {
+  gyro_get_axes_number, gyro_read_raw, gyro_read_cooked,
+  gyro_reset_calibration, gyro_calibrate
+};
 /*===========================================================================*/
 /* Driver exported functions.                                                */
 /*===========================================================================*/
@@ -356,6 +471,7 @@ static const struct LSM6DS0ACCVMT vmt_baseaccelerometer = {
 void lsm6ds0ObjectInit(LSM6DS0Driver *devp) {
 
   devp->vmt_baseaccelerometer = &vmt_baseaccelerometer;
+  devp->vmt_basegyroscope = &vmt_basegyroscope;
   devp->state = LSM6DS0_STOP;
   devp->config = NULL;
 }
diff --git a/os/ex/ST/lsm6ds0.h b/os/ex/ST/lsm6ds0.h
index 6adf58a94..06f41419c 100644
--- a/os/ex/ST/lsm6ds0.h
+++ b/os/ex/ST/lsm6ds0.h
@@ -74,6 +74,23 @@
 #if !defined(LSM6DS0_SHARED_I2C) || defined(__DOXYGEN__)
 #define LSM6DS0_SHARED_I2C                  FALSE
 #endif
+
+/**
+ * @brief   Number of acquisitions for gyroscope bias removal
+ * @details This is the number of acquisitions performed to compute the
+ *          bias. A repetition is required in order to remove noise.
+ */
+#if !defined(LSM6DS0_GYRO_BIAS_ACQ_TIMES) || defined(__DOXYGEN__)
+#define LSM6DS0_GYRO_BIAS_ACQ_TIMES         50
+#endif
+
+/**
+ * @brief   Settling time for gyroscope bias removal
+ * @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
+#endif
 /** @} */
 
 /*===========================================================================*/
@@ -518,7 +535,9 @@ struct LSM6DS0GYROVMT {
   /* Current accelerometer sensitivity.*/                                   \
   float                     accsens;                                        \
   /* Current gyroscope sensitivity.*/                                       \
-  float                     gyrosens;
+  float                     gyrosens;                                       \
+  /* Bias data.*/                                                           \
+  int32_t                   gyrobias[LSM6DS0_GYRO_NUMBER_OF_AXES];
   
 /**
  * @brief   LSM6DS0 6-axis accelerometer/gyroscope class.
diff --git a/os/ex/ST/lsm6ds0.mk b/os/ex/ST/lsm6ds0.mk
index 313301f0f..3acd24845 100644
--- a/os/ex/ST/lsm6ds0.mk
+++ b/os/ex/ST/lsm6ds0.mk
@@ -2,5 +2,5 @@
 LSM6DS0SRC := $(CHIBIOS)/os/ex/ST/lsm6ds0.c

 

 # Required include directories

-LSM6DS0INC := $(CHIBIOS)/os/hal/lib/peripherals/sensors/include \

+LSM6DS0INC := $(CHIBIOS)/os/hal/lib/peripherals/sensors \

               $(CHIBIOS)/os/ex/ST
\ No newline at end of file