From 610290753dff711fca3c84f50877b2ca0e921d01 Mon Sep 17 00:00:00 2001 From: Rocco Marco Guglielmi Date: Fri, 9 Mar 2018 23:14:37 +0000 Subject: Minor adjustments on L3GD20 and LSMS303DLHC drivers git-svn-id: svn://svn.code.sf.net/p/chibios/svn/trunk@11675 110e8d01-0319-4d1e-a829-52ad28d1bb01 --- os/ex/ST/l3gd20.c | 19 +++++++---------- os/ex/ST/lsm303dlhc.c | 57 ++++++++++++++++++++++++++++----------------------- 2 files changed, 38 insertions(+), 38 deletions(-) (limited to 'os') diff --git a/os/ex/ST/l3gd20.c b/os/ex/ST/l3gd20.c index c3093d40c..fcb6dd17d 100644 --- a/os/ex/ST/l3gd20.c +++ b/os/ex/ST/l3gd20.c @@ -176,16 +176,11 @@ static msg_t gyro_read_cooked(void *ip, float axes[]) { devp = objGetInstance(L3GD20Driver*, (BaseGyroscope*)ip); osalDbgAssert((devp->state == L3GD20_READY), - "gyro_read_cooked(), invalid state"); -#if L3GD20_USE_SPI - osalDbgAssert((devp->config->spip->state == SPI_READY), - "gyro_read_cooked(), channel not ready"); -#endif + "gyro_read_cooked(), invalid state"); msg = gyro_read_raw(ip, raw); for(i = 0; i < L3GD20_GYRO_NUMBER_OF_AXES; i++){ - axes[i] = raw[i] * devp->gyrosensitivity[i]; - axes[i] -= devp->gyrobias[i]; + axes[i] = (raw[i] * devp->gyrosensitivity[i]) - devp->gyrobias[i]; } return msg; } @@ -259,7 +254,7 @@ static msg_t gyro_set_bias(void *ip, float *bp) { /* Getting parent instance pointer.*/ devp = objGetInstance(L3GD20Driver*, (BaseGyroscope*)ip); - osalDbgAssert((devp->state != L3GD20_UNINIT), + osalDbgAssert((devp->state == L3GD20_READY), "gyro_set_bias(), invalid state"); for(i = 0; i < L3GD20_GYRO_NUMBER_OF_AXES; i++) { @@ -287,7 +282,7 @@ static msg_t gyro_reset_bias(void *ip) { /* Getting parent instance pointer.*/ devp = objGetInstance(L3GD20Driver*, (BaseGyroscope*)ip); - osalDbgAssert((devp->state != L3GD20_UNINIT), + osalDbgAssert((devp->state == L3GD20_READY), "gyro_reset_bias(), invalid state"); for(i = 0; i < L3GD20_GYRO_NUMBER_OF_AXES; i++) @@ -316,7 +311,7 @@ static msg_t gyro_set_sensivity(void *ip, float *sp) { /* Getting parent instance pointer.*/ devp = objGetInstance(L3GD20Driver*, (BaseGyroscope*)ip); - osalDbgAssert((devp->state != L3GD20_UNINIT), + osalDbgAssert((devp->state == L3GD20_READY), "gyro_set_sensivity(), invalid state"); for(i = 0; i < L3GD20_GYRO_NUMBER_OF_AXES; i++) { @@ -345,7 +340,7 @@ static msg_t gyro_reset_sensivity(void *ip) { /* Getting parent instance pointer.*/ devp = objGetInstance(L3GD20Driver*, (BaseGyroscope*)ip); - osalDbgAssert((devp->state != L3GD20_UNINIT), + osalDbgAssert((devp->state == L3GD20_READY), "gyro_reset_sensivity(), invalid state"); if(devp->config->gyrofullscale == L3GD20_FS_250DPS) @@ -384,7 +379,7 @@ static msg_t gyro_set_full_scale(L3GD20Driver *devp, l3gd20_fs_t fs) { osalDbgCheck(devp != NULL); - osalDbgAssert((devp->state != L3GD20_UNINIT), + osalDbgAssert((devp->state == L3GD20_READY), "gyro_set_full_scale(), invalid state"); #if L3GD20_USE_SPI osalDbgAssert((devp->config->spip->state == SPI_READY), diff --git a/os/ex/ST/lsm303dlhc.c b/os/ex/ST/lsm303dlhc.c index 98656c766..64f965607 100644 --- a/os/ex/ST/lsm303dlhc.c +++ b/os/ex/ST/lsm303dlhc.c @@ -191,13 +191,10 @@ static msg_t acc_read_cooked(void *ip, float axes[]) { osalDbgAssert((devp->state == LSM303DLHC_READY), "acc_read_cooked(), invalid state"); - osalDbgAssert((devp->config->i2cp->state == I2C_READY), - "acc_read_cooked(), channel not ready"); msg = acc_read_raw(ip, raw); for(i = 0; i < LSM303DLHC_ACC_NUMBER_OF_AXES; i++) { - axes[i] = raw[i] * devp->accsensitivity[i]; - axes[i] -= devp->accbias[i]; + axes[i] = (raw[i] * devp->accsensitivity[i]) - devp->accbias[i]; } return msg; } @@ -224,7 +221,7 @@ static msg_t acc_set_bias(void *ip, float *bp) { /* Getting parent instance pointer.*/ devp = objGetInstance(LSM303DLHCDriver*, (BaseAccelerometer*)ip); - osalDbgAssert((devp->state != LSM303DLHC_UNINIT), + osalDbgAssert((devp->state == LSM303DLHC_READY), "acc_set_bias(), invalid state"); for(i = 0; i < LSM303DLHC_ACC_NUMBER_OF_AXES; i++) { @@ -253,7 +250,7 @@ static msg_t acc_reset_bias(void *ip) { /* Getting parent instance pointer.*/ devp = objGetInstance(LSM303DLHCDriver*, (BaseAccelerometer*)ip); - osalDbgAssert((devp->state != LSM303DLHC_UNINIT), + osalDbgAssert((devp->state == LSM303DLHC_READY), "acc_reset_bias(), invalid state"); for(i = 0; i < LSM303DLHC_ACC_NUMBER_OF_AXES; i++) @@ -283,7 +280,7 @@ static msg_t acc_set_sensivity(void *ip, float *sp) { osalDbgCheck((ip != NULL) && (sp != NULL)); - osalDbgAssert((devp->state != LSM303DLHC_UNINIT), + osalDbgAssert((devp->state == LSM303DLHC_READY), "acc_set_sensivity(), invalid state"); for(i = 0; i < LSM303DLHC_ACC_NUMBER_OF_AXES; i++) { @@ -312,7 +309,7 @@ static msg_t acc_reset_sensivity(void *ip) { /* Getting parent instance pointer.*/ devp = objGetInstance(LSM303DLHCDriver*, (BaseAccelerometer*)ip); - osalDbgAssert((devp->state != LSM303DLHC_UNINIT), + osalDbgAssert((devp->state == LSM303DLHC_READY), "acc_reset_sensivity(), invalid state"); if(devp->config->accfullscale == LSM303DLHC_ACC_FS_2G) @@ -355,7 +352,7 @@ static msg_t acc_set_full_scale(LSM303DLHCDriver *devp, osalDbgCheck(devp != NULL); - osalDbgAssert((devp->state != LSM303DLHC_UNINIT), + osalDbgAssert((devp->state == LSM303DLHC_READY), "acc_set_full_scale(), invalid state"); osalDbgAssert((devp->config->i2cp->state == I2C_READY), "acc_set_full_scale(), channel not ready"); @@ -528,13 +525,10 @@ static msg_t comp_read_cooked(void *ip, float axes[]) { osalDbgAssert((devp->state == LSM303DLHC_READY), "comp_read_cooked(), invalid state"); - osalDbgAssert((devp->config->i2cp->state == I2C_READY), - "comp_read_cooked(), channel not ready"); msg = comp_read_raw(ip, raw); for(i = 0; i < LSM303DLHC_COMP_NUMBER_OF_AXES ; i++) { - axes[i] = raw[i] * devp->compsensitivity[i]; - axes[i] -= devp->compbias[i]; + axes[i] = (raw[i] * devp->compsensitivity[i]) - devp->compbias[i]; } return msg; } @@ -561,7 +555,7 @@ static msg_t comp_set_bias(void *ip, float *bp) { /* Getting parent instance pointer.*/ devp = objGetInstance(LSM303DLHCDriver*, (BaseCompass*)ip); - osalDbgAssert((devp->state != LSM303DLHC_UNINIT), + osalDbgAssert((devp->state == LSM303DLHC_READY), "comp_set_bias(), invalid state"); for(i = 0; i < LSM303DLHC_COMP_NUMBER_OF_AXES; i++) { @@ -590,7 +584,7 @@ static msg_t comp_reset_bias(void *ip) { /* Getting parent instance pointer.*/ devp = objGetInstance(LSM303DLHCDriver*, (BaseCompass*)ip); - osalDbgAssert((devp->state != LSM303DLHC_UNINIT), + osalDbgAssert((devp->state == LSM303DLHC_READY), "comp_reset_bias(), invalid state"); for(i = 0; i < LSM303DLHC_COMP_NUMBER_OF_AXES; i++) @@ -620,7 +614,7 @@ static msg_t comp_set_sensivity(void *ip, float *sp) { osalDbgCheck((ip != NULL) && (sp != NULL)); - osalDbgAssert((devp->state != LSM303DLHC_UNINIT), + osalDbgAssert((devp->state == LSM303DLHC_READY), "comp_set_sensivity(), invalid state"); for(i = 0; i < LSM303DLHC_COMP_NUMBER_OF_AXES; i++) { @@ -649,7 +643,7 @@ static msg_t comp_reset_sensivity(void *ip) { /* Getting parent instance pointer.*/ devp = objGetInstance(LSM303DLHCDriver*, (BaseCompass*)ip); - osalDbgAssert((devp->state != LSM303DLHC_UNINIT), + osalDbgAssert((devp->state == LSM303DLHC_READY), "comp_reset_sensivity(), invalid state"); if(devp->config->compfullscale == LSM303DLHC_COMP_FS_1P3GA) @@ -743,7 +737,7 @@ static msg_t comp_set_full_scale(LSM303DLHCDriver *devp, osalDbgCheck(devp != NULL); - osalDbgAssert((devp->state != LSM303DLHC_UNINIT), + osalDbgAssert((devp->state == LSM303DLHC_READY), "comp_set_full_scale(), invalid state"); osalDbgAssert((devp->config->i2cp->state == I2C_READY), "comp_set_full_scale(), channel not ready"); @@ -893,11 +887,6 @@ void lsm303dlhcStart(LSM303DLHCDriver *devp, const LSM303DLHCConfig *config) { devp->config = config; -#if LSM303DLHC_SHARED_I2C - i2cAcquireBus((devp)->config->i2cp); -#endif /* LSM303DLHC_SHARED_I2C */ - i2cStart((devp)->config->i2cp, (devp)->config->i2ccfg); - /* Configuring Accelerometer subsystem */ /* Multiple write starting address.*/ @@ -931,7 +920,17 @@ void lsm303dlhcStart(LSM303DLHCDriver *devp, const LSM303DLHCConfig *config) { devp->config->acchighresmode; #endif } + +#if LSM303DLHC_SHARED_I2C + i2cAcquireBus((devp)->config->i2cp); +#endif /* LSM303DLHC_SHARED_I2C */ + i2cStart((devp)->config->i2cp, (devp)->config->i2ccfg); + lsm303dlhcI2CWriteRegister(devp->config->i2cp, LSM303DLHC_SAD_ACC, cr, 4); + +#if LSM303DLHC_SHARED_I2C + i2cReleaseBus((devp)->config->i2cp); +#endif /* LSM303DLHC_SHARED_I2C */ /* Storing sensitivity according to user settings */ if(devp->config->accfullscale == LSM303DLHC_ACC_FS_2G) { @@ -1001,9 +1000,18 @@ void lsm303dlhcStart(LSM303DLHCDriver *devp, const LSM303DLHCConfig *config) { #endif } +#if LSM303DLHC_SHARED_I2C + i2cAcquireBus((devp)->config->i2cp); + i2cStart((devp)->config->i2cp, (devp)->config->i2ccfg); +#endif /* LSM303DLHC_SHARED_I2C */ + lsm303dlhcI2CWriteRegister(devp->config->i2cp, LSM303DLHC_SAD_COMP, cr, 3); +#if LSM303DLHC_SHARED_I2C + i2cReleaseBus((devp)->config->i2cp); +#endif /* LSM303DLHC_SHARED_I2C */ + if(devp->config->compfullscale == LSM303DLHC_COMP_FS_1P3GA) { devp->compfullscale = LSM303DLHC_COMP_1P3GA; for(i = 0; i < LSM303DLHC_COMP_NUMBER_OF_AXES; i++) { @@ -1128,9 +1136,6 @@ void lsm303dlhcStart(LSM303DLHCDriver *devp, const LSM303DLHCConfig *config) { osalThreadSleepMilliseconds(5); devp->state = LSM303DLHC_READY; -#if LSM303DLHC_SHARED_I2C - i2cReleaseBus((devp)->config->i2cp); -#endif /* LSM303DLHC_SHARED_I2C */ } /** -- cgit v1.2.3