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

LSM303DLHC driver completely updated 

git-svn-id: https://svn.code.sf.net/p/chibios/svn2/trunk@11591 110e8d01-0319-4d1e-a829-52ad28d1bb01
This commit is contained in:
Rocco Marco Guglielmi 2018-02-28 10:04:12 +00:00
parent 4fe88d34a4
commit 4e4b7ef08e
2 changed files with 571 additions and 325 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -312,16 +312,16 @@ static msg_t acc_reset_sensivity(void *ip) {
osalDbgAssert((devp->state != LSM303DLHC_UNINIT), osalDbgAssert((devp->state != LSM303DLHC_UNINIT),
"acc_reset_sensivity(), invalid state"); "acc_reset_sensivity(), invalid state");
if(devp->config->acccfg->fullscale == LSM303DLHC_ACC_FS_2G) if(devp->config->accfullscale == LSM303DLHC_ACC_FS_2G)
for(i = 0; i < LSM303DLHC_ACC_NUMBER_OF_AXES; i++) for(i = 0; i < LSM303DLHC_ACC_NUMBER_OF_AXES; i++)
devp->accsensitivity[i] = LSM303DLHC_ACC_SENS_2G; devp->accsensitivity[i] = LSM303DLHC_ACC_SENS_2G;
else if(devp->config->acccfg->fullscale == LSM303DLHC_ACC_FS_4G) else if(devp->config->accfullscale == LSM303DLHC_ACC_FS_4G)
for(i = 0; i < LSM303DLHC_ACC_NUMBER_OF_AXES; i++) for(i = 0; i < LSM303DLHC_ACC_NUMBER_OF_AXES; i++)
devp->accsensitivity[i] = LSM303DLHC_ACC_SENS_4G; devp->accsensitivity[i] = LSM303DLHC_ACC_SENS_4G;
else if(devp->config->acccfg->fullscale == LSM303DLHC_ACC_FS_8G) else if(devp->config->accfullscale == LSM303DLHC_ACC_FS_8G)
for(i = 0; i < LSM303DLHC_ACC_NUMBER_OF_AXES; i++) for(i = 0; i < LSM303DLHC_ACC_NUMBER_OF_AXES; i++)
devp->accsensitivity[i] = LSM303DLHC_ACC_SENS_8G; devp->accsensitivity[i] = LSM303DLHC_ACC_SENS_8G;
else if(devp->config->acccfg->fullscale == LSM303DLHC_ACC_FS_16G) else if(devp->config->accfullscale == LSM303DLHC_ACC_FS_16G)
for(i = 0; i < LSM303DLHC_ACC_NUMBER_OF_AXES; i++) for(i = 0; i < LSM303DLHC_ACC_NUMBER_OF_AXES; i++)
devp->accsensitivity[i] = LSM303DLHC_ACC_SENS_16G; devp->accsensitivity[i] = LSM303DLHC_ACC_SENS_16G;
else { else {
@ -646,7 +646,7 @@ static msg_t comp_reset_sensivity(void *ip) {
osalDbgAssert((devp->state != LSM303DLHC_UNINIT), osalDbgAssert((devp->state != LSM303DLHC_UNINIT),
"comp_reset_sensivity(), invalid state"); "comp_reset_sensivity(), invalid state");
if(devp->config->compcfg->fullscale == LSM303DLHC_COMP_FS_1P3GA) if(devp->config->compfullscale == LSM303DLHC_COMP_FS_1P3GA)
for(i = 0; i < LSM303DLHC_COMP_NUMBER_OF_AXES; i++) { for(i = 0; i < LSM303DLHC_COMP_NUMBER_OF_AXES; i++) {
if(i != 2) { if(i != 2) {
devp->compsensitivity[i] = LSM303DLHC_COMP_SENS_XY_1P3GA; devp->compsensitivity[i] = LSM303DLHC_COMP_SENS_XY_1P3GA;
@ -655,7 +655,7 @@ static msg_t comp_reset_sensivity(void *ip) {
devp->compsensitivity[i] = LSM303DLHC_COMP_SENS_Z_1P3GA; devp->compsensitivity[i] = LSM303DLHC_COMP_SENS_Z_1P3GA;
} }
} }
else if(devp->config->compcfg->fullscale == LSM303DLHC_COMP_FS_1P9GA) else if(devp->config->compfullscale == LSM303DLHC_COMP_FS_1P9GA)
for(i = 0; i < LSM303DLHC_COMP_NUMBER_OF_AXES; i++) { for(i = 0; i < LSM303DLHC_COMP_NUMBER_OF_AXES; i++) {
if(i != 2) { if(i != 2) {
devp->compsensitivity[i] = LSM303DLHC_COMP_SENS_XY_1P9GA; devp->compsensitivity[i] = LSM303DLHC_COMP_SENS_XY_1P9GA;
@ -664,7 +664,7 @@ static msg_t comp_reset_sensivity(void *ip) {
devp->compsensitivity[i] = LSM303DLHC_COMP_SENS_Z_1P9GA; devp->compsensitivity[i] = LSM303DLHC_COMP_SENS_Z_1P9GA;
} }
} }
else if(devp->config->compcfg->fullscale == LSM303DLHC_COMP_FS_2P5GA) else if(devp->config->compfullscale == LSM303DLHC_COMP_FS_2P5GA)
for(i = 0; i < LSM303DLHC_COMP_NUMBER_OF_AXES; i++) { for(i = 0; i < LSM303DLHC_COMP_NUMBER_OF_AXES; i++) {
if(i != 2) { if(i != 2) {
devp->compsensitivity[i] = LSM303DLHC_COMP_SENS_XY_2P5GA; devp->compsensitivity[i] = LSM303DLHC_COMP_SENS_XY_2P5GA;
@ -673,7 +673,7 @@ static msg_t comp_reset_sensivity(void *ip) {
devp->compsensitivity[i] = LSM303DLHC_COMP_SENS_Z_2P5GA; devp->compsensitivity[i] = LSM303DLHC_COMP_SENS_Z_2P5GA;
} }
} }
else if(devp->config->compcfg->fullscale == LSM303DLHC_COMP_FS_4P0GA) else if(devp->config->compfullscale == LSM303DLHC_COMP_FS_4P0GA)
for(i = 0; i < LSM303DLHC_COMP_NUMBER_OF_AXES; i++) { for(i = 0; i < LSM303DLHC_COMP_NUMBER_OF_AXES; i++) {
if(i != 2) { if(i != 2) {
devp->compsensitivity[i] = LSM303DLHC_COMP_SENS_XY_4P0GA; devp->compsensitivity[i] = LSM303DLHC_COMP_SENS_XY_4P0GA;
@ -682,7 +682,7 @@ static msg_t comp_reset_sensivity(void *ip) {
devp->compsensitivity[i] = LSM303DLHC_COMP_SENS_Z_4P0GA; devp->compsensitivity[i] = LSM303DLHC_COMP_SENS_Z_4P0GA;
} }
} }
else if(devp->config->compcfg->fullscale == LSM303DLHC_COMP_FS_4P7GA) else if(devp->config->compfullscale == LSM303DLHC_COMP_FS_4P7GA)
for(i = 0; i < LSM303DLHC_COMP_NUMBER_OF_AXES; i++) { for(i = 0; i < LSM303DLHC_COMP_NUMBER_OF_AXES; i++) {
if(i != 2) { if(i != 2) {
devp->compsensitivity[i] = LSM303DLHC_COMP_SENS_XY_4P7GA; devp->compsensitivity[i] = LSM303DLHC_COMP_SENS_XY_4P7GA;
@ -691,7 +691,7 @@ static msg_t comp_reset_sensivity(void *ip) {
devp->compsensitivity[i] = LSM303DLHC_COMP_SENS_Z_4P7GA; devp->compsensitivity[i] = LSM303DLHC_COMP_SENS_Z_4P7GA;
} }
} }
else if(devp->config->compcfg->fullscale == LSM303DLHC_COMP_FS_5P6GA) else if(devp->config->compfullscale == LSM303DLHC_COMP_FS_5P6GA)
for(i = 0; i < LSM303DLHC_COMP_NUMBER_OF_AXES; i++) { for(i = 0; i < LSM303DLHC_COMP_NUMBER_OF_AXES; i++) {
if(i != 2) { if(i != 2) {
devp->compsensitivity[i] = LSM303DLHC_COMP_SENS_XY_5P6GA; devp->compsensitivity[i] = LSM303DLHC_COMP_SENS_XY_5P6GA;
@ -700,7 +700,7 @@ static msg_t comp_reset_sensivity(void *ip) {
devp->compsensitivity[i] = LSM303DLHC_COMP_SENS_Z_5P6GA; devp->compsensitivity[i] = LSM303DLHC_COMP_SENS_Z_5P6GA;
} }
} }
else if(devp->config->compcfg->fullscale == LSM303DLHC_COMP_FS_8P1GA) else if(devp->config->compfullscale == LSM303DLHC_COMP_FS_8P1GA)
for(i = 0; i < LSM303DLHC_COMP_NUMBER_OF_AXES; i++) { for(i = 0; i < LSM303DLHC_COMP_NUMBER_OF_AXES; i++) {
if(i != 2) { if(i != 2) {
devp->compsensitivity[i] = LSM303DLHC_COMP_SENS_XY_8P1GA; devp->compsensitivity[i] = LSM303DLHC_COMP_SENS_XY_8P1GA;
@ -853,8 +853,8 @@ void lsm303dlhcObjectInit(LSM303DLHCDriver *devp) {
uint32_t i; uint32_t i;
devp->vmt = &vmt_device; devp->vmt = &vmt_device;
devp->accelerometer_if.vmt = &vmt_accelerometer; devp->acc_if.vmt = &vmt_accelerometer;
devp->compass_if.vmt = &vmt_compass; devp->comp_if.vmt = &vmt_compass;
devp->config = NULL; devp->config = NULL;
@ -893,16 +893,16 @@ void lsm303dlhcStart(LSM303DLHCDriver *devp, const LSM303DLHCConfig *config) {
i2cStart((devp)->config->i2cp, (devp)->config->i2ccfg); i2cStart((devp)->config->i2cp, (devp)->config->i2ccfg);
/* Configuring Accelerometer subsystem */ /* Configuring Accelerometer subsystem */
if((devp)->config->acccfg != NULL) {
/* Multiple write starting address.*/ /* Multiple write starting address.*/
cr[0] = LSM303DLHC_AD_ACC_CTRL_REG1; cr[0] = LSM303DLHC_AD_ACC_CTRL_REG1;
/* Control register 1 configuration block.*/ /* Control register 1 configuration block.*/
{ {
cr[1] = LSM303DLHC_CTRL_REG1_A_XEN | LSM303DLHC_CTRL_REG1_A_YEN | cr[1] = LSM303DLHC_CTRL_REG1_A_XEN | LSM303DLHC_CTRL_REG1_A_YEN |
LSM303DLHC_CTRL_REG1_A_ZEN | devp->config->acccfg->outdatarate; LSM303DLHC_CTRL_REG1_A_ZEN | devp->config->accoutdatarate;
#if LSM303DLHC_ACC_USE_ADVANCED || defined(__DOXYGEN__) #if LSM303DLHC_ACC_USE_ADVANCED || defined(__DOXYGEN__)
cr[1] |= devp->config->acccfg->lowpower; cr[1] |= devp->config->acclowpower;
#endif #endif
} }
@ -918,93 +918,90 @@ void lsm303dlhcStart(LSM303DLHCDriver *devp, const LSM303DLHCConfig *config) {
/* Control register 4 configuration block.*/ /* Control register 4 configuration block.*/
{ {
cr[4] = devp->config->acccfg->fullscale; cr[4] = devp->config->accfullscale;
#if LSM303DLHC_ACC_USE_ADVANCED || defined(__DOXYGEN__) #if LSM303DLHC_ACC_USE_ADVANCED || defined(__DOXYGEN__)
cr[4] |= devp->config->acccfg->endianess | cr[4] |= devp->config->accendianess |
devp->config->acccfg->blockdataupdate | devp->config->accblockdataupdate |
devp->config->acccfg->highresmode; devp->config->acchighresmode;
#endif #endif
} }
lsm303dlhcI2CWriteRegister(devp->config->i2cp, LSM303DLHC_SAD_ACC, cr, 4); lsm303dlhcI2CWriteRegister(devp->config->i2cp, LSM303DLHC_SAD_ACC, cr, 4);
/* Storing sensitivity according to user settings */ /* Storing sensitivity according to user settings */
if(devp->config->acccfg->fullscale == LSM303DLHC_ACC_FS_2G) { if(devp->config->accfullscale == LSM303DLHC_ACC_FS_2G) {
devp->accfullscale = LSM303DLHC_ACC_2G; devp->accfullscale = LSM303DLHC_ACC_2G;
for(i = 0; i < LSM303DLHC_ACC_NUMBER_OF_AXES; i++) { for(i = 0; i < LSM303DLHC_ACC_NUMBER_OF_AXES; i++) {
if(devp->config->acccfg->sensitivity == NULL) if(devp->config->accsensitivity == NULL)
devp->accsensitivity[i] = LSM303DLHC_ACC_SENS_2G; devp->accsensitivity[i] = LSM303DLHC_ACC_SENS_2G;
else else
devp->accsensitivity[i] = devp->config->acccfg->sensitivity[i]; devp->accsensitivity[i] = devp->config->accsensitivity[i];
} }
} }
else if(devp->config->acccfg->fullscale == LSM303DLHC_ACC_FS_4G) { else if(devp->config->accfullscale == LSM303DLHC_ACC_FS_4G) {
devp->accfullscale = LSM303DLHC_ACC_4G; devp->accfullscale = LSM303DLHC_ACC_4G;
for(i = 0; i < LSM303DLHC_ACC_NUMBER_OF_AXES; i++) { for(i = 0; i < LSM303DLHC_ACC_NUMBER_OF_AXES; i++) {
if(devp->config->acccfg->sensitivity == NULL) if(devp->config->accsensitivity == NULL)
devp->accsensitivity[i] = LSM303DLHC_ACC_SENS_4G; devp->accsensitivity[i] = LSM303DLHC_ACC_SENS_4G;
else else
devp->accsensitivity[i] = devp->config->acccfg->sensitivity[i]; devp->accsensitivity[i] = devp->config->accsensitivity[i];
} }
} }
else if(devp->config->acccfg->fullscale == LSM303DLHC_ACC_FS_8G) { else if(devp->config->accfullscale == LSM303DLHC_ACC_FS_8G) {
devp->accfullscale = LSM303DLHC_ACC_8G; devp->accfullscale = LSM303DLHC_ACC_8G;
for(i = 0; i < LSM303DLHC_ACC_NUMBER_OF_AXES; i++) { for(i = 0; i < LSM303DLHC_ACC_NUMBER_OF_AXES; i++) {
if(devp->config->acccfg->sensitivity == NULL) if(devp->config->accsensitivity == NULL)
devp->accsensitivity[i] = LSM303DLHC_ACC_SENS_8G; devp->accsensitivity[i] = LSM303DLHC_ACC_SENS_8G;
else else
devp->accsensitivity[i] = devp->config->acccfg->sensitivity[i]; devp->accsensitivity[i] = devp->config->accsensitivity[i];
} }
} }
else if(devp->config->acccfg->fullscale == LSM303DLHC_ACC_FS_16G) { else if(devp->config->accfullscale == LSM303DLHC_ACC_FS_16G) {
devp->accfullscale = LSM303DLHC_ACC_16G; devp->accfullscale = LSM303DLHC_ACC_16G;
for(i = 0; i < LSM303DLHC_ACC_NUMBER_OF_AXES; i++) { for(i = 0; i < LSM303DLHC_ACC_NUMBER_OF_AXES; i++) {
if(devp->config->acccfg->sensitivity == NULL) if(devp->config->accsensitivity == NULL)
devp->accsensitivity[i] = LSM303DLHC_ACC_SENS_16G; devp->accsensitivity[i] = LSM303DLHC_ACC_SENS_16G;
else else
devp->accsensitivity[i] = devp->config->acccfg->sensitivity[i]; devp->accsensitivity[i] = devp->config->accsensitivity[i];
} }
} }
else else
osalDbgAssert(FALSE, "lsm303dlhcStart(), accelerometer full scale issue"); osalDbgAssert(FALSE, "lsm303dlhcStart(), accelerometer full scale issue");
/* Storing bias information */ /* Storing bias information */
if(devp->config->acccfg->bias != NULL) if(devp->config->accbias != NULL)
for(i = 0; i < LSM303DLHC_ACC_NUMBER_OF_AXES; i++) for(i = 0; i < LSM303DLHC_ACC_NUMBER_OF_AXES; i++)
devp->accbias[i] = devp->config->acccfg->bias[i]; devp->accbias[i] = devp->config->accbias[i];
}
/* Configuring Compass subsystem */ /* Configuring Compass subsystem */
if((devp)->config->compcfg != NULL) {
/* Multiple write starting address.*/ /* Multiple write starting address.*/
cr[0] = LSM303DLHC_AD_COMP_CRA_REG; cr[0] = LSM303DLHC_AD_COMP_CRA_REG;
/* Control register A configuration block.*/ /* Control register A configuration block.*/
{ {
cr[1] = devp->config->compcfg->outputdatarate; cr[1] = devp->config->compoutputdatarate;
} }
/* Control register B configuration block.*/ /* Control register B configuration block.*/
{ {
cr[2] = devp->config->compcfg->fullscale; cr[2] = devp->config->compfullscale;
} }
/* Mode register configuration block.*/ /* Mode register configuration block.*/
{ {
cr[3] = 0; cr[3] = 0;
#if LSM303DLHC_COMP_USE_ADVANCED || defined(__DOXYGEN__) #if LSM303DLHC_COMP_USE_ADVANCED || defined(__DOXYGEN__)
cr[3] |= devp->config->compcfg->mode; cr[3] |= devp->config->compmode;
#endif #endif
} }
lsm303dlhcI2CWriteRegister(devp->config->i2cp, LSM303DLHC_SAD_COMP, lsm303dlhcI2CWriteRegister(devp->config->i2cp, LSM303DLHC_SAD_COMP,
cr, 3); cr, 3);
if(devp->config->compcfg->fullscale == 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++) {
if(devp->config->compcfg->sensitivity == NULL) { if(devp->config->compsensitivity == NULL) {
if(i != 2) { if(i != 2) {
devp->compsensitivity[i] = LSM303DLHC_COMP_SENS_XY_1P3GA; devp->compsensitivity[i] = LSM303DLHC_COMP_SENS_XY_1P3GA;
} }
@ -1013,14 +1010,14 @@ void lsm303dlhcStart(LSM303DLHCDriver *devp, const LSM303DLHCConfig *config) {
} }
} }
else { else {
devp->compsensitivity[i] = devp->config->compcfg->sensitivity[i]; devp->compsensitivity[i] = devp->config->compsensitivity[i];
} }
} }
} }
else if(devp->config->compcfg->fullscale == LSM303DLHC_COMP_FS_1P9GA) { else if(devp->config->compfullscale == LSM303DLHC_COMP_FS_1P9GA) {
devp->compfullscale = LSM303DLHC_COMP_1P9GA; devp->compfullscale = LSM303DLHC_COMP_1P9GA;
for(i = 0; i < LSM303DLHC_COMP_NUMBER_OF_AXES; i++) { for(i = 0; i < LSM303DLHC_COMP_NUMBER_OF_AXES; i++) {
if(devp->config->compcfg->sensitivity == NULL) { if(devp->config->compsensitivity == NULL) {
if(i != 2) { if(i != 2) {
devp->compsensitivity[i] = LSM303DLHC_COMP_SENS_XY_1P9GA; devp->compsensitivity[i] = LSM303DLHC_COMP_SENS_XY_1P9GA;
} }
@ -1029,14 +1026,14 @@ void lsm303dlhcStart(LSM303DLHCDriver *devp, const LSM303DLHCConfig *config) {
} }
} }
else { else {
devp->compsensitivity[i] = devp->config->compcfg->sensitivity[i]; devp->compsensitivity[i] = devp->config->compsensitivity[i];
} }
} }
} }
else if(devp->config->compcfg->fullscale == LSM303DLHC_COMP_FS_2P5GA) { else if(devp->config->compfullscale == LSM303DLHC_COMP_FS_2P5GA) {
devp->compfullscale = LSM303DLHC_COMP_2P5GA; devp->compfullscale = LSM303DLHC_COMP_2P5GA;
for(i = 0; i < LSM303DLHC_COMP_NUMBER_OF_AXES; i++) { for(i = 0; i < LSM303DLHC_COMP_NUMBER_OF_AXES; i++) {
if(devp->config->compcfg->sensitivity == NULL) { if(devp->config->compsensitivity == NULL) {
if(i != 2) { if(i != 2) {
devp->compsensitivity[i] = LSM303DLHC_COMP_SENS_XY_2P5GA; devp->compsensitivity[i] = LSM303DLHC_COMP_SENS_XY_2P5GA;
} }
@ -1045,14 +1042,14 @@ void lsm303dlhcStart(LSM303DLHCDriver *devp, const LSM303DLHCConfig *config) {
} }
} }
else { else {
devp->compsensitivity[i] = devp->config->compcfg->sensitivity[i]; devp->compsensitivity[i] = devp->config->compsensitivity[i];
} }
} }
} }
else if(devp->config->compcfg->fullscale == LSM303DLHC_COMP_FS_4P0GA) { else if(devp->config->compfullscale == LSM303DLHC_COMP_FS_4P0GA) {
devp->compfullscale = LSM303DLHC_COMP_4P0GA; devp->compfullscale = LSM303DLHC_COMP_4P0GA;
for(i = 0; i < LSM303DLHC_COMP_NUMBER_OF_AXES; i++) { for(i = 0; i < LSM303DLHC_COMP_NUMBER_OF_AXES; i++) {
if(devp->config->compcfg->sensitivity == NULL) { if(devp->config->compsensitivity == NULL) {
if(i != 2) { if(i != 2) {
devp->compsensitivity[i] = LSM303DLHC_COMP_SENS_XY_4P0GA; devp->compsensitivity[i] = LSM303DLHC_COMP_SENS_XY_4P0GA;
} }
@ -1061,14 +1058,14 @@ void lsm303dlhcStart(LSM303DLHCDriver *devp, const LSM303DLHCConfig *config) {
} }
} }
else { else {
devp->compsensitivity[i] = devp->config->compcfg->sensitivity[i]; devp->compsensitivity[i] = devp->config->compsensitivity[i];
} }
} }
} }
else if(devp->config->compcfg->fullscale == LSM303DLHC_COMP_FS_4P7GA) { else if(devp->config->compfullscale == LSM303DLHC_COMP_FS_4P7GA) {
devp->compfullscale = LSM303DLHC_COMP_4P7GA; devp->compfullscale = LSM303DLHC_COMP_4P7GA;
for(i = 0; i < LSM303DLHC_COMP_NUMBER_OF_AXES; i++) { for(i = 0; i < LSM303DLHC_COMP_NUMBER_OF_AXES; i++) {
if(devp->config->compcfg->sensitivity == NULL) { if(devp->config->compsensitivity == NULL) {
if(i != 2) { if(i != 2) {
devp->compsensitivity[i] = LSM303DLHC_COMP_SENS_XY_4P7GA; devp->compsensitivity[i] = LSM303DLHC_COMP_SENS_XY_4P7GA;
} }
@ -1077,14 +1074,14 @@ void lsm303dlhcStart(LSM303DLHCDriver *devp, const LSM303DLHCConfig *config) {
} }
} }
else { else {
devp->compsensitivity[i] = devp->config->compcfg->sensitivity[i]; devp->compsensitivity[i] = devp->config->compsensitivity[i];
} }
} }
} }
else if(devp->config->compcfg->fullscale == LSM303DLHC_COMP_FS_5P6GA) { else if(devp->config->compfullscale == LSM303DLHC_COMP_FS_5P6GA) {
devp->compfullscale = LSM303DLHC_COMP_5P6GA; devp->compfullscale = LSM303DLHC_COMP_5P6GA;
for(i = 0; i < LSM303DLHC_COMP_NUMBER_OF_AXES; i++) { for(i = 0; i < LSM303DLHC_COMP_NUMBER_OF_AXES; i++) {
if(devp->config->compcfg->sensitivity == NULL) { if(devp->config->compsensitivity == NULL) {
if(i != 2) { if(i != 2) {
devp->compsensitivity[i] = LSM303DLHC_COMP_SENS_XY_5P6GA; devp->compsensitivity[i] = LSM303DLHC_COMP_SENS_XY_5P6GA;
} }
@ -1093,14 +1090,14 @@ void lsm303dlhcStart(LSM303DLHCDriver *devp, const LSM303DLHCConfig *config) {
} }
} }
else { else {
devp->compsensitivity[i] = devp->config->compcfg->sensitivity[i]; devp->compsensitivity[i] = devp->config->compsensitivity[i];
} }
} }
} }
else if(devp->config->compcfg->fullscale == LSM303DLHC_COMP_FS_8P1GA) { else if(devp->config->compfullscale == LSM303DLHC_COMP_FS_8P1GA) {
devp->compfullscale = LSM303DLHC_COMP_8P1GA; devp->compfullscale = LSM303DLHC_COMP_8P1GA;
for(i = 0; i < LSM303DLHC_COMP_NUMBER_OF_AXES; i++) { for(i = 0; i < LSM303DLHC_COMP_NUMBER_OF_AXES; i++) {
if(devp->config->compcfg->sensitivity == NULL) { if(devp->config->compsensitivity == NULL) {
if(i != 2) { if(i != 2) {
devp->compsensitivity[i] = LSM303DLHC_COMP_SENS_XY_8P1GA; devp->compsensitivity[i] = LSM303DLHC_COMP_SENS_XY_8P1GA;
} }
@ -1109,7 +1106,7 @@ void lsm303dlhcStart(LSM303DLHCDriver *devp, const LSM303DLHCConfig *config) {
} }
} }
else { else {
devp->compsensitivity[i] = devp->config->compcfg->sensitivity[i]; devp->compsensitivity[i] = devp->config->compsensitivity[i];
} }
} }
} }
@ -1117,10 +1114,9 @@ void lsm303dlhcStart(LSM303DLHCDriver *devp, const LSM303DLHCConfig *config) {
osalDbgAssert(FALSE, "lsm303dlhcStart(), compass full scale issue"); osalDbgAssert(FALSE, "lsm303dlhcStart(), compass full scale issue");
/* Storing bias information */ /* Storing bias information */
if(devp->config->compcfg->bias != NULL) if(devp->config->compbias != NULL)
for(i = 0; i < LSM303DLHC_COMP_NUMBER_OF_AXES; i++) for(i = 0; i < LSM303DLHC_COMP_NUMBER_OF_AXES; i++)
devp->compbias[i] = devp->config->compcfg->bias[i]; devp->compbias[i] = devp->config->compbias[i];
}
/* This is the MEMS transient recovery time */ /* This is the MEMS transient recovery time */
osalThreadSleepMilliseconds(5); osalThreadSleepMilliseconds(5);
@ -1150,20 +1146,21 @@ void lsm303dlhcStop(LSM303DLHCDriver *devp) {
i2cAcquireBus((devp)->config->i2cp); i2cAcquireBus((devp)->config->i2cp);
i2cStart((devp)->config->i2cp, (devp)->config->i2ccfg); i2cStart((devp)->config->i2cp, (devp)->config->i2ccfg);
#endif /* LSM303DLHC_SHARED_I2C */ #endif /* LSM303DLHC_SHARED_I2C */
if((devp)->config->acccfg != NULL) {
/* Disabling accelerometer. */
cr[0] = LSM303DLHC_AD_ACC_CTRL_REG1; cr[0] = LSM303DLHC_AD_ACC_CTRL_REG1;
cr[1] = LSM303DLHC_ACC_AE_DISABLED | LSM303DLHC_ACC_ODR_PD; cr[1] = LSM303DLHC_ACC_AE_DISABLED | LSM303DLHC_ACC_ODR_PD;
lsm303dlhcI2CWriteRegister(devp->config->i2cp, LSM303DLHC_SAD_ACC, lsm303dlhcI2CWriteRegister(devp->config->i2cp, LSM303DLHC_SAD_ACC,
cr, 1); cr, 1);
}
if((devp)->config->compcfg != NULL) { /* Disabling compass. */
cr[0] = LSM303DLHC_AD_COMP_MR_REG; cr[0] = LSM303DLHC_AD_COMP_MR_REG;
cr[1] = LSM303DLHC_COMP_MD_SLEEP; cr[1] = LSM303DLHC_COMP_MD_SLEEP;
lsm303dlhcI2CWriteRegister(devp->config->i2cp, LSM303DLHC_SAD_ACC, lsm303dlhcI2CWriteRegister(devp->config->i2cp, LSM303DLHC_SAD_ACC,
cr, 1); cr, 1);
lsm303dlhcI2CWriteRegister(devp->config->i2cp, LSM303DLHC_SAD_COMP, lsm303dlhcI2CWriteRegister(devp->config->i2cp, LSM303DLHC_SAD_COMP,
cr, 1); cr, 1);
}
i2cStop((devp)->config->i2cp); i2cStop((devp)->config->i2cp);
#if LSM303DLHC_SHARED_I2C #if LSM303DLHC_SHARED_I2C
i2cReleaseBus((devp)->config->i2cp); i2cReleaseBus((devp)->config->i2cp);

403
os/ex/ST/lsm303dlhc.h
View File

@ -428,47 +428,6 @@ typedef enum {
LSM303DLHC_ACC_END_BIG = 0x40 /**< Big Endian */ LSM303DLHC_ACC_END_BIG = 0x40 /**< Big Endian */
} lsm303dlhc_acc_end_t; } lsm303dlhc_acc_end_t;
/**
* @brief LSM303DLHC accelerometer subsystem configuration structure.
*/
typedef struct {
/**
* @brief LSM303DLHC initial sensitivity.
*/
float *sensitivity;
/**
* @brief LSM303DLHC initial bias.
*/
float *bias;
/**
* @brief LSM303DLHC accelerometer subsystem initial full scale.
*/
lsm303dlhc_acc_fs_t fullscale;
/**
* @brief LSM303DLHC accelerometer subsystem output data rate.
*/
lsm303dlhc_acc_odr_t outdatarate;
#if LSM303DLHC_ACC_USE_ADVANCED || defined(__DOXYGEN__)
/**
* @brief LSM303DLHC accelerometer subsystem low power mode.
*/
lsm303dlhc_acc_lp_t lowpower;
/**
* @brief LSM303DLHC accelerometer subsystem high resolution mode.
*/
lsm303dlhc_acc_hr_t highresmode;
/**
* @brief LSM303DLHC accelerometer subsystem block data update.
*/
lsm303dlhc_acc_bdu_t blockdataupdate;
/**
* @brief LSM303DLHC accelerometer endianness.
*/
lsm303dlhc_acc_end_t endianess;
#endif
} LSM303DLHCAccConfig;
/** @} */
/** /**
* @name LSM303DLHC compass subsystem data structures and types. * @name LSM303DLHC compass subsystem data structures and types.
* @{ * @{
@ -509,40 +468,10 @@ typedef enum {
LSM303DLHC_COMP_MD_SLEEP = 0x02 /**< Sleep Mode */ LSM303DLHC_COMP_MD_SLEEP = 0x02 /**< Sleep Mode */
} lsm303dlhc_comp_md_t; } lsm303dlhc_comp_md_t;
/**
* @brief LSM303DLHC compass subsystem configuration structure.
*/
typedef struct {
/**
* @brief LSM303DLHC compass initial sensitivity.
*/
float *sensitivity;
/**
* @brief LSM303DLHC compass initial bias.
*/
float *bias;
/**
* @brief LSM303DLHC compass subsystem initial full scale.
*/
lsm303dlhc_comp_fs_t fullscale;
/**
* @brief LSM303DLHC compass subsystem output data rate.
*/
lsm303dlhc_comp_odr_t outputdatarate;
#if LSM303DLHC_COMP_USE_ADVANCED || defined(__DOXYGEN__)
/**
* @brief LSM303DLHC compass subsystem working mode.
*/
lsm303dlhc_comp_md_t mode;
#endif
} LSM303DLHCCompConfig;
/** @} */
/** /**
* @name LSM303DLHC main system data structures and types. * @name LSM303DLHC main system data structures and types.
* @{ * @{
*/ */
/** /**
* @brief Driver state machine possible states. * @brief Driver state machine possible states.
*/ */
@ -566,13 +495,61 @@ typedef struct {
*/ */
const I2CConfig *i2ccfg; const I2CConfig *i2ccfg;
/** /**
* @brief LSM303DLHC accelerometer subsystem configuration structure * @brief LSM303DLHC initial sensitivity.
*/ */
const LSM303DLHCAccConfig *acccfg; float *accsensitivity;
/** /**
* @brief LSM303DLHC compass subsystem configuration structure * @brief LSM303DLHC initial bias.
*/ */
const LSM303DLHCCompConfig *compcfg; float *accbias;
/**
* @brief LSM303DLHC accelerometer subsystem initial full scale.
*/
lsm303dlhc_acc_fs_t accfullscale;
/**
* @brief LSM303DLHC accelerometer subsystem output data rate.
*/
lsm303dlhc_acc_odr_t accoutdatarate;
#if LSM303DLHC_ACC_USE_ADVANCED || defined(__DOXYGEN__)
/**
* @brief LSM303DLHC accelerometer subsystem low power mode.
*/
lsm303dlhc_acc_lp_t acclowpower;
/**
* @brief LSM303DLHC accelerometer subsystem high resolution mode.
*/
lsm303dlhc_acc_hr_t acchighresmode;
/**
* @brief LSM303DLHC accelerometer subsystem block data update.
*/
lsm303dlhc_acc_bdu_t accblockdataupdate;
/**
* @brief LSM303DLHC accelerometer endianness.
*/
lsm303dlhc_acc_end_t accendianess;
#endif
/**
* @brief LSM303DLHC compass initial sensitivity.
*/
float *compsensitivity;
/**
* @brief LSM303DLHC compass initial bias.
*/
float *compbias;
/**
* @brief LSM303DLHC compass subsystem initial full scale.
*/
lsm303dlhc_comp_fs_t compfullscale;
/**
* @brief LSM303DLHC compass subsystem output data rate.
*/
lsm303dlhc_comp_odr_t compoutputdatarate;
#if LSM303DLHC_COMP_USE_ADVANCED || defined(__DOXYGEN__)
/**
* @brief LSM303DLHC compass subsystem working mode.
*/
lsm303dlhc_comp_md_t compmode;
#endif
} LSM303DLHCConfig; } LSM303DLHCConfig;
/** /**
@ -635,9 +612,9 @@ struct LSM303DLHCDriver {
/** @brief Virtual Methods Table.*/ /** @brief Virtual Methods Table.*/
const struct LSM303DLHCVMT *vmt; const struct LSM303DLHCVMT *vmt;
/** @brief Accelerometer interface.*/ /** @brief Accelerometer interface.*/
BaseAccelerometer accelerometer_if; BaseAccelerometer acc_if;
/** @brief Compass interface.*/ /** @brief Compass interface.*/
BaseCompass compass_if; BaseCompass comp_if;
_lsm303dlhc_data _lsm303dlhc_data
}; };
/** @} */ /** @} */
@ -645,7 +622,279 @@ struct LSM303DLHCDriver {
/*===========================================================================*/ /*===========================================================================*/
/* Driver macros. */ /* Driver macros. */
/*===========================================================================*/ /*===========================================================================*/
/**
* @brief Return the number of axes of the BaseAccelerometer.
*
* @param[in] ip pointer to @p BaseAccelerometer interface
*
* @return the number of axes.
*
* @api
*/
#define lsm303dlhcAccelerometerGetAxesNumber(devp) \
accelerometerGetAxesNumber(&((devp)->acc_if))
/**
* @brief Retrieves raw data from the BaseAccelerometer.
* @note This data is retrieved from MEMS register without any algebrical
* manipulation.
* @note The axes array must be at least the same size of the
* BaseAccelerometer axes number.
*
* @param[in] ip pointer to @p BaseAccelerometer 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.
*
* @api
*/
#define lsm303dlhcAccelerometerReadRaw(devp, axes) \
accelerometerReadRaw(&((devp)->acc_if), axes)
/**
* @brief Retrieves cooked data from the BaseAccelerometer.
* @note This data is manipulated according to the formula
* cooked = (raw * sensitivity) - bias.
* @note Final data is expressed as milli-G.
* @note The axes array must be at least the same size of the
* BaseAccelerometer axes number.
*
* @param[in] ip pointer to @p BaseAccelerometer interface.
* @param[out] axes a buffer which would be filled with cooked 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.
*
* @api
*/
#define lsm303dlhcAccelerometerReadCooked(devp, axes) \
accelerometerReadCooked(&((devp)->acc_if), axes)
/**
* @brief Set bias values for the BaseAccelerometer.
* @note Bias must be expressed as milli-G.
* @note The bias buffer must be at least the same size of the
* BaseAccelerometer axes number.
*
* @param[in] ip pointer to @p BaseAccelerometer interface.
* @param[in] bp a buffer which contains biases.
*
* @return The operation status.
* @retval MSG_OK if the function succeeded.
*
* @api
*/
#define lsm303dlhcAccelerometerSetBias(devp, bp) \
accelerometerSetBias(&((devp)->acc_if), bp)
/**
* @brief Reset bias values for the BaseAccelerometer.
* @note Default biases value are obtained from device datasheet when
* available otherwise they are considered zero.
*
* @param[in] ip pointer to @p BaseAccelerometer interface.
*
* @return The operation status.
* @retval MSG_OK if the function succeeded.
*
* @api
*/
#define lsm303dlhcAccelerometerResetBias(devp) \
accelerometerResetBias(&((devp)->acc_if))
/**
* @brief Set sensitivity values for the BaseAccelerometer.
* @note Sensitivity must be expressed as milli-G/LSB.
* @note The sensitivity buffer must be at least the same size of the
* BaseAccelerometer axes number.
*
* @param[in] ip pointer to @p BaseAccelerometer interface.
* @param[in] sp a buffer which contains sensitivities.
*
* @return The operation status.
* @retval MSG_OK if the function succeeded.
*
* @api
*/
#define lsm303dlhcAccelerometerSetSensitivity(devp, sp) \
accelerometerSetSensitivity(&((devp)->acc_if), sp)
/**
* @brief Reset sensitivity values for the BaseAccelerometer.
* @note Default sensitivities value are obtained from device datasheet.
*
* @param[in] ip pointer to @p BaseAccelerometer interface.
*
* @return The operation status.
* @retval MSG_OK if the function succeeded.
* @retval MSG_RESET otherwise.
*
* @api
*/
#define lsm303dlhcAccelerometerResetSensitivity(devp) \
accelerometerResetSensitivity(&((devp)->acc_if))
/**
* @brief Changes the LSM303DLHCDriver 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] ip pointer to @p LSM303DLHCDriver interface.
* @param[in] fs new fullscale value.
*
* @return The operation status.
* @retval MSG_OK if the function succeeded.
* @retval MSG_RESET otherwise.
*
* @api
*/
#define lsm303dlhcAccelerometerSetFullScale(devp, fs) \
(devp)->vmt->acc_set_full_scale(devp, fs)
/**
* @brief Return the number of axes of the BaseCompass.
*
* @param[in] ip pointer to @p BaseCompass interface
*
* @return the number of axes.
*
* @api
*/
#define lsm303dlhcCompassGetAxesNumber(devp) \
compassGetAxesNumber(&((devp)->comp_if))
/**
* @brief Retrieves raw data from the BaseCompass.
* @note This data is retrieved from MEMS register without any algebrical
* manipulation.
* @note The axes array must be at least the same size of the
* BaseCompass axes number.
*
* @param[in] ip pointer to @p BaseCompass 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.
*
* @api
*/
#define lsm303dlhcCompassReadRaw(devp, axes) \
compassReadRaw(&((devp)->comp_if), axes)
/**
* @brief Retrieves cooked data from the BaseCompass.
* @note This data is manipulated according to the formula
* cooked = (raw * sensitivity) - bias.
* @note Final data is expressed as Ga.
* @note The axes array must be at least the same size of the
* BaseCompass axes number.
*
* @param[in] ip pointer to @p BaseCompass interface.
* @param[out] axes a buffer which would be filled with cooked 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.
*
* @api
*/
#define lsm303dlhcCompassReadCooked(devp, axes) \
compassReadCooked(&((devp)->comp_if), axes)
/**
* @brief Set bias values for the BaseCompass.
* @note Bias must be expressed as Ga.
* @note The bias buffer must be at least the same size of the
* BaseCompass axes number.
*
* @param[in] ip pointer to @p BaseCompass interface.
* @param[in] bp a buffer which contains biases.
*
* @return The operation status.
* @retval MSG_OK if the function succeeded.
*
* @api
*/
#define lsm303dlhcCompassSetBias(devp, bp) \
compassSetBias(&((devp)->comp_if), bp)
/**
* @brief Reset bias values for the BaseCompass.
* @note Default biases value are obtained from device datasheet when
* available otherwise they are considered zero.
*
* @param[in] ip pointer to @p BaseCompass interface.
*
* @return The operation status.
* @retval MSG_OK if the function succeeded.
*
* @api
*/
#define lsm303dlhcCompassResetBias(devp) \
compassResetBias(&((devp)->comp_if))
/**
* @brief Set sensitivity values for the BaseCompass.
* @note Sensitivity must be expressed as Ga/LSB.
* @note The sensitivity buffer must be at least the same size of the
* BaseCompass axes number.
*
* @param[in] ip pointer to @p BaseCompass interface.
* @param[in] sp a buffer which contains sensitivities.
*
* @return The operation status.
* @retval MSG_OK if the function succeeded.
*
* @api
*/
#define lsm303dlhcCompassSetSensitivity(devp, sp) \
compassSetSensitivity(&((devp)->comp_if), sp)
/**
* @brief Reset sensitivity values for the BaseCompass.
* @note Default sensitivities value are obtained from device datasheet.
*
* @param[in] ip pointer to @p BaseCompass interface.
*
* @return The operation status.
* @retval MSG_OK if the function succeeded.
* @retval MSG_RESET otherwise.
*
* @api
*/
#define lsm303dlhcCompassResetSensitivity(devp) \
compassResetSensitivity(&((devp)->comp_if))
/**
* @brief Changes the LSM303DLHCDriver compass 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] ip pointer to @p LSM303DLHCDriver interface.
* @param[in] fs new fullscale value.
*
* @return The operation status.
* @retval MSG_OK if the function succeeded.
* @retval MSG_RESET otherwise.
*
* @api
*/
#define lsm303dlhcCompassSetFullScale(devp, fs) \
(devp)->vmt->comp_set_full_scale(devp, fs)
/*===========================================================================*/ /*===========================================================================*/
/* External declarations. */ /* External declarations. */
/*===========================================================================*/ /*===========================================================================*/