lsm6dsl.c

00001 /**
00002  ******************************************************************************
00003  * @file    lsm6dsl.c
00004  * @author  MCD Application Team
00005  * @version V1.0.0
00006  * @date    14-February-2017
00007  * @brief   This file provides a set of functions needed to manage the LSM6DSL
00008  *          accelero and gyro devices
00009  ******************************************************************************
00010  * @attention
00011  *
00012  * <h2><center>&copy; COPYRIGHT(c) 2017 STMicroelectronics</center></h2>
00013  *
00014  * Redistribution and use in source and binary forms, with or without modification,
00015  * are permitted provided that the following conditions are met:
00016  *   1. Redistributions of source code must retain the above copyright notice,
00017  *      this list of conditions and the following disclaimer.
00018  *   2. Redistributions in binary form must reproduce the above copyright notice,
00019  *      this list of conditions and the following disclaimer in the documentation
00020  *      and/or other materials provided with the distribution.
00021  *   3. Neither the name of STMicroelectronics nor the names of its contributors
00022  *      may be used to endorse or promote products derived from this software
00023  *      without specific prior written permission.
00024  *
00025  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00026  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00027  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00028  * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
00029  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
00030  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
00031  * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00032  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
00033  * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
00034  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00035  *
00036  ******************************************************************************
00037  */
00038 
00039 /* Includes ------------------------------------------------------------------*/
00040 #include "lsm6dsl.h"
00041 
00042 /** @addtogroup BSP
00043   * @{
00044   */
00045 
00046 /** @addtogroup Component
00047   * @{
00048   */
00049 
00050 /** @defgroup LSM6DSL LSM6DSL
00051   * @{
00052   */
00053 
00054 /** @defgroup LSM6DSL_Private_Variables LSM6DSL Private Variables
00055   * @{
00056   */ 
00057 ACCELERO_DrvTypeDef Lsm6dslAccDrv =
00058 {
00059   LSM6DSL_AccInit,
00060   LSM6DSL_AccDeInit,
00061   LSM6DSL_AccReadID,
00062   0,
00063   LSM6DSL_AccLowPower,
00064   0,
00065   0,
00066   0,
00067   0,
00068   0,
00069   0,
00070   0,
00071   LSM6DSL_AccReadXYZ
00072 };
00073 
00074 GYRO_DrvTypeDef Lsm6dslGyroDrv =
00075 {
00076   LSM6DSL_GyroInit,
00077   LSM6DSL_GyroDeInit,
00078   LSM6DSL_GyroReadID,
00079   0,
00080   LSM6DSL_GyroLowPower,
00081   0,
00082   0,
00083   0,
00084   0,
00085   0,
00086   0,
00087   0,
00088   LSM6DSL_GyroReadXYZAngRate
00089 };
00090 /**
00091   * @}
00092   */ 
00093 
00094 /** @defgroup LSM6DSL_ACC_Private_Functions LSM6DSL ACC Private Functions
00095   * @{
00096   */
00097 /**
00098   * @brief  Set LSM6DSL Accelerometer Initialization.
00099   * @param  InitStruct: Init parameters
00100   */
00101 void LSM6DSL_AccInit(uint16_t InitStruct)
00102 {  
00103   uint8_t ctrl = 0x00;
00104   uint8_t tmp;
00105 
00106   /* Read CTRL1_XL */
00107   tmp = SENSOR_IO_Read(LSM6DSL_ACC_GYRO_I2C_ADDRESS_LOW, LSM6DSL_ACC_GYRO_CTRL1_XL);
00108 
00109   /* Write value to ACC MEMS CTRL1_XL register: FS and Data Rate */
00110   ctrl = (uint8_t) InitStruct;
00111   tmp &= ~(0xFC);
00112   tmp |= ctrl;
00113   SENSOR_IO_Write(LSM6DSL_ACC_GYRO_I2C_ADDRESS_LOW, LSM6DSL_ACC_GYRO_CTRL1_XL, tmp);
00114 
00115   /* Read CTRL3_C */
00116   tmp = SENSOR_IO_Read(LSM6DSL_ACC_GYRO_I2C_ADDRESS_LOW, LSM6DSL_ACC_GYRO_CTRL3_C);
00117 
00118   /* Write value to ACC MEMS CTRL3_C register: BDU and Auto-increment */
00119   ctrl = ((uint8_t) (InitStruct >> 8));
00120   tmp &= ~(0x44);
00121   tmp |= ctrl; 
00122   SENSOR_IO_Write(LSM6DSL_ACC_GYRO_I2C_ADDRESS_LOW, LSM6DSL_ACC_GYRO_CTRL3_C, tmp);
00123 }
00124 
00125 /**
00126   * @brief  LSM6DSL Accelerometer De-initialization.
00127   */
00128 void LSM6DSL_AccDeInit(void)
00129 {
00130   uint8_t ctrl = 0x00;
00131   
00132   /* Read control register 1 value */
00133   ctrl = SENSOR_IO_Read(LSM6DSL_ACC_GYRO_I2C_ADDRESS_LOW, LSM6DSL_ACC_GYRO_CTRL1_XL);
00134 
00135   /* Clear ODR bits */
00136   ctrl &= ~(LSM6DSL_ODR_BITPOSITION);
00137 
00138   /* Set Power down */
00139   ctrl |= LSM6DSL_ODR_POWER_DOWN;
00140   
00141   /* write back control register */
00142   SENSOR_IO_Write(LSM6DSL_ACC_GYRO_I2C_ADDRESS_LOW, LSM6DSL_ACC_GYRO_CTRL1_XL, ctrl);
00143 }
00144 
00145 /**
00146   * @brief  Read LSM6DSL ID.
00147   * @retval ID 
00148   */
00149 uint8_t LSM6DSL_AccReadID(void)
00150 {  
00151   /* IO interface initialization */
00152   SENSOR_IO_Init();
00153   /* Read value at Who am I register address */
00154   return (SENSOR_IO_Read(LSM6DSL_ACC_GYRO_I2C_ADDRESS_LOW, LSM6DSL_ACC_GYRO_WHO_AM_I_REG));
00155 }
00156 
00157 /**
00158   * @brief  Set/Unset Accelerometer in low power mode.
00159   * @param  status 0 means disable Low Power Mode, otherwise Low Power Mode is enabled
00160   */
00161 void LSM6DSL_AccLowPower(uint16_t status)
00162 {
00163   uint8_t ctrl = 0x00;
00164   
00165   /* Read CTRL6_C value */
00166   ctrl = SENSOR_IO_Read(LSM6DSL_ACC_GYRO_I2C_ADDRESS_LOW, LSM6DSL_ACC_GYRO_CTRL6_C);
00167 
00168   /* Clear Low Power Mode bit */
00169   ctrl &= ~(0x10);
00170 
00171   /* Set Low Power Mode */
00172   if(status)
00173   {
00174     ctrl |= LSM6DSL_ACC_GYRO_LP_XL_ENABLED;
00175   }else
00176   {
00177     ctrl |= LSM6DSL_ACC_GYRO_LP_XL_DISABLED;
00178   }
00179   
00180   /* write back control register */
00181   SENSOR_IO_Write(LSM6DSL_ACC_GYRO_I2C_ADDRESS_LOW, LSM6DSL_ACC_GYRO_CTRL6_C, ctrl);
00182 }
00183 
00184 /**
00185   * @brief  Read X, Y & Z Acceleration values 
00186   * @param  pData: Data out pointer
00187   */
00188 void LSM6DSL_AccReadXYZ(int16_t* pData)
00189 {
00190   int16_t pnRawData[3];
00191   uint8_t ctrlx= 0;
00192   uint8_t buffer[6];
00193   uint8_t i = 0;
00194   float sensitivity = 0;
00195   
00196   /* Read the acceleration control register content */
00197   ctrlx = SENSOR_IO_Read(LSM6DSL_ACC_GYRO_I2C_ADDRESS_LOW, LSM6DSL_ACC_GYRO_CTRL1_XL);
00198   
00199   /* Read output register X, Y & Z acceleration */
00200   SENSOR_IO_ReadMultiple(LSM6DSL_ACC_GYRO_I2C_ADDRESS_LOW, LSM6DSL_ACC_GYRO_OUTX_L_XL, buffer, 6);
00201   
00202   for(i=0; i<3; i++)
00203   {
00204     pnRawData[i]=((((uint16_t)buffer[2*i+1]) << 8) + (uint16_t)buffer[2*i]);
00205   }
00206   
00207   /* Normal mode */
00208   /* Switch the sensitivity value set in the CRTL1_XL */
00209   switch(ctrlx & 0x0C)
00210   {
00211   case LSM6DSL_ACC_FULLSCALE_2G:
00212     sensitivity = LSM6DSL_ACC_SENSITIVITY_2G;
00213     break;
00214   case LSM6DSL_ACC_FULLSCALE_4G:
00215     sensitivity = LSM6DSL_ACC_SENSITIVITY_4G;
00216     break;
00217   case LSM6DSL_ACC_FULLSCALE_8G:
00218     sensitivity = LSM6DSL_ACC_SENSITIVITY_8G;
00219     break;
00220   case LSM6DSL_ACC_FULLSCALE_16G:
00221     sensitivity = LSM6DSL_ACC_SENSITIVITY_16G;
00222     break;    
00223   }
00224   
00225   /* Obtain the mg value for the three axis */
00226   for(i=0; i<3; i++)
00227   {
00228     pData[i]=( int16_t )(pnRawData[i] * sensitivity);
00229   }
00230 }
00231 
00232 /**
00233   * @}
00234   */ 
00235 
00236 /** @defgroup LSM6DSL_GYRO_Private_Functions LSM6DSL GYRO Private Functions
00237   * @{
00238   */
00239 
00240 /**
00241   * @brief  Set LSM6DSL Gyroscope Initialization.
00242   * @param  InitStruct: pointer to a LSM6DSL_InitTypeDef structure 
00243   *         that contains the configuration setting for the LSM6DSL.
00244   */
00245 void LSM6DSL_GyroInit(uint16_t InitStruct)
00246 {  
00247   uint8_t ctrl = 0x00;
00248   uint8_t tmp;
00249 
00250   /* Read CTRL2_G */
00251   tmp = SENSOR_IO_Read(LSM6DSL_ACC_GYRO_I2C_ADDRESS_LOW, LSM6DSL_ACC_GYRO_CTRL2_G);
00252 
00253   /* Write value to GYRO MEMS CTRL2_G register: FS and Data Rate */
00254   ctrl = (uint8_t) InitStruct;
00255   tmp &= ~(0xFC);
00256   tmp |= ctrl;
00257   SENSOR_IO_Write(LSM6DSL_ACC_GYRO_I2C_ADDRESS_LOW, LSM6DSL_ACC_GYRO_CTRL2_G, tmp);
00258 
00259   /* Read CTRL3_C */
00260   tmp = SENSOR_IO_Read(LSM6DSL_ACC_GYRO_I2C_ADDRESS_LOW, LSM6DSL_ACC_GYRO_CTRL3_C);
00261 
00262   /* Write value to GYRO MEMS CTRL3_C register: BDU and Auto-increment */
00263   ctrl = ((uint8_t) (InitStruct >> 8));
00264   tmp &= ~(0x44);
00265   tmp |= ctrl; 
00266   SENSOR_IO_Write(LSM6DSL_ACC_GYRO_I2C_ADDRESS_LOW, LSM6DSL_ACC_GYRO_CTRL3_C, tmp);
00267 }
00268 
00269 
00270 /**
00271   * @brief LSM6DSL Gyroscope De-initialization
00272   */
00273 void LSM6DSL_GyroDeInit(void)
00274 {
00275   uint8_t ctrl = 0x00;
00276   
00277   /* Read control register 1 value */
00278   ctrl = SENSOR_IO_Read(LSM6DSL_ACC_GYRO_I2C_ADDRESS_LOW, LSM6DSL_ACC_GYRO_CTRL2_G);
00279 
00280   /* Clear ODR bits */
00281   ctrl &= ~(LSM6DSL_ODR_BITPOSITION);
00282 
00283   /* Set Power down */
00284   ctrl |= LSM6DSL_ODR_POWER_DOWN;
00285   
00286   /* write back control register */
00287   SENSOR_IO_Write(LSM6DSL_ACC_GYRO_I2C_ADDRESS_LOW, LSM6DSL_ACC_GYRO_CTRL2_G, ctrl);
00288 }
00289 
00290 /**
00291   * @brief  Read ID address of LSM6DSL
00292   * @retval ID 
00293   */
00294 uint8_t LSM6DSL_GyroReadID(void)
00295 {
00296   /* IO interface initialization */
00297   SENSOR_IO_Init();  
00298   /* Read value at Who am I register address */
00299   return SENSOR_IO_Read(LSM6DSL_ACC_GYRO_I2C_ADDRESS_LOW, LSM6DSL_ACC_GYRO_WHO_AM_I_REG);
00300 }
00301 
00302 /**
00303   * @brief Set/Unset LSM6DSL Gyroscope in low power mode
00304   * @param  status 0 means disable Low Power Mode, otherwise Low Power Mode is enabled 
00305   */
00306 void LSM6DSL_GyroLowPower(uint16_t status)
00307 {  
00308   uint8_t ctrl = 0x00;
00309   
00310   /* Read CTRL7_G value */
00311   ctrl = SENSOR_IO_Read(LSM6DSL_ACC_GYRO_I2C_ADDRESS_LOW, LSM6DSL_ACC_GYRO_CTRL7_G);
00312 
00313   /* Clear Low Power Mode bit */
00314   ctrl &= ~(0x80);
00315 
00316   /* Set Low Power Mode */
00317   if(status)
00318   {
00319     ctrl |= LSM6DSL_ACC_GYRO_LP_G_ENABLED;
00320   }else
00321   {
00322     ctrl |= LSM6DSL_ACC_GYRO_LP_G_DISABLED;
00323   }
00324   
00325   /* write back control register */
00326   SENSOR_IO_Write(LSM6DSL_ACC_GYRO_I2C_ADDRESS_LOW, LSM6DSL_ACC_GYRO_CTRL7_G, ctrl);
00327 }
00328 
00329 /**
00330 * @brief  Calculate the LSM6DSL angular data.
00331 * @param  pfData: Data out pointer
00332 */
00333 void LSM6DSL_GyroReadXYZAngRate(float *pfData)
00334 {
00335   int16_t pnRawData[3];
00336   uint8_t ctrlg= 0;
00337   uint8_t buffer[6];
00338   uint8_t i = 0;
00339   float sensitivity = 0;
00340   
00341   /* Read the gyro control register content */
00342   ctrlg = SENSOR_IO_Read(LSM6DSL_ACC_GYRO_I2C_ADDRESS_LOW, LSM6DSL_ACC_GYRO_CTRL2_G);
00343   
00344   /* Read output register X, Y & Z acceleration */
00345   SENSOR_IO_ReadMultiple(LSM6DSL_ACC_GYRO_I2C_ADDRESS_LOW, LSM6DSL_ACC_GYRO_OUTX_L_G, buffer, 6);
00346   
00347   for(i=0; i<3; i++)
00348   {
00349     pnRawData[i]=((((uint16_t)buffer[2*i+1]) << 8) + (uint16_t)buffer[2*i]);
00350   }
00351   
00352   /* Normal mode */
00353   /* Switch the sensitivity value set in the CRTL2_G */
00354   switch(ctrlg & 0x0C)
00355   {
00356   case LSM6DSL_GYRO_FS_245:
00357     sensitivity = LSM6DSL_GYRO_SENSITIVITY_245DPS;
00358     break;
00359   case LSM6DSL_GYRO_FS_500:
00360     sensitivity = LSM6DSL_GYRO_SENSITIVITY_500DPS;
00361     break;
00362   case LSM6DSL_GYRO_FS_1000:
00363     sensitivity = LSM6DSL_GYRO_SENSITIVITY_1000DPS;
00364     break;
00365   case LSM6DSL_GYRO_FS_2000:
00366     sensitivity = LSM6DSL_GYRO_SENSITIVITY_2000DPS;
00367     break;    
00368   }
00369   
00370   /* Obtain the mg value for the three axis */
00371   for(i=0; i<3; i++)
00372   {
00373     pfData[i]=( float )(pnRawData[i] * sensitivity);
00374   }
00375 }
00376 
00377 /**
00378   * @}
00379   */ 
00380 
00381 /**
00382   * @}
00383   */ 
00384   
00385 /**
00386   * @}
00387   */ 
00388   
00389 /**
00390   * @}
00391   */ 
00392 
00393 /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
00394