lsm6ds0.c

00001 /**
00002  ******************************************************************************
00003  * @file    lsm6ds0.c
00004  * @author  MEMS Application Team
00005  * @version V1.0.0
00006  * @date    30-July-2014
00007  * @brief   This file provides a set of functions needed to manage the lsm6ds0.
00008  ******************************************************************************
00009  * @attention
00010  *
00011  * <h2><center>&copy; COPYRIGHT(c) 2014 STMicroelectronics</center></h2>
00012  *
00013  * Redistribution and use in source and binary forms, with or without modification,
00014  * are permitted provided that the following conditions are met:
00015  *   1. Redistributions of source code must retain the above copyright notice,
00016  *      this list of conditions and the following disclaimer.
00017  *   2. Redistributions in binary form must reproduce the above copyright notice,
00018  *      this list of conditions and the following disclaimer in the documentation
00019  *      and/or other materials provided with the distribution.
00020  *   3. Neither the name of STMicroelectronics nor the names of its contributors
00021  *      may be used to endorse or promote products derived from this software
00022  *      without specific prior written permission.
00023  *
00024  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00025  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00026  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00027  * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
00028  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
00029  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
00030  * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00031  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
00032  * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
00033  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00034  *
00035  ******************************************************************************
00036  */
00037 /* Includes ------------------------------------------------------------------*/
00038 #include "lsm6ds0.h"
00039 #include <math.h>
00040 
00041 /** @addtogroup BSP
00042  * @{
00043  */
00044 
00045 /** @addtogroup MEMS_SHIELD
00046  * @{
00047  */
00048 
00049 /** @addtogroup LSM6DS0
00050  * @{
00051  */
00052 
00053 /** @defgroup LSM6DS0_Private_TypesDefinitions
00054  * @{
00055  */
00056 
00057 /**
00058  * @}
00059  */
00060 
00061 /** @defgroup LSM6DS0_Private_Defines
00062  * @{
00063  */
00064 
00065 /**
00066  * @}
00067  */
00068 
00069 /** @defgroup LSM6DS0_Private_Macros
00070  * @{
00071  */
00072 
00073 /**
00074  * @}
00075  */
00076 
00077 /** @defgroup LSM6DS0_Private_Variables
00078  * @{
00079  */
00080 
00081 IMU_6AXES_DrvTypeDef LSM6DS0Drv =
00082 {
00083  LSM6DS0_Init,
00084  LSM6DS0_Read_XG_ID,
00085  LSM6DS0_X_GetAxes,
00086  LSM6DS0_G_GetAxes
00087 };
00088 
00089 /**
00090  * @}
00091  */
00092 
00093 /** @defgroup LSM6DS0_Private_FunctionPrototypes
00094  * @{
00095  */
00096 
00097 void LSM6DS0_X_GetAxesRaw(int16_t *pData);
00098 void LSM6DS0_G_GetAxesRaw(int16_t *pData);
00099 
00100 /**
00101  * @}
00102  */
00103 
00104 /** @defgroup LSM6DS0_Private_Functions
00105  * @{
00106  */
00107 
00108 
00109 
00110 /**
00111  * @brief  Set LSM6DS0 Initialization.
00112  * @param  InitStruct: it contains the configuration setting for the LSM6DS0.
00113  * @retval None
00114  */
00115 void LSM6DS0_Init(IMU_6AXES_InitTypeDef *LSM6DS0_Init)
00116 {
00117     uint8_t tmp1 = 0x00;
00118 
00119     /* Configure the low level interface ---------------------------------------*/
00120     IMU_6AXES_IO_Init();
00121 
00122 /******* Gyroscope init *******/
00123 
00124     IMU_6AXES_IO_Read(&tmp1, LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_CTRL_REG1_G, 1);
00125 
00126     /* Output Data Rate selection */
00127     tmp1 &= ~(LSM6DS0_G_ODR_MASK);
00128     tmp1 |= LSM6DS0_Init->G_OutputDataRate;
00129 
00130     /* Full scale selection */
00131     tmp1 &= ~(LSM6DS0_G_FS_MASK);
00132     tmp1 |= LSM6DS0_Init->G_FullScale;
00133 
00134     IMU_6AXES_IO_Write(&tmp1, LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_CTRL_REG1_G, 1);
00135 
00136 
00137     IMU_6AXES_IO_Read(&tmp1, LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_CTRL_REG4, 1);
00138 
00139     /* Enable X axis selection */
00140     tmp1 &= ~(LSM6DS0_G_XEN_MASK);
00141     tmp1 |= LSM6DS0_Init->G_X_Axis;
00142 
00143     /* Enable Y axis selection */
00144     tmp1 &= ~(LSM6DS0_G_YEN_MASK);
00145     tmp1 |= LSM6DS0_Init->G_Y_Axis;
00146 
00147     /* Enable Z axis selection */
00148     tmp1 &= ~(LSM6DS0_G_ZEN_MASK);
00149     tmp1 |= LSM6DS0_Init->G_Z_Axis;
00150 
00151     IMU_6AXES_IO_Write(&tmp1, LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_CTRL_REG4, 1);
00152 
00153 /******************************/
00154 
00155 /***** Accelerometer init *****/
00156 
00157     IMU_6AXES_IO_Read(&tmp1, LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_CTRL_REG6_XL, 1);
00158 
00159     /* Output Data Rate selection */
00160     tmp1 &= ~(LSM6DS0_XL_ODR_MASK);
00161     tmp1 |= LSM6DS0_Init->X_OutputDataRate;
00162 
00163     /* Full scale selection */
00164     tmp1 &= ~(LSM6DS0_XL_FS_MASK);
00165     tmp1 |= LSM6DS0_Init->X_FullScale;
00166 
00167     IMU_6AXES_IO_Write(&tmp1, LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_CTRL_REG6_XL, 1);
00168 
00169 
00170     IMU_6AXES_IO_Read(&tmp1, LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_CTRL_REG5_XL, 1);
00171 
00172     /* Enable X axis selection */
00173     tmp1 &= ~(LSM6DS0_XL_XEN_MASK);
00174     tmp1 |= LSM6DS0_Init->X_X_Axis;
00175 
00176     /* Enable Y axis selection */
00177     tmp1 &= ~(LSM6DS0_XL_YEN_MASK);
00178     tmp1 |= LSM6DS0_Init->X_Y_Axis;
00179 
00180     /* Enable Z axis selection */
00181     tmp1 &= ~(LSM6DS0_XL_ZEN_MASK);
00182     tmp1 |= LSM6DS0_Init->X_Z_Axis;
00183 
00184     IMU_6AXES_IO_Write(&tmp1, LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_CTRL_REG5_XL, 1);
00185 
00186 /******************************/
00187 }
00188 
00189 
00190 /**
00191  * @brief  Read ID of LSM6DS0 Accelerometer and Gyroscope
00192  * @param  Device ID
00193  * @retval ID name
00194  */
00195 uint8_t LSM6DS0_Read_XG_ID(void)
00196 {
00197     uint8_t tmp = 0x00;
00198 
00199     /* Read WHO I AM register */
00200     IMU_6AXES_IO_Read(&tmp, LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_WHO_AM_I_ADDR, 1);
00201 
00202     /* Return the ID */
00203     return (uint8_t)tmp;
00204 }
00205 
00206 
00207 /**
00208  * @brief Read raw data from LSM6DS0 Accelerometer output register.
00209  * @param float *pfData
00210  * @retval None.
00211  */
00212 void LSM6DS0_X_GetAxesRaw(int16_t *pData)
00213 {
00214     uint8_t tempReg[2] = {0,0};
00215 
00216 
00217     IMU_6AXES_IO_Read(&tempReg[0], LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_OUT_X_L_XL + 0x80, 2);
00218 
00219     pData[0] = ((((int16_t)tempReg[1]) << 8)+(int16_t)tempReg[0]);
00220 
00221     IMU_6AXES_IO_Read(&tempReg[0], LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_OUT_Y_L_XL + 0x80, 2);
00222 
00223     pData[1] = ((((int16_t)tempReg[1]) << 8)+(int16_t)tempReg[0]);
00224 
00225     IMU_6AXES_IO_Read(&tempReg[0], LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_OUT_Z_L_XL + 0x80, 2);
00226 
00227     pData[2] = ((((int16_t)tempReg[1]) << 8)+(int16_t)tempReg[0]);
00228 }
00229 
00230 
00231 /**
00232  * @brief Read data from LSM6DS0 Accelerometer and calculate linear acceleration in mg.
00233  * @param float *pfData
00234  * @retval None.
00235  */
00236 void LSM6DS0_X_GetAxes(int32_t *pData)
00237 {
00238 
00239   uint8_t tempReg = 0x00;
00240   int16_t pDataRaw[3];
00241   float sensitivity = 0;
00242 
00243   LSM6DS0_X_GetAxesRaw(pDataRaw);
00244 
00245   IMU_6AXES_IO_Read(&tempReg, LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_CTRL_REG6_XL, 1);
00246 
00247   tempReg &= LSM6DS0_XL_FS_MASK;
00248 
00249   switch(tempReg)
00250   {
00251     case LSM6DS0_XL_FS_2G:
00252       sensitivity = 0.061;
00253       break;
00254     case LSM6DS0_XL_FS_4G:
00255       sensitivity = 0.122;
00256       break;
00257     case LSM6DS0_XL_FS_8G:
00258       sensitivity = 0.244;
00259       break;
00260   }
00261 
00262   pData[0] = (int32_t)(pDataRaw[0] * sensitivity);
00263   pData[1] = (int32_t)(pDataRaw[1] * sensitivity);
00264   pData[2] = (int32_t)(pDataRaw[2] * sensitivity);
00265 }
00266 
00267 
00268 /**
00269  * @brief Read raw data from LSM6DS0 Gyroscope output register.
00270  * @param float *pfData
00271  * @retval None.
00272  */
00273 void LSM6DS0_G_GetAxesRaw(int16_t *pData)
00274 {
00275     uint8_t tempReg[2] = {0,0};
00276 
00277 
00278     IMU_6AXES_IO_Read(&tempReg[0], LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_OUT_X_L_G + 0x80, 2);
00279 
00280     pData[0] = ((((int16_t)tempReg[1]) << 8)+(int16_t)tempReg[0]);
00281 
00282     IMU_6AXES_IO_Read(&tempReg[0], LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_OUT_Y_L_G + 0x80, 2);
00283 
00284     pData[1] = ((((int16_t)tempReg[1]) << 8)+(int16_t)tempReg[0]);
00285 
00286     IMU_6AXES_IO_Read(&tempReg[0], LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_OUT_Z_L_G + 0x80, 2);
00287 
00288     pData[2] = ((((int16_t)tempReg[1]) << 8)+(int16_t)tempReg[0]);
00289 }
00290 
00291 
00292 /**
00293  * @brief Read data from LSM6DS0 Gyroscope and calculate angular rate in mdps.
00294  * @param float *pfData
00295  * @retval None.
00296  */
00297 void LSM6DS0_G_GetAxes(int32_t *pData)
00298 {
00299 
00300   uint8_t tempReg = 0x00;
00301   int16_t pDataRaw[3];
00302   float sensitivity = 0;
00303 
00304   LSM6DS0_G_GetAxesRaw(pDataRaw);
00305 
00306   IMU_6AXES_IO_Read(&tempReg, LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_CTRL_REG1_G, 1);
00307 
00308   tempReg &= LSM6DS0_G_FS_MASK;
00309 
00310   switch(tempReg)
00311   {
00312     case LSM6DS0_G_FS_245:
00313       sensitivity = 8.75;
00314       break;
00315     case LSM6DS0_G_FS_500:
00316       sensitivity = 17.50;
00317       break;
00318     case LSM6DS0_G_FS_2000:
00319       sensitivity = 70;
00320       break;
00321   }
00322 
00323   pData[0] = (int32_t)(pDataRaw[0] * sensitivity);
00324   pData[1] = (int32_t)(pDataRaw[1] * sensitivity);
00325   pData[2] = (int32_t)(pDataRaw[2] * sensitivity);
00326 }
00327 
00328 /**
00329  * @}
00330  */
00331 
00332 /**
00333  * @}
00334  */
00335 
00336 /**
00337  * @}
00338  */
00339 
00340 /**
00341  * @}
00342  */
00343 
00344 
00345 /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
00346