diff options
| author | Rocco Marco Guglielmi <roccomarco.guglielmi@live.com> | 2016-07-28 10:38:43 +0000 |
|---|---|---|
| committer | Rocco Marco Guglielmi <roccomarco.guglielmi@live.com> | 2016-07-28 10:38:43 +0000 |
| commit | 52bc6794fa660ad3b1a338576ada3d0fb68f5b16 (patch) | |
| tree | b001f0b542d1f1e4a0a1be97b2c0f51467ed723a /os/ex/ST | |
| parent | 7a987641412812a451c0d720b2243e2be3922f2a (diff) | |
| download | ChibiOS-52bc6794fa660ad3b1a338576ada3d0fb68f5b16.tar.gz ChibiOS-52bc6794fa660ad3b1a338576ada3d0fb68f5b16.tar.bz2 ChibiOS-52bc6794fa660ad3b1a338576ada3d0fb68f5b16.zip | |
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
Diffstat (limited to 'os/ex/ST')
| -rw-r--r-- | os/ex/ST/lsm303dlhc.c | 40 | ||||
| -rw-r--r-- | 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)
|