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

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

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

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

@ -428,47 +428,6 @@ typedef enum {
LSM303DLHC_ACC_END_BIG = 0x40 /**< Big Endian */
} 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.
* @{
@ -509,40 +468,10 @@ typedef enum {
LSM303DLHC_COMP_MD_SLEEP = 0x02 /**< Sleep Mode */
} 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.
* @{
*/
/**
* @brief Driver state machine possible states.
*/
@ -566,13 +495,61 @@ typedef struct {
*/
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;
/**
@ -635,9 +612,9 @@ struct LSM303DLHCDriver {
/** @brief Virtual Methods Table.*/
const struct LSM303DLHCVMT *vmt;
/** @brief Accelerometer interface.*/
BaseAccelerometer accelerometer_if;
BaseAccelerometer acc_if;
/** @brief Compass interface.*/
BaseCompass compass_if;
BaseCompass comp_if;
_lsm303dlhc_data
};
/** @} */
@ -645,7 +622,279 @@ struct LSM303DLHCDriver {
/*===========================================================================*/
/* 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. */
/*===========================================================================*/