diff options
| author | Rocco Marco Guglielmi <roccomarco.guglielmi@gmail.com> | 2018-03-12 21:37:27 +0000 |
|---|---|---|
| committer | Rocco Marco Guglielmi <roccomarco.guglielmi@gmail.com> | 2018-03-12 21:37:27 +0000 |
| commit | 9ac42f8cd7a7613827ddd78209d9ad02112714f2 (patch) | |
| tree | c0e388fe27f46747aa4449801ff5bec9859b3654 | |
| parent | 9dc196bbdc63a97e28062058001d0ea2b98951c5 (diff) | |
| download | ChibiOS-9ac42f8cd7a7613827ddd78209d9ad02112714f2.tar.gz ChibiOS-9ac42f8cd7a7613827ddd78209d9ad02112714f2.tar.bz2 ChibiOS-9ac42f8cd7a7613827ddd78209d9ad02112714f2.zip | |
Updated LSM6DS0 driver
git-svn-id: svn://svn.code.sf.net/p/chibios/svn/trunk@11710 110e8d01-0319-4d1e-a829-52ad28d1bb01
| -rw-r--r-- | os/ex/ST/lsm6ds0.c | 1261 | ||||
| -rw-r--r-- | os/ex/ST/lsm6ds0.h | 599 | ||||
| -rw-r--r-- | testex/STM32/STM32F4xx/I2C-LPS25H/main.c | 2 | ||||
| -rw-r--r-- | testex/STM32/STM32F4xx/I2C-LSM6DS0/Makefile | 32 | ||||
| -rw-r--r-- | testex/STM32/STM32F4xx/I2C-LSM6DS0/main.c | 277 |
5 files changed, 1227 insertions, 944 deletions
diff --git a/os/ex/ST/lsm6ds0.c b/os/ex/ST/lsm6ds0.c index 81b35d559..781e2236a 100644 --- a/os/ex/ST/lsm6ds0.c +++ b/os/ex/ST/lsm6ds0.c @@ -1,5 +1,5 @@ /*
- ChibiOS - Copyright (C) 2016 Rocco Marco Guglielmi
+ ChibiOS - Copyright (C) 2016-2018 Rocco Marco Guglielmi
This file is part of ChibiOS.
@@ -85,411 +85,648 @@ msg_t lsm6ds0I2CReadRegister(I2CDriver *i2cp, lsm6ds0_sad_t sad, uint8_t reg, TIME_INFINITE)
#endif /* LSM6DS0_USE_I2C */
-/*
- * Interface implementation.
+/**
+ * @brief Return the number of axes of the BaseAccelerometer.
+ *
+ * @param[in] ip pointer to @p BaseAccelerometer interface.
+ *
+ * @return the number of axes.
*/
static size_t acc_get_axes_number(void *ip) {
-
- osalDbgCheck(ip != NULL);
+ (void)ip;
+
return 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) {
+/**
+ * @brief Retrieves raw data from the BaseAccelerometer.
+ * @note This data is retrieved from MEMS register without any algebraical
+ * manipulation.
+ * @note The axes array must be at least the same size of the
+ * BaseAccelerometer axes number.
+ *
+ * @param[in] ip pointer to @p BaseAccelerometer interface.
+ * @param[out] axes a buffer which would be filled with raw data.
+ *
+ * @return The operation status.
+ * @retval MSG_OK if the function succeeded.
+ * @retval MSG_RESET if one or more I2C errors occurred, the errors can
+ * be retrieved using @p i2cGetErrors().
+ * @retval MSG_TIMEOUT if a timeout occurred before operation end.
+ */
+static msg_t acc_read_raw(void *ip, int32_t axes[]) {
+ LSM6DS0Driver* devp;
+ uint8_t buff [LSM6DS0_ACC_NUMBER_OF_AXES * 2], i;
int16_t tmp;
- uint8_t i, buff[2 * LSM6DS0_ACC_NUMBER_OF_AXES];
- msg_t msg = MSG_OK;
- osalDbgCheck(((ip != NULL) && (axes != NULL)) &&
- (((LSM6DS0Driver *)ip)->config->acccfg != NULL));
- osalDbgAssert((((LSM6DS0Driver *)ip)->state == LSM6DS0_READY),
- "acc_read_raw(), invalid state");
+ msg_t msg;
+ osalDbgCheck((ip != NULL) && (axes != NULL));
+
+ /* Getting parent instance pointer.*/
+ devp = objGetInstance(LSM6DS0Driver*, (BaseAccelerometer*)ip);
+
+ osalDbgAssert((devp->state == LSM6DS0_READY),
+ "acc_read_raw(), invalid state");
#if LSM6DS0_USE_I2C
- osalDbgAssert((((LSM6DS0Driver *)ip)->config->i2cp->state == I2C_READY),
+ osalDbgAssert((devp->config->i2cp->state == I2C_READY),
"acc_read_raw(), channel not ready");
+
#if LSM6DS0_SHARED_I2C
- i2cAcquireBus(((LSM6DS0Driver *)ip)->config->i2cp);
- i2cStart(((LSM6DS0Driver *)ip)->config->i2cp,
- ((LSM6DS0Driver *)ip)->config->i2ccfg);
+ i2cAcquireBus(devp->config->i2cp);
+ i2cStart(devp->config->i2cp,
+ devp->config->i2ccfg);
#endif /* LSM6DS0_SHARED_I2C */
- msg = lsm6ds0I2CReadRegister(((LSM6DS0Driver *)ip)->config->i2cp,
- ((LSM6DS0Driver *)ip)->config->slaveaddress,
- LSM6DS0_AD_OUT_X_L_XL, buff, 2 * LSM6DS0_ACC_NUMBER_OF_AXES);
+
+ msg = lsm6ds0I2CReadRegister(devp->config->i2cp, devp->config->slaveaddress,
+ LSM6DS0_AD_OUT_X_L_XL, buff,
+ LSM6DS0_ACC_NUMBER_OF_AXES * 2);
+
#if LSM6DS0_SHARED_I2C
- i2cReleaseBus(((LSM6DS0Driver *)ip)->config->i2cp);
+ i2cReleaseBus(devp->config->i2cp);
#endif /* LSM6DS0_SHARED_I2C */
#endif /* LSM6DS0_USE_I2C */
-
- for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES; i++) {
- if(msg == MSG_OK) {
- tmp = buff[2*i] + (buff[2*i+1] << 8);
+ if(msg == MSG_OK)
+ for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES; i++) {
+ tmp = buff[2 * i] + (buff[2 * i + 1] << 8);
axes[i] = (int32_t)tmp;
}
- else{
- axes[i] = 0;
- }
+ return msg;
+}
+
+/**
+ * @brief Retrieves cooked data from the BaseAccelerometer.
+ * @note This data is manipulated according to the formula
+ * cooked = (raw * sensitivity) - bias.
+ * @note Final data is expressed as milli-G.
+ * @note The axes array must be at least the same size of the
+ * BaseAccelerometer axes number.
+ *
+ * @param[in] ip pointer to @p BaseAccelerometer interface.
+ * @param[out] axes a buffer which would be filled with cooked data.
+ *
+ * @return The operation status.
+ * @retval MSG_OK if the function succeeded.
+ * @retval MSG_RESET if one or more I2C errors occurred, the errors can
+ * be retrieved using @p i2cGetErrors().
+ * @retval MSG_TIMEOUT if a timeout occurred before operation end.
+ */
+static msg_t acc_read_cooked(void *ip, float axes[]) {
+ LSM6DS0Driver* devp;
+ uint32_t i;
+ int32_t raw[LSM6DS0_ACC_NUMBER_OF_AXES];
+ msg_t msg;
+
+ osalDbgCheck((ip != NULL) && (axes != NULL));
+
+ /* Getting parent instance pointer.*/
+ devp = objGetInstance(LSM6DS0Driver*, (BaseAccelerometer*)ip);
+
+ osalDbgAssert((devp->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] * devp->accsensitivity[i]) - devp->accbias[i];
}
return msg;
}
-static msg_t gyro_read_raw(void *ip, int32_t* axes) {
- int16_t tmp;
- uint8_t i, buff[2 * LSM6DS0_GYRO_NUMBER_OF_AXES];
+/**
+ * @brief Set bias values for the BaseAccelerometer.
+ * @note Bias must be expressed as milli-G.
+ * @note The bias buffer must be at least the same size of the
+ * BaseAccelerometer axes number.
+ *
+ * @param[in] ip pointer to @p BaseAccelerometer interface.
+ * @param[in] bp a buffer which contains biases.
+ *
+ * @return The operation status.
+ * @retval MSG_OK if the function succeeded.
+ */
+static msg_t acc_set_bias(void *ip, float *bp) {
+ LSM6DS0Driver* devp;
+ uint32_t i;
msg_t msg = MSG_OK;
- 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 */
- msg = lsm6ds0I2CReadRegister(((LSM6DS0Driver *)ip)->config->i2cp,
- ((LSM6DS0Driver *)ip)->config->slaveaddress,
- LSM6DS0_AD_OUT_X_L_G, buff, 2 * LSM6DS0_GYRO_NUMBER_OF_AXES);
-#if LSM6DS0_SHARED_I2C
- i2cReleaseBus(((LSM6DS0Driver *)ip)->config->i2cp);
-#endif /* LSM6DS0_SHARED_I2C */
-#endif /* LSM6DS0_USE_I2C */
+ osalDbgCheck((ip != NULL) && (bp != NULL));
- for(i = 0; i < LSM6DS0_GYRO_NUMBER_OF_AXES; i++) {
- if(msg == MSG_OK) {
- tmp = buff[2*i] + (buff[2*i+1] << 8);
- axes[i] = (int32_t)tmp;
- }
- else{
- axes[i] = 0;
- }
+ /* Getting parent instance pointer.*/
+ devp = objGetInstance(LSM6DS0Driver*, (BaseAccelerometer*)ip);
+
+ osalDbgAssert((devp->state == LSM6DS0_READY),
+ "acc_set_bias(), invalid state");
+
+ for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES; i++) {
+ devp->accbias[i] = bp[i];
}
return msg;
}
-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);
- }
+/**
+ * @brief Reset bias values for the BaseAccelerometer.
+ * @note Default biases value are obtained from device datasheet when
+ * available otherwise they are considered zero.
+ *
+ * @param[in] ip pointer to @p BaseAccelerometer interface.
+ *
+ * @return The operation status.
+ * @retval MSG_OK if the function succeeded.
+ */
+static msg_t acc_reset_bias(void *ip) {
+ LSM6DS0Driver* devp;
+ uint32_t i;
+ msg_t msg = MSG_OK;
+
+ osalDbgCheck(ip != NULL);
+
+ /* Getting parent instance pointer.*/
+ devp = objGetInstance(LSM6DS0Driver*, (BaseAccelerometer*)ip);
+
+ osalDbgAssert((devp->state == LSM6DS0_READY),
+ "acc_reset_bias(), invalid state");
+
+ for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES; i++)
+ devp->accbias[i] = LSM6DS0_ACC_BIAS;
return msg;
}
-
-static msg_t acc_read_cooked(void *ip, float axes[]) {
+
+/**
+ * @brief Set sensitivity values for the BaseAccelerometer.
+ * @note Sensitivity must be expressed as milli-G/LSB.
+ * @note The sensitivity buffer must be at least the same size of the
+ * BaseAccelerometer axes number.
+ *
+ * @param[in] ip pointer to @p BaseAccelerometer interface.
+ * @param[in] sp a buffer which contains sensitivities.
+ *
+ * @return The operation status.
+ * @retval MSG_OK if the function succeeded.
+ */
+static msg_t acc_set_sensivity(void *ip, float *sp) {
+ LSM6DS0Driver* devp;
uint32_t i;
- int32_t raw[LSM6DS0_ACC_NUMBER_OF_AXES];
- msg_t msg;
+ msg_t msg = MSG_OK;
- osalDbgCheck(((ip != NULL) && (axes != NULL)) &&
- (((LSM6DS0Driver *)ip)->config->acccfg != NULL));
+ /* Getting parent instance pointer.*/
+ devp = objGetInstance(LSM6DS0Driver*, (BaseAccelerometer*)ip);
- osalDbgAssert((((LSM6DS0Driver *)ip)->state == LSM6DS0_READY),
- "acc_read_cooked(), invalid state");
+ osalDbgCheck((ip != NULL) && (sp != NULL));
- msg = acc_read_raw(ip, raw);
- for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES ; i++){
- axes[i] = raw[i] * ((LSM6DS0Driver *)ip)->accsensitivity[i];
- axes[i] -= ((LSM6DS0Driver *)ip)->accbias[i];
+ osalDbgAssert((devp->state == LSM6DS0_READY),
+ "acc_set_sensivity(), invalid state");
+
+ for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES; i++) {
+ devp->accsensitivity[i] = sp[i];
}
return msg;
}
-static msg_t gyro_read_cooked(void *ip, float axes[]) {
+/**
+ * @brief Reset sensitivity values for the BaseAccelerometer.
+ * @note Default sensitivities value are obtained from device datasheet.
+ *
+ * @param[in] ip pointer to @p BaseAccelerometer interface.
+ *
+ * @return The operation status.
+ * @retval MSG_OK if the function succeeded.
+ * @retval MSG_RESET otherwise.
+ */
+static msg_t acc_reset_sensivity(void *ip) {
+ LSM6DS0Driver* devp;
uint32_t i;
- int32_t raw[LSM6DS0_GYRO_NUMBER_OF_AXES];
- msg_t msg;
+ msg_t msg = MSG_OK;
- osalDbgCheck(((ip != NULL) && (axes != NULL)) &&
- (((LSM6DS0Driver *)ip)->config->gyrocfg != NULL));
+ osalDbgCheck(ip != NULL);
+
+ /* Getting parent instance pointer.*/
+ devp = objGetInstance(LSM6DS0Driver*, (BaseAccelerometer*)ip);
- osalDbgAssert((((LSM6DS0Driver *)ip)->state == LSM6DS0_READY),
- "gyro_read_cooked(), invalid state");
+ osalDbgAssert((devp->state == LSM6DS0_READY),
+ "acc_reset_sensivity(), invalid state");
- 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];
+ if(devp->config->accfullscale == LSM6DS0_ACC_FS_2G)
+ for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES; i++)
+ devp->accsensitivity[i] = LSM6DS0_ACC_SENS_2G;
+ else if(devp->config->accfullscale == LSM6DS0_ACC_FS_4G)
+ for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES; i++)
+ devp->accsensitivity[i] = LSM6DS0_ACC_SENS_4G;
+ else if(devp->config->accfullscale == LSM6DS0_ACC_FS_8G)
+ for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES; i++)
+ devp->accsensitivity[i] = LSM6DS0_ACC_SENS_8G;
+ else if(devp->config->accfullscale == LSM6DS0_ACC_FS_16G)
+ for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES; i++)
+ devp->accsensitivity[i] = LSM6DS0_ACC_SENS_16G;
+ else {
+ osalDbgAssert(FALSE, "reset_sensivity(), accelerometer full scale issue");
+ msg = MSG_RESET;
}
return msg;
}
-static msg_t sens_read_cooked(void *ip, float axes[]) {
- float* bp = axes;
+/**
+ * @brief Changes the LSM6DS0Driver accelerometer fullscale value.
+ * @note This function also rescale sensitivities and biases based on
+ * previous and next fullscale value.
+ * @note A recalibration is highly suggested after calling this function.
+ *
+ * @param[in] ip pointer to @p LSM6DS0Driver interface.
+ * @param[in] fs new fullscale value.
+ *
+ * @return The operation status.
+ * @retval MSG_OK if the function succeeded.
+ * @retval MSG_RESET otherwise.
+ */
+static msg_t acc_set_full_scale(LSM6DS0Driver *devp,
+ lsm6ds0_acc_fs_t fs) {
+ float newfs, scale;
+ uint8_t i, buff[2];
msg_t msg;
- if (((LSM6DS0Driver *)ip)->config->acccfg != NULL) {
- msg = acc_read_cooked(ip, bp);
- if(msg != MSG_OK)
+
+ osalDbgCheck(devp != NULL);
+
+ osalDbgAssert((devp->state == LSM6DS0_READY),
+ "acc_set_full_scale(), invalid state");
+ osalDbgAssert((devp->config->i2cp->state == I2C_READY),
+ "acc_set_full_scale(), channel not ready");
+
+ /* Computing new fullscale value.*/
+ 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 {
+ msg = MSG_RESET;
return msg;
- bp += LSM6DS0_ACC_NUMBER_OF_AXES;
}
- if (((LSM6DS0Driver *)ip)->config->gyrocfg != NULL) {
- msg = gyro_read_cooked(ip, bp);
+
+ if(newfs != devp->accfullscale) {
+ /* Computing scale value.*/
+ scale = newfs / devp->accfullscale;
+ devp->accfullscale = newfs;
+
+#if LSM6DS0_SHARED_I2C
+ i2cAcquireBus(devp->config->i2cp);
+ i2cStart(devp->config->i2cp,
+ devp->config->i2ccfg);
+#endif /* LSM6DS0_SHARED_I2C */
+
+ /* Updating register.*/
+ msg = lsm6ds0I2CReadRegister(devp->config->i2cp,
+ devp->config->slaveaddress,
+ LSM6DS0_AD_CTRL_REG6_XL, &buff[1], 1);
+
+#if LSM6DS0_SHARED_I2C
+ i2cReleaseBus(devp->config->i2cp);
+#endif /* LSM6DS0_SHARED_I2C */
+
+ if(msg != MSG_OK)
+ return msg;
+
+ buff[1] &= ~(LSM6DS0_CTRL_REG6_XL_FS_MASK);
+ buff[1] |= fs;
+ buff[0] = LSM6DS0_AD_CTRL_REG6_XL;
+
+#if LSM6DS0_SHARED_I2C
+ i2cAcquireBus(devp->config->i2cp);
+ i2cStart(devp->config->i2cp, devp->config->i2ccfg);
+#endif /* LSM6DS0_SHARED_I2C */
+
+ msg = lsm6ds0I2CWriteRegister(devp->config->i2cp,
+ devp->config->slaveaddress, buff, 1);
+
+#if LSM6DS0_SHARED_I2C
+ i2cReleaseBus(devp->config->i2cp);
+#endif /* LSM6DS0_SHARED_I2C */
+
+ 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++) {
+ devp->accsensitivity[i] *= scale;
+ devp->accbias[i] *= scale;
+ }
+ }
+ return msg;
+}
+
+/**
+ * @brief Return the number of axes of the BaseGyroscope.
+ *
+ * @param[in] ip pointer to @p BaseGyroscope interface.
+ *
+ * @return the number of axes.
+ */
+static size_t gyro_get_axes_number(void *ip) {
+ (void)ip;
+
+ return LSM6DS0_GYRO_NUMBER_OF_AXES;
+}
+
+/**
+ * @brief Retrieves raw data from the BaseGyroscope.
+ * @note This data is retrieved from MEMS register without any algebraical
+ * manipulation.
+ * @note The axes array must be at least the same size of the
+ * BaseGyroscope axes number.
+ *
+ * @param[in] ip pointer to @p BaseGyroscope interface.
+ * @param[out] axes a buffer which would be filled with raw data.
+ *
+ * @return The operation status.
+ * @retval MSG_OK if the function succeeded.
+ * @retval MSG_RESET if one or more I2C errors occurred, the errors can
+ * be retrieved using @p i2cGetErrors().
+ * @retval MSG_TIMEOUT if a timeout occurred before operation end.
+ */
+static msg_t gyro_read_raw(void *ip, int32_t axes[LSM6DS0_GYRO_NUMBER_OF_AXES]) {
+ LSM6DS0Driver* devp;
+ int16_t tmp;
+ uint8_t i, buff [2 * LSM6DS0_GYRO_NUMBER_OF_AXES];
+ msg_t msg = MSG_OK;
+
+ osalDbgCheck((ip != NULL) && (axes != NULL));
+
+ /* Getting parent instance pointer.*/
+ devp = objGetInstance(LSM6DS0Driver*, (BaseGyroscope*)ip);
+
+ osalDbgAssert((devp->state == LSM6DS0_READY),
+ "gyro_read_raw(), invalid state");
+#if LSM6DS0_USE_I2C
+ osalDbgAssert((devp->config->i2cp->state == I2C_READY),
+ "gyro_read_raw(), channel not ready");
+
+#if LSM6DS0_SHARED_I2C
+ i2cAcquireBus(devp->config->i2cp);
+ i2cStart(devp->config->i2cp,
+ devp->config->i2ccfg);
+#endif /* LSM6DS0_SHARED_I2C */
+
+ msg = lsm6ds0I2CReadRegister(devp->config->i2cp, devp->config->slaveaddress,
+ LSM6DS0_AD_OUT_X_L_G, buff,
+ LSM6DS0_GYRO_NUMBER_OF_AXES * 2);
+
+#if LSM6DS0_SHARED_I2C
+ i2cReleaseBus(devp->config->i2cp);
+#endif /* LSM6DS0_SHARED_I2C */
+#endif /* LSM6DS0_USE_I2C */
+
+ for(i = 0; i < LSM6DS0_GYRO_NUMBER_OF_AXES; i++) {
+ tmp = buff[2 * i] + (buff[2 * i + 1] << 8);
+ axes[i] = (int32_t)tmp;
+ }
+ return msg;
+}
+
+/**
+ * @brief Retrieves cooked data from the BaseGyroscope.
+ * @note This data is manipulated according to the formula
+ * cooked = (raw * sensitivity) - bias.
+ * @note Final data is expressed as DPS.
+ * @note The axes array must be at least the same size of the
+ * BaseGyroscope axes number.
+ *
+ * @param[in] ip pointer to @p BaseGyroscope interface.
+ * @param[out] axes a buffer which would be filled with cooked data.
+ *
+ * @return The operation status.
+ * @retval MSG_OK if the function succeeded.
+ * @retval MSG_RESET if one or more I2C errors occurred, the errors can
+ * be retrieved using @p i2cGetErrors().
+ * @retval MSG_TIMEOUT if a timeout occurred before operation end.
+ */
+static msg_t gyro_read_cooked(void *ip, float axes[]) {
+ LSM6DS0Driver* devp;
+ uint32_t i;
+ int32_t raw[LSM6DS0_GYRO_NUMBER_OF_AXES];
+ msg_t msg;
+
+ osalDbgCheck((ip != NULL) && (axes != NULL));
+
+ /* Getting parent instance pointer.*/
+ devp = objGetInstance(LSM6DS0Driver*, (BaseGyroscope*)ip);
+
+ osalDbgAssert((devp->state == LSM6DS0_READY),
+ "gyro_read_cooked(), invalid state");
+
+ msg = gyro_read_raw(ip, raw);
+ for(i = 0; i < LSM6DS0_GYRO_NUMBER_OF_AXES; i++){
+ axes[i] = (raw[i] * devp->gyrosensitivity[i]) - devp->gyrobias[i];
}
return msg;
}
+/**
+ * @brief Samples bias values for the BaseGyroscope.
+ * @note The LSM6DS0 shall not be moved during the whole procedure.
+ * @note After this function internal bias is automatically updated.
+ * @note The behavior of this function depends on @P LSM6DS0_BIAS_ACQ_TIMES
+ * and @p LSM6DS0_BIAS_SETTLING_US.
+ *
+ * @param[in] ip pointer to @p BaseGyroscope interface.
+ *
+ * @return The operation status.
+ * @retval MSG_OK if the function succeeded.
+ */
static msg_t gyro_sample_bias(void *ip) {
+ LSM6DS0Driver* devp;
uint32_t i, j;
int32_t raw[LSM6DS0_GYRO_NUMBER_OF_AXES];
- float buff[LSM6DS0_GYRO_NUMBER_OF_AXES] = {0.0f, 0.0f, 0.0f};
+ int32_t buff[LSM6DS0_GYRO_NUMBER_OF_AXES] = {0, 0, 0};
msg_t msg;
-
+
osalDbgCheck(ip != NULL);
- osalDbgAssert((((LSM6DS0Driver *)ip)->state == LSM6DS0_READY),
- "sample_bias(), invalid state");
+ /* Getting parent instance pointer.*/
+ devp = objGetInstance(LSM6DS0Driver*, (BaseGyroscope*)ip);
+
+ osalDbgAssert((devp->state == LSM6DS0_READY),
+ "gyro_sample_bias(), invalid state");
+#if LSM6DS0_USE_I2C
+ osalDbgAssert((devp->config->i2cp->state == I2C_READY),
+ "gyro_sample_bias(), channel not ready");
+#endif
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];
}
- osalThreadSleepMicroseconds(LSM6DS0_GYRO_BIAS_SETTLING_uS);
+ 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;
- ((LSM6DS0Driver *)ip)->gyrobias[i] *= ((LSM6DS0Driver *)ip)->gyrosensitivity[i];
+ devp->gyrobias[i] = (buff[i] / LSM6DS0_GYRO_BIAS_ACQ_TIMES);
+ devp->gyrobias[i] *= devp->gyrosensitivity[i];
}
return msg;
}
-static msg_t acc_set_bias(void *ip, float *bp) {
+/**
+ * @brief Set bias values for the BaseGyroscope.
+ * @note Bias must be expressed as DPS.
+ * @note The bias buffer must be at least the same size of the BaseGyroscope
+ * axes number.
+ *
+ * @param[in] ip pointer to @p BaseGyroscope interface.
+ * @param[in] bp a buffer which contains biases.
+ *
+ * @return The operation status.
+ * @retval MSG_OK if the function succeeded.
+ */
+static msg_t gyro_set_bias(void *ip, float *bp) {
+ LSM6DS0Driver* devp;
uint32_t i;
+ msg_t msg = MSG_OK;
- osalDbgCheck((ip != NULL) && (bp !=NULL));
-
- osalDbgAssert((((LSM6DS0Driver *)ip)->state == LSM6DS0_READY) ||
- (((LSM6DS0Driver *)ip)->state == LSM6DS0_STOP),
- "acc_set_bias(), invalid state");
+ osalDbgCheck((ip != NULL) && (bp != NULL));
- 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, float *bp) {
- uint32_t i;
+ /* Getting parent instance pointer.*/
+ devp = objGetInstance(LSM6DS0Driver*, (BaseGyroscope*)ip);
- osalDbgCheck((ip != NULL) && (bp !=NULL));
-
- osalDbgAssert((((LSM6DS0Driver *)ip)->state == LSM6DS0_READY) ||
- (((LSM6DS0Driver *)ip)->state == LSM6DS0_STOP),
+ osalDbgAssert((devp->state == LSM6DS0_READY),
"gyro_set_bias(), invalid state");
for(i = 0; i < LSM6DS0_GYRO_NUMBER_OF_AXES; i++) {
- ((LSM6DS0Driver *)ip)->gyrobias[i] = bp[i];
+ devp->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.0f;
- return MSG_OK;
+ return msg;
}
+/**
+ * @brief Reset bias values for the BaseGyroscope.
+ * @note Default biases value are obtained from device datasheet when
+ * available otherwise they are considered zero.
+ *
+ * @param[in] ip pointer to @p BaseGyroscope interface.
+ *
+ * @return The operation status.
+ * @retval MSG_OK if the function succeeded.
+ */
static msg_t gyro_reset_bias(void *ip) {
+ LSM6DS0Driver* devp;
uint32_t i;
+ msg_t msg = MSG_OK;
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.0f;
- 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");
+ /* Getting parent instance pointer.*/
+ devp = objGetInstance(LSM6DS0Driver*, (BaseGyroscope*)ip);
- for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES; i++) {
- ((LSM6DS0Driver *)ip)->accsensitivity[i] = sp[i];
- }
- return MSG_OK;
+ osalDbgAssert((devp->state == LSM6DS0_READY),
+ "gyro_reset_bias(), invalid state");
+
+ for(i = 0; i < LSM6DS0_GYRO_NUMBER_OF_AXES; i++)
+ devp->gyrobias[i] = LSM6DS0_GYRO_BIAS;
+ return msg;
}
+/**
+ * @brief Set sensitivity values for the BaseGyroscope.
+ * @note Sensitivity must be expressed as DPS/LSB.
+ * @note The sensitivity buffer must be at least the same size of the
+ * BaseGyroscope axes number.
+ *
+ * @param[in] ip pointer to @p BaseGyroscope interface.
+ * @param[in] sp a buffer which contains sensitivities.
+ *
+ * @return The operation status.
+ * @retval MSG_OK if the function succeeded.
+ */
static msg_t gyro_set_sensivity(void *ip, float *sp) {
+ LSM6DS0Driver* devp;
uint32_t i;
+ msg_t msg = MSG_OK;
osalDbgCheck((ip != NULL) && (sp !=NULL));
- osalDbgAssert((((LSM6DS0Driver *)ip)->state == LSM6DS0_READY),
+ /* Getting parent instance pointer.*/
+ devp = objGetInstance(LSM6DS0Driver*, (BaseGyroscope*)ip);
+
+ osalDbgAssert((devp->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;
- else {
- osalDbgAssert(FALSE, "acc_reset_sensivity(), accelerometer full scale issue");
- return MSG_RESET;
+ devp->gyrosensitivity[i] = sp[i];
}
- return MSG_OK;
+ return msg;
}
+/**
+ * @brief Reset sensitivity values for the BaseGyroscope.
+ * @note Default sensitivities value are obtained from device datasheet.
+ *
+ * @param[in] ip pointer to @p BaseGyroscope interface.
+ *
+ * @return The operation status.
+ * @retval MSG_OK if the function succeeded.
+ * @retval MSG_RESET otherwise.
+ */
static msg_t gyro_reset_sensivity(void *ip) {
+ LSM6DS0Driver* devp;
uint32_t i;
-
+ msg_t msg = MSG_OK;
+
osalDbgCheck(ip != NULL);
- osalDbgAssert((((LSM6DS0Driver *)ip)->state == LSM6DS0_READY),
+ /* Getting parent instance pointer.*/
+ devp = objGetInstance(LSM6DS0Driver*, (BaseGyroscope*)ip);
+
+ osalDbgAssert((devp->state == LSM6DS0_READY),
"gyro_reset_sensivity(), invalid state");
- 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_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_2000DPS)
- for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES; i++)
- ((LSM6DS0Driver *)ip)->gyrosensitivity[i] = LSM6DS0_GYRO_SENS_2000DPS;
+ if(devp->config->gyrofullscale == 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->gyrofullscale == 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->gyrofullscale == LSM6DS0_GYRO_FS_2000DPS)
+ for(i = 0; i < LSM6DS0_GYRO_NUMBER_OF_AXES; i++)
+ devp->gyrosensitivity[i] = LSM6DS0_GYRO_SENS_2000DPS;
else {
- osalDbgAssert(FALSE, "gyro_reset_sensivity(), gyroscope full scale issue");
+ osalDbgAssert(FALSE, "gyro_reset_sensivity(), full scale issue");
return MSG_RESET;
}
- return MSG_OK;
+ return msg;
}
-static msg_t acc_set_full_scale(void *ip, lsm6ds0_acc_fs_t fs) {
+/**
+ * @brief Changes the LSM6DS0Driver gyroscope fullscale value.
+ * @note This function also rescale sensitivities and biases based on
+ * previous and next fullscale value.
+ * @note A recalibration is highly suggested after calling this function.
+ *
+ * @param[in] ip pointer to @p BaseGyroscope interface.
+ * @param[in] fs new fullscale value.
+ *
+ * @return The operation status.
+ * @retval MSG_OK if the function succeeded.
+ * @retval MSG_RESET otherwise.
+ */
+static msg_t gyro_set_full_scale(LSM6DS0Driver *devp, lsm6ds0_gyro_fs_t fs) {
float newfs, scale;
- uint8_t i, cr[2];
- 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;
-
-#if LSM6DS0_SHARED_I2C
- i2cAcquireBus(((LSM6DS0Driver *)ip)->config->i2cp);
- i2cStart(((LSM6DS0Driver *)ip)->config->i2cp,
- ((LSM6DS0Driver *)ip)->config->i2ccfg);
-#endif /* LSM6DS0_SHARED_I2C */
-
- /* Updating register.*/
- msg = lsm6ds0I2CReadRegister(((LSM6DS0Driver *)ip)->config->i2cp,
- ((LSM6DS0Driver *)ip)->config->slaveaddress,
- LSM6DS0_AD_CTRL_REG6_XL, &cr[1], 1);
-#if LSM6DS0_SHARED_I2C
- i2cReleaseBus(((LSM6DS0Driver *)ip)->config->i2cp);
-#endif /* LSM6DS0_SHARED_I2C */
- if(msg != MSG_OK)
- return msg;
+ uint8_t i, buff[2];
+ msg_t msg = MSG_OK;
- cr[0] = LSM6DS0_AD_CTRL_REG6_XL;
- cr[1] &= ~(LSM6DS0_CTRL_REG6_XL_FS_MASK);
- cr[1] |= fs;
-
-#if LSM6DS0_SHARED_I2C
- i2cAcquireBus(((LSM6DS0Driver *)ip)->config->i2cp);
- i2cStart(((LSM6DS0Driver *)ip)->config->i2cp,
- ((LSM6DS0Driver *)ip)->config->i2ccfg);
-#endif /* LSM6DS0_SHARED_I2C */
-
- msg = lsm6ds0I2CWriteRegister(((LSM6DS0Driver *)ip)->config->i2cp,
- ((LSM6DS0Driver *)ip)->config->slaveaddress, cr, 1);
-#if LSM6DS0_SHARED_I2C
- i2cReleaseBus(((LSM6DS0Driver *)ip)->config->i2cp);
-#endif /* LSM6DS0_SHARED_I2C */
- 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;
-}
+ osalDbgCheck(devp != NULL);
-static msg_t gyro_set_full_scale(void *ip, lsm6ds0_gyro_fs_t fs) {
- float newfs, scale;
- uint8_t i, cr[2];
- msg_t msg;
+ osalDbgAssert((devp->state == LSM6DS0_READY),
+ "gyro_set_full_scale(), invalid state");
+#if LSM6DS0_USE_I2C
+ osalDbgAssert((devp->config->i2cp->state == I2C_READY),
+ "gyro_set_full_scale(), channel not ready");
+#endif
if(fs == LSM6DS0_GYRO_FS_245DPS) {
newfs = LSM6DS0_GYRO_245DPS;
@@ -504,70 +741,71 @@ static msg_t gyro_set_full_scale(void *ip, lsm6ds0_gyro_fs_t fs) { return MSG_RESET;
}
- if(newfs != ((LSM6DS0Driver *)ip)->gyrofullscale) {
- scale = newfs / ((LSM6DS0Driver *)ip)->gyrofullscale;
- ((LSM6DS0Driver *)ip)->gyrofullscale = newfs;
-
-#if LSM6DS0_SHARED_I2C
- i2cAcquireBus(((LSM6DS0Driver *)ip)->config->i2cp);
- i2cStart(((LSM6DS0Driver *)ip)->config->i2cp,
- ((LSM6DS0Driver *)ip)->config->i2ccfg);
-#endif /* LSM6DS0_SHARED_I2C */
+ if(newfs != devp->gyrofullscale) {
+ scale = newfs / devp->gyrofullscale;
+ devp->gyrofullscale = newfs;
+
+#if LSM6DS0_USE_I2C
+#if LSM6DS0_SHARED_I2C
+ i2cAcquireBus(devp->config->i2cp);
+ i2cStart(devp->config->i2cp,
+ devp->config->i2ccfg);
+#endif /* LSM6DS0_SHARED_I2C */
/* Updating register.*/
- msg = lsm6ds0I2CReadRegister(((LSM6DS0Driver *)ip)->config->i2cp,
- ((LSM6DS0Driver *)ip)->config->slaveaddress,
- LSM6DS0_AD_CTRL_REG1_G, &cr[1], 1);
+ msg = lsm6ds0I2CReadRegister(devp->config->i2cp,
+ devp->config->slaveaddress,
+ LSM6DS0_AD_CTRL_REG1_G, &buff[1], 1);
-#if LSM6DS0_SHARED_I2C
- i2cReleaseBus(((LSM6DS0Driver *)ip)->config->i2cp);
-#endif /* LSM6DS0_SHARED_I2C */
- if(msg != MSG_OK)
- return msg;
+#if LSM6DS0_SHARED_I2C
+ i2cReleaseBus(devp->config->i2cp);
+#endif /* LSM6DS0_SHARED_I2C */
+#endif /* LSM6DS0_USE_I2C */
- cr[0] = LSM6DS0_AD_CTRL_REG1_G;
- cr[1] &= ~(LSM6DS0_CTRL_REG1_G_FS_MASK);
- cr[1] |= fs;
-
-#if LSM6DS0_SHARED_I2C
- i2cAcquireBus(((LSM6DS0Driver *)ip)->config->i2cp);
- i2cStart(((LSM6DS0Driver *)ip)->config->i2cp,
- ((LSM6DS0Driver *)ip)->config->i2ccfg);
-#endif /* LSM6DS0_SHARED_I2C */
-
- msg = lsm6ds0I2CWriteRegister(((LSM6DS0Driver *)ip)->config->i2cp,
- ((LSM6DS0Driver *)ip)->config->slaveaddress,
- cr, 1);
+ buff[1] &= ~(LSM6DS0_CTRL_REG1_G_FS_MASK);
+ buff[1] |= fs;
+ buff[0] = LSM6DS0_AD_CTRL_REG1_G;
+
+#if LSM6DS0_USE_I2C
+#if LSM6DS0_SHARED_I2C
+ i2cAcquireBus(devp->config->i2cp);
+ i2cStart(devp->config->i2cp,
+ devp->config->i2ccfg);
+#endif /* LSM6DS0_SHARED_I2C */
+
+ lsm6ds0I2CWriteRegister(devp->config->i2cp, devp->config->slaveaddress,
+ buff, 1);
+
+#if LSM6DS0_SHARED_I2C
+ i2cReleaseBus(devp->config->i2cp);
+#endif /* LSM6DS0_SHARED_I2C */
+#endif /* LSM6DS0_USE_I2C */
-#if LSM6DS0_SHARED_I2C
- i2cReleaseBus(((LSM6DS0Driver *)ip)->config->i2cp);
-#endif /* LSM6DS0_SHARED_I2C */
- 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;
+ devp->gyrosensitivity[i] *= scale;
+ devp->gyrobias[i] *= scale;
}
- }
+ }
return msg;
}
-static const struct BaseSensorVMT vmt_sensor = {
- sens_get_axes_number, sens_read_raw, sens_read_cooked
+static const struct LSM6DS0VMT vmt_device = {
+ (size_t)0,
+ acc_set_full_scale, gyro_set_full_scale
};
-static const struct LSM6DS0AccelerometerVMT vmt_accelerometer = {
+static const struct BaseAccelerometerVMT vmt_accelerometer = {
+ sizeof(struct LSM6DS0VMT*),
acc_get_axes_number, acc_read_raw, acc_read_cooked,
- acc_set_bias, acc_reset_bias, acc_set_sensivity, acc_reset_sensivity,
- acc_set_full_scale
+ acc_set_bias, acc_reset_bias, acc_set_sensivity, acc_reset_sensivity
};
-static const struct LSM6DS0GyroscopeVMT vmt_gyroscope = {
+static const struct BaseGyroscopeVMT vmt_gyroscope = {
+ sizeof(struct LSM6DS0VMT*) + sizeof(BaseAccelerometer),
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, gyro_set_full_scale
+ gyro_set_sensivity, gyro_reset_sensivity
};
/*===========================================================================*/
@@ -582,16 +820,16 @@ static const struct LSM6DS0GyroscopeVMT vmt_gyroscope = { * @init
*/
void lsm6ds0ObjectInit(LSM6DS0Driver *devp) {
- uint32_t i;
- devp->vmt_sensor = &vmt_sensor;
- devp->vmt_accelerometer = &vmt_accelerometer;
- devp->vmt_gyroscope = &vmt_gyroscope;
+ devp->vmt = &vmt_device;
+ devp->acc_if.vmt = &vmt_accelerometer;
+ devp->gyro_if.vmt = &vmt_gyroscope;
+
devp->config = NULL;
- for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES; i++)
- devp->accbias[i] = 0.0f;
- for(i = 0; i < LSM6DS0_GYRO_NUMBER_OF_AXES; i++)
- devp->gyrobias[i] = 0.0f;
- devp->state = LSM6DS0_STOP;
+
+ devp->accaxes = LSM6DS0_ACC_NUMBER_OF_AXES;
+ devp->gyroaxes = LSM6DS0_GYRO_NUMBER_OF_AXES;
+
+ devp->state = LSM6DS0_STOP;
}
/**
@@ -605,15 +843,16 @@ void lsm6ds0ObjectInit(LSM6DS0Driver *devp) { void lsm6ds0Start(LSM6DS0Driver *devp, const LSM6DS0Config *config) {
uint32_t i;
uint8_t cr[5];
- osalDbgCheck((devp != NULL) && (config != NULL) &&
- ((devp->config->acccfg != NULL) ||
- (devp->config->gyrocfg != NULL)));
+ osalDbgCheck((devp != NULL) && (config != NULL));
- osalDbgAssert((devp->state == LSM6DS0_STOP) || (devp->state == LSM6DS0_READY),
- "lsm6ds0Start(), invalid state");
+ osalDbgAssert((devp->state == LSM6DS0_STOP) ||
+ (devp->state == LSM6DS0_READY),
+ "lsm6ds0Start(), invalid state");
devp->config = config;
+ /* Configuring common registers.*/
+
/* Control register 8 configuration block.*/
{
cr[0] = LSM6DS0_AD_CTRL_REG8;
@@ -624,194 +863,206 @@ void lsm6ds0Start(LSM6DS0Driver *devp, const LSM6DS0Config *config) { }
#if LSM6DS0_USE_I2C
#if LSM6DS0_SHARED_I2C
- i2cAcquireBus((devp)->config->i2cp);
+ i2cAcquireBus(devp->config->i2cp);
#endif /* LSM6DS0_SHARED_I2C */
- i2cStart((devp)->config->i2cp, (devp)->config->i2ccfg);
+ i2cStart(devp->config->i2cp, devp->config->i2ccfg);
lsm6ds0I2CWriteRegister(devp->config->i2cp, devp->config->slaveaddress,
cr, 1);
#if LSM6DS0_SHARED_I2C
- i2cReleaseBus((devp)->config->i2cp);
+ i2cReleaseBus(devp->config->i2cp);
#endif /* LSM6DS0_SHARED_I2C */
#endif /* LSM6DS0_USE_I2C */
- if((devp)->config->acccfg != NULL) {
-
- cr[0] = LSM6DS0_AD_CTRL_REG5_XL;
- /* Control register 5 configuration block.*/
- {
- cr[1] = LSM6DS0_CTRL_REG5_XL_XEN_XL | LSM6DS0_CTRL_REG5_XL_YEN_XL |
- LSM6DS0_CTRL_REG5_XL_ZEN_XL;
+ /* Configuring Accelerometer subsystem.*/
+ /* Multiple write starting address.*/
+ cr[0] = LSM6DS0_AD_CTRL_REG5_XL;
+ /* Control register 5 configuration block.*/
+ {
+ cr[1] = 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[1] |= devp->config->acccfg->decmode;
+ cr[1] |= devp->config->accdecmode;
#endif
- }
+ }
- /* Control register 6 configuration block.*/
- {
- cr[2] = devp->config->acccfg->outdatarate |
- devp->config->acccfg->fullscale;
- }
+ /* Control register 6 configuration block.*/
+ {
+ cr[2] = devp->config->accoutdatarate |
+ devp->config->accfullscale;
+ }
+
#if LSM6DS0_USE_I2C
#if LSM6DS0_SHARED_I2C
- i2cAcquireBus((devp)->config->i2cp);
- i2cStart((devp)->config->i2cp, (devp)->config->i2ccfg);
+ i2cAcquireBus(devp->config->i2cp);
+ i2cStart(devp->config->i2cp, devp->config->i2ccfg);
#endif /* LSM6DS0_SHARED_I2C */
- lsm6ds0I2CWriteRegister(devp->config->i2cp, devp->config->slaveaddress,
- cr, 2);
+ lsm6ds0I2CWriteRegister(devp->config->i2cp, devp->config->slaveaddress,
+ cr, 2);
#if LSM6DS0_SHARED_I2C
- i2cReleaseBus((devp)->config->i2cp);
+ i2cReleaseBus(devp->config->i2cp);
#endif /* LSM6DS0_SHARED_I2C */
#endif /* LSM6DS0_USE_I2C */
- }
- if((devp)->config->gyrocfg != NULL) {
+ /* Storing sensitivity according to user settings */
+ if(devp->config->accfullscale == LSM6DS0_ACC_FS_2G) {
+ for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES; i++) {
+ if(devp->config->accsensitivity == NULL)
+ devp->accsensitivity[i] = LSM6DS0_ACC_SENS_2G;
+ else
+ devp->accsensitivity[i] = devp->config->accsensitivity[i];
+ }
+ devp->accfullscale = LSM6DS0_ACC_2G;
+ }
+ else if(devp->config->accfullscale == LSM6DS0_ACC_FS_4G) {
+ for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES; i++) {
+ if(devp->config->accsensitivity == NULL)
+ devp->accsensitivity[i] = LSM6DS0_ACC_SENS_4G;
+ else
+ devp->accsensitivity[i] = devp->config->accsensitivity[i];
+ }
+ devp->accfullscale = LSM6DS0_ACC_4G;
+ }
+ else if(devp->config->accfullscale == LSM6DS0_ACC_FS_8G) {
+ for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES; i++) {
+ if(devp->config->accsensitivity == NULL)
+ devp->accsensitivity[i] = LSM6DS0_ACC_SENS_8G;
+ else
+ devp->accsensitivity[i] = devp->config->accsensitivity[i];
+ }
+ devp->accfullscale = LSM6DS0_ACC_8G;
+ }
+ else if(devp->config->accfullscale == LSM6DS0_ACC_FS_16G) {
+ for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES; i++) {
+ if(devp->config->accsensitivity == NULL)
+ devp->accsensitivity[i] = LSM6DS0_ACC_SENS_16G;
+ else
+ devp->accsensitivity[i] = devp->config->accsensitivity[i];
+ }
+ devp->accfullscale = LSM6DS0_ACC_16G;
+ }
+ else
+ osalDbgAssert(FALSE, "lsm6ds0Start(), accelerometer full scale issue");
- cr[0] = LSM6DS0_AD_CTRL_REG1_G;
+ /* Storing bias information */
+ if(devp->config->accbias != NULL)
+ for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES; i++)
+ devp->accbias[i] = devp->config->accbias[i];
+ else
+ for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES; i++)
+ devp->accbias[i] = LSM6DS0_ACC_BIAS;
+
+ /* Configuring Gyroscope subsystem.*/
+ /* Multiple write starting address.*/
+ cr[0] = LSM6DS0_AD_CTRL_REG1_G;
- /* Control register 1 configuration block.*/
- {
- cr[1] = devp->config->gyrocfg->fullscale |
- devp->config->gyrocfg->outdatarate;
+ /* Control register 1 configuration block.*/
+ {
+ cr[1] = devp->config->gyrofullscale |
+ devp->config->gyrooutdatarate;
#if LSM6DS0_GYRO_USE_ADVANCED || defined(__DOXYGEN__)
- cr[1] |= devp->config->acccfg->decmode;
+ cr[1] |= devp->config->gyrodecmode;
#endif
- }
+ }
- /* Control register 2 configuration block.*/
- {
- cr[2] = 0;
+ /* Control register 2 configuration block.*/
+ {
+ cr[2] = 0;
#if LSM6DS0_GYRO_USE_ADVANCED || defined(__DOXYGEN__)
- cr[2] |= devp->config->gyrocfg->outsel;
+ cr[2] |= devp->config->gyrooutsel;
#endif
- }
+ }
- /* Control register 3 configuration block.*/
- {
- cr[3] = 0;
+ /* Control register 3 configuration block.*/
+ {
+ cr[3] = 0;
#if LSM6DS0_GYRO_USE_ADVANCED || defined(__DOXYGEN__)
- cr[3] |= devp->config->gyrocfg->hpfenable |
- devp->config->gyrocfg->lowmodecfg |
- devp->config->gyrocfg->hpcfg;
+ cr[3] |= devp->config->gyrohpfenable |
+ devp->config->gyrolowmodecfg |
+ devp->config->gyrohpcfg;
#endif
- }
+ }
- /* Control register 4 configuration block.*/
- {
- cr[4] = LSM6DS0_CTRL_REG4_XEN_G | LSM6DS0_CTRL_REG4_YEN_G |
- LSM6DS0_CTRL_REG4_ZEN_G;
- }
+ /* Control register 4 configuration block.*/
+ {
+ cr[4] = LSM6DS0_CTRL_REG4_XEN_G | LSM6DS0_CTRL_REG4_YEN_G |
+ LSM6DS0_CTRL_REG4_ZEN_G;
+ }
#if LSM6DS0_USE_I2C
#if LSM6DS0_SHARED_I2C
- i2cAcquireBus((devp)->config->i2cp);
- i2cStart((devp)->config->i2cp, (devp)->config->i2ccfg);
+ i2cAcquireBus(devp->config->i2cp);
+ i2cStart(devp->config->i2cp, devp->config->i2ccfg);
#endif /* LSM6DS0_SHARED_I2C */
- lsm6ds0I2CWriteRegister(devp->config->i2cp, devp->config->slaveaddress,
- cr, 4);
+ lsm6ds0I2CWriteRegister(devp->config->i2cp, devp->config->slaveaddress,
+ cr, 4);
#if LSM6DS0_SHARED_I2C
- i2cReleaseBus((devp)->config->i2cp);
+ i2cReleaseBus(devp->config->i2cp);
#endif /* LSM6DS0_SHARED_I2C */
#endif /* LSM6DS0_USE_I2C */
- cr[0] = LSM6DS0_AD_CTRL_REG9;
- /* Control register 9 configuration block.*/
- {
- cr[1] = 0;
- }
+ cr[0] = LSM6DS0_AD_CTRL_REG9;
+ /* Control register 9 configuration block.*/
+ {
+ cr[1] = 0;
+ }
#if LSM6DS0_USE_I2C
#if LSM6DS0_SHARED_I2C
- i2cAcquireBus((devp)->config->i2cp);
- i2cStart((devp)->config->i2cp, (devp)->config->i2ccfg);
+ i2cAcquireBus(devp->config->i2cp);
+ i2cStart(devp->config->i2cp, devp->config->i2ccfg);
#endif /* LSM6DS0_SHARED_I2C */
- lsm6ds0I2CWriteRegister(devp->config->i2cp, devp->config->slaveaddress,
- cr, 1);
+ lsm6ds0I2CWriteRegister(devp->config->i2cp, devp->config->slaveaddress,
+ cr, 1);
#if LSM6DS0_SHARED_I2C
- i2cReleaseBus((devp)->config->i2cp);
+ i2cReleaseBus(devp->config->i2cp);
#endif /* LSM6DS0_SHARED_I2C */
#endif /* LSM6DS0_USE_I2C */
- }
- /* 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++) {
- if((devp)->config->acccfg->bias == NULL)
- devp->accsensitivity[i] = LSM6DS0_ACC_SENS_2G;
- else
- devp->accsensitivity[i] = devp->config->acccfg->sensitivity[i];
- }
- 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++) {
- if((devp)->config->acccfg->bias == NULL)
- devp->accsensitivity[i] = LSM6DS0_ACC_SENS_4G;
- else
- devp->accsensitivity[i] = devp->config->acccfg->sensitivity[i];
- }
- 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++) {
- if((devp)->config->acccfg->bias == NULL)
- devp->accsensitivity[i] = LSM6DS0_ACC_SENS_8G;
- else
- devp->accsensitivity[i] = devp->config->acccfg->sensitivity[i];
- }
- 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++) {
- if((devp)->config->acccfg->bias == NULL)
- devp->accsensitivity[i] = LSM6DS0_ACC_SENS_16G;
- else
- devp->accsensitivity[i] = devp->config->acccfg->sensitivity[i];
- }
- 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_245DPS) {
- for(i = 0; i < LSM6DS0_GYRO_NUMBER_OF_AXES; i++) {
- if((devp)->config->acccfg->bias == NULL)
- devp->gyrosensitivity[i] = LSM6DS0_GYRO_SENS_245DPS;
- else
- devp->gyrosensitivity[i] = devp->config->gyrocfg->sensitivity[i];
- }
- devp->gyrofullscale = LSM6DS0_GYRO_245DPS;
+ if(devp->config->gyrofullscale == LSM6DS0_GYRO_FS_245DPS) {
+ for(i = 0; i < LSM6DS0_GYRO_NUMBER_OF_AXES; i++) {
+ if(devp->config->gyrosensitivity == NULL)
+ devp->gyrosensitivity[i] = LSM6DS0_GYRO_SENS_245DPS;
+ else
+ devp->gyrosensitivity[i] = devp->config->gyrosensitivity[i];
}
- else if(devp->config->gyrocfg->fullscale == LSM6DS0_GYRO_FS_500DPS) {
- for(i = 0; i < LSM6DS0_GYRO_NUMBER_OF_AXES; i++) {
- if((devp)->config->acccfg->bias == NULL)
- devp->gyrosensitivity[i] = LSM6DS0_GYRO_SENS_500DPS;
- else
- devp->gyrosensitivity[i] = devp->config->gyrocfg->sensitivity[i];
- }
- devp->gyrofullscale = LSM6DS0_GYRO_500DPS;
+ devp->gyrofullscale = LSM6DS0_GYRO_245DPS;
+ }
+ else if(devp->config->gyrofullscale == LSM6DS0_GYRO_FS_500DPS) {
+ for(i = 0; i < LSM6DS0_GYRO_NUMBER_OF_AXES; i++) {
+ if(devp->config->gyrosensitivity == NULL)
+ devp->gyrosensitivity[i] = LSM6DS0_GYRO_SENS_500DPS;
+ else
+ devp->gyrosensitivity[i] = devp->config->gyrosensitivity[i];
}
- else if(devp->config->gyrocfg->fullscale == LSM6DS0_GYRO_FS_2000DPS) {
- for(i = 0; i < LSM6DS0_GYRO_NUMBER_OF_AXES; i++) {
- if((devp)->config->acccfg->bias == NULL)
- devp->gyrosensitivity[i] = LSM6DS0_GYRO_SENS_2000DPS;
- else
- devp->gyrosensitivity[i] = devp->config->gyrocfg->sensitivity[i];
- }
- devp->gyrofullscale = LSM6DS0_GYRO_2000DPS;
+ devp->gyrofullscale = LSM6DS0_GYRO_500DPS;
+ }
+ else if(devp->config->gyrofullscale == LSM6DS0_GYRO_FS_2000DPS) {
+ for(i = 0; i < LSM6DS0_GYRO_NUMBER_OF_AXES; i++) {
+ if(devp->config->gyrosensitivity == NULL)
+ devp->gyrosensitivity[i] = LSM6DS0_GYRO_SENS_2000DPS;
+ else
+ devp->gyrosensitivity[i] = devp->config->gyrosensitivity[i];
}
- else
- osalDbgAssert(FALSE, "lsm6ds0Start(), gyroscope full scale issue");
+ devp->gyrofullscale = LSM6DS0_GYRO_2000DPS;
}
- /* This is the Gyroscope transient recovery time */
+ else
+ osalDbgAssert(FALSE, "lsm6ds0Start(), gyroscope full scale issue");
+
+ /* Storing bias information */
+ if(devp->config->gyrobias != NULL)
+ for(i = 0; i < LSM6DS0_GYRO_NUMBER_OF_AXES; i++)
+ devp->gyrobias[i] = devp->config->gyrobias[i];
+ else
+ for(i = 0; i < LSM6DS0_GYRO_NUMBER_OF_AXES; i++)
+ devp->gyrobias[i] = LSM6DS0_GYRO_BIAS;
+
+ /* This is the MEMS transient recovery time */
osalThreadSleepMilliseconds(5);
devp->state = LSM6DS0_READY;
@@ -833,47 +1084,29 @@ void lsm6ds0Stop(LSM6DS0Driver *devp) { "lsm6ds0Stop(), invalid state");
if (devp->state == LSM6DS0_READY) {
- if((devp)->config->acccfg != NULL) {
- cr[0] = LSM6DS0_AD_CTRL_REG6_XL;
- /* Control register 6 configuration block.*/
- {
- cr[1] = 0;
- }
#if LSM6DS0_USE_I2C
#if LSM6DS0_SHARED_I2C
- i2cAcquireBus((devp)->config->i2cp);
- i2cStart((devp)->config->i2cp, (devp)->config->i2ccfg);
+ i2cAcquireBus(devp->config->i2cp);
+ i2cStart(devp->config->i2cp, devp->config->i2ccfg);
#endif /* LSM6DS0_SHARED_I2C */
-
+
+ /* Disabling accelerometer.*/
+ cr[0] = LSM6DS0_AD_CTRL_REG6_XL;
+ cr[1] = 0;
lsm6ds0I2CWriteRegister(devp->config->i2cp, devp->config->slaveaddress,
cr, 1);
-#if LSM6DS0_SHARED_I2C
- i2cReleaseBus((devp)->config->i2cp);
-#endif /* LSM6DS0_SHARED_I2C */
-#endif /* LSM6DS0_USE_I2C */
- }
- if((devp)->config->gyrocfg != NULL) {
+ /* Disabling gyroscope.*/
cr[0] = LSM6DS0_AD_CTRL_REG9;
- /* Control register 9 configuration block.*/
- {
- cr[1] = LSM6DS0_CTRL_REG9_SLEEP_G;
- }
-#if LSM6DS0_USE_I2C
-#if LSM6DS0_SHARED_I2C
- i2cAcquireBus((devp)->config->i2cp);
- i2cStart((devp)->config->i2cp, (devp)->config->i2ccfg);
-#endif /* LSM6DS0_SHARED_I2C */
-
+ cr[1] = LSM6DS0_CTRL_REG9_SLEEP_G;
lsm6ds0I2CWriteRegister(devp->config->i2cp, devp->config->slaveaddress,
cr, 1);
- i2cStop((devp)->config->i2cp);
+ i2cStop(devp->config->i2cp);
#if LSM6DS0_SHARED_I2C
- i2cReleaseBus((devp)->config->i2cp);
+ i2cReleaseBus(devp->config->i2cp);
#endif /* LSM6DS0_SHARED_I2C */
#endif /* LSM6DS0_USE_I2C */
- }
}
devp->state = LSM6DS0_STOP;
}
diff --git a/os/ex/ST/lsm6ds0.h b/os/ex/ST/lsm6ds0.h index 59abeb336..8dbc542d5 100644 --- a/os/ex/ST/lsm6ds0.h +++ b/os/ex/ST/lsm6ds0.h @@ -1,5 +1,5 @@ /*
- ChibiOS - Copyright (C) 2016 Rocco Marco Guglielmi
+ ChibiOS - Copyright (C) 2016-2018 Rocco Marco Guglielmi
This file is part of ChibiOS.
@@ -43,7 +43,7 @@ /**
* @brief LSM6DS0 driver version string.
*/
-#define EX_LSM6DS0_VERSION "1.0.4"
+#define EX_LSM6DS0_VERSION "1.0.5"
/**
* @brief LSM6DS0 driver version major number.
@@ -58,11 +58,14 @@ /**
* @brief LSM6DS0 driver version patch number.
*/
-#define EX_LSM6DS0_PATCH 4
+#define EX_LSM6DS0_PATCH 5
/** @} */
/**
* @brief LSM6DS0 accelerometer subsystem characteristics.
+ * @note Sensitivity is expressed as milli-G/LSB whereas
+ * 1 milli-G = 0.00980665 m/s^2.
+ * @note Bias is expressed as milli-G.
*
* @{
*/
@@ -77,10 +80,15 @@ #define LSM6DS0_ACC_SENS_4G 0.122f
#define LSM6DS0_ACC_SENS_8G 0.244f
#define LSM6DS0_ACC_SENS_16G 0.732f
+
+#define LSM6DS0_ACC_BIAS 0.0f
/** @} */
/**
- * @brief LSM6DS0 gyroscope subsystem characteristics.
+ * @brief L3GD20 gyroscope system characteristics.
+ * @note Sensitivity is expressed as DPS/LSB whereas DPS stand for Degree
+ * per second [°/s].
+ * @note Bias is expressed as DPS.
*
* @{
*/
@@ -93,6 +101,8 @@ #define LSM6DS0_GYRO_SENS_245DPS 0.00875f
#define LSM6DS0_GYRO_SENS_500DPS 0.01750f
#define LSM6DS0_GYRO_SENS_2000DPS 0.07000f
+
+#define LSM6DS0_GYRO_BIAS 0.0f
/** @} */
/**
@@ -287,11 +297,8 @@ #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. */
/*===========================================================================*/
@@ -301,7 +308,7 @@ * @{
*/
/**
- * @brief LSM6DS0 SPI interface selector.
+ * @brief LSM6DS0 SPI interface switch.
* @details If set to @p TRUE the support for SPI is included.
* @note The default is @p FALSE.
*/
@@ -310,7 +317,17 @@ #endif
/**
- * @brief LSM6DS0 I2C interface selector.
+ * @brief LSM6DS0 shared SPI switch.
+ * @details If set to @p TRUE the device acquires SPI bus ownership
+ * on each transaction.
+ * @note The default is @p FALSE. Requires SPI_USE_MUTUAL_EXCLUSION.
+ */
+#if !defined(LSM6DS0_SHARED_SPI) || defined(__DOXYGEN__)
+#define LSM6DS0_SHARED_SPI FALSE
+#endif
+
+/**
+ * @brief LSM6DS0 I2C interface switch.
* @details If set to @p TRUE the support for I2C is included.
* @note The default is @p TRUE.
*/
@@ -329,7 +346,7 @@ #endif
/**
- * @brief LSM6DS0 subsystem advanced configurations switch.
+ * @brief LSM6DS0 advanced configurations switch.
* @details If set to @p TRUE more configurations are available.
* @note The default is @p FALSE.
*/
@@ -370,8 +387,8 @@ * @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
+#if !defined(LSM6DS0_GYRO_BIAS_SETTLING_US) || defined(__DOXYGEN__)
+#define LSM6DS0_GYRO_BIAS_SETTLING_US 5000
#endif
/** @} */
@@ -379,20 +396,31 @@ /* Derived constants and error checks. */
/*===========================================================================*/
-#if LSM6DS0_USE_SPI && LSM6DS0_USE_I2C
-#error "LSM6DS0_USE_SPI and LSM6DS0_USE_I2C cannot be both true"
+#if !(LSM6DS0_USE_SPI ^ LSM6DS0_USE_I2C)
+#error "LSM6DS0_USE_SPI and LSM6DS0_USE_I2C cannot be both true or both false"
#endif
#if LSM6DS0_USE_SPI && !HAL_USE_SPI
#error "LSM6DS0_USE_SPI requires HAL_USE_SPI"
#endif
+#if LSM6DS0_SHARED_SPI && !SPI_USE_MUTUAL_EXCLUSION
+#error "LSM6DS0_SHARED_SPI requires SPI_USE_MUTUAL_EXCLUSION"
+#endif
+
#if LSM6DS0_USE_I2C && !HAL_USE_I2C
#error "LSM6DS0_USE_I2C requires HAL_USE_I2C"
#endif
#if LSM6DS0_SHARED_I2C && !I2C_USE_MUTUAL_EXCLUSION
-#error "LSM6DS0_SHARED_SPI requires I2C_USE_MUTUAL_EXCLUSION"
+#error "LSM6DS0_SHARED_I2C requires I2C_USE_MUTUAL_EXCLUSION"
+#endif
+
+/**
+ * @todo Add support for LSM6DS0 over SPI.
+ */
+#if LSM6DS0_USE_SPI
+#error "LSM6DS0 over SPI still not supported"
#endif
/*===========================================================================*/
@@ -400,10 +428,23 @@ /*===========================================================================*/
/**
- * @name LSM6DS0 accelerometer subsystem data structures and types.
+ * @name LSM6DS0 data structures and types.
* @{
*/
/**
+ * @brief Structure representing a LSM6DS0 driver.
+ */
+typedef struct LSM6DS0Driver LSM6DS0Driver;
+
+/**
+ * @brief Accelerometer and Gyroscope Slave Address.
+ */
+typedef enum {
+ LSM6DS0_SAD_GND = 0x6A, /**< SAD pin connected to GND. */
+ LSM6DS0_SAD_VCC = 0x6B /**< SAD pin connected to VCC. */
+} lsm6ds0_sad_t;
+
+/**
* @brief LSM6DS0 accelerometer subsystem full scale.
*/
typedef enum {
@@ -437,39 +478,6 @@ typedef enum { } lsm6ds0_acc_dec_t;
/**
- * @brief LSM6DS0 accelerometer subsystem configuration structure.
- */
-typedef struct {
- /**
- * @brief LSM6DS0 accelerometer initial sensitivity.
- */
- float* sensitivity;
- /**
- * @brief LSM6DS0 accelerometer initial bias.
- */
- float* bias;
- /**
- * @brief LSM6DS0 accelerometer subsystem full scale.
- */
- lsm6ds0_acc_fs_t fullscale;
- /**
- * @brief LSM6DS0 accelerometer subsystem output data rate.
- */
- lsm6ds0_acc_odr_t outdatarate;
-#if LSM6DS0_ACC_USE_ADVANCED || defined(__DOXYGEN__)
- /**
- * @brief LSM6DS0 accelerometer subsystem decimation mode.
- */
- lsm6ds0_acc_dec_t decmode;
-#endif /* LSM6DS0_ACC_USE_ADVANCED */
-} LSM6DS0AccConfig;
-/** @} */
-
-/**
- * @name LSM6DS0 gyroscope subsystem data structures and types.
- * @{
- */
-/**
* @brief LSM6DS0 gyroscope subsystem full scale.
*/
typedef enum {
@@ -544,59 +552,6 @@ typedef enum { } lsm6ds0_gyro_hpcf_t;
/**
- * @brief LSM6DS0 gyroscope subsystem configuration structure.
- */
-typedef struct {
- /**
- * @brief LSM6DS0 gyroscope initial sensitivity.
- */
- float* sensitivity;
- /**
- * @brief LSM6DS0 gyroscope initial bias.
- */
- float* bias;
- /**
- * @brief LSM6DS0 gyroscope subsystem full scale.
- */
- lsm6ds0_gyro_fs_t fullscale;
- /**
- * @brief LSM6DS0 gyroscope subsystem output data rate.
- */
- lsm6ds0_gyro_odr_t outdatarate;
-#if LSM6DS0_GYRO_USE_ADVANCED || defined(__DOXYGEN__)
- /**
- * @brief LSM6DS0 gyroscope subsystem low mode configuration.
- */
- lsm6ds0_gyro_lp_t lowmodecfg;
- /**
- * @brief LSM6DS0 gyroscope subsystem output selection.
- */
- lsm6ds0_gyro_out_sel_t outsel;
- /**
- * @brief LSM6DS0 gyroscope subsystem high pass filter.
- */
- 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.
- * @{
- */
-/**
- * @brief Accelerometer and Gyroscope Slave Address.
- */
-typedef enum {
- LSM6DS0_SAD_GND = 0x6A, /**< SAD pin connected to GND. */
- LSM6DS0_SAD_VCC = 0x6B /**< SAD pin connected to VCC. */
-} lsm6ds0_sad_t;
-
-/**
* @brief LSM6DS0 block data update.
*/
typedef enum {
@@ -635,11 +590,6 @@ typedef struct { * subsystem.
*/
const SPIConfig *accspicfg;
- /**
- * @brief SPI configuration associated to this LSM6DS0 compass
- * subsystem.
- */
- const SPIConfig *gyrospicfg;
#endif /* LSM6DS0_USE_SPI */
#if (LSM6DS0_USE_I2C) || defined(__DOXYGEN__)
/**
@@ -651,19 +601,67 @@ typedef struct { * subsystem.
*/
const I2CConfig *i2ccfg;
+ /**
+ * @brief LSM6DS0 Slave Address
+ */
+ lsm6ds0_sad_t slaveaddress;
#endif /* LSM6DS0_USE_I2C */
/**
- * @brief LSM6DS0 accelerometer subsystem configuration structure
+ * @brief LSM6DS0 accelerometer subsystem initial sensitivity.
*/
- const LSM6DS0AccConfig *acccfg;
+ float *accsensitivity;
/**
- * @brief LSM6DS0 gyroscope subsystem configuration structure
+ * @brief LSM6DS0 accelerometer subsystem initial bias.
*/
- const LSM6DS0GyroConfig *gyrocfg;
+ float *accbias;
/**
- * @brief Accelerometer and Gyroscope Slave Address
+ * @brief LSM6DS0 accelerometer subsystem full scale.
*/
- lsm6ds0_sad_t slaveaddress;
+ lsm6ds0_acc_fs_t accfullscale;
+ /**
+ * @brief LSM6DS0 accelerometer subsystem output data rate.
+ */
+ lsm6ds0_acc_odr_t accoutdatarate;
+#if LSM6DS0_ACC_USE_ADVANCED || defined(__DOXYGEN__)
+ /**
+ * @brief LSM6DS0 accelerometer subsystem decimation mode.
+ */
+ lsm6ds0_acc_dec_t accdecmode;
+#endif /* LSM6DS0_ACC_USE_ADVANCED */
+ /**
+ * @brief LSM6DS0 gyroscope subsystem initial sensitivity.
+ */
+ float *gyrosensitivity;
+ /**
+ * @brief LSM6DS0 gyroscope subsystem initial bias.
+ */
+ float *gyrobias;
+ /**
+ * @brief LSM6DS0 gyroscope subsystem full scale.
+ */
+ lsm6ds0_gyro_fs_t gyrofullscale;
+ /**
+ * @brief LSM6DS0 gyroscope subsystem output data rate.
+ */
+ lsm6ds0_gyro_odr_t gyrooutdatarate;
+#if LSM6DS0_GYRO_USE_ADVANCED || defined(__DOXYGEN__)
+ /**
+ * @brief LSM6DS0 gyroscope subsystem low mode configuration.
+ */
+ lsm6ds0_gyro_lp_t gyrolowmodecfg;
+ /**
+ * @brief LSM6DS0 gyroscope subsystem output selection.
+ */
+ lsm6ds0_gyro_out_sel_t gyrooutsel;
+ /**
+ * @brief LSM6DS0 gyroscope subsystem high pass filter.
+ */
+ lsm6ds0_gyro_hp_t gyrohpfenable;
+ /**
+ * @brief LSM6DS0 gyroscope subsystem high pass filter configuration.
+ */
+ lsm6ds0_gyro_hpcf_t gyrohpcfg;
+ #endif /* LSM6DS0_GYRO_USE_ADVANCED */
#if (LSM6DS0_USE_ADVANCED) || defined(__DOXYGEN__)
/**
* @brief LSM6DS0 block data update
@@ -677,95 +675,68 @@ typedef struct { } LSM6DS0Config;
/**
- * @brief @p LSM6DS0 accelerometer subsystem specific methods.
+ * @brief @p LSM6DS0 specific methods.
*/
-#define _lsm6ds0_accelerometer_methods_alone \
+#define _lsm6ds0_methods_alone \
/* Change full scale value of LSM6DS0 accelerometer subsystem .*/ \
- msg_t (*set_full_scale)(void *instance, lsm6ds0_acc_fs_t fs);
-
-
-/**
- * @brief @p LSM6DS0 accelerometer subsystem specific methods with inherited
- * ones.
- */
-#define _lsm6ds0_accelerometer_methods \
- _base_accelerometer_methods \
- _lsm6ds0_accelerometer_methods_alone
-
-/**
- * @brief @p LSM6DS0 gyroscope subsystem specific methods.
- */
-#define _lsm6ds0_gyroscope_methods_alone \
+ msg_t (*acc_set_full_scale)(LSM6DS0Driver *devp, lsm6ds0_acc_fs_t fs); \
/* Change full scale value of LSM6DS0 gyroscope subsystem .*/ \
- msg_t (*set_full_scale)(void *instance, lsm6ds0_gyro_fs_t fs);
-
+ msg_t (*gyro_set_full_scale)(LSM6DS0Driver *devp, lsm6ds0_gyro_fs_t fs);
/**
- * @brief @p LSM6DS0 gyroscope subsystem specific methods with inherited ones.
+ * @brief @p LSM6DS0 specific methods with inherited ones.
*/
-#define _lsm6ds0_gyroscope_methods \
- _base_gyroscope_methods \
- _lsm6ds0_gyroscope_methods_alone
-
-/**
- * @extends BaseAccelerometerVMT
- *
- * @brief @p LSM6DS0 accelerometer virtual methods table.
- */
-struct LSM6DS0AccelerometerVMT {
- _lsm6ds0_accelerometer_methods
-};
+#define _lsm6ds0_methods \
+ _base_object_methods \
+ _lsm6ds0_methods_alone
/**
- * @extends BaseCompassVMT
+ * @extends BaseObjectVMT
*
- * @brief @p LSM6DS0 gyroscope virtual methods table.
+ * @brief @p LSM6DS0 virtual methods table.
*/
-struct LSM6DS0GyroscopeVMT {
- _lsm6ds0_gyroscope_methods
+struct LSM6DS0VMT {
+ _lsm6ds0_methods
};
/**
* @brief @p LSM6DS0Driver specific data.
*/
#define _lsm6ds0_data \
+ _base_sensor_data \
/* Driver state.*/ \
lsm6ds0_state_t state; \
/* Current configuration data.*/ \
const LSM6DS0Config *config; \
- /* Current accelerometer sensitivity.*/ \
+ /* Accelerometer subsystem axes number.*/ \
+ size_t accaxes; \
+ /* Accelerometer subsystem current sensitivity.*/ \
float accsensitivity[LSM6DS0_ACC_NUMBER_OF_AXES]; \
- /* Accelerometer bias data.*/ \
+ /* Accelerometer subsystem current bias .*/ \
float accbias[LSM6DS0_ACC_NUMBER_OF_AXES]; \
- /* Current accelerometer full scale value.*/ \
+ /* Accelerometer subsystem current full scale value.*/ \
float accfullscale; \
- /* Current gyroscope sensitivity.*/ \
+ /* Gyroscope subsystem axes number.*/ \
+ size_t gyroaxes; \
+ /* Gyroscope subsystem current sensitivity.*/ \
float gyrosensitivity[LSM6DS0_GYRO_NUMBER_OF_AXES]; \
- /* Bias data.*/ \
+ /* Gyroscope subsystem current Bias.*/ \
float gyrobias[LSM6DS0_GYRO_NUMBER_OF_AXES]; \
- /* Current gyroscope full scale value.*/ \
+ /* Gyroscope subsystem current full scale value.*/ \
float gyrofullscale;
/**
* @brief LSM6DS0 6-axis accelerometer/gyroscope class.
*/
struct LSM6DS0Driver {
- /** @brief BaseSensor Virtual Methods Table. */
- const struct BaseSensorVMT *vmt_sensor;
- _base_sensor_data
- /** @brief LSM6DS0 Accelerometer Virtual Methods Table. */
- const struct LSM6DS0AccelerometerVMT *vmt_accelerometer;
- _base_accelerometer_data
- /** @brief LSM6DS0 Gyroscope Virtual Methods Table. */
- const struct LSM6DS0GyroscopeVMT *vmt_gyroscope;
- _base_gyroscope_data
+ /** @brief Virtual Methods Table.*/
+ const struct LSM6DS0VMT *vmt;
+ /** @brief Base accelerometer interface.*/
+ BaseAccelerometer acc_if;
+ /** @brief Base gyroscope interface.*/
+ BaseGyroscope gyro_if;
_lsm6ds0_data
};
-
-/**
- * @brief Structure representing a LSM6DS0 driver.
- */
-typedef struct LSM6DS0Driver LSM6DS0Driver;
/** @} */
/*===========================================================================*/
@@ -773,32 +744,298 @@ typedef struct LSM6DS0Driver LSM6DS0Driver; /*===========================================================================*/
/**
- * @brief Change accelerometer fullscale value.
+ * @brief Return the number of axes of the BaseAccelerometer.
+ *
+ * @param[in] devp pointer to @p LSM6DS0Driver.
+ *
+ * @return the number of axes.
+ *
+ * @api
+ */
+#define lsm6ds0AccelerometerGetAxesNumber(devp) \
+ accelerometerGetAxesNumber(&((devp)->acc_if))
+
+/**
+ * @brief Retrieves raw data from the BaseAccelerometer.
+ * @note This data is retrieved from MEMS register without any algebraical
+ * manipulation.
+ * @note The axes array must be at least the same size of the
+ * BaseAccelerometer axes number.
+ *
+ * @param[in] devp pointer to @p LSM6DS0Driver.
+ * @param[out] axes a buffer which would be filled with raw data.
+ *
+ * @return The operation status.
+ * @retval MSG_OK if the function succeeded.
+ * @retval MSG_RESET if one or more I2C errors occurred, the errors can
+ * be retrieved using @p i2cGetErrors().
+ * @retval MSG_TIMEOUT if a timeout occurred before operation end.
+ *
+ * @api
+ */
+#define lsm6ds0AccelerometerReadRaw(devp, axes) \
+ accelerometerReadRaw(&((devp)->acc_if), axes)
+
+/**
+ * @brief Retrieves cooked data from the BaseAccelerometer.
+ * @note This data is manipulated according to the formula
+ * cooked = (raw * sensitivity) - bias.
+ * @note Final data is expressed as milli-G.
+ * @note The axes array must be at least the same size of the
+ * BaseAccelerometer axes number.
+ *
+ * @param[in] devp pointer to @p LSM6DS0Driver.
+ * @param[out] axes a buffer which would be filled with cooked data.
+ *
+ * @return The operation status.
+ * @retval MSG_OK if the function succeeded.
+ * @retval MSG_RESET if one or more I2C errors occurred, the errors can
+ * be retrieved using @p i2cGetErrors().
+ * @retval MSG_TIMEOUT if a timeout occurred before operation end.
+ *
+ * @api
+ */
+#define lsm6ds0AccelerometerReadCooked(devp, axes) \
+ accelerometerReadCooked(&((devp)->acc_if), axes)
+
+/**
+ * @brief Set bias values for the BaseAccelerometer.
+ * @note Bias must be expressed as milli-G.
+ * @note The bias buffer must be at least the same size of the
+ * BaseAccelerometer axes number.
+ *
+ * @param[in] devp pointer to @p LSM6DS0Driver.
+ * @param[in] bp a buffer which contains biases.
+ *
+ * @return The operation status.
+ * @retval MSG_OK if the function succeeded.
+ *
+ * @api
+ */
+#define lsm6ds0AccelerometerSetBias(devp, bp) \
+ accelerometerSetBias(&((devp)->acc_if), bp)
+
+/**
+ * @brief Reset bias values for the BaseAccelerometer.
+ * @note Default biases value are obtained from device datasheet when
+ * available otherwise they are considered zero.
+ *
+ * @param[in] devp pointer to @p LSM6DS0Driver.
+ *
+ * @return The operation status.
+ * @retval MSG_OK if the function succeeded.
+ *
+ * @api
+ */
+#define lsm6ds0AccelerometerResetBias(devp) \
+ accelerometerResetBias(&((devp)->acc_if))
+
+/**
+ * @brief Set sensitivity values for the BaseAccelerometer.
+ * @note Sensitivity must be expressed as milli-G/LSB.
+ * @note The sensitivity buffer must be at least the same size of the
+ * BaseAccelerometer axes number.
*
- * @param[in] ip pointer to a @p LSM6DS0Driver class.
- * @param[in] fs the new full scale value.
+ * @param[in] devp pointer to @p LSM6DS0Driver.
+ * @param[in] sp a buffer which contains sensitivities.
*
* @return The operation status.
* @retval MSG_OK if the function succeeded.
- * @retval MSG_RESET if one or more errors occurred.
+ *
* @api
*/
-#define accelerometerSetFullScale(ip, fs) \
- (ip)->vmt_accelerometer->set_full_scale(ip, fs)
+#define lsm6ds0AccelerometerSetSensitivity(devp, sp) \
+ accelerometerSetSensitivity(&((devp)->acc_if), sp)
/**
- * @brief Change compass fullscale value.
+ * @brief Reset sensitivity values for the BaseAccelerometer.
+ * @note Default sensitivities value are obtained from device datasheet.
*
- * @param[in] ip pointer to a @p LSM6DS0Driver class.
- * @param[in] fs the new full scale value.
+ * @param[in] devp pointer to @p LSM6DS0Driver.
*
* @return The operation status.
* @retval MSG_OK if the function succeeded.
- * @retval MSG_RESET if one or more errors occurred.
+ * @retval MSG_RESET otherwise.
+ *
+ * @api
+ */
+#define lsm6ds0AccelerometerResetSensitivity(devp) \
+ accelerometerResetSensitivity(&((devp)->acc_if))
+
+/**
+ * @brief Changes the LSM6DS0Driver accelerometer fullscale value.
+ * @note This function also rescale sensitivities and biases based on
+ * previous and next fullscale value.
+ * @note A recalibration is highly suggested after calling this function.
+ *
+ * @param[in] devp pointer to @p LSM6DS0Driver.
+ * @param[in] fs new fullscale value.
+ *
+ * @return The operation status.
+ * @retval MSG_OK if the function succeeded.
+ * @retval MSG_RESET otherwise.
+ *
+ * @api
+ */
+#define lsm6ds0AccelerometerSetFullScale(devp, fs) \
+ (devp)->vmt->acc_set_full_scale(devp, fs)
+
+/**
+ * @brief Return the number of axes of the BaseGyroscope.
+ *
+ * @param[in] devp pointer to @p LSM6DS0Driver.
+ *
+ * @return the number of axes.
+ *
+ * @api
+ */
+#define lsm6ds0GyroscopeGetAxesNumber(devp) \
+ gyroscopeGetAxesNumber(&((devp)->gyro_if))
+
+/**
+ * @brief Retrieves raw data from the BaseGyroscope.
+ * @note This data is retrieved from MEMS register without any algebraical
+ * manipulation.
+ * @note The axes array must be at least the same size of the
+ * BaseGyroscope axes number.
+ *
+ * @param[in] devp pointer to @p LSM6DS0Driver.
+ * @param[out] axes a buffer which would be filled with raw data.
+ *
+ * @return The operation status.
+ * @retval MSG_OK if the function succeeded.
+ * @retval MSG_RESET if one or more I2C errors occurred, the errors can
+ * be retrieved using @p i2cGetErrors().
+ * @retval MSG_TIMEOUT if a timeout occurred before operation end.
+ *
+ * @api
+ */
+#define lsm6ds0GyroscopeReadRaw(devp, axes) \
+ gyroscopeReadRaw(&((devp)->gyro_if), axes)
+
+/**
+ * @brief Retrieves cooked data from the BaseGyroscope.
+ * @note This data is manipulated according to the formula
+ * cooked = (raw * sensitivity) - bias.
+ * @note Final data is expressed as DPS.
+ * @note The axes array must be at least the same size of the
+ * BaseGyroscope axes number.
+ *
+ * @param[in] devp pointer to @p LSM6DS0Driver.
+ * @param[out] axes a buffer which would be filled with cooked data.
+ *
+ * @return The operation status.
+ * @retval MSG_OK if the function succeeded.
+ * @retval MSG_RESET if one or more I2C errors occurred, the errors can
+ * be retrieved using @p i2cGetErrors().
+ * @retval MSG_TIMEOUT if a timeout occurred before operation end.
+ *
+ * @api
+ */
+#define lsm6ds0GyroscopeReadCooked(devp, axes) \
+ gyroscopeReadCooked(&((devp)->gyro_if), axes)
+
+/**
+ * @brief Samples bias values for the BaseGyroscope.
+ * @note The LSM6DS0 shall not be moved during the whole procedure.
+ * @note After this function internal bias is automatically updated.
+ * @note The behavior of this function depends on @P LSM6DS0_BIAS_ACQ_TIMES
+ * and @p LSM6DS0_BIAS_SETTLING_US.
+ *
+ * @param[in] devp pointer to @p LSM6DS0Driver.
+ *
+ * @return The operation status.
+ * @retval MSG_OK if the function succeeded.
+ * @retval MSG_RESET if one or more I2C errors occurred, the errors can
+ * be retrieved using @p i2cGetErrors().
+ * @retval MSG_TIMEOUT if a timeout occurred before operation end.
+ *
+ * @api
+ */
+#define lsm6ds0GyroscopeSampleBias(devp) \
+ gyroscopeSampleBias(&((devp)->gyro_if))
+
+/**
+ * @brief Set bias values for the BaseGyroscope.
+ * @note Bias must be expressed as DPS.
+ * @note The bias buffer must be at least the same size of the BaseGyroscope
+ * axes number.
+ *
+ * @param[in] devp pointer to @p LSM6DS0Driver.
+ * @param[in] bp a buffer which contains biases.
+ *
+ * @return The operation status.
+ * @retval MSG_OK if the function succeeded.
+ *
+ * @api
+ */
+#define lsm6ds0GyroscopeSetBias(devp, bp) \
+ gyroscopeSetBias(&((devp)->gyro_if), bp)
+
+/**
+ * @brief Reset bias values for the BaseGyroscope.
+ * @note Default biases value are obtained from device datasheet when
+ * available otherwise they are considered zero.
+ *
+ * @param[in] devp pointer to @p LSM6DS0Driver.
+ *
+ * @return The operation status.
+ * @retval MSG_OK if the function succeeded.
+ *
+ * @api
+ */
+#define lsm6ds0GyroscopeResetBias(devp) \
+ gyroscopeResetBias(&((devp)->gyro_if))
+
+/**
+ * @brief Set sensitivity values for the BaseGyroscope.
+ * @note Sensitivity must be expressed as DPS/LSB.
+ * @note The sensitivity buffer must be at least the same size of the
+ * BaseGyroscope axes number.
+ *
+ * @param[in] devp pointer to @p LSM6DS0Driver.
+ * @param[in] sp a buffer which contains sensitivities.
+ *
+ * @return The operation status.
+ * @retval MSG_OK if the function succeeded.
+ *
+ * @api
+ */
+#define lsm6ds0GyroscopeSetSensitivity(devp, sp) \
+ gyroscopeSetSensitivity(&((devp)->gyro_if), sp)
+
+/**
+ * @brief Reset sensitivity values for the BaseGyroscope.
+ * @note Default sensitivities value are obtained from device datasheet.
+ *
+ * @param[in] devp pointer to @p LSM6DS0Driver.
+ *
+ * @return The operation status.
+ * @retval MSG_OK if the function succeeded.
+ * @retval MSG_RESET otherwise.
+ *
+ * @api
+ */
+#define lsm6ds0GyroscopeResetSensitivity(devp) \
+ gyroscopeResetSensitivity(&((devp)->gyro_if))
+
+/**
+ * @brief Changes the LSM6DS0Driver gyroscope fullscale value.
+ * @note This function also rescale sensitivities and biases based on
+ * previous and next fullscale value.
+ * @note A recalibration is highly suggested after calling this function.
+ *
+ * @param[in] devp pointer to @p LSM6DS0Driver.
+ * @param[in] fs new fullscale value.
+ *
+ * @return The operation status.
+ * @retval MSG_OK if the function succeeded.
+ * @retval MSG_RESET otherwise.
+ *
* @api
*/
-#define gyroscopeSetFullScale(ip, fs) \
- (ip)->vmt_gyroscope->set_full_scale(ip, fs)
+#define lsm6ds0GyroscopeSetFullScale(devp, fs) \
+ (devp)->vmt->acc_set_full_scale(devp, fs)
/*===========================================================================*/
/* External declarations. */
diff --git a/testex/STM32/STM32F4xx/I2C-LPS25H/main.c b/testex/STM32/STM32F4xx/I2C-LPS25H/main.c index 640a82c91..014af067e 100644 --- a/testex/STM32/STM32F4xx/I2C-LPS25H/main.c +++ b/testex/STM32/STM32F4xx/I2C-LPS25H/main.c @@ -97,7 +97,7 @@ int main(void) { palSetLineMode(LINE_ARD_D14, PAL_MODE_ALTERNATE(4) |
PAL_STM32_OSPEED_HIGHEST | PAL_STM32_OTYPE_OPENDRAIN);
- /* Activates the serial driver 1 using the driver default configuration.*/
+ /* Activates the serial driver 2 using the driver default configuration.*/
sdStart(&SD2, NULL);
/* Creates the blinker thread.*/
diff --git a/testex/STM32/STM32F4xx/I2C-LSM6DS0/Makefile b/testex/STM32/STM32F4xx/I2C-LSM6DS0/Makefile index f205016a5..b404ccd51 100644 --- a/testex/STM32/STM32F4xx/I2C-LSM6DS0/Makefile +++ b/testex/STM32/STM32F4xx/I2C-LSM6DS0/Makefile @@ -87,6 +87,9 @@ PROJECT = ch # Imported source files and paths
CHIBIOS = ../../../..
+
+# Licensing files.
+include $(CHIBIOS)/os/license/license.mk
# Startup files.
include $(CHIBIOS)/os/common/startup/ARMCMx/compilers/GCC/mk/startup_stm32f4xx.mk
# HAL-OSAL files (optional).
@@ -97,31 +100,23 @@ include $(CHIBIOS)/os/hal/osal/rt/osal.mk # RTOS files (optional).
include $(CHIBIOS)/os/rt/rt.mk
include $(CHIBIOS)/os/common/ports/ARMCMx/compilers/GCC/mk/port_v7m.mk
-# Other files (optional).
+# EX files (optional).
include $(CHIBIOS)/os/ex/ST/lsm6ds0.mk
+# Other files (optional).
include $(CHIBIOS)/os/hal/lib/streams/streams.mk
-include $(CHIBIOS)/os/various/shell/shell.mk
# Define linker script file here
LDSCRIPT= $(STARTUPLD)/STM32F401xE.ld
# C sources that can be compiled in ARM or THUMB mode depending on the global
# setting.
-CSRC = $(STARTUPSRC) \
- $(KERNSRC) \
- $(PORTSRC) \
- $(OSALSRC) \
- $(HALSRC) \
- $(PLATFORMSRC) \
- $(BOARDSRC) \
- $(LSM6DS0SRC) \
- $(STREAMSSRC) \
- $(SHELLSRC) \
+CSRC = $(ALLCSRC) \
+ $(TESTSRC) \
main.c
# C++ sources that can be compiled in ARM or THUMB mode depending on the global
# setting.
-CPPSRC =
+CPPSRC = $(ALLCPPSRC)
# C sources to be compiled in ARM mode regardless of the global setting.
# NOTE: Mixing ARM and THUMB mode enables the -mthumb-interwork compiler
@@ -144,13 +139,10 @@ TCSRC = TCPPSRC =
# List ASM source files here
-ASMSRC =
-ASMXSRC = $(STARTUPASM) $(PORTASM) $(OSALASM)
+ASMSRC = $(ALLASMSRC)
+ASMXSRC = $(ALLXASMSRC)
-INCDIR = $(CHIBIOS)/os/license \
- $(STARTUPINC) $(KERNINC) $(PORTINC) $(OSALINC) \
- $(HALINC) $(PLATFORMINC) $(BOARDINC) $(LSM6DS0INC) \
- $(STREAMSINC) $(SHELLINC)
+INCDIR = $(ALLINC) $(TESTINC)
#
# Project, sources and paths
@@ -200,7 +192,7 @@ CPPWARN = -Wall -Wextra -Wundef #
# List all user C define here, like -D_DEBUG=1
-UDEFS = -DCHPRINTF_USE_FLOAT=1 -DSHELL_CMD_TEST_ENABLED=0 \
+UDEFS = -DCHPRINTF_USE_FLOAT=1 \
-DLSM6DS0_USE_ADVANCED=0 -DLSM6DS0_GYRO_USE_ADVANCED=0 \
-DLSM6DS0_ACC_USE_ADVANCED=0 -DLSM6DS0_SHARED_I2C=0
diff --git a/testex/STM32/STM32F4xx/I2C-LSM6DS0/main.c b/testex/STM32/STM32F4xx/I2C-LSM6DS0/main.c index 73cd07b3f..bd1bd5d6a 100644 --- a/testex/STM32/STM32F4xx/I2C-LSM6DS0/main.c +++ b/testex/STM32/STM32F4xx/I2C-LSM6DS0/main.c @@ -2,7 +2,7 @@ ChibiOS - Copyright (C) 2006..2018 Giovanni Di Sirio
Licensed under the Apache License, Version 2.0 (the "License");
- you may not use this file except in compliance with the License.
+ you may not use this file except in gyroliance with the License.
You may obtain a copy of the License at
http://www.apache.org/licenses/LICENSE-2.0
@@ -17,22 +17,23 @@ #include "ch.h"
#include "hal.h"
-#include "string.h"
-#include "shell.h"
#include "chprintf.h"
-
#include "lsm6ds0.h"
+#define cls(chp) chprintf(chp, "\033[2J\033[1;1H")
+
/*===========================================================================*/
/* LSM6DS0 related. */
/*===========================================================================*/
+
/* LSM6DS0 Driver: This object represent an LSM6DS0 instance */
static LSM6DS0Driver LSM6DS0D1;
-static int32_t rawdata[LSM6DS0_ACC_NUMBER_OF_AXES +
- LSM6DS0_GYRO_NUMBER_OF_AXES];
-static float cookeddata[LSM6DS0_ACC_NUMBER_OF_AXES +
- LSM6DS0_GYRO_NUMBER_OF_AXES];
+static int32_t accraw[LSM6DS0_ACC_NUMBER_OF_AXES];
+static int32_t gyroraw[LSM6DS0_GYRO_NUMBER_OF_AXES];
+
+static float acccooked[LSM6DS0_ACC_NUMBER_OF_AXES];
+static float gyrocooked[LSM6DS0_GYRO_NUMBER_OF_AXES];
static char axisID[LSM6DS0_ACC_NUMBER_OF_AXES] = {'X', 'Y', 'Z'};
static uint32_t i;
@@ -43,7 +44,10 @@ static const I2CConfig i2ccfg = { FAST_DUTY_CYCLE_2,
};
-static const LSM6DS0AccConfig lsm6ds0acccfg = {
+static const LSM6DS0Config lsm6ds0cfg = {
+ &I2CD1,
+ &i2ccfg,
+ LSM6DS0_SAD_VCC,
NULL,
NULL,
LSM6DS0_ACC_FS_2G,
@@ -51,9 +55,6 @@ static const LSM6DS0AccConfig lsm6ds0acccfg = { #if LSM6DS0_ACC_USE_ADVANCED
LSM6DS0_ACC_DEC_X4,
#endif
-};
-
-static const LSM6DS0GyroConfig lsm6ds0gyrocfg = {
NULL,
NULL,
LSM6DS0_GYRO_FS_245DPS,
@@ -64,14 +65,6 @@ static const LSM6DS0GyroConfig lsm6ds0gyrocfg = { LSM6DS0_GYRO_HP_DISABLED,
LSM6DS0_GYRO_HPCF_0
#endif
-};
-
-static const LSM6DS0Config lsm6ds0cfg = {
- &I2CD1,
- &i2ccfg,
- &lsm6ds0acccfg,
- &lsm6ds0gyrocfg,
- LSM6DS0_SAD_VCC,
#if LSM6DS0_USE_ADVANCED || defined(__DOXYGEN__)
LSM6DS0_BDU_BLOCKED,
LSM6DS0_END_LITTLE
@@ -79,190 +72,10 @@ static const LSM6DS0Config lsm6ds0cfg = { };
/*===========================================================================*/
-/* Command line related. */
-/*===========================================================================*/
-
-/* Enable use of special ANSI escape sequences */
-#define CHPRINTF_USE_ANSI_CODE TRUE
-#define SHELL_WA_SIZE THD_WORKING_AREA_SIZE(2048)
-
-static void cmd_read(BaseSequentialStream *chp, int argc, char *argv[]) {
- (void)argv;
- if (argc != 2) {
- chprintf(chp, "Usage: read [acc|gyro|both] [raw|cooked]\r\n");
- return;
- }
-
- while (chnGetTimeout((BaseChannel *)chp, 150) == Q_TIMEOUT) {
- if (!strcmp (argv[0], "acc")) {
- if (!strcmp (argv[1], "raw")) {
-#if CHPRINTF_USE_ANSI_CODE
- chprintf(chp, "\033[2J\033[1;1H");
-#endif
- accelerometerReadRaw(&LSM6DS0D1, rawdata);
- chprintf(chp, "LSM6DS0 Accelerometer raw data...\r\n");
- for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES; i++) {
- chprintf(chp, "%c-axis: %d\r\n", axisID[i], rawdata[i]);
- }
- }
- else if (!strcmp (argv[1], "cooked")) {
-#if CHPRINTF_USE_ANSI_CODE
- chprintf(chp, "\033[2J\033[1;1H");
-#endif
- accelerometerReadCooked(&LSM6DS0D1, cookeddata);
- chprintf(chp, "LSM6DS0 Accelerometer cooked data...\r\n");
- for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES; i++) {
- chprintf(chp, "%c-axis: %.4f mG\r\n", axisID[i], cookeddata[i]);
- }
- }
- else {
- chprintf(chp, "Usage: read [acc|gyro|both] [raw|cooked]\r\n");
- return;
- }
- }
- else if (!strcmp (argv[0], "gyro")) {
- if (!strcmp (argv[1], "raw")) {
-#if CHPRINTF_USE_ANSI_CODE
- chprintf(chp, "\033[2J\033[1;1H");
-#endif
- gyroscopeReadRaw(&LSM6DS0D1, rawdata);
- chprintf(chp, "LSM6DS0 Gyroscope raw data...\r\n");
- for(i = 0; i < LSM6DS0_GYRO_NUMBER_OF_AXES; i++) {
- chprintf(chp, "%c-axis: %d\r\n", axisID[i], rawdata[i]);
- }
- }
- else if (!strcmp (argv[1], "cooked")) {
-#if CHPRINTF_USE_ANSI_CODE
- chprintf(chp, "\033[2J\033[1;1H");
-#endif
- gyroscopeReadCooked(&LSM6DS0D1, cookeddata);
- chprintf(chp, "LSM6DS0 Gyroscope cooked data...\r\n");
- for(i = 0; i < LSM6DS0_GYRO_NUMBER_OF_AXES; i++) {
- chprintf(chp, "%c-axis: %.4f DPS\r\n", axisID[i], cookeddata[i]);
- }
- }
- else {
- chprintf(chp, "Usage: read [acc|gyro|both] [raw|cooked]\r\n");
- return;
- }
- }
- else if (!strcmp (argv[0], "both")) {
- if (!strcmp (argv[1], "raw")) {
-#if CHPRINTF_USE_ANSI_CODE
- chprintf(chp, "\033[2J\033[1;1H");
-#endif
- sensorReadRaw(&LSM6DS0D1, rawdata);
- chprintf(chp, "LSM6DS0 Accelerometer raw data...\r\n");
- for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES; i++) {
- chprintf(chp, "%c-axis: %d\r\n", axisID[i], rawdata[i]);
- }
- chprintf(chp, "LSM6DS0 Gyroscope raw data...\r\n");
- for(i = 0; i < LSM6DS0_GYRO_NUMBER_OF_AXES; i++) {
- chprintf(chp, "%c-axis: %d\r\n", axisID[i],
- rawdata[i + LSM6DS0_ACC_NUMBER_OF_AXES]);
- }
- }
- else if (!strcmp (argv[1], "cooked")) {
-#if CHPRINTF_USE_ANSI_CODE
- chprintf(chp, "\033[2J\033[1;1H");
-#endif
- sensorReadCooked(&LSM6DS0D1, cookeddata);
- chprintf(chp, "LSM6DS0 Accelerometer cooked data...\r\n");
- for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES; i++) {
- chprintf(chp, "%c-axis: %.4f mG\r\n", axisID[i], cookeddata[i]);
- }
- chprintf(chp, "LSM6DS0 Gyroscope cooked data...\r\n");
- for(i = 0; i < LSM6DS0_GYRO_NUMBER_OF_AXES; i++) {
- chprintf(chp, "%c-axis: %.4f DPS\r\n", axisID[i],
- cookeddata[i + LSM6DS0_ACC_NUMBER_OF_AXES]);
- }
- }
- else {
- chprintf(chp, "Usage: read [acc|gyro|both] [raw|cooked]\r\n");
- return;
- }
- }
- else {
- chprintf(chp, "Usage: read [acc|gyro|both] [raw|cooked]\r\n");
- return;
- }
- }
- chprintf(chp, "Stopped\r\n");
-}
-
-static void cmd_fullscale(BaseSequentialStream *chp, int argc, char *argv[]) {
- (void)argv;
- if (argc < 1) {
- chprintf(chp, "Usage: fullscale [acc|gyro] [value]\r\n");
- return;
- }
- if (!strcmp (argv[0], "acc")) {
-#if CHPRINTF_USE_ANSI_CODE
- chprintf(chp, "\033[2J\033[1;1H");
-#endif
- if(!strcmp (argv[1], "2G")) {
- accelerometerSetFullScale(&LSM6DS0D1, LSM6DS0_ACC_FS_2G);
- chprintf(chp, "LSM6DS0 Accelerometer full scale set to 2G...\r\n");
- }
- else if(!strcmp (argv[1], "4G")) {
- accelerometerSetFullScale(&LSM6DS0D1, LSM6DS0_ACC_FS_4G);
- chprintf(chp, "LSM6DS0 Accelerometer full scale set to 4G...\r\n");
- }
- else if(!strcmp (argv[1], "8G")) {
- accelerometerSetFullScale(&LSM6DS0D1, LSM6DS0_ACC_FS_8G);
- chprintf(chp, "LSM6DS0 Accelerometer full scale set to 8G...\r\n");
- }
- else if(!strcmp (argv[1], "16G")) {
- accelerometerSetFullScale(&LSM6DS0D1, LSM6DS0_ACC_FS_16G);
- chprintf(chp, "LSM6DS0 Accelerometer full scale set to 16G...\r\n");
- }
- else {
- chprintf(chp, "Usage: fullscale acc [2G|4G|8G|16G]\r\n");
- return;
- }
- }
- else if (!strcmp (argv[0], "gyro")) {
-#if CHPRINTF_USE_ANSI_CODE
- chprintf(chp, "\033[2J\033[1;1H");
-#endif
- if(!strcmp (argv[1], "245")) {
- gyroscopeSetFullScale(&LSM6DS0D1, LSM6DS0_GYRO_FS_245DPS);
- chprintf(chp, "LSM6DS0 Gyroscope full scale set to 245 DPS...\r\n");
- }
- else if(!strcmp (argv[1], "500")) {
- gyroscopeSetFullScale(&LSM6DS0D1, LSM6DS0_GYRO_FS_500DPS);
- chprintf(chp, "LSM6DS0 Gyroscope full scale set to 500 DPS...\r\n");
- }
- else if(!strcmp (argv[1], "2000")) {
- gyroscopeSetFullScale(&LSM6DS0D1, LSM6DS0_GYRO_FS_2000DPS);
- chprintf(chp, "LSM6DS0 Gyroscope full scale set to 2000 DPS...\r\n");
- }
- else {
- chprintf(chp, "Usage: fullscale gyro [245|500|2000]\r\n");
- return;
- }
- }
- else {
- chprintf(chp, "Usage: fullscale [acc|gyro] [value]\r\n");
- return;
- }
-}
-
-static const ShellCommand commands[] = {
- {"read", cmd_read},
- {"fullscale", cmd_fullscale},
- {NULL, NULL}
-};
-
-static const ShellConfig shell_cfg1 = {
- (BaseSequentialStream *)&SD2,
- commands
-};
-
-/*===========================================================================*/
-/* Main code. */
+/* Generic code. */
/*===========================================================================*/
+static BaseSequentialStream* chp = (BaseSequentialStream*)&SD2;
/*
* LED blinker thread, times are in milliseconds.
*/
@@ -272,13 +85,12 @@ static THD_FUNCTION(Thread1, arg) { (void)arg;
chRegSetThreadName("blinker");
while (true) {
- palClearLine(LINE_LED_GREEN);
- chThdSleepMilliseconds(500);
- palSetLine(LINE_LED_GREEN);
+ palToggleLine(LINE_LED_GREEN);
chThdSleepMilliseconds(500);
}
}
+
/*
* Application entry point.
*/
@@ -294,44 +106,53 @@ int main(void) { halInit();
chSysInit();
+ /* Configuring I2C SCK and I2C SDA related GPIOs .*/
palSetLineMode(LINE_ARD_D15, PAL_MODE_ALTERNATE(4) |
PAL_STM32_OSPEED_HIGHEST | PAL_STM32_OTYPE_OPENDRAIN);
palSetLineMode(LINE_ARD_D14, PAL_MODE_ALTERNATE(4) |
PAL_STM32_OSPEED_HIGHEST | PAL_STM32_OTYPE_OPENDRAIN);
- /*
- * Activates the serial driver 2 using the driver default configuration.
- */
+ /* Activates the serial driver 2 using the driver default configuration.*/
sdStart(&SD2, NULL);
- /*
- * Creates the blinker thread.
- */
+ /* Creates the blinker thread.*/
chThdCreateStatic(waThread1, sizeof(waThread1), NORMALPRIO + 1, Thread1, NULL);
- /*
- * LSM6DS0 Object Initialization
- */
+ /* LSM6DS0 Object Initialization.*/
lsm6ds0ObjectInit(&LSM6DS0D1);
- /*
- * Activates the LSM6DS0 driver.
- */
+ /* Activates the LSM6DS0 driver.*/
lsm6ds0Start(&LSM6DS0D1, &lsm6ds0cfg);
- /*
- * Shell manager initialization.
- */
- shellInit();
+ lsm6ds0GyroscopeSampleBias(&LSM6DS0D1);
+
+ /* Normal main() thread activity, printing MEMS data on the SDU1.*/
+ while (true) {
+ lsm6ds0AccelerometerReadRaw(&LSM6DS0D1, accraw);
+ chprintf(chp, "LSM6DS0 Accelerometer raw data...\r\n");
+ for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES; i++) {
+ chprintf(chp, "%c-axis: %d\r\n", axisID[i], accraw[i]);
+ }
- while(TRUE) {
+ lsm6ds0GyroscopeReadRaw(&LSM6DS0D1, gyroraw);
+ chprintf(chp, "LSM6DS0 Gyroscope raw data...\r\n");
+ for(i = 0; i < LSM6DS0_GYRO_NUMBER_OF_AXES; i++) {
+ chprintf(chp, "%c-axis: %d\r\n", axisID[i], gyroraw[i]);
+ }
+
+ lsm6ds0AccelerometerReadCooked(&LSM6DS0D1, acccooked);
+ chprintf(chp, "LSM6DS0 Accelerometer cooked data...\r\n");
+ for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES; i++) {
+ chprintf(chp, "%c-axis: %.3f\r\n", axisID[i], acccooked[i]);
+ }
- thread_t *shelltp = chThdCreateFromHeap(NULL, SHELL_WA_SIZE,
- "shell", NORMALPRIO + 1,
- shellThread, (void *)&shell_cfg1);
- chThdWait(shelltp); /* Waiting termination. */
- chThdSleepMilliseconds(1000);
+ lsm6ds0GyroscopeReadCooked(&LSM6DS0D1, gyrocooked);
+ chprintf(chp, "LSM6DS0 Gyroscope cooked data...\r\n");
+ for(i = 0; i < LSM6DS0_GYRO_NUMBER_OF_AXES; i++) {
+ chprintf(chp, "%c-axis: %.3f\r\n", axisID[i], gyrocooked[i]);
+ }
+ chThdSleepMilliseconds(100);
+ cls(chp);
}
lsm6ds0Stop(&LSM6DS0D1);
- return 0;
}
|