/** * @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; }