andreika-git
/
ChibiOS
mirror of https://github.com/andreika-git/ChibiOS.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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
This commit is contained in:
Rocco Marco Guglielmi 2018-03-09 23:14:37 +00:00
parent ae9e1ec08f
commit 610290753d
2 changed files with 38 additions and 38 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
os/ex/ST/l3gd20.c
View File

@ -177,15 +177,10 @@ static msg_t gyro_read_cooked(void *ip, float axes[]) {
osalDbgAssert((devp->state == L3GD20_READY), osalDbgAssert((devp->state == L3GD20_READY),
"gyro_read_cooked(), invalid state"); "gyro_read_cooked(), invalid state");
#if L3GD20_USE_SPI
osalDbgAssert((devp->config->spip->state == SPI_READY),
"gyro_read_cooked(), channel not ready");
#endif
msg = gyro_read_raw(ip, raw); msg = gyro_read_raw(ip, raw);
for(i = 0; i < L3GD20_GYRO_NUMBER_OF_AXES; i++){ for(i = 0; i < L3GD20_GYRO_NUMBER_OF_AXES; i++){
axes[i] = raw[i] * devp->gyrosensitivity[i]; axes[i] = (raw[i] * devp->gyrosensitivity[i]) - devp->gyrobias[i];
axes[i] -= devp->gyrobias[i];
} }
return msg; return msg;
} }
@ -259,7 +254,7 @@ static msg_t gyro_set_bias(void *ip, float *bp) {
/* Getting parent instance pointer.*/ /* Getting parent instance pointer.*/
devp = objGetInstance(L3GD20Driver*, (BaseGyroscope*)ip); devp = objGetInstance(L3GD20Driver*, (BaseGyroscope*)ip);
osalDbgAssert((devp->state != L3GD20_UNINIT), osalDbgAssert((devp->state == L3GD20_READY),
"gyro_set_bias(), invalid state"); "gyro_set_bias(), invalid state");
for(i = 0; i < L3GD20_GYRO_NUMBER_OF_AXES; i++) { 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.*/ /* Getting parent instance pointer.*/
devp = objGetInstance(L3GD20Driver*, (BaseGyroscope*)ip); devp = objGetInstance(L3GD20Driver*, (BaseGyroscope*)ip);
osalDbgAssert((devp->state != L3GD20_UNINIT), osalDbgAssert((devp->state == L3GD20_READY),
"gyro_reset_bias(), invalid state"); "gyro_reset_bias(), invalid state");
for(i = 0; i < L3GD20_GYRO_NUMBER_OF_AXES; i++) 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.*/ /* Getting parent instance pointer.*/
devp = objGetInstance(L3GD20Driver*, (BaseGyroscope*)ip); devp = objGetInstance(L3GD20Driver*, (BaseGyroscope*)ip);
osalDbgAssert((devp->state != L3GD20_UNINIT), osalDbgAssert((devp->state == L3GD20_READY),
"gyro_set_sensivity(), invalid state"); "gyro_set_sensivity(), invalid state");
for(i = 0; i < L3GD20_GYRO_NUMBER_OF_AXES; i++) { 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.*/ /* Getting parent instance pointer.*/
devp = objGetInstance(L3GD20Driver*, (BaseGyroscope*)ip); devp = objGetInstance(L3GD20Driver*, (BaseGyroscope*)ip);
osalDbgAssert((devp->state != L3GD20_UNINIT), osalDbgAssert((devp->state == L3GD20_READY),
"gyro_reset_sensivity(), invalid state"); "gyro_reset_sensivity(), invalid state");
if(devp->config->gyrofullscale == L3GD20_FS_250DPS) 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); osalDbgCheck(devp != NULL);
osalDbgAssert((devp->state != L3GD20_UNINIT), osalDbgAssert((devp->state == L3GD20_READY),
"gyro_set_full_scale(), invalid state"); "gyro_set_full_scale(), invalid state");
#if L3GD20_USE_SPI #if L3GD20_USE_SPI
osalDbgAssert((devp->config->spip->state == SPI_READY), osalDbgAssert((devp->config->spip->state == SPI_READY),

57
os/ex/ST/lsm303dlhc.c
View File

@ -191,13 +191,10 @@ static msg_t acc_read_cooked(void *ip, float axes[]) {
osalDbgAssert((devp->state == LSM303DLHC_READY), osalDbgAssert((devp->state == LSM303DLHC_READY),
"acc_read_cooked(), invalid state"); "acc_read_cooked(), invalid state");
osalDbgAssert((devp->config->i2cp->state == I2C_READY),
"acc_read_cooked(), channel not ready");
msg = acc_read_raw(ip, raw); msg = acc_read_raw(ip, raw);
for(i = 0; i < LSM303DLHC_ACC_NUMBER_OF_AXES; i++) { for(i = 0; i < LSM303DLHC_ACC_NUMBER_OF_AXES; i++) {
axes[i] = raw[i] * devp->accsensitivity[i]; axes[i] = (raw[i] * devp->accsensitivity[i]) - devp->accbias[i];
axes[i] -= devp->accbias[i];
} }
return msg; return msg;
} }
@ -224,7 +221,7 @@ static msg_t acc_set_bias(void *ip, float *bp) {
/* Getting parent instance pointer.*/ /* Getting parent instance pointer.*/
devp = objGetInstance(LSM303DLHCDriver*, (BaseAccelerometer*)ip); devp = objGetInstance(LSM303DLHCDriver*, (BaseAccelerometer*)ip);
osalDbgAssert((devp->state != LSM303DLHC_UNINIT), osalDbgAssert((devp->state == LSM303DLHC_READY),
"acc_set_bias(), invalid state"); "acc_set_bias(), invalid state");
for(i = 0; i < LSM303DLHC_ACC_NUMBER_OF_AXES; i++) { 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.*/ /* Getting parent instance pointer.*/
devp = objGetInstance(LSM303DLHCDriver*, (BaseAccelerometer*)ip); devp = objGetInstance(LSM303DLHCDriver*, (BaseAccelerometer*)ip);
osalDbgAssert((devp->state != LSM303DLHC_UNINIT), osalDbgAssert((devp->state == LSM303DLHC_READY),
"acc_reset_bias(), invalid state"); "acc_reset_bias(), invalid state");
for(i = 0; i < LSM303DLHC_ACC_NUMBER_OF_AXES; i++) 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)); osalDbgCheck((ip != NULL) && (sp != NULL));
osalDbgAssert((devp->state != LSM303DLHC_UNINIT), osalDbgAssert((devp->state == LSM303DLHC_READY),
"acc_set_sensivity(), invalid state"); "acc_set_sensivity(), invalid state");
for(i = 0; i < LSM303DLHC_ACC_NUMBER_OF_AXES; i++) { 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.*/ /* Getting parent instance pointer.*/
devp = objGetInstance(LSM303DLHCDriver*, (BaseAccelerometer*)ip); devp = objGetInstance(LSM303DLHCDriver*, (BaseAccelerometer*)ip);
osalDbgAssert((devp->state != LSM303DLHC_UNINIT), osalDbgAssert((devp->state == LSM303DLHC_READY),
"acc_reset_sensivity(), invalid state"); "acc_reset_sensivity(), invalid state");
if(devp->config->accfullscale == LSM303DLHC_ACC_FS_2G) if(devp->config->accfullscale == LSM303DLHC_ACC_FS_2G)
@ -355,7 +352,7 @@ static msg_t acc_set_full_scale(LSM303DLHCDriver *devp,
osalDbgCheck(devp != NULL); osalDbgCheck(devp != NULL);
osalDbgAssert((devp->state != LSM303DLHC_UNINIT), osalDbgAssert((devp->state == LSM303DLHC_READY),
"acc_set_full_scale(), invalid state"); "acc_set_full_scale(), invalid state");
osalDbgAssert((devp->config->i2cp->state == I2C_READY), osalDbgAssert((devp->config->i2cp->state == I2C_READY),
"acc_set_full_scale(), channel not 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), osalDbgAssert((devp->state == LSM303DLHC_READY),
"comp_read_cooked(), invalid state"); "comp_read_cooked(), invalid state");
osalDbgAssert((devp->config->i2cp->state == I2C_READY),
"comp_read_cooked(), channel not ready");
msg = comp_read_raw(ip, raw); msg = comp_read_raw(ip, raw);
for(i = 0; i < LSM303DLHC_COMP_NUMBER_OF_AXES ; i++) { for(i = 0; i < LSM303DLHC_COMP_NUMBER_OF_AXES ; i++) {
axes[i] = raw[i] * devp->compsensitivity[i]; axes[i] = (raw[i] * devp->compsensitivity[i]) - devp->compbias[i];
axes[i] -= devp->compbias[i];
} }
return msg; return msg;
} }
@ -561,7 +555,7 @@ static msg_t comp_set_bias(void *ip, float *bp) {
/* Getting parent instance pointer.*/ /* Getting parent instance pointer.*/
devp = objGetInstance(LSM303DLHCDriver*, (BaseCompass*)ip); devp = objGetInstance(LSM303DLHCDriver*, (BaseCompass*)ip);
osalDbgAssert((devp->state != LSM303DLHC_UNINIT), osalDbgAssert((devp->state == LSM303DLHC_READY),
"comp_set_bias(), invalid state"); "comp_set_bias(), invalid state");
for(i = 0; i < LSM303DLHC_COMP_NUMBER_OF_AXES; i++) { 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.*/ /* Getting parent instance pointer.*/
devp = objGetInstance(LSM303DLHCDriver*, (BaseCompass*)ip); devp = objGetInstance(LSM303DLHCDriver*, (BaseCompass*)ip);
osalDbgAssert((devp->state != LSM303DLHC_UNINIT), osalDbgAssert((devp->state == LSM303DLHC_READY),
"comp_reset_bias(), invalid state"); "comp_reset_bias(), invalid state");
for(i = 0; i < LSM303DLHC_COMP_NUMBER_OF_AXES; i++) 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)); osalDbgCheck((ip != NULL) && (sp != NULL));
osalDbgAssert((devp->state != LSM303DLHC_UNINIT), osalDbgAssert((devp->state == LSM303DLHC_READY),
"comp_set_sensivity(), invalid state"); "comp_set_sensivity(), invalid state");
for(i = 0; i < LSM303DLHC_COMP_NUMBER_OF_AXES; i++) { 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.*/ /* Getting parent instance pointer.*/
devp = objGetInstance(LSM303DLHCDriver*, (BaseCompass*)ip); devp = objGetInstance(LSM303DLHCDriver*, (BaseCompass*)ip);
osalDbgAssert((devp->state != LSM303DLHC_UNINIT), osalDbgAssert((devp->state == LSM303DLHC_READY),
"comp_reset_sensivity(), invalid state"); "comp_reset_sensivity(), invalid state");
if(devp->config->compfullscale == LSM303DLHC_COMP_FS_1P3GA) if(devp->config->compfullscale == LSM303DLHC_COMP_FS_1P3GA)
@ -743,7 +737,7 @@ static msg_t comp_set_full_scale(LSM303DLHCDriver *devp,
osalDbgCheck(devp != NULL); osalDbgCheck(devp != NULL);
osalDbgAssert((devp->state != LSM303DLHC_UNINIT), osalDbgAssert((devp->state == LSM303DLHC_READY),
"comp_set_full_scale(), invalid state"); "comp_set_full_scale(), invalid state");
osalDbgAssert((devp->config->i2cp->state == I2C_READY), osalDbgAssert((devp->config->i2cp->state == I2C_READY),
"comp_set_full_scale(), channel not ready"); "comp_set_full_scale(), channel not ready");
@ -893,11 +887,6 @@ void lsm303dlhcStart(LSM303DLHCDriver *devp, const LSM303DLHCConfig *config) {
devp->config = 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 */ /* Configuring Accelerometer subsystem */
/* Multiple write starting address.*/ /* Multiple write starting address.*/
@ -931,8 +920,18 @@ void lsm303dlhcStart(LSM303DLHCDriver *devp, const LSM303DLHCConfig *config) {
devp->config->acchighresmode; devp->config->acchighresmode;
#endif #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); 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 */ /* Storing sensitivity according to user settings */
if(devp->config->accfullscale == LSM303DLHC_ACC_FS_2G) { if(devp->config->accfullscale == LSM303DLHC_ACC_FS_2G) {
devp->accfullscale = LSM303DLHC_ACC_2G; devp->accfullscale = LSM303DLHC_ACC_2G;
@ -1001,9 +1000,18 @@ void lsm303dlhcStart(LSM303DLHCDriver *devp, const LSM303DLHCConfig *config) {
#endif #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, lsm303dlhcI2CWriteRegister(devp->config->i2cp, LSM303DLHC_SAD_COMP,
cr, 3); cr, 3);
#if LSM303DLHC_SHARED_I2C
i2cReleaseBus((devp)->config->i2cp);
#endif /* LSM303DLHC_SHARED_I2C */
if(devp->config->compfullscale == LSM303DLHC_COMP_FS_1P3GA) { if(devp->config->compfullscale == LSM303DLHC_COMP_FS_1P3GA) {
devp->compfullscale = LSM303DLHC_COMP_1P3GA; devp->compfullscale = LSM303DLHC_COMP_1P3GA;
for(i = 0; i < LSM303DLHC_COMP_NUMBER_OF_AXES; i++) { for(i = 0; i < LSM303DLHC_COMP_NUMBER_OF_AXES; i++) {
@ -1128,9 +1136,6 @@ void lsm303dlhcStart(LSM303DLHCDriver *devp, const LSM303DLHCConfig *config) {
osalThreadSleepMilliseconds(5); osalThreadSleepMilliseconds(5);
devp->state = LSM303DLHC_READY; devp->state = LSM303DLHC_READY;
#if LSM303DLHC_SHARED_I2C
i2cReleaseBus((devp)->config->i2cp);
#endif /* LSM303DLHC_SHARED_I2C */
} }
/** /**