From 638d27eabf74c2ea7f12f61336836f467a52d05b Mon Sep 17 00:00:00 2001 From: Rocco Marco Guglielmi Date: Fri, 30 Sep 2016 14:37:45 +0000 Subject: Improved LIS3DSH driver and related demos: improved bias and sensitivity handling. git-svn-id: svn://svn.code.sf.net/p/chibios/svn/trunk@9830 35acf78f-673a-0410-8e92-d51de3d6d3f4 --- os/ex/ST/lis3dsh.c | 134 +++++++++++++++++++++++++++++++++++++++++------------ os/ex/ST/lis3dsh.h | 20 ++++---- 2 files changed, 115 insertions(+), 39 deletions(-) (limited to 'os/ex/ST') diff --git a/os/ex/ST/lis3dsh.c b/os/ex/ST/lis3dsh.c index d8c9ca68f..b8d98bbf8 100644 --- a/os/ex/ST/lis3dsh.c +++ b/os/ex/ST/lis3dsh.c @@ -107,21 +107,25 @@ static msg_t read_raw(void *ip, int32_t axes[LIS3DSH_NUMBER_OF_AXES]) { #if LIS3DSH_USE_SPI osalDbgAssert((((LIS3DSHDriver *)ip)->config->spip->state == SPI_READY), "read_raw(), channel not ready"); + #if LIS3DSH_SHARED_SPI spiAcquireBus(((LIS3DSHDriver *)ip)->config->spip); spiStart(((LIS3DSHDriver *)ip)->config->spip, ((LIS3DSHDriver *)ip)->config->spicfg); -#endif /* LIS3DSH_SHARED_SPI */ +#endif /* LIS3DSH_SHARED_SPI */ + lis3dshSPIReadRegister(((LIS3DSHDriver *)ip)->config->spip, LIS3DSH_AD_OUT_X_L, LIS3DSH_NUMBER_OF_AXES * 2, buff); - for(i = 0; i < LIS3DSH_NUMBER_OF_AXES; i++) { - tmp = buff[2*i] + (buff[2*i+1] << 8); - axes[i] = (int32_t)tmp; - } + #if LIS3DSH_SHARED_SPI spiReleaseBus(((LIS3DSHDriver *)ip)->config->spip); #endif /* LIS3DSH_SHARED_SPI */ -#endif /* LIS3DSH_USE_SPI */ +#endif /* LIS3DSH_USE_SPI */ + + for(i = 0; i < LIS3DSH_NUMBER_OF_AXES; i++) { + tmp = buff[2*i] + (buff[2*i+1] << 8); + axes[i] = (int32_t)tmp; + } return MSG_OK; } @@ -244,13 +248,34 @@ static msg_t set_full_scale(void *ip, lis3dsh_fs_t fs) { scale = newfs / ((LIS3DSHDriver *)ip)->fullscale; ((LIS3DSHDriver *)ip)->fullscale = newfs; - /* Updating register.*/ +#if LIS3DSH_USE_SPI +#if LIS3DSH_SHARED_SPI + spiAcquireBus(((LIS3DSHDriver *)ip)->config->spip); + spiStart(((LIS3DSHDriver *)ip)->config->spip, + ((LIS3DSHDriver *)ip)->config->spicfg); +#endif /* LIS3DSH_SHARED_SPI */ lis3dshSPIReadRegister(((LIS3DSHDriver *)ip)->config->spip, LIS3DSH_AD_CTRL_REG5, 1, &cr); +#if LIS3DSH_SHARED_SPI + spiReleaseBus(((LIS3DSHDriver *)ip)->config->spip); +#endif /* LIS3DSH_SHARED_SPI */ +#endif /* LIS3DSH_USE_SPI */ + cr &= ~(LIS3DSH_CTRL_REG5_FS_MASK); cr |= fs; + +#if LIS3DSH_USE_SPI +#if LIS3DSH_SHARED_SPI + spiAcquireBus(((LIS3DSHDriver *)ip)->config->spip); + spiStart(((LIS3DSHDriver *)ip)->config->spip, + ((LIS3DSHDriver *)ip)->config->spicfg); +#endif /* LIS3DSH_SHARED_SPI */ lis3dshSPIWriteRegister(((LIS3DSHDriver *)ip)->config->spip, LIS3DSH_AD_CTRL_REG5, 1, &cr); +#if LIS3DSH_SHARED_SPI + spiReleaseBus(((LIS3DSHDriver *)ip)->config->spip); +#endif /* LIS3DSH_SHARED_SPI */ +#endif /* LIS3DSH_USE_SPI */ /* Scaling sensitivity and bias. Re-calibration is suggested anyway. */ for(i = 0; i < LIS3DSH_NUMBER_OF_AXES; i++) { @@ -294,7 +319,7 @@ void lis3dshObjectInit(LIS3DSHDriver *devp) { devp->vmt_lis3dsh = &vmt_lis3dsh; devp->config = NULL; for(i = 0; i < LIS3DSH_NUMBER_OF_AXES; i++) - devp->bias[i] = 0; + devp->bias[i] = 0.0f; devp->state = LIS3DSH_STOP; } @@ -315,13 +340,6 @@ void lis3dshStart(LIS3DSHDriver *devp, const LIS3DSHConfig *config) { devp->config = config; -#if LIS3DSH_USE_SPI -#if LIS3DSH_SHARED_SPI - spiAcquireBus((devp)->config->spip); -#endif /* LIS3DSH_SHARED_SPI */ - spiStart((devp)->config->spip, - (devp)->config->spicfg); - /* Control register 4 configuration block.*/ { cr = LIS3DSH_CTRL_REG4_XEN | LIS3DSH_CTRL_REG4_YEN | LIS3DSH_CTRL_REG4_ZEN | @@ -330,9 +348,21 @@ void lis3dshStart(LIS3DSHDriver *devp, const LIS3DSHConfig *config) { cr |= devp->config->blockdataupdate; #endif } + +#if LIS3DSH_USE_SPI +#if LIS3DSH_SHARED_SPI + spiAcquireBus((devp)->config->spip); +#endif /* LIS3DSH_SHARED_SPI */ + spiStart((devp)->config->spip, (devp)->config->spicfg); + lis3dshSPIWriteRegister(devp->config->spip, LIS3DSH_AD_CTRL_REG4, 1, &cr); +#if LIS3DSH_SHARED_SPI + spiReleaseBus((devp)->config->spip); +#endif /* LIS3DSH_SHARED_SPI */ +#endif /* LIS3DSH_USE_SPI */ + /* Control register 5 configuration block.*/ { cr = devp->config->fullscale; @@ -340,9 +370,21 @@ void lis3dshStart(LIS3DSHDriver *devp, const LIS3DSHConfig *config) { cr |= devp->config->antialiasing; #endif } + +#if LIS3DSH_USE_SPI +#if LIS3DSH_SHARED_SPI + spiAcquireBus((devp)->config->spip); + spiStart((devp)->config->spip, (devp)->config->spicfg); +#endif /* LIS3DSH_SHARED_SPI */ + lis3dshSPIWriteRegister(devp->config->spip, LIS3DSH_AD_CTRL_REG5, 1, &cr); +#if LIS3DSH_SHARED_SPI + spiReleaseBus((devp)->config->spip); +#endif /* LIS3DSH_SHARED_SPI */ +#endif /* LIS3DSH_USE_SPI */ + /* Control register 6 configuration block.*/ { cr = LIS3DSH_CTRL_REG6_ADD_INC; @@ -350,42 +392,76 @@ void lis3dshStart(LIS3DSHDriver *devp, const LIS3DSHConfig *config) { cr |= devp->config->blockdataupdate; #endif } + +#if LIS3DSH_USE_SPI +#if LIS3DSH_SHARED_SPI + spiAcquireBus((devp)->config->spip); + spiStart((devp)->config->spip, (devp)->config->spicfg); +#endif /* LIS3DSH_SHARED_SPI */ + lis3dshSPIWriteRegister(devp->config->spip, LIS3DSH_AD_CTRL_REG6, 1, &cr); + #if LIS3DSH_SHARED_SPI spiReleaseBus((devp)->config->spip); #endif /* LIS3DSH_SHARED_SPI */ #endif /* LIS3DSH_USE_SPI */ - /* Storing sensitivity information according to full scale value */ + /* Storing sensitivity information according to user setting */ if(devp->config->fullscale == LIS3DSH_FS_2G) { devp->fullscale = LIS3DSH_2G; - for(i = 0; i < LIS3DSH_NUMBER_OF_AXES; i++) - devp->sensitivity[i] = LIS3DSH_SENS_2G; + if(devp->config->sensitivity == NULL) + for(i = 0; i < LIS3DSH_NUMBER_OF_AXES; i++) + devp->sensitivity[i] = LIS3DSH_SENS_2G; + else + for(i = 0; i < LIS3DSH_NUMBER_OF_AXES; i++) + devp->sensitivity[i] = devp->config->sensitivity[i]; } else if(devp->config->fullscale == LIS3DSH_FS_4G) { devp->fullscale = LIS3DSH_4G; - for(i = 0; i < LIS3DSH_NUMBER_OF_AXES; i++) - devp->sensitivity[i] = LIS3DSH_SENS_4G; + if(devp->config->sensitivity == NULL) + for(i = 0; i < LIS3DSH_NUMBER_OF_AXES; i++) + devp->sensitivity[i] = LIS3DSH_SENS_4G; + else + for(i = 0; i < LIS3DSH_NUMBER_OF_AXES; i++) + devp->sensitivity[i] = devp->config->sensitivity[i]; } else if(devp->config->fullscale == LIS3DSH_FS_6G) { devp->fullscale = LIS3DSH_6G; - for(i = 0; i < LIS3DSH_NUMBER_OF_AXES; i++) - devp->sensitivity[i] = LIS3DSH_SENS_6G; + if(devp->config->sensitivity == NULL) + for(i = 0; i < LIS3DSH_NUMBER_OF_AXES; i++) + devp->sensitivity[i] = LIS3DSH_SENS_6G; + else + for(i = 0; i < LIS3DSH_NUMBER_OF_AXES; i++) + devp->sensitivity[i] = devp->config->sensitivity[i]; } else if(devp->config->fullscale == LIS3DSH_FS_8G) { devp->fullscale = LIS3DSH_8G; - for(i = 0; i < LIS3DSH_NUMBER_OF_AXES; i++) - devp->sensitivity[i] = LIS3DSH_SENS_8G; + if(devp->config->sensitivity == NULL) + for(i = 0; i < LIS3DSH_NUMBER_OF_AXES; i++) + devp->sensitivity[i] = LIS3DSH_SENS_8G; + else + for(i = 0; i < LIS3DSH_NUMBER_OF_AXES; i++) + devp->sensitivity[i] = devp->config->sensitivity[i]; } else if(devp->config->fullscale == LIS3DSH_FS_16G) { devp->fullscale = LIS3DSH_16G; - for(i = 0; i < LIS3DSH_NUMBER_OF_AXES; i++) - devp->sensitivity[i] = LIS3DSH_SENS_16G; + if(devp->config->sensitivity == NULL) + for(i = 0; i < LIS3DSH_NUMBER_OF_AXES; i++) + devp->sensitivity[i] = LIS3DSH_SENS_16G; + else + for(i = 0; i < LIS3DSH_NUMBER_OF_AXES; i++) + devp->sensitivity[i] = devp->config->sensitivity[i]; } else { osalDbgAssert(FALSE, "lis3dshStart(), accelerometer full scale issue"); } + + /* Storing bias information according to user setting */ + if(devp->config->bias != NULL) + for(i = 0; i < LIS3DSH_NUMBER_OF_AXES; i++) + devp->bias[i] = devp->config->bias[i]; + /* This is the Accelerometer transient recovery time */ osalThreadSleepMilliseconds(10); @@ -406,8 +482,8 @@ void lis3dshStop(LIS3DSHDriver *devp) { osalDbgAssert((devp->state == LIS3DSH_STOP) || (devp->state == LIS3DSH_READY), "lis3dshStop(), invalid state"); + if (devp->state == LIS3DSH_READY) { #if (LIS3DSH_USE_SPI) - if (devp->state == LIS3DSH_STOP) { #if LIS3DSH_SHARED_SPI spiAcquireBus((devp)->config->spip); spiStart((devp)->config->spip, @@ -420,9 +496,9 @@ void lis3dshStop(LIS3DSHDriver *devp) { spiStop((devp)->config->spip); #if LIS3DSH_SHARED_SPI spiReleaseBus((devp)->config->spip); -#endif /* LIS3DSH_SHARED_SPI */ - } +#endif /* LIS3DSH_SHARED_SPI */ #endif /* LIS3DSH_USE_SPI */ + } devp->state = LIS3DSH_STOP; } /** @} */ diff --git a/os/ex/ST/lis3dsh.h b/os/ex/ST/lis3dsh.h index 79e611e53..22a18f4a7 100644 --- a/os/ex/ST/lis3dsh.h +++ b/os/ex/ST/lis3dsh.h @@ -41,7 +41,7 @@ /** * @brief LIS3DSH driver version string. */ -#define EX_LIS3DSH_VERSION "1.0.1" +#define EX_LIS3DSH_VERSION "1.0.2" /** * @brief LIS3DSH driver version major number. @@ -56,7 +56,7 @@ /** * @brief LIS3DSH driver version patch number. */ -#define EX_LIS3DSH_PATCH 1 +#define EX_LIS3DSH_PATCH 2 /** @} */ /** @@ -435,28 +435,28 @@ typedef struct { /** * @brief LIS3DSH initial sensitivity. */ - float sensitivity[LIS3DSH_NUMBER_OF_AXES]; + float *sensitivity; /** * @brief LIS3DSH initial bias. */ - float bias[LIS3DSH_NUMBER_OF_AXES]; + float *bias; /** * @brief LIS3DSH full scale value. */ - lis3dsh_fs_t fullscale; + lis3dsh_fs_t fullscale; /** * @brief LIS3DSH output data rate selection. */ - lis3dsh_odr_t outputdatarate; + lis3dsh_odr_t outputdatarate; #if LIS3DSH_USE_ADVANCED || defined(__DOXYGEN__) /** * @brief LIS3DSH anti-aliasing bandwidth. */ - lis3dsh_bw_t antialiasing; + lis3dsh_bw_t antialiasing; /** * @brief LIS3DSH block data update. */ - lis3dsh_bdu_t blockdataupdate; + lis3dsh_bdu_t blockdataupdate; #endif } LIS3DSHConfig; @@ -488,9 +488,9 @@ struct LIS3DSHVMT { #define _lis3dsh_data \ _base_accelerometer_data \ /* Driver state.*/ \ - lis3dsh_state_t state; \ + lis3dsh_state_t state; \ /* Current configuration data.*/ \ - const LIS3DSHConfig *config; \ + const LIS3DSHConfig *config; \ /* Current sensitivity.*/ \ float sensitivity[LIS3DSH_NUMBER_OF_AXES]; \ /* Bias data.*/ \ -- cgit v1.2.3