diff options
| author | Rocco Marco Guglielmi <roccomarco.guglielmi@live.com> | 2016-04-30 15:33:42 +0000 |
|---|---|---|
| committer | Rocco Marco Guglielmi <roccomarco.guglielmi@live.com> | 2016-04-30 15:33:42 +0000 |
| commit | 4ec01316ea530b9bd9b40a1947a7a45f737c4ef6 (patch) | |
| tree | 7df8ed43ac340a422f016d1ea98e9ce5255d61c2 /os/ex/ST/lis3mdl.c | |
| parent | 5c953ae7a200b9a2cd7c13f9cecd4d5aa7098d5f (diff) | |
| download | ChibiOS-4ec01316ea530b9bd9b40a1947a7a45f737c4ef6.tar.gz ChibiOS-4ec01316ea530b9bd9b40a1947a7a45f737c4ef6.tar.bz2 ChibiOS-4ec01316ea530b9bd9b40a1947a7a45f737c4ef6.zip | |
ChibiOS/EX: improved MEMS get_axes functions
Fixed bug related to LSM6DS0 get_temperature
git-svn-id: svn://svn.code.sf.net/p/chibios/svn/trunk@9390 35acf78f-673a-0410-8e92-d51de3d6d3f4
Diffstat (limited to 'os/ex/ST/lis3mdl.c')
| -rw-r--r-- | os/ex/ST/lis3mdl.c | 48 |
1 files changed, 24 insertions, 24 deletions
diff --git a/os/ex/ST/lis3mdl.c b/os/ex/ST/lis3mdl.c index e8022ae2c..9d1a7e160 100644 --- a/os/ex/ST/lis3mdl.c +++ b/os/ex/ST/lis3mdl.c @@ -189,7 +189,7 @@ static size_t get_axes_number(void *ip) { } static msg_t read_raw(void *ip, int32_t axes[LIS3MDL_NUMBER_OF_AXES]) { - + uint16_t tmp; osalDbgCheck((ip != NULL) && (axes != NULL)); osalDbgAssert((((LIS3MDLDriver *)ip)->state == LIS3MDL_READY), "read_raw(), invalid state"); @@ -203,29 +203,29 @@ static msg_t read_raw(void *ip, int32_t axes[LIS3MDL_NUMBER_OF_AXES]) { ((LIS3MDLDriver *)ip)->config->i2ccfg); #endif /* LIS3MDL_SHARED_I2C */ - axes[0] = (int16_t)(lis3mdlI2CReadRegister(((LIS3MDLDriver *)ip)->config->i2cp, - ((LIS3MDLDriver *)ip)->config->slaveaddress, - LIS3MDL_AD_OUT_X_L, NULL)); - axes[0] += (int16_t)(lis3mdlI2CReadRegister(((LIS3MDLDriver *)ip)->config->i2cp, - ((LIS3MDLDriver *)ip)->config->slaveaddress, - LIS3MDL_AD_OUT_X_H, NULL) << 8); - axes[0] -= ((LIS3MDLDriver *)ip)->bias[0]; - - axes[1] = (int16_t)(lis3mdlI2CReadRegister(((LIS3MDLDriver *)ip)->config->i2cp, - ((LIS3MDLDriver *)ip)->config->slaveaddress, - LIS3MDL_AD_OUT_Y_L, NULL)); - axes[1] += (int16_t)(lis3mdlI2CReadRegister(((LIS3MDLDriver *)ip)->config->i2cp, - ((LIS3MDLDriver *)ip)->config->slaveaddress, - LIS3MDL_AD_OUT_Y_H, NULL) << 8); - axes[1] -= ((LIS3MDLDriver *)ip)->bias[1]; - - axes[2] = (int16_t)(lis3mdlI2CReadRegister(((LIS3MDLDriver *)ip)->config->i2cp, - ((LIS3MDLDriver *)ip)->config->slaveaddress, - LIS3MDL_AD_OUT_Z_L, NULL)); - axes[2] += (int16_t)(lis3mdlI2CReadRegister(((LIS3MDLDriver *)ip)->config->i2cp, - ((LIS3MDLDriver *)ip)->config->slaveaddress, - LIS3MDL_AD_OUT_Z_H, NULL) << 8); - axes[2] -= ((LIS3MDLDriver *)ip)->bias[2]; + tmp = lis3mdlI2CReadRegister(((LIS3MDLDriver *)ip)->config->i2cp, + ((LIS3MDLDriver *)ip)->config->slaveaddress, + LIS3MDL_AD_OUT_X_L, NULL); + tmp += lis3mdlI2CReadRegister(((LIS3MDLDriver *)ip)->config->i2cp, + ((LIS3MDLDriver *)ip)->config->slaveaddress, + LIS3MDL_AD_OUT_X_H, NULL) << 8; + axes[0] = (int32_t)tmp - ((LIS3MDLDriver *)ip)->bias[0]; + + tmp = lis3mdlI2CReadRegister(((LIS3MDLDriver *)ip)->config->i2cp, + ((LIS3MDLDriver *)ip)->config->slaveaddress, + LIS3MDL_AD_OUT_Y_L, NULL); + tmp += lis3mdlI2CReadRegister(((LIS3MDLDriver *)ip)->config->i2cp, + ((LIS3MDLDriver *)ip)->config->slaveaddress, + LIS3MDL_AD_OUT_Y_H, NULL) << 8; + axes[1] = (int32_t)tmp - ((LIS3MDLDriver *)ip)->bias[1]; + + tmp = lis3mdlI2CReadRegister(((LIS3MDLDriver *)ip)->config->i2cp, + ((LIS3MDLDriver *)ip)->config->slaveaddress, + LIS3MDL_AD_OUT_Z_L, NULL); + tmp += lis3mdlI2CReadRegister(((LIS3MDLDriver *)ip)->config->i2cp, + ((LIS3MDLDriver *)ip)->config->slaveaddress, + LIS3MDL_AD_OUT_Z_H, NULL) << 8; + axes[2] = (int32_t)tmp - ((LIS3MDLDriver *)ip)->bias[2]; #if LIS3MDL_SHARED_I2C i2cReleaseBus(((LIS3MDLDriver *)ip)->config->i2cp); |