From 7a987641412812a451c0d720b2243e2be3922f2a Mon Sep 17 00:00:00 2001 From: Rocco Marco Guglielmi Date: Wed, 27 Jul 2016 21:07:54 +0000 Subject: Fixed bug in L3GD20 set_fullscale method. git-svn-id: svn://svn.code.sf.net/p/chibios/svn/trunk@9726 35acf78f-673a-0410-8e92-d51de3d6d3f4 --- os/ex/ST/l3gd20.c | 15 +++++++++++---- os/ex/ST/l3gd20.h | 4 ++-- 2 files changed, 13 insertions(+), 6 deletions(-) (limited to 'os') diff --git a/os/ex/ST/l3gd20.c b/os/ex/ST/l3gd20.c index d214b9e9d..fba1440da 100644 --- a/os/ex/ST/l3gd20.c +++ b/os/ex/ST/l3gd20.c @@ -33,8 +33,6 @@ /* Driver local definitions. */ /*===========================================================================*/ -#define PI 3.14159f - /*===========================================================================*/ /* Driver exported variables. */ /*===========================================================================*/ @@ -237,7 +235,7 @@ static msg_t reset_sensivity(void *ip) { static msg_t set_full_scale(void *ip, l3gd20_fs_t fs) { float newfs, scale; - unsigned i; + uint8_t i, cr; if(fs == L3GD20_FS_250DPS) { newfs = L3GD20_250DPS; @@ -254,7 +252,16 @@ static msg_t set_full_scale(void *ip, l3gd20_fs_t fs) { if(newfs != ((L3GD20Driver *)ip)->fullscale) { scale = newfs / ((L3GD20Driver *)ip)->fullscale; - ((L3GD20Driver *)ip)->fullscale = newfs; + ((L3GD20Driver *)ip)->fullscale = newfs; + + /* Updating register.*/ + l3gd20SPIReadRegister(((L3GD20Driver *)ip)->config->spip, + L3GD20_AD_CTRL_REG4, 1, &cr); + cr &= ~(L3GD20_CTRL_REG4_FS_MASK); + cr |= fs; + l3gd20SPIWriteRegister(((L3GD20Driver *)ip)->config->spip, + L3GD20_AD_CTRL_REG4, 1, &cr); + /* Scaling sensitivity and bias. Re-calibration is suggested anyway. */ for(i = 0; i < L3GD20_NUMBER_OF_AXES; i++) { ((L3GD20Driver *)ip)->sensitivity[i] *= scale; diff --git a/os/ex/ST/l3gd20.h b/os/ex/ST/l3gd20.h index f6b1780a7..4a3761c30 100644 --- a/os/ex/ST/l3gd20.h +++ b/os/ex/ST/l3gd20.h @@ -59,7 +59,7 @@ /** @} */ /** - * @brief L3GD20 characteristics + * @brief L3GD20 characteristics. * * @{ */ @@ -167,7 +167,7 @@ */ #define L3GD20_CTRL_REG4_MASK 0xF1 /**< L3GD20_CTRL_REG4 mask */ #define L3GD20_CTRL_REG4_SIM (1 << 0) /**< SPI mode */ -#define L3GD20_CTRL_REG4_FS_MASK (3 << 4) /**< Full scale mask */ +#define L3GD20_CTRL_REG4_FS_MASK 0x30 /**< Full scale field mask */ #define L3GD20_CTRL_REG4_FS0 (1 << 4) /**< Full scale bit 0 */ #define L3GD20_CTRL_REG4_FS1 (1 << 5) /**< Full scale bit 1 */ #define L3GD20_CTRL_REG4_BLE (1 << 6) /**< Big/little endian data */ -- cgit v1.2.3