andreika-git
/
ChibiOS
mirror of https://github.com/andreika-git/ChibiOS.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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
This commit is contained in:
Rocco Marco Guglielmi 2016-07-28 10:38:43 +00:00
parent 7a98764141
commit 52bc6794fa
2 changed files with 39 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

40
os/ex/ST/lsm303dlhc.c
View File

@ -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) { static msg_t acc_set_full_scale(void *ip, lsm303dlhc_acc_fs_t fs) {
float newfs, scale; float newfs, scale;
unsigned i; uint8_t i, buff[2];
msg_t msg;
if(fs == LSM303DLHC_ACC_FS_2G) { if(fs == LSM303DLHC_ACC_FS_2G) {
newfs = LSM303DLHC_ACC_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) { if(newfs != ((LSM303DLHCDriver *)ip)->accfullscale) {
scale = newfs / ((LSM303DLHCDriver *)ip)->accfullscale; scale = newfs / ((LSM303DLHCDriver *)ip)->accfullscale;
((LSM303DLHCDriver *)ip)->accfullscale = newfs; ((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. */ /* Scaling sensitivity and bias. Re-calibration is suggested anyway. */
for(i = 0; i < LSM303DLHC_ACC_NUMBER_OF_AXES; i++) { for(i = 0; i < LSM303DLHC_ACC_NUMBER_OF_AXES; i++) {
((LSM303DLHCDriver *)ip)->accsensitivity[i] *= scale; ((LSM303DLHCDriver *)ip)->accsensitivity[i] *= scale;
((LSM303DLHCDriver *)ip)->accbias[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) { static msg_t comp_set_full_scale(void *ip, lsm303dlhc_comp_fs_t fs) {
float newfs, scale; float newfs, scale;
unsigned i; uint8_t i, buff[2];
msg_t msg;
if(fs == LSM303DLHC_COMP_FS_1P3GA) { if(fs == LSM303DLHC_COMP_FS_1P3GA) {
newfs = LSM303DLHC_COMP_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) { if(newfs != ((LSM303DLHCDriver *)ip)->compfullscale) {
scale = newfs / ((LSM303DLHCDriver *)ip)->compfullscale; scale = newfs / ((LSM303DLHCDriver *)ip)->compfullscale;
((LSM303DLHCDriver *)ip)->compfullscale = newfs; ((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. */ /* Scaling sensitivity and bias. Re-calibration is suggested anyway. */
for(i = 0; i < LSM303DLHC_COMP_NUMBER_OF_AXES; i++) { for(i = 0; i < LSM303DLHC_COMP_NUMBER_OF_AXES; i++) {
((LSM303DLHCDriver *)ip)->compsensitivity[i] *= scale; ((LSM303DLHCDriver *)ip)->compsensitivity[i] *= scale;

2
os/ex/ST/lsm303dlhc.h
View File

@ -222,6 +222,7 @@
#define LSM303DLHC_CTRL_REG4_A_MASK 0xF9 #define LSM303DLHC_CTRL_REG4_A_MASK 0xF9
#define LSM303DLHC_CTRL_REG4_A_SIM (1 << 0) #define LSM303DLHC_CTRL_REG4_A_SIM (1 << 0)
#define LSM303DLHC_CTRL_REG4_A_HR (1 << 3) #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_FS0 (1 << 4)
#define LSM303DLHC_CTRL_REG4_A_FS1 (1 << 5) #define LSM303DLHC_CTRL_REG4_A_FS1 (1 << 5)
#define LSM303DLHC_CTRL_REG4_A_BLE (1 << 6) #define LSM303DLHC_CTRL_REG4_A_BLE (1 << 6)
@ -270,6 +271,7 @@
* @{ * @{
*/ */
#define LSM303DLHC_CRB_REG_M_MASK 0xE0 #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_GN0 (1 << 5)
#define LSM303DLHC_CRB_REG_M_GN1 (1 << 6) #define LSM303DLHC_CRB_REG_M_GN1 (1 << 6)
#define LSM303DLHC_CRB_REG_M_GN2 (1 << 7) #define LSM303DLHC_CRB_REG_M_GN2 (1 << 7)