diff options
Diffstat (limited to 'os/ex/ST/lsm6ds0.c')
| -rw-r--r-- | os/ex/ST/lsm6ds0.c | 1261 |
1 files changed, 747 insertions, 514 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;
}
|