static msg_t sens_get_temperature(void *ip, float* tempp) {
int16_t temp;
#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 */
temp = lsm6ds0I2CReadRegister(((LSM6DS0Driver *)ip)->config->i2cp,
((LSM6DS0Driver *)ip)->config->slaveaddress,
LSM6DS0_AD_OUT_TEMP_L, NULL);
temp += lsm6ds0I2CReadRegister(((LSM6DS0Driver *)ip)->config->i2cp,
((LSM6DS0Driver *)ip)->config->slaveaddress,
LSM6DS0_AD_OUT_TEMP_H, NULL) << 8;
#if LSM6DS0_SHARED_I2C
i2cReleaseBus(((LSM6DS0Driver *)ip)->config->i2cp);
#endif /* LSM6DS0_SHARED_I2C */
#endif /* LSM6DS0_USE_I2C */
*tempp = ((float)temp / LSM6DS0_TEMP_SENS) + LSM6DS0_TEMP_SENS_OFF;
return MSG_OK;
}
static msg_t gyro_read_raw(void *ip, int32_t axes[]) {
int16_t tmp;
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 */
if(((LSM6DS0Driver *)ip)->config->gyrocfg->axesenabling & LSM6DS0_GYRO_AE_X){
tmp = lsm6ds0I2CReadRegister(((LSM6DS0Driver *)ip)->config->i2cp,
((LSM6DS0Driver *)ip)->config->slaveaddress,
LSM6DS0_AD_OUT_X_L_G, NULL);
tmp += lsm6ds0I2CReadRegister(((LSM6DS0Driver *)ip)->config->i2cp,
((LSM6DS0Driver *)ip)->config->slaveaddress,
LSM6DS0_AD_OUT_X_H_G, NULL) << 8;
axes[0] = (int32_t)tmp - ((LSM6DS0Driver *)ip)->gyrobias[0];
}
if(((LSM6DS0Driver *)ip)->config->acccfg->axesenabling & LSM6DS0_GYRO_AE_Y){
tmp = lsm6ds0I2CReadRegister(((LSM6DS0Driver *)ip)->config->i2cp,
((LSM6DS0Driver *)ip)->config->slaveaddress,
LSM6DS0_AD_OUT_Y_L_G, NULL);
tmp += lsm6ds0I2CReadRegister(((LSM6DS0Driver *)ip)->config->i2cp,
((LSM6DS0Driver *)ip)->config->slaveaddress,
LSM6DS0_AD_OUT_Y_H_G, NULL) << 8;
axes[1] = (int32_t)tmp - ((LSM6DS0Driver *)ip)->gyrobias[1];
}
if(((LSM6DS0Driver *)ip)->config->acccfg->axesenabling & LSM6DS0_GYRO_AE_Z){
tmp = lsm6ds0I2CReadRegister(((LSM6DS0Driver *)ip)->config->i2cp,
((LSM6DS0Driver *)ip)->config->slaveaddress,
LSM6DS0_AD_OUT_Z_L_G, NULL);
tmp += lsm6ds0I2CReadRegister(((LSM6DS0Driver *)ip)->config->i2cp,
((LSM6DS0Driver *)ip)->config->slaveaddress,
LSM6DS0_AD_OUT_Z_H_G, NULL) << 8;
axes[2] = (int32_t)tmp - ((LSM6DS0Driver *)ip)->gyrobias[2];
}
#if LSM6DS0_SHARED_I2C
i2cReleaseBus(((LSM6DS0Driver *)ip)->config->i2cp);
#endif /* LSM6DS0_SHARED_I2C */
#endif /* LSM6DS0_USE_I2C */
return MSG_OK;
}
/**
* @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;
}
static msg_t gyro_read_raw(void *ip, int32_t* axes) {
int16_t tmp;
uint8_t i, buff[2 * LSM6DS0_GYRO_NUMBER_OF_AXES];
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 */
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;
}
}
return msg;
}
/**
* @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;
}
