From 3d3363b7dac3cdb44ed96a34ea601fe3e0096534 Mon Sep 17 00:00:00 2001
From: Rocco Marco Guglielmi <roccomarco.guglielmi@gmail.com>
Date: Wed, 14 Mar 2018 20:32:08 +0000
Subject: Minor adjustments

git-svn-id: svn://svn.code.sf.net/p/chibios/svn/trunk@11756 110e8d01-0319-4d1e-a829-52ad28d1bb01
---
 os/ex/ST/lsm303agr.c | 66 ++++++++++++++++++++++++++++++++++++++++++++++------
 1 file changed, 59 insertions(+), 7 deletions(-)

(limited to 'os/ex/ST/lsm303agr.c')

diff --git a/os/ex/ST/lsm303agr.c b/os/ex/ST/lsm303agr.c
index 5620826dd..17b312ee0 100644
--- a/os/ex/ST/lsm303agr.c
+++ b/os/ex/ST/lsm303agr.c
@@ -709,6 +709,7 @@ void lsm303agrObjectInit(LSM303AGRDriver *devp) {
  * @api
  */
 void lsm303agrStart(LSM303AGRDriver *devp, const LSM303AGRConfig *config) {
+  uint32_t i;
   uint8_t cr[6];
   osalDbgCheck((devp != NULL) && (config != NULL));
 
@@ -753,6 +754,54 @@ void lsm303agrStart(LSM303AGRDriver *devp, const LSM303AGRConfig *config) {
 #endif
   }
 
+  /* Storing sensitivity according to user settings */
+  if(devp->config->accfullscale == LSM303AGR_ACC_FS_2G) {
+    devp->accfullscale = LSM303AGR_ACC_2G;
+    for(i = 0; i < LSM303AGR_ACC_NUMBER_OF_AXES; i++) {
+      if(devp->config->accsensitivity == NULL)
+        devp->accsensitivity[i] = LSM303AGR_ACC_SENS_2G;
+      else
+        devp->accsensitivity[i] = devp->config->accsensitivity[i];
+    }
+  }
+  else if(devp->config->accfullscale == LSM303AGR_ACC_FS_4G) {
+    devp->accfullscale = LSM303AGR_ACC_4G;
+    for(i = 0; i < LSM303AGR_ACC_NUMBER_OF_AXES; i++) {
+      if(devp->config->accsensitivity == NULL)
+        devp->accsensitivity[i] = LSM303AGR_ACC_SENS_4G;
+      else
+        devp->accsensitivity[i] = devp->config->accsensitivity[i];
+    }
+  }
+  else if(devp->config->accfullscale == LSM303AGR_ACC_FS_8G) {
+    devp->accfullscale = LSM303AGR_ACC_8G;
+    for(i = 0; i < LSM303AGR_ACC_NUMBER_OF_AXES; i++) {
+      if(devp->config->accsensitivity == NULL)
+        devp->accsensitivity[i] = LSM303AGR_ACC_SENS_8G;
+      else
+        devp->accsensitivity[i] = devp->config->accsensitivity[i];
+    }
+  }
+  else if(devp->config->accfullscale == LSM303AGR_ACC_FS_16G) {
+    devp->accfullscale = LSM303AGR_ACC_16G;
+    for(i = 0; i < LSM303AGR_ACC_NUMBER_OF_AXES; i++) {
+      if(devp->config->accsensitivity == NULL)
+        devp->accsensitivity[i] = LSM303AGR_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 < LSM303AGR_ACC_NUMBER_OF_AXES; i++)
+      devp->accbias[i] = devp->config->accbias[i];
+  else
+    for(i = 0; i < LSM303AGR_ACC_NUMBER_OF_AXES; i++)
+      devp->accbias[i] = LSM303AGR_ACC_BIAS;
+
 #if LSM303AGR_SHARED_I2C
   i2cAcquireBus((devp)->config->i2cp);
 #endif /* LSM303AGR_SHARED_I2C */
@@ -798,17 +847,20 @@ void lsm303agrStart(LSM303AGRDriver *devp, const LSM303AGRConfig *config) {
   i2cReleaseBus((devp)->config->i2cp);
 #endif /* LSM303AGR_SHARED_I2C */
 
+  devp->compfullscale = LSM303AGR_COMP_50GA;
+  for(i = 0; i < LSM303AGR_COMP_NUMBER_OF_AXES; i++) {
+    if(devp->config->compsensitivity == NULL) {
+      devp->compsensitivity[i] = LSM303AGR_COMP_SENS_50GA;
+    }
+    else {
+      devp->compsensitivity[i] = devp->config->compsensitivity[i];
+    }
+  }
+    
   /* This is the MEMS transient recovery time */
   osalThreadSleepMilliseconds(5);
 
   devp->state = LSM303AGR_READY;
-  
-  /* Configuring sensitivity and bias of accelerometer.*/
-  acc_reset_sensivity(&(devp->acc_if));
-  acc_reset_bias(&(devp->acc_if));
-  /* Configuring sensitivity and bias of compass.*/ 
-  comp_reset_sensivity(&(devp->comp_if));
-  comp_reset_bias(&(devp->comp_if));
 }
 
 /**
-- 
cgit v1.2.3