From 52bc6794fa660ad3b1a338576ada3d0fb68f5b16 Mon Sep 17 00:00:00 2001
From: Rocco Marco Guglielmi <roccomarco.guglielmi@live.com>
Date: Thu, 28 Jul 2016 10:38:43 +0000
Subject: Fixed bug in LSM303DLHC acc_set_fullscale and comp_set_fullscale
 method.

git-svn-id: svn://svn.code.sf.net/p/chibios/svn/trunk@9727 35acf78f-673a-0410-8e92-d51de3d6d3f4
---
 os/ex/ST/lsm303dlhc.c | 40 +++++++++++++++++++++++++++++++++++++---
 os/ex/ST/lsm303dlhc.h |  2 ++
 2 files changed, 39 insertions(+), 3 deletions(-)

diff --git a/os/ex/ST/lsm303dlhc.c b/os/ex/ST/lsm303dlhc.c
index c4adc8aea..310eb44e9 100644
--- a/os/ex/ST/lsm303dlhc.c
+++ b/os/ex/ST/lsm303dlhc.c
@@ -449,7 +449,8 @@ static msg_t comp_reset_sensivity(void *ip) {
 
 static msg_t acc_set_full_scale(void *ip, lsm303dlhc_acc_fs_t fs) {
   float newfs, scale;
-  unsigned i;
+  uint8_t i, buff[2];
+  msg_t msg;
 
   if(fs == LSM303DLHC_ACC_FS_2G) {
     newfs = LSM303DLHC_ACC_2G;
@@ -470,18 +471,35 @@ static msg_t acc_set_full_scale(void *ip, lsm303dlhc_acc_fs_t fs) {
   if(newfs != ((LSM303DLHCDriver *)ip)->accfullscale) {
     scale = newfs / ((LSM303DLHCDriver *)ip)->accfullscale;
     ((LSM303DLHCDriver *)ip)->accfullscale = newfs;
+
+    /* Updating register.*/
+    msg = lsm303dlhcI2CReadRegister(((LSM303DLHCDriver *)ip)->config->i2cp,
+                                   LSM303DLHC_SAD_ACC,
+                                   LSM303DLHC_AD_ACC_CTRL_REG4,
+                                   &buff[1], 1);
+    if(msg != MSG_OK)
+      return msg;
+    buff[1] &= ~(LSM303DLHC_CTRL_REG4_A_FS_MASK);
+    buff[1] |= fs;
+    buff[0] = LSM303DLHC_AD_ACC_CTRL_REG4;
+    msg = lsm303dlhcI2CWriteRegister(((LSM303DLHCDriver *)ip)->config->i2cp,
+                                    LSM303DLHC_SAD_ACC, buff, 1);
+    if(msg != MSG_OK)
+      return msg;
+
     /* Scaling sensitivity and bias. Re-calibration is suggested anyway. */
     for(i = 0; i < LSM303DLHC_ACC_NUMBER_OF_AXES; i++) {
       ((LSM303DLHCDriver *)ip)->accsensitivity[i] *= scale;
       ((LSM303DLHCDriver *)ip)->accbias[i] *= scale;
     }
   }
-  return MSG_OK;
+  return msg;
 }
 
 static msg_t comp_set_full_scale(void *ip, lsm303dlhc_comp_fs_t fs) {
   float newfs, scale;
-  unsigned i;
+  uint8_t i, buff[2];
+  msg_t msg;
 
   if(fs == LSM303DLHC_COMP_FS_1P3GA) {
     newfs = LSM303DLHC_COMP_1P3GA;
@@ -511,6 +529,22 @@ static msg_t comp_set_full_scale(void *ip, lsm303dlhc_comp_fs_t fs) {
   if(newfs != ((LSM303DLHCDriver *)ip)->compfullscale) {
     scale = newfs / ((LSM303DLHCDriver *)ip)->compfullscale;
     ((LSM303DLHCDriver *)ip)->compfullscale = newfs;
+
+    /* Updating register.*/
+    msg = lsm303dlhcI2CReadRegister(((LSM303DLHCDriver *)ip)->config->i2cp,
+                                   LSM303DLHC_SAD_COMP,
+                                   LSM303DLHC_AD_COMP_CRB_REG,
+                                   &buff[1], 1);
+    if(msg != MSG_OK)
+      return msg;
+    buff[1] &= ~(LSM303DLHC_CRB_REG_M_GN_MASK);
+    buff[1] |= fs;
+    buff[0] = LSM303DLHC_AD_COMP_CRB_REG;
+    msg = lsm303dlhcI2CWriteRegister(((LSM303DLHCDriver *)ip)->config->i2cp,
+                                     LSM303DLHC_SAD_COMP, buff, 1);
+    if(msg != MSG_OK)
+      return msg;
+
     /* Scaling sensitivity and bias. Re-calibration is suggested anyway. */
     for(i = 0; i < LSM303DLHC_COMP_NUMBER_OF_AXES; i++) {
       ((LSM303DLHCDriver *)ip)->compsensitivity[i] *= scale;
diff --git a/os/ex/ST/lsm303dlhc.h b/os/ex/ST/lsm303dlhc.h
index 7500fa95b..470d8b77e 100644
--- a/os/ex/ST/lsm303dlhc.h
+++ b/os/ex/ST/lsm303dlhc.h
@@ -222,6 +222,7 @@
 #define LSM303DLHC_CTRL_REG4_A_MASK         0xF9
 #define LSM303DLHC_CTRL_REG4_A_SIM          (1 << 0)
 #define LSM303DLHC_CTRL_REG4_A_HR           (1 << 3)
+#define LSM303DLHC_CTRL_REG4_A_FS_MASK      0x30
 #define LSM303DLHC_CTRL_REG4_A_FS0          (1 << 4)
 #define LSM303DLHC_CTRL_REG4_A_FS1          (1 << 5)
 #define LSM303DLHC_CTRL_REG4_A_BLE          (1 << 6)
@@ -270,6 +271,7 @@
  * @{
  */
 #define LSM303DLHC_CRB_REG_M_MASK           0xE0
+#define LSM303DLHC_CRB_REG_M_GN_MASK        0xE0
 #define LSM303DLHC_CRB_REG_M_GN0            (1 << 5)
 #define LSM303DLHC_CRB_REG_M_GN1            (1 << 6)
 #define LSM303DLHC_CRB_REG_M_GN2            (1 << 7)
-- 
cgit v1.2.3