
Example for Bluetooth low energy interface
Dependencies: mbed HC_SR04_Ultrasonic_Library
Diff: Sensor_Shield/lsm6ds0.c
- Revision:
- 1:bdbf36f8408d
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Sensor_Shield/lsm6ds0.c Tue May 19 16:53:44 2015 +0000 @@ -0,0 +1,346 @@ +/** + ****************************************************************************** + * @file lsm6ds0.c + * @author MEMS Application Team + * @version V1.0.0 + * @date 30-July-2014 + * @brief This file provides a set of functions needed to manage the lsm6ds0. + ****************************************************************************** + * @attention + * + * <h2><center>© COPYRIGHT(c) 2014 STMicroelectronics</center></h2> + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * 3. Neither the name of STMicroelectronics nor the names of its contributors + * may be used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************** + */ +/* Includes ------------------------------------------------------------------*/ +#include "lsm6ds0.h" +#include <math.h> + +/** @addtogroup BSP + * @{ + */ + +/** @addtogroup MEMS_SHIELD + * @{ + */ + +/** @addtogroup LSM6DS0 + * @{ + */ + +/** @defgroup LSM6DS0_Private_TypesDefinitions + * @{ + */ + +/** + * @} + */ + +/** @defgroup LSM6DS0_Private_Defines + * @{ + */ + +/** + * @} + */ + +/** @defgroup LSM6DS0_Private_Macros + * @{ + */ + +/** + * @} + */ + +/** @defgroup LSM6DS0_Private_Variables + * @{ + */ + +IMU_6AXES_DrvTypeDef LSM6DS0Drv = +{ + LSM6DS0_Init, + LSM6DS0_Read_XG_ID, + LSM6DS0_X_GetAxes, + LSM6DS0_G_GetAxes +}; + +/** + * @} + */ + +/** @defgroup LSM6DS0_Private_FunctionPrototypes + * @{ + */ + +void LSM6DS0_X_GetAxesRaw(int16_t *pData); +void LSM6DS0_G_GetAxesRaw(int16_t *pData); + +/** + * @} + */ + +/** @defgroup LSM6DS0_Private_Functions + * @{ + */ + + + +/** + * @brief Set LSM6DS0 Initialization. + * @param InitStruct: it contains the configuration setting for the LSM6DS0. + * @retval None + */ +void LSM6DS0_Init(IMU_6AXES_InitTypeDef *LSM6DS0_Init) +{ + uint8_t tmp1 = 0x00; + + /* Configure the low level interface ---------------------------------------*/ + IMU_6AXES_IO_Init(); + +/******* Gyroscope init *******/ + + IMU_6AXES_IO_Read(&tmp1, LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_CTRL_REG1_G, 1); + + /* Output Data Rate selection */ + tmp1 &= ~(LSM6DS0_G_ODR_MASK); + tmp1 |= LSM6DS0_Init->G_OutputDataRate; + + /* Full scale selection */ + tmp1 &= ~(LSM6DS0_G_FS_MASK); + tmp1 |= LSM6DS0_Init->G_FullScale; + + IMU_6AXES_IO_Write(&tmp1, LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_CTRL_REG1_G, 1); + + + IMU_6AXES_IO_Read(&tmp1, LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_CTRL_REG4, 1); + + /* Enable X axis selection */ + tmp1 &= ~(LSM6DS0_G_XEN_MASK); + tmp1 |= LSM6DS0_Init->G_X_Axis; + + /* Enable Y axis selection */ + tmp1 &= ~(LSM6DS0_G_YEN_MASK); + tmp1 |= LSM6DS0_Init->G_Y_Axis; + + /* Enable Z axis selection */ + tmp1 &= ~(LSM6DS0_G_ZEN_MASK); + tmp1 |= LSM6DS0_Init->G_Z_Axis; + + IMU_6AXES_IO_Write(&tmp1, LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_CTRL_REG4, 1); + +/******************************/ + +/***** Accelerometer init *****/ + + IMU_6AXES_IO_Read(&tmp1, LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_CTRL_REG6_XL, 1); + + /* Output Data Rate selection */ + tmp1 &= ~(LSM6DS0_XL_ODR_MASK); + tmp1 |= LSM6DS0_Init->X_OutputDataRate; + + /* Full scale selection */ + tmp1 &= ~(LSM6DS0_XL_FS_MASK); + tmp1 |= LSM6DS0_Init->X_FullScale; + + IMU_6AXES_IO_Write(&tmp1, LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_CTRL_REG6_XL, 1); + + + IMU_6AXES_IO_Read(&tmp1, LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_CTRL_REG5_XL, 1); + + /* Enable X axis selection */ + tmp1 &= ~(LSM6DS0_XL_XEN_MASK); + tmp1 |= LSM6DS0_Init->X_X_Axis; + + /* Enable Y axis selection */ + tmp1 &= ~(LSM6DS0_XL_YEN_MASK); + tmp1 |= LSM6DS0_Init->X_Y_Axis; + + /* Enable Z axis selection */ + tmp1 &= ~(LSM6DS0_XL_ZEN_MASK); + tmp1 |= LSM6DS0_Init->X_Z_Axis; + + IMU_6AXES_IO_Write(&tmp1, LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_CTRL_REG5_XL, 1); + +/******************************/ +} + + +/** + * @brief Read ID of LSM6DS0 Accelerometer and Gyroscope + * @param Device ID + * @retval ID name + */ +uint8_t LSM6DS0_Read_XG_ID(void) +{ + uint8_t tmp = 0x00; + + /* Read WHO I AM register */ + IMU_6AXES_IO_Read(&tmp, LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_WHO_AM_I_ADDR, 1); + + /* Return the ID */ + return (uint8_t)tmp; +} + + +/** + * @brief Read raw data from LSM6DS0 Accelerometer output register. + * @param float *pfData + * @retval None. + */ +void LSM6DS0_X_GetAxesRaw(int16_t *pData) +{ + uint8_t tempReg[2] = {0,0}; + + + IMU_6AXES_IO_Read(&tempReg[0], LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_OUT_X_L_XL + 0x80, 2); + + pData[0] = ((((int16_t)tempReg[1]) << 8)+(int16_t)tempReg[0]); + + IMU_6AXES_IO_Read(&tempReg[0], LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_OUT_Y_L_XL + 0x80, 2); + + pData[1] = ((((int16_t)tempReg[1]) << 8)+(int16_t)tempReg[0]); + + IMU_6AXES_IO_Read(&tempReg[0], LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_OUT_Z_L_XL + 0x80, 2); + + pData[2] = ((((int16_t)tempReg[1]) << 8)+(int16_t)tempReg[0]); +} + + +/** + * @brief Read data from LSM6DS0 Accelerometer and calculate linear acceleration in mg. + * @param float *pfData + * @retval None. + */ +void LSM6DS0_X_GetAxes(int32_t *pData) +{ + + uint8_t tempReg = 0x00; + int16_t pDataRaw[3]; + float sensitivity = 0; + + LSM6DS0_X_GetAxesRaw(pDataRaw); + + IMU_6AXES_IO_Read(&tempReg, LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_CTRL_REG6_XL, 1); + + tempReg &= LSM6DS0_XL_FS_MASK; + + switch(tempReg) + { + case LSM6DS0_XL_FS_2G: + sensitivity = 0.061; + break; + case LSM6DS0_XL_FS_4G: + sensitivity = 0.122; + break; + case LSM6DS0_XL_FS_8G: + sensitivity = 0.244; + break; + } + + pData[0] = (int32_t)(pDataRaw[0] * sensitivity); + pData[1] = (int32_t)(pDataRaw[1] * sensitivity); + pData[2] = (int32_t)(pDataRaw[2] * sensitivity); +} + + +/** + * @brief Read raw data from LSM6DS0 Gyroscope output register. + * @param float *pfData + * @retval None. + */ +void LSM6DS0_G_GetAxesRaw(int16_t *pData) +{ + uint8_t tempReg[2] = {0,0}; + + + IMU_6AXES_IO_Read(&tempReg[0], LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_OUT_X_L_G + 0x80, 2); + + pData[0] = ((((int16_t)tempReg[1]) << 8)+(int16_t)tempReg[0]); + + IMU_6AXES_IO_Read(&tempReg[0], LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_OUT_Y_L_G + 0x80, 2); + + pData[1] = ((((int16_t)tempReg[1]) << 8)+(int16_t)tempReg[0]); + + IMU_6AXES_IO_Read(&tempReg[0], LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_OUT_Z_L_G + 0x80, 2); + + pData[2] = ((((int16_t)tempReg[1]) << 8)+(int16_t)tempReg[0]); +} + + +/** + * @brief Read data from LSM6DS0 Gyroscope and calculate angular rate in mdps. + * @param float *pfData + * @retval None. + */ +void LSM6DS0_G_GetAxes(int32_t *pData) +{ + + uint8_t tempReg = 0x00; + int16_t pDataRaw[3]; + float sensitivity = 0; + + LSM6DS0_G_GetAxesRaw(pDataRaw); + + IMU_6AXES_IO_Read(&tempReg, LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_CTRL_REG1_G, 1); + + tempReg &= LSM6DS0_G_FS_MASK; + + switch(tempReg) + { + case LSM6DS0_G_FS_245: + sensitivity = 8.75; + break; + case LSM6DS0_G_FS_500: + sensitivity = 17.50; + break; + case LSM6DS0_G_FS_2000: + sensitivity = 70; + break; + } + + pData[0] = (int32_t)(pDataRaw[0] * sensitivity); + pData[1] = (int32_t)(pDataRaw[1] * sensitivity); + pData[2] = (int32_t)(pDataRaw[2] * sensitivity); +} + +/** + * @} + */ + +/** + * @} + */ + +/** + * @} + */ + +/** + * @} + */ + + +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ +