LSM303DLHC driver completely updated

git-svn-id: https://svn.code.sf.net/p/chibios/svn2/trunk@11591 110e8d01-0319-4d1e-a829-52ad28d1bb01

2 files changed, 521 insertions, 275 deletions

diff --git a/os/ex/ST/lsm303dlhc.c b/os/ex/ST/lsm303dlhc.c
index f172063c3..39b7f31c6 100644
--- a/os/ex/ST/lsm303dlhc.c
+++ b/os/ex/ST/lsm303dlhc.c
@@ -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,234 +893,230 @@ 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 2 configuration block.*/

+  {

+    cr[2] = 0;

+  }

 

-    /* Control register 3 configuration block.*/

-    {

-      cr[3] = 0;

-    }

+  /* Control register 3 configuration block.*/

+  {

+    cr[3] = 0;

+  }

 

-    /* Control register 4 configuration block.*/

-    {

-      cr[4] = devp->config->acccfg->fullscale;

+  /* Control register 4 configuration block.*/

+  {

+    cr[4] = devp->config->accfullscale;

 #if LSM303DLHC_ACC_USE_ADVANCED || defined(__DOXYGEN__)

-      cr[4] |= devp->config->acccfg->endianess |

-               devp->config->acccfg->blockdataupdate |

-               devp->config->acccfg->highresmode;

+    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];

     }

-    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->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->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->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->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 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");

+  }

+  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];

+  /* 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 A configuration block.*/

+  {

+    cr[1] = devp->config->compoutputdatarate;

+  }

 

-    /* Control register B configuration block.*/

-    {

-      cr[2] = devp->config->compcfg->fullscale;

-    }

+  /* Control register B configuration block.*/

+  {

+    cr[2] = devp->config->compfullscale;

+  }

 

-    /* Mode register configuration block.*/

-    {

-      cr[3] = 0;

+  /* Mode register configuration block.*/

+  {

+    cr[3] = 0;

 #if LSM303DLHC_COMP_USE_ADVANCED || defined(__DOXYGEN__)

-      cr[3] |= devp->config->compcfg->mode;

+    cr[3] |= devp->config->compmode;

 #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;

-          }

+  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] = devp->config->compcfg->sensitivity[i];

+          devp->compsensitivity[i] = LSM303DLHC_COMP_SENS_Z_1P3GA;

         }

       }

+      else {

+        devp->compsensitivity[i] = devp->config->compsensitivity[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 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] = devp->config->compcfg->sensitivity[i];

+          devp->compsensitivity[i] = LSM303DLHC_COMP_SENS_Z_1P9GA;

         }

       }

+      else {

+        devp->compsensitivity[i] = devp->config->compsensitivity[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 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] = devp->config->compcfg->sensitivity[i];

+          devp->compsensitivity[i] = LSM303DLHC_COMP_SENS_Z_2P5GA;

         }

       }

+      else {

+        devp->compsensitivity[i] = devp->config->compsensitivity[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 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] = devp->config->compcfg->sensitivity[i];

+          devp->compsensitivity[i] = LSM303DLHC_COMP_SENS_Z_4P0GA;

         }

       }

+      else {

+        devp->compsensitivity[i] = devp->config->compsensitivity[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 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] = devp->config->compcfg->sensitivity[i];

+          devp->compsensitivity[i] = LSM303DLHC_COMP_SENS_Z_4P7GA;

         }

       }

+      else {

+        devp->compsensitivity[i] = devp->config->compsensitivity[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 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] = devp->config->compcfg->sensitivity[i];

+          devp->compsensitivity[i] = LSM303DLHC_COMP_SENS_Z_5P6GA;

         }

       }

+      else {

+        devp->compsensitivity[i] = devp->config->compsensitivity[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 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] = devp->config->compcfg->sensitivity[i];

+          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->compcfg->bias != NULL)

-      for(i = 0; i < LSM303DLHC_COMP_NUMBER_OF_AXES; i++)

-        devp->compbias[i] = devp->config->compcfg->bias[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);

diff --git a/os/ex/ST/lsm303dlhc.h b/os/ex/ST/lsm303dlhc.h
index 36f0a74c4..e58fd7a39 100644
--- a/os/ex/ST/lsm303dlhc.h
+++ b/os/ex/ST/lsm303dlhc.h
@@ -429,47 +429,6 @@ typedef enum {
 } 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.

  * @{

  */

@@ -510,39 +469,9 @@ typedef enum {
 } 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.

+   */

+  float                     *accsensitivity;

+  /**

+   * @brief LSM303DLHC initial bias.

+   */

+  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.

    */

-  const LSM303DLHCAccConfig *acccfg;

+  lsm303dlhc_acc_lp_t       acclowpower;

   /**

-   * @brief LSM303DLHC compass subsystem configuration structure

+   * @brief LSM303DLHC accelerometer subsystem high resolution mode.

    */

-  const LSM303DLHCCompConfig *compcfg;

+  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.                                                    */

 /*===========================================================================*/