Minor adjustments on L3GD20 and LSMS303DLHC drivers

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

2 files changed, 38 insertions, 38 deletions

diff --git a/os/ex/ST/l3gd20.c b/os/ex/ST/l3gd20.c
index c3093d40c..fcb6dd17d 100644
--- a/os/ex/ST/l3gd20.c
+++ b/os/ex/ST/l3gd20.c
@@ -176,16 +176,11 @@ static msg_t gyro_read_cooked(void *ip, float axes[]) {
   devp = objGetInstance(L3GD20Driver*, (BaseGyroscope*)ip);

   
   osalDbgAssert((devp->state == L3GD20_READY),
-                "gyro_read_cooked(), invalid state");
-#if L3GD20_USE_SPI

-  osalDbgAssert((devp->config->spip->state == SPI_READY),

-                "gyro_read_cooked(), channel not ready");

-#endif

+                "gyro_read_cooked(), invalid state");

 
   msg = gyro_read_raw(ip, raw);
   for(i = 0; i < L3GD20_GYRO_NUMBER_OF_AXES; i++){
-    axes[i] = raw[i] * devp->gyrosensitivity[i];
-    axes[i] -= devp->gyrobias[i];
+    axes[i] = (raw[i] * devp->gyrosensitivity[i]) - devp->gyrobias[i];
   }
   return msg;
 }
@@ -259,7 +254,7 @@ static msg_t gyro_set_bias(void *ip, float *bp) {
   /* Getting parent instance pointer.*/

   devp = objGetInstance(L3GD20Driver*, (BaseGyroscope*)ip);

   
-  osalDbgAssert((devp->state != L3GD20_UNINIT),
+  osalDbgAssert((devp->state == L3GD20_READY),
                 "gyro_set_bias(), invalid state");
   
   for(i = 0; i < L3GD20_GYRO_NUMBER_OF_AXES; i++) {
@@ -287,7 +282,7 @@ static msg_t gyro_reset_bias(void *ip) {
   /* Getting parent instance pointer.*/

   devp = objGetInstance(L3GD20Driver*, (BaseGyroscope*)ip);

   
-  osalDbgAssert((devp->state != L3GD20_UNINIT),

+  osalDbgAssert((devp->state == L3GD20_READY),

                 "gyro_reset_bias(), invalid state");
 
   for(i = 0; i < L3GD20_GYRO_NUMBER_OF_AXES; i++)
@@ -316,7 +311,7 @@ static msg_t gyro_set_sensivity(void *ip, float *sp) {
   /* Getting parent instance pointer.*/

   devp = objGetInstance(L3GD20Driver*, (BaseGyroscope*)ip);

   

-  osalDbgAssert((devp->state != L3GD20_UNINIT),

+  osalDbgAssert((devp->state == L3GD20_READY),

                 "gyro_set_sensivity(), invalid state");
   
   for(i = 0; i < L3GD20_GYRO_NUMBER_OF_AXES; i++) {
@@ -345,7 +340,7 @@ static msg_t gyro_reset_sensivity(void *ip) {
   /* Getting parent instance pointer.*/

   devp = objGetInstance(L3GD20Driver*, (BaseGyroscope*)ip);

   

-  osalDbgAssert((devp->state != L3GD20_UNINIT),

+  osalDbgAssert((devp->state == L3GD20_READY),

                 "gyro_reset_sensivity(), invalid state");
 
   if(devp->config->gyrofullscale == L3GD20_FS_250DPS)
@@ -384,7 +379,7 @@ static msg_t gyro_set_full_scale(L3GD20Driver *devp, l3gd20_fs_t fs) {
 
   osalDbgCheck(devp != NULL);

 

-  osalDbgAssert((devp->state != L3GD20_UNINIT),

+  osalDbgAssert((devp->state == L3GD20_READY),

                 "gyro_set_full_scale(), invalid state");

 #if L3GD20_USE_SPI

   osalDbgAssert((devp->config->spip->state == SPI_READY),

diff --git a/os/ex/ST/lsm303dlhc.c b/os/ex/ST/lsm303dlhc.c
index 98656c766..64f965607 100644
--- a/os/ex/ST/lsm303dlhc.c
+++ b/os/ex/ST/lsm303dlhc.c
@@ -191,13 +191,10 @@ static msg_t acc_read_cooked(void *ip, float axes[]) {
 

   osalDbgAssert((devp->state == LSM303DLHC_READY),

                 "acc_read_cooked(), invalid state");

-  osalDbgAssert((devp->config->i2cp->state == I2C_READY),

-                "acc_read_cooked(), channel not ready");

 

   msg = acc_read_raw(ip, raw);

   for(i = 0; i < LSM303DLHC_ACC_NUMBER_OF_AXES; i++) {

-    axes[i] = raw[i] * devp->accsensitivity[i];

-    axes[i] -= devp->accbias[i];

+    axes[i] = (raw[i] * devp->accsensitivity[i]) - devp->accbias[i];

   }

   return msg;

 }

@@ -224,7 +221,7 @@ static msg_t acc_set_bias(void *ip, float *bp) {
   /* Getting parent instance pointer.*/

   devp = objGetInstance(LSM303DLHCDriver*, (BaseAccelerometer*)ip);

 

-  osalDbgAssert((devp->state != LSM303DLHC_UNINIT),

+  osalDbgAssert((devp->state == LSM303DLHC_READY),

                 "acc_set_bias(), invalid state");

 

   for(i = 0; i < LSM303DLHC_ACC_NUMBER_OF_AXES; i++) {

@@ -253,7 +250,7 @@ static msg_t acc_reset_bias(void *ip) {
   /* Getting parent instance pointer.*/

   devp = objGetInstance(LSM303DLHCDriver*, (BaseAccelerometer*)ip);

 

-  osalDbgAssert((devp->state != LSM303DLHC_UNINIT),

+  osalDbgAssert((devp->state == LSM303DLHC_READY),

                 "acc_reset_bias(), invalid state");

 

   for(i = 0; i < LSM303DLHC_ACC_NUMBER_OF_AXES; i++)

@@ -283,7 +280,7 @@ static msg_t acc_set_sensivity(void *ip, float *sp) {
 

   osalDbgCheck((ip != NULL) && (sp != NULL));

 

-  osalDbgAssert((devp->state != LSM303DLHC_UNINIT),

+  osalDbgAssert((devp->state == LSM303DLHC_READY),

                 "acc_set_sensivity(), invalid state");

 

   for(i = 0; i < LSM303DLHC_ACC_NUMBER_OF_AXES; i++) {

@@ -312,7 +309,7 @@ static msg_t acc_reset_sensivity(void *ip) {
   /* Getting parent instance pointer.*/

   devp = objGetInstance(LSM303DLHCDriver*, (BaseAccelerometer*)ip);

 

-  osalDbgAssert((devp->state != LSM303DLHC_UNINIT),

+  osalDbgAssert((devp->state == LSM303DLHC_READY),

                 "acc_reset_sensivity(), invalid state");

 

   if(devp->config->accfullscale == LSM303DLHC_ACC_FS_2G)

@@ -355,7 +352,7 @@ static msg_t acc_set_full_scale(LSM303DLHCDriver *devp,
 

   osalDbgCheck(devp != NULL);

 

-  osalDbgAssert((devp->state != LSM303DLHC_UNINIT),

+  osalDbgAssert((devp->state == LSM303DLHC_READY),

                 "acc_set_full_scale(), invalid state");

   osalDbgAssert((devp->config->i2cp->state == I2C_READY),

                 "acc_set_full_scale(), channel not ready");

@@ -528,13 +525,10 @@ static msg_t comp_read_cooked(void *ip, float axes[]) {
   

   osalDbgAssert((devp->state == LSM303DLHC_READY),

                 "comp_read_cooked(), invalid state");

-  osalDbgAssert((devp->config->i2cp->state == I2C_READY),

-                "comp_read_cooked(), channel not ready");

                 

   msg = comp_read_raw(ip, raw);

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

-    axes[i] = raw[i] * devp->compsensitivity[i];

-    axes[i] -= devp->compbias[i];

+    axes[i] = (raw[i] * devp->compsensitivity[i]) - devp->compbias[i];

   }

   return msg;

 }

@@ -561,7 +555,7 @@ static msg_t comp_set_bias(void *ip, float *bp) {
   /* Getting parent instance pointer.*/

   devp = objGetInstance(LSM303DLHCDriver*, (BaseCompass*)ip);

 

-  osalDbgAssert((devp->state != LSM303DLHC_UNINIT),

+  osalDbgAssert((devp->state == LSM303DLHC_READY),

                 "comp_set_bias(), invalid state");

 

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

@@ -590,7 +584,7 @@ static msg_t comp_reset_bias(void *ip) {
   /* Getting parent instance pointer.*/

   devp = objGetInstance(LSM303DLHCDriver*, (BaseCompass*)ip);

 

-  osalDbgAssert((devp->state != LSM303DLHC_UNINIT),

+  osalDbgAssert((devp->state == LSM303DLHC_READY),

                 "comp_reset_bias(), invalid state");

 

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

@@ -620,7 +614,7 @@ static msg_t comp_set_sensivity(void *ip, float *sp) {
 

   osalDbgCheck((ip != NULL) && (sp != NULL));

 

-  osalDbgAssert((devp->state != LSM303DLHC_UNINIT),

+  osalDbgAssert((devp->state == LSM303DLHC_READY),

                 "comp_set_sensivity(), invalid state");

 

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

@@ -649,7 +643,7 @@ static msg_t comp_reset_sensivity(void *ip) {
   /* Getting parent instance pointer.*/

   devp = objGetInstance(LSM303DLHCDriver*, (BaseCompass*)ip);

 

-  osalDbgAssert((devp->state != LSM303DLHC_UNINIT),

+  osalDbgAssert((devp->state == LSM303DLHC_READY),

                 "comp_reset_sensivity(), invalid state");

 

   if(devp->config->compfullscale == LSM303DLHC_COMP_FS_1P3GA)

@@ -743,7 +737,7 @@ static msg_t comp_set_full_scale(LSM303DLHCDriver *devp,
   

   osalDbgCheck(devp != NULL);

 

-  osalDbgAssert((devp->state != LSM303DLHC_UNINIT),

+  osalDbgAssert((devp->state == LSM303DLHC_READY),

                 "comp_set_full_scale(), invalid state");

   osalDbgAssert((devp->config->i2cp->state == I2C_READY),

                 "comp_set_full_scale(), channel not ready");

@@ -893,11 +887,6 @@ void lsm303dlhcStart(LSM303DLHCDriver *devp, const LSM303DLHCConfig *config) {
 

   devp->config = config;

 

-#if LSM303DLHC_SHARED_I2C

-  i2cAcquireBus((devp)->config->i2cp);

-#endif /* LSM303DLHC_SHARED_I2C */

-  i2cStart((devp)->config->i2cp, (devp)->config->i2ccfg);

-

   /* Configuring Accelerometer subsystem */

 

   /* Multiple write starting address.*/

@@ -931,7 +920,17 @@ void lsm303dlhcStart(LSM303DLHCDriver *devp, const LSM303DLHCConfig *config) {
              devp->config->acchighresmode;

 #endif

   }

+  

+#if LSM303DLHC_SHARED_I2C

+  i2cAcquireBus((devp)->config->i2cp);

+#endif /* LSM303DLHC_SHARED_I2C */

+  i2cStart((devp)->config->i2cp, (devp)->config->i2ccfg);

+  

   lsm303dlhcI2CWriteRegister(devp->config->i2cp, LSM303DLHC_SAD_ACC, cr, 4);

+  

+#if LSM303DLHC_SHARED_I2C

+  i2cReleaseBus((devp)->config->i2cp);

+#endif /* LSM303DLHC_SHARED_I2C */

 

   /* Storing sensitivity according to user settings */

   if(devp->config->accfullscale == LSM303DLHC_ACC_FS_2G) {

@@ -1001,9 +1000,18 @@ void lsm303dlhcStart(LSM303DLHCDriver *devp, const LSM303DLHCConfig *config) {
 #endif

   }

 

+#if LSM303DLHC_SHARED_I2C

+  i2cAcquireBus((devp)->config->i2cp);

+  i2cStart((devp)->config->i2cp, (devp)->config->i2ccfg);

+#endif /* LSM303DLHC_SHARED_I2C */

+  

   lsm303dlhcI2CWriteRegister(devp->config->i2cp, LSM303DLHC_SAD_COMP,

                              cr, 3);

 

+#if LSM303DLHC_SHARED_I2C

+  i2cReleaseBus((devp)->config->i2cp);

+#endif /* LSM303DLHC_SHARED_I2C */

+

   if(devp->config->compfullscale == LSM303DLHC_COMP_FS_1P3GA) {

     devp->compfullscale = LSM303DLHC_COMP_1P3GA;

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

@@ -1128,9 +1136,6 @@ void lsm303dlhcStart(LSM303DLHCDriver *devp, const LSM303DLHCConfig *config) {
   osalThreadSleepMilliseconds(5);

 

   devp->state = LSM303DLHC_READY;

-#if LSM303DLHC_SHARED_I2C

-  i2cReleaseBus((devp)->config->i2cp);

-#endif /* LSM303DLHC_SHARED_I2C */

 }

 

 /**