lsm6dsl.c

00001 /**
00002  ******************************************************************************
00003  * @file    lsm6dsl.c
00004  * @author  MCD Application Team
00005  * @brief   This file provides a set of functions needed to manage the LSM6DSL
00006  *          accelero and gyro devices
00007  ******************************************************************************
00008  * @attention
00009  *
00010  * <<h2><center>&copy; Copyright (c) 2017 STMicroelectronics.
00011  * All rights reserved.</center></h2>
00012  *
00013  * This software component is licensed by ST under BSD 3-Clause license,
00014  * the "License"; You may not use this file except in compliance with the
00015  * License. You may obtain a copy of the License at:
00016  *                        opensource.org/licenses/BSD-3-Clause
00017  *
00018  ******************************************************************************
00019  */
00020 
00021 /* Includes ------------------------------------------------------------------*/
00022 #include "lsm6dsl.h"
00023 
00024 /** @addtogroup BSP
00025   * @{
00026   */
00027 
00028 /** @addtogroup Component
00029   * @{
00030   */
00031 
00032 /** @defgroup LSM6DSL LSM6DSL
00033   * @{
00034   */
00035 
00036 /** @defgroup LSM6DSL_Private_Variables LSM6DSL Private Variables
00037   * @{
00038   */ 
00039 ACCELERO_DrvTypeDef Lsm6dslAccDrv =
00040 {
00041   LSM6DSL_AccInit,
00042   LSM6DSL_AccDeInit,
00043   LSM6DSL_AccReadID,
00044   0,
00045   LSM6DSL_AccLowPower,
00046   0,
00047   0,
00048   0,
00049   0,
00050   0,
00051   0,
00052   0,
00053   LSM6DSL_AccReadXYZ
00054 };
00055 
00056 GYRO_DrvTypeDef Lsm6dslGyroDrv =
00057 {
00058   LSM6DSL_GyroInit,
00059   LSM6DSL_GyroDeInit,
00060   LSM6DSL_GyroReadID,
00061   0,
00062   LSM6DSL_GyroLowPower,
00063   0,
00064   0,
00065   0,
00066   0,
00067   0,
00068   0,
00069   0,
00070   LSM6DSL_GyroReadXYZAngRate
00071 };
00072 /**
00073   * @}
00074   */ 
00075 
00076 /** @defgroup LSM6DSL_ACC_Private_Functions LSM6DSL ACC Private Functions
00077   * @{
00078   */
00079 /**
00080   * @brief  Set LSM6DSL Accelerometer Initialization.
00081   * @param  InitStruct: Init parameters
00082   */
00083 void LSM6DSL_AccInit(uint16_t InitStruct)
00084 {  
00085   uint8_t ctrl = 0x00;
00086   uint8_t tmp;
00087 
00088   /* Read CTRL1_XL */
00089   tmp = SENSOR_IO_Read(LSM6DSL_ACC_GYRO_I2C_ADDRESS_LOW, LSM6DSL_ACC_GYRO_CTRL1_XL);
00090 
00091   /* Write value to ACC MEMS CTRL1_XL register: FS and Data Rate */
00092   ctrl = (uint8_t) InitStruct;
00093   tmp &= ~(0xFC);
00094   tmp |= ctrl;
00095   SENSOR_IO_Write(LSM6DSL_ACC_GYRO_I2C_ADDRESS_LOW, LSM6DSL_ACC_GYRO_CTRL1_XL, tmp);
00096 
00097   /* Read CTRL3_C */
00098   tmp = SENSOR_IO_Read(LSM6DSL_ACC_GYRO_I2C_ADDRESS_LOW, LSM6DSL_ACC_GYRO_CTRL3_C);
00099 
00100   /* Write value to ACC MEMS CTRL3_C register: BDU and Auto-increment */
00101   ctrl = ((uint8_t) (InitStruct >> 8));
00102   tmp &= ~(0x44);
00103   tmp |= ctrl; 
00104   SENSOR_IO_Write(LSM6DSL_ACC_GYRO_I2C_ADDRESS_LOW, LSM6DSL_ACC_GYRO_CTRL3_C, tmp);
00105 }
00106 
00107 /**
00108   * @brief  LSM6DSL Accelerometer De-initialization.
00109   */
00110 void LSM6DSL_AccDeInit(void)
00111 {
00112   uint8_t ctrl = 0x00;
00113   
00114   /* Read control register 1 value */
00115   ctrl = SENSOR_IO_Read(LSM6DSL_ACC_GYRO_I2C_ADDRESS_LOW, LSM6DSL_ACC_GYRO_CTRL1_XL);
00116 
00117   /* Clear ODR bits */
00118   ctrl &= ~(LSM6DSL_ODR_BITPOSITION);
00119 
00120   /* Set Power down */
00121   ctrl |= LSM6DSL_ODR_POWER_DOWN;
00122   
00123   /* write back control register */
00124   SENSOR_IO_Write(LSM6DSL_ACC_GYRO_I2C_ADDRESS_LOW, LSM6DSL_ACC_GYRO_CTRL1_XL, ctrl);
00125 }
00126 
00127 /**
00128   * @brief  Read LSM6DSL ID.
00129   * @retval ID 
00130   */
00131 uint8_t LSM6DSL_AccReadID(void)
00132 {  
00133   /* IO interface initialization */
00134   SENSOR_IO_Init();
00135   /* Read value at Who am I register address */
00136   return (SENSOR_IO_Read(LSM6DSL_ACC_GYRO_I2C_ADDRESS_LOW, LSM6DSL_ACC_GYRO_WHO_AM_I_REG));
00137 }
00138 
00139 /**
00140   * @brief  Set/Unset Accelerometer in low power mode.
00141   * @param  status 0 means disable Low Power Mode, otherwise Low Power Mode is enabled
00142   */
00143 void LSM6DSL_AccLowPower(uint16_t status)
00144 {
00145   uint8_t ctrl = 0x00;
00146   
00147   /* Read CTRL6_C value */
00148   ctrl = SENSOR_IO_Read(LSM6DSL_ACC_GYRO_I2C_ADDRESS_LOW, LSM6DSL_ACC_GYRO_CTRL6_C);
00149 
00150   /* Clear Low Power Mode bit */
00151   ctrl &= ~(0x10);
00152 
00153   /* Set Low Power Mode */
00154   if(status)
00155   {
00156     ctrl |= LSM6DSL_ACC_GYRO_LP_XL_ENABLED;
00157   }else
00158   {
00159     ctrl |= LSM6DSL_ACC_GYRO_LP_XL_DISABLED;
00160   }
00161   
00162   /* write back control register */
00163   SENSOR_IO_Write(LSM6DSL_ACC_GYRO_I2C_ADDRESS_LOW, LSM6DSL_ACC_GYRO_CTRL6_C, ctrl);
00164 }
00165 
00166 /**
00167   * @brief  Read X, Y & Z Acceleration values 
00168   * @param  pData: Data out pointer
00169   */
00170 void LSM6DSL_AccReadXYZ(int16_t* pData)
00171 {
00172   int16_t pnRawData[3];
00173   uint8_t ctrlx= 0;
00174   uint8_t buffer[6];
00175   uint8_t i = 0;
00176   float sensitivity = 0;
00177   
00178   /* Read the acceleration control register content */
00179   ctrlx = SENSOR_IO_Read(LSM6DSL_ACC_GYRO_I2C_ADDRESS_LOW, LSM6DSL_ACC_GYRO_CTRL1_XL);
00180   
00181   /* Read output register X, Y & Z acceleration */
00182   SENSOR_IO_ReadMultiple(LSM6DSL_ACC_GYRO_I2C_ADDRESS_LOW, LSM6DSL_ACC_GYRO_OUTX_L_XL, buffer, 6);
00183   
00184   for(i=0; i<3; i++)
00185   {
00186     pnRawData[i]=((((uint16_t)buffer[2*i+1]) << 8) + (uint16_t)buffer[2*i]);
00187   }
00188   
00189   /* Normal mode */
00190   /* Switch the sensitivity value set in the CRTL1_XL */
00191   switch(ctrlx & 0x0C)
00192   {
00193   case LSM6DSL_ACC_FULLSCALE_2G:
00194     sensitivity = LSM6DSL_ACC_SENSITIVITY_2G;
00195     break;
00196   case LSM6DSL_ACC_FULLSCALE_4G:
00197     sensitivity = LSM6DSL_ACC_SENSITIVITY_4G;
00198     break;
00199   case LSM6DSL_ACC_FULLSCALE_8G:
00200     sensitivity = LSM6DSL_ACC_SENSITIVITY_8G;
00201     break;
00202   case LSM6DSL_ACC_FULLSCALE_16G:
00203     sensitivity = LSM6DSL_ACC_SENSITIVITY_16G;
00204     break;    
00205   }
00206   
00207   /* Obtain the mg value for the three axis */
00208   for(i=0; i<3; i++)
00209   {
00210     pData[i]=( int16_t )(pnRawData[i] * sensitivity);
00211   }
00212 }
00213 
00214 /**
00215   * @}
00216   */ 
00217 
00218 /** @defgroup LSM6DSL_GYRO_Private_Functions LSM6DSL GYRO Private Functions
00219   * @{
00220   */
00221 
00222 /**
00223   * @brief  Set LSM6DSL Gyroscope Initialization.
00224   * @param  InitStruct: pointer to a LSM6DSL_InitTypeDef structure 
00225   *         that contains the configuration setting for the LSM6DSL.
00226   */
00227 void LSM6DSL_GyroInit(uint16_t InitStruct)
00228 {  
00229   uint8_t ctrl = 0x00;
00230   uint8_t tmp;
00231 
00232   /* Read CTRL2_G */
00233   tmp = SENSOR_IO_Read(LSM6DSL_ACC_GYRO_I2C_ADDRESS_LOW, LSM6DSL_ACC_GYRO_CTRL2_G);
00234 
00235   /* Write value to GYRO MEMS CTRL2_G register: FS and Data Rate */
00236   ctrl = (uint8_t) InitStruct;
00237   tmp &= ~(0xFC);
00238   tmp |= ctrl;
00239   SENSOR_IO_Write(LSM6DSL_ACC_GYRO_I2C_ADDRESS_LOW, LSM6DSL_ACC_GYRO_CTRL2_G, tmp);
00240 
00241   /* Read CTRL3_C */
00242   tmp = SENSOR_IO_Read(LSM6DSL_ACC_GYRO_I2C_ADDRESS_LOW, LSM6DSL_ACC_GYRO_CTRL3_C);
00243 
00244   /* Write value to GYRO MEMS CTRL3_C register: BDU and Auto-increment */
00245   ctrl = ((uint8_t) (InitStruct >> 8));
00246   tmp &= ~(0x44);
00247   tmp |= ctrl; 
00248   SENSOR_IO_Write(LSM6DSL_ACC_GYRO_I2C_ADDRESS_LOW, LSM6DSL_ACC_GYRO_CTRL3_C, tmp);
00249 }
00250 
00251 
00252 /**
00253   * @brief LSM6DSL Gyroscope De-initialization
00254   */
00255 void LSM6DSL_GyroDeInit(void)
00256 {
00257   uint8_t ctrl = 0x00;
00258   
00259   /* Read control register 1 value */
00260   ctrl = SENSOR_IO_Read(LSM6DSL_ACC_GYRO_I2C_ADDRESS_LOW, LSM6DSL_ACC_GYRO_CTRL2_G);
00261 
00262   /* Clear ODR bits */
00263   ctrl &= ~(LSM6DSL_ODR_BITPOSITION);
00264 
00265   /* Set Power down */
00266   ctrl |= LSM6DSL_ODR_POWER_DOWN;
00267   
00268   /* write back control register */
00269   SENSOR_IO_Write(LSM6DSL_ACC_GYRO_I2C_ADDRESS_LOW, LSM6DSL_ACC_GYRO_CTRL2_G, ctrl);
00270 }
00271 
00272 /**
00273   * @brief  Read ID address of LSM6DSL
00274   * @retval ID 
00275   */
00276 uint8_t LSM6DSL_GyroReadID(void)
00277 {
00278   /* IO interface initialization */
00279   SENSOR_IO_Init();  
00280   /* Read value at Who am I register address */
00281   return SENSOR_IO_Read(LSM6DSL_ACC_GYRO_I2C_ADDRESS_LOW, LSM6DSL_ACC_GYRO_WHO_AM_I_REG);
00282 }
00283 
00284 /**
00285   * @brief Set/Unset LSM6DSL Gyroscope in low power mode
00286   * @param  status 0 means disable Low Power Mode, otherwise Low Power Mode is enabled 
00287   */
00288 void LSM6DSL_GyroLowPower(uint16_t status)
00289 {  
00290   uint8_t ctrl = 0x00;
00291   
00292   /* Read CTRL7_G value */
00293   ctrl = SENSOR_IO_Read(LSM6DSL_ACC_GYRO_I2C_ADDRESS_LOW, LSM6DSL_ACC_GYRO_CTRL7_G);
00294 
00295   /* Clear Low Power Mode bit */
00296   ctrl &= ~(0x80);
00297 
00298   /* Set Low Power Mode */
00299   if(status)
00300   {
00301     ctrl |= LSM6DSL_ACC_GYRO_LP_G_ENABLED;
00302   }else
00303   {
00304     ctrl |= LSM6DSL_ACC_GYRO_LP_G_DISABLED;
00305   }
00306   
00307   /* write back control register */
00308   SENSOR_IO_Write(LSM6DSL_ACC_GYRO_I2C_ADDRESS_LOW, LSM6DSL_ACC_GYRO_CTRL7_G, ctrl);
00309 }
00310 
00311 /**
00312 * @brief  Calculate the LSM6DSL angular data.
00313 * @param  pfData: Data out pointer
00314 */
00315 void LSM6DSL_GyroReadXYZAngRate(float *pfData)
00316 {
00317   int16_t pnRawData[3];
00318   uint8_t ctrlg= 0;
00319   uint8_t buffer[6];
00320   uint8_t i = 0;
00321   float sensitivity = 0;
00322   
00323   /* Read the gyro control register content */
00324   ctrlg = SENSOR_IO_Read(LSM6DSL_ACC_GYRO_I2C_ADDRESS_LOW, LSM6DSL_ACC_GYRO_CTRL2_G);
00325   
00326   /* Read output register X, Y & Z acceleration */
00327   SENSOR_IO_ReadMultiple(LSM6DSL_ACC_GYRO_I2C_ADDRESS_LOW, LSM6DSL_ACC_GYRO_OUTX_L_G, buffer, 6);
00328   
00329   for(i=0; i<3; i++)
00330   {
00331     pnRawData[i]=((((uint16_t)buffer[2*i+1]) << 8) + (uint16_t)buffer[2*i]);
00332   }
00333   
00334   /* Normal mode */
00335   /* Switch the sensitivity value set in the CRTL2_G */
00336   switch(ctrlg & 0x0C)
00337   {
00338   case LSM6DSL_GYRO_FS_245:
00339     sensitivity = LSM6DSL_GYRO_SENSITIVITY_245DPS;
00340     break;
00341   case LSM6DSL_GYRO_FS_500:
00342     sensitivity = LSM6DSL_GYRO_SENSITIVITY_500DPS;
00343     break;
00344   case LSM6DSL_GYRO_FS_1000:
00345     sensitivity = LSM6DSL_GYRO_SENSITIVITY_1000DPS;
00346     break;
00347   case LSM6DSL_GYRO_FS_2000:
00348     sensitivity = LSM6DSL_GYRO_SENSITIVITY_2000DPS;
00349     break;    
00350   }
00351   
00352   /* Obtain the mg value for the three axis */
00353   for(i=0; i<3; i++)
00354   {
00355     pfData[i]=( float )(pnRawData[i] * sensitivity);
00356   }
00357 }
00358 
00359 /**
00360   * @}
00361   */ 
00362 
00363 /**
00364   * @}
00365   */ 
00366   
00367 /**
00368   * @}
00369   */ 
00370   
00371 /**
00372   * @}
00373   */ 
00374 
00375 /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
00376