/**
* @brief Deactivates the LSM6DS0 Complex Driver peripheral.
*
* @param[in] devp pointer to the @p LSM6DS0Driver object
*
* @api
*/
void lsm6ds0Stop(LSM6DS0Driver *devp) {
osalDbgCheck(devp != NULL);
osalDbgAssert((devp->state == LSM6DS0_STOP) || (devp->state == LSM6DS0_READY),
"lsm6ds0Stop(), invalid state");
#if (LSM6DS0_USE_I2C)
if (devp->state == LSM6DS0_STOP) {
#if LSM6DS0_SHARED_I2C
i2cAcquireBus((devp)->config->i2cp);
i2cStart((devp)->config->i2cp,
(devp)->config->i2ccfg);
#endif /* LSM6DS0_SHARED_I2C */
if((devp)->config->acccfg != NULL) {
lsm6ds0I2CWriteRegister(devp->config->i2cp,
devp->config->slaveaddress,
LSM6DS0_AD_CTRL_REG6_XL,
LSM6DS0_ACC_ODR_PD);
}
if((devp)->config->gyrocfg != NULL) {
lsm6ds0I2CWriteRegister(devp->config->i2cp,
devp->config->slaveaddress,
LSM6DS0_AD_CTRL_REG9,
LSM6DS0_GYRO_SLP_ENABLED);
}
i2cStop((devp)->config->i2cp);
#if LSM6DS0_SHARED_I2C
i2cReleaseBus((devp)->config->i2cp);
#endif /* LSM6DS0_SHARED_I2C */
}
#endif /* LSM6DS0_USE_I2C */
devp->state = LSM6DS0_STOP;
}
/**
* @brief Deactivates the LSM6DS0 Complex Driver peripheral.
*
* @param[in] devp pointer to the @p LSM6DS0Driver object
*
* @api
*/
void lsm6ds0Stop(LSM6DS0Driver *devp) {
uint8_t cr[2];
osalDbgCheck(devp != NULL);
osalDbgAssert((devp->state == LSM6DS0_STOP) || (devp->state == LSM6DS0_READY),
"lsm6ds0Stop(), invalid state");
if (devp->state == LSM6DS0_READY) {
#if LSM6DS0_USE_I2C
#if LSM6DS0_SHARED_I2C
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);
/* Disabling gyroscope.*/
cr[0] = LSM6DS0_AD_CTRL_REG9;
cr[1] = LSM6DS0_CTRL_REG9_SLEEP_G;
lsm6ds0I2CWriteRegister(devp->config->i2cp, devp->config->slaveaddress,
cr, 1);
i2cStop(devp->config->i2cp);
#if LSM6DS0_SHARED_I2C
i2cReleaseBus(devp->config->i2cp);
#endif /* LSM6DS0_SHARED_I2C */
#endif /* LSM6DS0_USE_I2C */
}
devp->state = LSM6DS0_STOP;
}
/**
* @brief Deactivates the LSM6DS0 Complex Driver peripheral.
*
* @param[in] devp pointer to the @p LSM6DS0Driver object
*
* @api
*/
void lsm6ds0Stop(LSM6DS0Driver *devp) {
uint8_t cr[2];
osalDbgCheck(devp != NULL);
osalDbgAssert((devp->state == LSM6DS0_STOP) || (devp->state == LSM6DS0_READY),
"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);
#endif /* LSM6DS0_SHARED_I2C */
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) {
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 */
lsm6ds0I2CWriteRegister(devp->config->i2cp, devp->config->slaveaddress,
cr, 1);
i2cStop((devp)->config->i2cp);
#if LSM6DS0_SHARED_I2C
i2cReleaseBus((devp)->config->i2cp);
#endif /* LSM6DS0_SHARED_I2C */
#endif /* LSM6DS0_USE_I2C */
}
}
devp->state = LSM6DS0_STOP;
}
/**
* @brief Configures and activates LSM6DS0 Complex Driver peripheral.
*
* @param[in] devp pointer to the @p LSM6DS0Driver object
* @param[in] config pointer to the @p LSM6DS0Config object
*
* @api
*/
void lsm6ds0Start(LSM6DS0Driver *devp, const LSM6DS0Config *config) {
uint32_t i;
osalDbgCheck((devp != NULL) && (config != NULL));
osalDbgAssert((devp->state == LSM6DS0_STOP) || (devp->state == LSM6DS0_READY),
"lsm6ds0Start(), invalid state");
devp->config = config;
#if LSM6DS0_USE_I2C
#if LSM6DS0_SHARED_I2C
i2cAcquireBus((devp)->config->i2cp);
#endif /* LSM6DS0_SHARED_I2C */
i2cStart((devp)->config->i2cp,
(devp)->config->i2ccfg);
if((devp)->config->acccfg != NULL) {
lsm6ds0I2CWriteRegister(devp->config->i2cp,
devp->config->slaveaddress,
LSM6DS0_AD_CTRL_REG5_XL,
devp->config->acccfg->decmode |
devp->config->acccfg->axesenabling);
lsm6ds0I2CWriteRegister(devp->config->i2cp,
devp->config->slaveaddress,
LSM6DS0_AD_CTRL_REG6_XL,
devp->config->acccfg->outdatarate |
devp->config->acccfg->fullscale );
}
if((devp)->config->gyrocfg != NULL) {
lsm6ds0I2CWriteRegister(devp->config->i2cp,
devp->config->slaveaddress,
LSM6DS0_AD_CTRL_REG1_G,
devp->config->gyrocfg->fullscale |
devp->config->gyrocfg->outdatarate);
lsm6ds0I2CWriteRegister(devp->config->i2cp,
devp->config->slaveaddress,
LSM6DS0_AD_CTRL_REG2_G,
devp->config->gyrocfg->outsel);
lsm6ds0I2CWriteRegister(devp->config->i2cp,
devp->config->slaveaddress,
LSM6DS0_AD_CTRL_REG3_G,
devp->config->gyrocfg->hpfenable |
devp->config->gyrocfg->lowmodecfg |
devp->config->gyrocfg->hpcfg);
lsm6ds0I2CWriteRegister(devp->config->i2cp,
devp->config->slaveaddress,
LSM6DS0_AD_CTRL_REG4,
devp->config->gyrocfg->axesenabling);
lsm6ds0I2CWriteRegister(devp->config->i2cp,
devp->config->slaveaddress,
LSM6DS0_AD_CTRL_REG9,
LSM6DS0_GYRO_SLP_DISABLED);
}
lsm6ds0I2CWriteRegister(devp->config->i2cp,
devp->config->slaveaddress,
LSM6DS0_AD_CTRL_REG8,
devp->config->endianness |
devp->config->blockdataupdate);
#if LSM6DS0_SHARED_I2C
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++)
devp->accsensitivity[i] = LSM6DS0_ACC_SENS_2G;
else if(devp->config->acccfg->fullscale == 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->acccfg->fullscale == 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->acccfg->fullscale == LSM6DS0_ACC_FS_16G)
for(i = 0; i < LSM6DS0_ACC_NUMBER_OF_AXES; i++)
devp->accsensitivity[i] = LSM6DS0_ACC_SENS_16G;
else
osalDbgAssert(FALSE, "lsm6ds0Start(), accelerometer full scale issue");
}
if((devp)->config->gyrocfg != NULL) {
if(devp->config->gyrocfg->fullscale == LSM6DS0_GYRO_FS_245DSP)
for(i = 0; i < LSM6DS0_GYRO_NUMBER_OF_AXES; i++)
devp->gyrosensitivity[i] = LSM6DS0_GYRO_SENS_245DPS;
else if(devp->config->gyrocfg->fullscale == LSM6DS0_GYRO_FS_500DSP)
for(i = 0; i < LSM6DS0_GYRO_NUMBER_OF_AXES; i++)
devp->gyrosensitivity[i] = LSM6DS0_GYRO_SENS_500DPS;
else if(devp->config->gyrocfg->fullscale == LSM6DS0_GYRO_FS_2000DSP)
for(i = 0; i < LSM6DS0_GYRO_NUMBER_OF_AXES; i++)
devp->gyrosensitivity[i] = LSM6DS0_GYRO_SENS_2000DPS;
else
osalDbgAssert(FALSE, "lsm6ds0Start(), gyroscope full scale issue");
}
/* This is the Gyroscope transient recovery time */
osalThreadSleepMilliseconds(5);
devp->state = LSM6DS0_READY;
}
/**
* @brief Configures and activates LSM6DS0 Complex Driver peripheral.
*
* @param[in] devp pointer to the @p LSM6DS0Driver object
* @param[in] config pointer to the @p LSM6DS0Config object
*
* @api
*/
void lsm6ds0Start(LSM6DS0Driver *devp, const LSM6DS0Config *config) {
uint32_t i;
uint8_t cr[5];
osalDbgCheck((devp != NULL) && (config != NULL));
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;
cr[1] = LSM6DS0_CTRL_REG8_IF_ADD_INC;
#if LSM6DS0_USE_ADVANCED || defined(__DOXYGEN__)
cr[1] |= devp->config->endianness | devp->config->blockdataupdate;
#endif
}
#if LSM6DS0_USE_I2C
#if LSM6DS0_SHARED_I2C
i2cAcquireBus(devp->config->i2cp);
#endif /* LSM6DS0_SHARED_I2C */
i2cStart(devp->config->i2cp, devp->config->i2ccfg);
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 */
/* 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_USE_ADVANCED || defined(__DOXYGEN__)
cr[1] |= devp->config->accdecmode;
#endif
}
/* 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);
#endif /* LSM6DS0_SHARED_I2C */
lsm6ds0I2CWriteRegister(devp->config->i2cp, devp->config->slaveaddress,
cr, 2);
#if LSM6DS0_SHARED_I2C
i2cReleaseBus(devp->config->i2cp);
#endif /* LSM6DS0_SHARED_I2C */
#endif /* LSM6DS0_USE_I2C */
/* 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");
/* 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->gyrofullscale |
devp->config->gyrooutdatarate;
}
/* Control register 2 configuration block.*/
{
cr[2] = 0;
#if LSM6DS0_USE_ADVANCED || defined(__DOXYGEN__)
cr[2] |= devp->config->gyrooutsel;
#endif
}
/* Control register 3 configuration block.*/
{
cr[3] = 0;
#if LSM6DS0_USE_ADVANCED || defined(__DOXYGEN__)
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;
}
#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,
cr, 4);
#if LSM6DS0_SHARED_I2C
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;
}
#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,
cr, 1);
#if LSM6DS0_SHARED_I2C
i2cReleaseBus(devp->config->i2cp);
#endif /* LSM6DS0_SHARED_I2C */
#endif /* LSM6DS0_USE_I2C */
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];
}
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];
}
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];
}
devp->gyrofullscale = LSM6DS0_GYRO_2000DPS;
}
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;
}
/**
* @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 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, buff[2];
msg_t msg = MSG_OK;
osalDbgCheck(devp != NULL);
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;
}
else if(fs == LSM6DS0_GYRO_FS_500DPS) {
newfs = LSM6DS0_GYRO_500DPS;
}
else if(fs == LSM6DS0_GYRO_FS_2000DPS) {
newfs = LSM6DS0_GYRO_2000DPS;
}
else {
return MSG_RESET;
}
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(devp->config->i2cp,
devp->config->slaveaddress,
LSM6DS0_AD_CTRL_REG1_G, &buff[1], 1);
#if LSM6DS0_SHARED_I2C
i2cReleaseBus(devp->config->i2cp);
#endif /* LSM6DS0_SHARED_I2C */
#endif /* LSM6DS0_USE_I2C */
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 */
/* Scaling sensitivity and bias. Re-calibration is suggested anyway. */
for(i = 0; i < LSM6DS0_GYRO_NUMBER_OF_AXES; i++) {
devp->gyrosensitivity[i] *= scale;
devp->gyrobias[i] *= scale;
}
}
return msg;
}
/**
* @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 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;
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;
}
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;
}
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;
if(fs == LSM6DS0_GYRO_FS_245DPS) {
newfs = LSM6DS0_GYRO_245DPS;
}
else if(fs == LSM6DS0_GYRO_FS_500DPS) {
newfs = LSM6DS0_GYRO_500DPS;
}
else if(fs == LSM6DS0_GYRO_FS_2000DPS) {
newfs = LSM6DS0_GYRO_2000DPS;
}
else {
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 */
/* Updating register.*/
msg = lsm6ds0I2CReadRegister(((LSM6DS0Driver *)ip)->config->i2cp,
((LSM6DS0Driver *)ip)->config->slaveaddress,
LSM6DS0_AD_CTRL_REG1_G, &cr[1], 1);
#if LSM6DS0_SHARED_I2C
i2cReleaseBus(((LSM6DS0Driver *)ip)->config->i2cp);
#endif /* LSM6DS0_SHARED_I2C */
if(msg != MSG_OK)
return msg;
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);
#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;
}
}
return msg;
}
