From 9ac42f8cd7a7613827ddd78209d9ad02112714f2 Mon Sep 17 00:00:00 2001 From: Rocco Marco Guglielmi Date: Mon, 12 Mar 2018 21:37:27 +0000 Subject: Updated LSM6DS0 driver git-svn-id: svn://svn.code.sf.net/p/chibios/svn/trunk@11710 110e8d01-0319-4d1e-a829-52ad28d1bb01 --- os/ex/ST/lsm6ds0.c | 1261 ++++++++++++++++----------- os/ex/ST/lsm6ds0.h | 599 +++++++++---- testex/STM32/STM32F4xx/I2C-LPS25H/main.c | 2 +- testex/STM32/STM32F4xx/I2C-LSM6DS0/Makefile | 32 +- 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,9 +428,22 @@ /*===========================================================================*/ /** - * @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. */ @@ -436,39 +477,6 @@ typedef enum { LSM6DS0_ACC_DEC_X8 = 0xC0 /**< Output updated every 8 samples. */ } 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. */ @@ -543,59 +551,6 @@ typedef enum { LSM6DS0_GYRO_HPCF_9 = 0x09 } 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. */ @@ -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; } -- cgit v1.2.3