Read and store X_NUCLEO_IKS01A1 data. Also calculates compass heading.
Dependencies: ST_INTERFACES X_NUCLEO_COMMON
Dependents: HelloWorld_IKS01A1
Fork of X_NUCLEO_IKS01A1 by
Components/lsm6ds3/lsm6ds3_class.cpp
- Committer:
- Wosser1sProductions
- Date:
- 2016-10-16
- Revision:
- 91:1dd4cdaede06
- Parent:
- 57:04563dd74269
File content as of revision 91:1dd4cdaede06:
/** ****************************************************************************** * @file lsm6ds3_class.cpp * @author AST / EST * @version V0.0.1 * @date 14-April-2015 * @brief Implementation file for the LSM6DS3 driver class ****************************************************************************** * @attention * * <h2><center>© COPYRIGHT(c) 2015 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 "lsm6ds3_class.h" #include "lsm6ds3.h" /* Methods -------------------------------------------------------------------*/ /* betzw - based on: X-CUBE-MEMS1/trunk/Drivers/BSP/Components/lsm6ds3/lsm6ds3.c: revision #400, X-CUBE-MEMS1/trunk: revision #416 */ /** * @brief Set LSM6DS3 Initialization * @param LSM6DS3_Init the configuration setting for the LSM6DS3 * @retval IMU_6AXES_OK in case of success, an error code otherwise */ IMU_6AXES_StatusTypeDef LSM6DS3::LSM6DS3_Init( IMU_6AXES_InitTypeDef *LSM6DS3_Init ) { /*Here we have to add the check if the parameters are valid*/ /* Configure the low level interface -------------------------------------*/ if(LSM6DS3_IO_Init() != IMU_6AXES_OK) { return IMU_6AXES_ERROR; } /******** Common init *********/ if(LSM6DS3_Common_Sensor_Enable() != IMU_6AXES_OK) { return IMU_6AXES_ERROR; } /******* Gyroscope init *******/ if(LSM6DS3_G_Set_ODR( LSM6DS3_Init->G_OutputDataRate ) != IMU_6AXES_OK) { return IMU_6AXES_ERROR; } if(LSM6DS3_G_Set_FS( LSM6DS3_Init->G_FullScale ) != IMU_6AXES_OK) { return IMU_6AXES_ERROR; } if(LSM6DS3_G_Set_Axes_Status(LSM6DS3_Init->G_X_Axis, LSM6DS3_Init->G_Y_Axis, LSM6DS3_Init->G_Z_Axis) != IMU_6AXES_OK) { return IMU_6AXES_ERROR; } /***** Accelerometer init *****/ if(LSM6DS3_X_Set_ODR( LSM6DS3_Init->X_OutputDataRate ) != IMU_6AXES_OK) { return IMU_6AXES_ERROR; } if(LSM6DS3_X_Set_FS( LSM6DS3_Init->X_FullScale ) != IMU_6AXES_OK) { return IMU_6AXES_ERROR; } if(LSM6DS3_X_Set_Axes_Status(LSM6DS3_Init->X_X_Axis, LSM6DS3_Init->X_Y_Axis, LSM6DS3_Init->X_Z_Axis) != IMU_6AXES_OK) { return IMU_6AXES_ERROR; } /* Configure interrupt lines */ LSM6DS3_IO_ITConfig(); return IMU_6AXES_OK; } /** * @brief Read ID of LSM6DS3 Accelerometer and Gyroscope * @param xg_id the pointer where the ID of the device is stored * @retval IMU_6AXES_OK in case of success, an error code otherwise */ IMU_6AXES_StatusTypeDef LSM6DS3::LSM6DS3_Read_XG_ID( uint8_t *xg_id) { if(!xg_id) { return IMU_6AXES_ERROR; } return LSM6DS3_IO_Read(xg_id, LSM6DS3_XG_WHO_AM_I_ADDR, 1); } /** * @brief Set LSM6DS3 common initialization * @retval IMU_6AXES_OK in case of success, an error code otherwise */ IMU_6AXES_StatusTypeDef LSM6DS3::LSM6DS3_Common_Sensor_Enable(void) { uint8_t tmp1 = 0x00; if(LSM6DS3_IO_Read(&tmp1, LSM6DS3_XG_CTRL3_C, 1) != IMU_6AXES_OK) { return IMU_6AXES_ERROR; } /* Enable register address automatically incremented during a multiple byte access with a serial interface (I2C or SPI) */ tmp1 &= ~(LSM6DS3_XG_IF_INC_MASK); tmp1 |= LSM6DS3_XG_IF_INC; if(LSM6DS3_IO_Write(&tmp1, LSM6DS3_XG_CTRL3_C, 1) != IMU_6AXES_OK) { return IMU_6AXES_ERROR; } if(LSM6DS3_IO_Read(&tmp1, LSM6DS3_XG_FIFO_CTRL5, 1) != IMU_6AXES_OK) { return IMU_6AXES_ERROR; } /* FIFO ODR selection */ tmp1 &= ~(LSM6DS3_XG_FIFO_ODR_MASK); tmp1 |= LSM6DS3_XG_FIFO_ODR_NA; /* FIFO mode selection */ tmp1 &= ~(LSM6DS3_XG_FIFO_MODE_MASK); tmp1 |= LSM6DS3_XG_FIFO_MODE_BYPASS; if(LSM6DS3_IO_Write(&tmp1, LSM6DS3_XG_FIFO_CTRL5, 1) != IMU_6AXES_OK) { return IMU_6AXES_ERROR; } return IMU_6AXES_OK; } /** * @brief Read raw data from LSM6DS3 Accelerometer output register * @param pData the pointer where the accelerometer raw data are stored * @retval IMU_6AXES_OK in case of success, an error code otherwise */ IMU_6AXES_StatusTypeDef LSM6DS3::LSM6DS3_X_GetAxesRaw( int16_t *pData ) { /*Here we have to add the check if the parameters are valid*/ uint8_t tempReg[2] = {0, 0}; if(LSM6DS3_IO_Read(&tempReg[0], LSM6DS3_XG_OUT_X_L_XL, 2) != IMU_6AXES_OK) { return IMU_6AXES_ERROR; } pData[0] = ((((int16_t)tempReg[1]) << 8) + (int16_t)tempReg[0]); if(LSM6DS3_IO_Read(&tempReg[0], LSM6DS3_XG_OUT_Y_L_XL, 2) != IMU_6AXES_OK) { return IMU_6AXES_ERROR; } pData[1] = ((((int16_t)tempReg[1]) << 8) + (int16_t)tempReg[0]); if(LSM6DS3_IO_Read(&tempReg[0], LSM6DS3_XG_OUT_Z_L_XL, 2) != IMU_6AXES_OK) { return IMU_6AXES_ERROR; } pData[2] = ((((int16_t)tempReg[1]) << 8) + (int16_t)tempReg[0]); return IMU_6AXES_OK; } /** * @brief Read data from LSM6DS3 Accelerometer and calculate linear acceleration in mg * @param pData the pointer where the accelerometer data are stored * @retval IMU_6AXES_OK in case of success, an error code otherwise */ IMU_6AXES_StatusTypeDef LSM6DS3::LSM6DS3_X_GetAxes( int32_t *pData ) { /*Here we have to add the check if the parameters are valid*/ int16_t pDataRaw[3]; float sensitivity = 0.0f; if(LSM6DS3_X_GetAxesRaw(pDataRaw) != IMU_6AXES_OK) { return IMU_6AXES_ERROR; } if(LSM6DS3_X_GetSensitivity( &sensitivity ) != IMU_6AXES_OK) { return IMU_6AXES_ERROR; } pData[0] = (int32_t)(pDataRaw[0] * sensitivity); pData[1] = (int32_t)(pDataRaw[1] * sensitivity); pData[2] = (int32_t)(pDataRaw[2] * sensitivity); return IMU_6AXES_OK; } /** * @brief Read raw data from LSM6DS3 Gyroscope output register * @param pData the pointer where the gyroscope raw data are stored * @retval IMU_6AXES_OK in case of success, an error code otherwise */ IMU_6AXES_StatusTypeDef LSM6DS3::LSM6DS3_G_GetAxesRaw( int16_t *pData ) { /*Here we have to add the check if the parameters are valid*/ uint8_t tempReg[2] = {0, 0}; if(LSM6DS3_IO_Read(&tempReg[0], LSM6DS3_XG_OUT_X_L_G, 2) != IMU_6AXES_OK) { return IMU_6AXES_ERROR; } pData[0] = ((((int16_t)tempReg[1]) << 8) + (int16_t)tempReg[0]); if(LSM6DS3_IO_Read(&tempReg[0], LSM6DS3_XG_OUT_Y_L_G, 2) != IMU_6AXES_OK) { return IMU_6AXES_ERROR; } pData[1] = ((((int16_t)tempReg[1]) << 8) + (int16_t)tempReg[0]); if(LSM6DS3_IO_Read(&tempReg[0], LSM6DS3_XG_OUT_Z_L_G, 2) != IMU_6AXES_OK) { return IMU_6AXES_ERROR; } pData[2] = ((((int16_t)tempReg[1]) << 8) + (int16_t)tempReg[0]); return IMU_6AXES_OK; } /** * @brief Set the status of the axes for accelerometer * @param enableX the status of the x axis to be set * @param enableY the status of the y axis to be set * @param enableZ the status of the z axis to be set * @retval IMU_6AXES_OK in case of success, an error code otherwise */ IMU_6AXES_StatusTypeDef LSM6DS3::LSM6DS3_X_Set_Axes_Status(uint8_t enableX, uint8_t enableY, uint8_t enableZ) { uint8_t tmp1 = 0x00; uint8_t eX = 0x00; uint8_t eY = 0x00; uint8_t eZ = 0x00; eX = ( enableX == 0 ) ? LSM6DS3_XL_XEN_DISABLE : LSM6DS3_XL_XEN_ENABLE; eY = ( enableY == 0 ) ? LSM6DS3_XL_YEN_DISABLE : LSM6DS3_XL_YEN_ENABLE; eZ = ( enableZ == 0 ) ? LSM6DS3_XL_ZEN_DISABLE : LSM6DS3_XL_ZEN_ENABLE; if(LSM6DS3_IO_Read(&tmp1, LSM6DS3_XG_CTRL9_XL, 1) != IMU_6AXES_OK) { return IMU_6AXES_ERROR; } /* Enable X axis selection */ tmp1 &= ~(LSM6DS3_XL_XEN_MASK); tmp1 |= eX; /* Enable Y axis selection */ tmp1 &= ~(LSM6DS3_XL_YEN_MASK); tmp1 |= eY; /* Enable Z axis selection */ tmp1 &= ~(LSM6DS3_XL_ZEN_MASK); tmp1 |= eZ; if(LSM6DS3_IO_Write(&tmp1, LSM6DS3_XG_CTRL9_XL, 1) != IMU_6AXES_OK) { return IMU_6AXES_ERROR; } return IMU_6AXES_OK; } /** * @brief Set the status of the axes for gyroscope * @param enableX the status of the x axis to be set * @param enableY the status of the y axis to be set * @param enableZ the status of the z axis to be set * @retval IMU_6AXES_OK in case of success, an error code otherwise */ IMU_6AXES_StatusTypeDef LSM6DS3::LSM6DS3_G_Set_Axes_Status(uint8_t enableX, uint8_t enableY, uint8_t enableZ) { uint8_t tmp1 = 0x00; uint8_t eX = 0x00; uint8_t eY = 0x00; uint8_t eZ = 0x00; eX = ( enableX == 0 ) ? LSM6DS3_G_XEN_DISABLE : LSM6DS3_G_XEN_ENABLE; eY = ( enableY == 0 ) ? LSM6DS3_G_YEN_DISABLE : LSM6DS3_G_YEN_ENABLE; eZ = ( enableZ == 0 ) ? LSM6DS3_G_ZEN_DISABLE : LSM6DS3_G_ZEN_ENABLE; if(LSM6DS3_IO_Read(&tmp1, LSM6DS3_XG_CTRL10_C, 1) != IMU_6AXES_OK) { return IMU_6AXES_ERROR; } /* Enable X axis selection */ tmp1 &= ~(LSM6DS3_G_XEN_MASK); tmp1 |= eX; /* Enable Y axis selection */ tmp1 &= ~(LSM6DS3_G_YEN_MASK); tmp1 |= eY; /* Enable Z axis selection */ tmp1 &= ~(LSM6DS3_G_ZEN_MASK); tmp1 |= eZ; if(LSM6DS3_IO_Write(&tmp1, LSM6DS3_XG_CTRL10_C, 1) != IMU_6AXES_OK) { return IMU_6AXES_ERROR; } return IMU_6AXES_OK; } /** * @brief Read data from LSM6DS3 Gyroscope and calculate angular rate in mdps * @param pData the pointer where the gyroscope data are stored * @retval IMU_6AXES_OK in case of success, an error code otherwise */ IMU_6AXES_StatusTypeDef LSM6DS3::LSM6DS3_G_GetAxes( int32_t *pData ) { /*Here we have to add the check if the parameters are valid*/ int16_t pDataRaw[3]; float sensitivity = 0.0f; if(LSM6DS3_G_GetAxesRaw(pDataRaw) != IMU_6AXES_OK) { return IMU_6AXES_ERROR; } if(LSM6DS3_G_GetSensitivity( &sensitivity ) != IMU_6AXES_OK) { return IMU_6AXES_ERROR; } pData[0] = (int32_t)(pDataRaw[0] * sensitivity); pData[1] = (int32_t)(pDataRaw[1] * sensitivity); pData[2] = (int32_t)(pDataRaw[2] * sensitivity); return IMU_6AXES_OK; } /** * @brief Read Accelero Output Data Rate * @param odr the pointer where the accelerometer output data rate is stored * @retval IMU_6AXES_OK in case of success, an error code otherwise */ IMU_6AXES_StatusTypeDef LSM6DS3::LSM6DS3_X_Get_ODR( float *odr ) { /*Here we have to add the check if the parameters are valid*/ uint8_t tempReg = 0x00; if(LSM6DS3_IO_Read( &tempReg, LSM6DS3_XG_CTRL1_XL, 1 ) != IMU_6AXES_OK) { return IMU_6AXES_ERROR; } tempReg &= LSM6DS3_XL_ODR_MASK; switch( tempReg ) { case LSM6DS3_XL_ODR_PD: *odr = 0.0f; break; case LSM6DS3_XL_ODR_13HZ: *odr = 13.0f; break; case LSM6DS3_XL_ODR_26HZ: *odr = 26.0f; break; case LSM6DS3_XL_ODR_52HZ: *odr = 52.0f; break; case LSM6DS3_XL_ODR_104HZ: *odr = 104.0f; break; case LSM6DS3_XL_ODR_208HZ: *odr = 208.0f; break; case LSM6DS3_XL_ODR_416HZ: *odr = 416.0f; break; case LSM6DS3_XL_ODR_833HZ: *odr = 833.0f; break; case LSM6DS3_XL_ODR_1660HZ: *odr = 1660.0f; break; case LSM6DS3_XL_ODR_3330HZ: *odr = 3330.0f; break; case LSM6DS3_XL_ODR_6660HZ: *odr = 6660.0f; break; default: break; } return IMU_6AXES_OK; } /** * @brief Write Accelero Output Data Rate * @param odr the accelerometer output data rate to be set * @retval IMU_6AXES_OK in case of success, an error code otherwise */ IMU_6AXES_StatusTypeDef LSM6DS3::LSM6DS3_X_Set_ODR( float odr ) { uint8_t new_odr = 0x00; uint8_t tempReg = 0x00; new_odr = ( odr <= 0.0f ) ? LSM6DS3_XL_ODR_PD /* Power Down */ : ( odr <= 13.0f ) ? LSM6DS3_XL_ODR_13HZ : ( odr <= 26.0f ) ? LSM6DS3_XL_ODR_26HZ : ( odr <= 52.0f ) ? LSM6DS3_XL_ODR_52HZ : ( odr <= 104.0f ) ? LSM6DS3_XL_ODR_104HZ : ( odr <= 208.0f ) ? LSM6DS3_XL_ODR_208HZ : ( odr <= 416.0f ) ? LSM6DS3_XL_ODR_416HZ : ( odr <= 833.0f ) ? LSM6DS3_XL_ODR_833HZ : ( odr <= 1660.0f ) ? LSM6DS3_XL_ODR_1660HZ : ( odr <= 3330.0f ) ? LSM6DS3_XL_ODR_3330HZ : LSM6DS3_XL_ODR_6660HZ; if(LSM6DS3_IO_Read( &tempReg, LSM6DS3_XG_CTRL1_XL, 1 ) != IMU_6AXES_OK) { return IMU_6AXES_ERROR; } tempReg &= ~(LSM6DS3_XL_ODR_MASK); tempReg |= new_odr; if(LSM6DS3_IO_Write(&tempReg, LSM6DS3_XG_CTRL1_XL, 1) != IMU_6AXES_OK) { return IMU_6AXES_ERROR; } return IMU_6AXES_OK; } /** * @brief Read Accelero Sensitivity * @param pfData the pointer where the accelerometer sensitivity is stored * @retval IMU_6AXES_OK in case of success, an error code otherwise */ IMU_6AXES_StatusTypeDef LSM6DS3::LSM6DS3_X_GetSensitivity( float *pfData ) { /*Here we have to add the check if the parameters are valid*/ uint8_t tempReg = 0x00; if(LSM6DS3_IO_Read( &tempReg, LSM6DS3_XG_CTRL1_XL, 1 ) != IMU_6AXES_OK) { return IMU_6AXES_ERROR; } tempReg &= LSM6DS3_XL_FS_MASK; switch( tempReg ) { case LSM6DS3_XL_FS_2G: *pfData = 0.061f; break; case LSM6DS3_XL_FS_4G: *pfData = 0.122f; break; case LSM6DS3_XL_FS_8G: *pfData = 0.244f; break; case LSM6DS3_XL_FS_16G: *pfData = 0.488f; break; default: break; } return IMU_6AXES_OK; } /** * @brief Read Accelero Full Scale * @param fullScale the pointer where the accelerometer full scale is stored * @retval IMU_6AXES_OK in case of success, an error code otherwise */ IMU_6AXES_StatusTypeDef LSM6DS3::LSM6DS3_X_Get_FS( float *fullScale ) { /*Here we have to add the check if the parameters are valid*/ uint8_t tempReg = 0x00; if(LSM6DS3_IO_Read( &tempReg, LSM6DS3_XG_CTRL1_XL, 1 ) != IMU_6AXES_OK) { return IMU_6AXES_ERROR; } tempReg &= LSM6DS3_XL_FS_MASK; switch( tempReg ) { case LSM6DS3_XL_FS_2G: *fullScale = 2.0f; break; case LSM6DS3_XL_FS_4G: *fullScale = 4.0f; break; case LSM6DS3_XL_FS_8G: *fullScale = 8.0f; break; case LSM6DS3_XL_FS_16G: *fullScale = 16.0f; break; default: break; } return IMU_6AXES_OK; } /** * @brief Write Accelero Full Scale * @param fullScale the accelerometer full scale to be set * @retval IMU_6AXES_OK in case of success, an error code otherwise */ IMU_6AXES_StatusTypeDef LSM6DS3::LSM6DS3_X_Set_FS( float fullScale ) { uint8_t new_fs = 0x00; uint8_t tempReg = 0x00; new_fs = ( fullScale <= 2.0f ) ? LSM6DS3_XL_FS_2G : ( fullScale <= 4.0f ) ? LSM6DS3_XL_FS_4G : ( fullScale <= 8.0f ) ? LSM6DS3_XL_FS_8G : LSM6DS3_XL_FS_16G; if(LSM6DS3_IO_Read( &tempReg, LSM6DS3_XG_CTRL1_XL, 1 ) != IMU_6AXES_OK) { return IMU_6AXES_ERROR; } tempReg &= ~(LSM6DS3_XL_FS_MASK); tempReg |= new_fs; if(LSM6DS3_IO_Write(&tempReg, LSM6DS3_XG_CTRL1_XL, 1) != IMU_6AXES_OK) { return IMU_6AXES_ERROR; } return IMU_6AXES_OK; } /** * @brief Read Gyro Output Data Rate * @param odr the pointer where the gyroscope output data rate is stored * @retval IMU_6AXES_OK in case of success, an error code otherwise */ IMU_6AXES_StatusTypeDef LSM6DS3::LSM6DS3_G_Get_ODR( float *odr ) { /*Here we have to add the check if the parameters are valid*/ uint8_t tempReg = 0x00; if(LSM6DS3_IO_Read( &tempReg, LSM6DS3_XG_CTRL2_G, 1 ) != IMU_6AXES_OK) { return IMU_6AXES_ERROR; } tempReg &= LSM6DS3_G_ODR_MASK; switch( tempReg ) { case LSM6DS3_G_ODR_PD: *odr = 0.0f; break; case LSM6DS3_G_ODR_13HZ: *odr = 13.0f; break; case LSM6DS3_G_ODR_26HZ: *odr = 26.0f; break; case LSM6DS3_G_ODR_52HZ: *odr = 52.0f; break; case LSM6DS3_G_ODR_104HZ: *odr = 104.0f; break; case LSM6DS3_G_ODR_208HZ: *odr = 208.0f; break; case LSM6DS3_G_ODR_416HZ: *odr = 416.0f; break; case LSM6DS3_G_ODR_833HZ: *odr = 833.0f; break; case LSM6DS3_G_ODR_1660HZ: *odr = 1660.0f; break; default: break; } return IMU_6AXES_OK; } /** * @brief Write Gyro Output Data Rate * @param odr the gyroscope output data rate to be set * @retval IMU_6AXES_OK in case of success, an error code otherwise */ IMU_6AXES_StatusTypeDef LSM6DS3::LSM6DS3_G_Set_ODR( float odr ) { uint8_t new_odr = 0x00; uint8_t tempReg = 0x00; new_odr = ( odr <= 0.0f ) ? LSM6DS3_G_ODR_PD /* Power Down */ : ( odr <= 13.0f ) ? LSM6DS3_G_ODR_13HZ : ( odr <= 26.0f ) ? LSM6DS3_G_ODR_26HZ : ( odr <= 52.0f ) ? LSM6DS3_G_ODR_52HZ : ( odr <= 104.0f ) ? LSM6DS3_G_ODR_104HZ : ( odr <= 208.0f ) ? LSM6DS3_G_ODR_208HZ : ( odr <= 416.0f ) ? LSM6DS3_G_ODR_416HZ : ( odr <= 833.0f ) ? LSM6DS3_G_ODR_833HZ : LSM6DS3_G_ODR_1660HZ; if(LSM6DS3_IO_Read( &tempReg, LSM6DS3_XG_CTRL2_G, 1 ) != IMU_6AXES_OK) { return IMU_6AXES_ERROR; } tempReg &= ~(LSM6DS3_G_ODR_MASK); tempReg |= new_odr; if(LSM6DS3_IO_Write(&tempReg, LSM6DS3_XG_CTRL2_G, 1) != IMU_6AXES_OK) { return IMU_6AXES_ERROR; } return IMU_6AXES_OK; } /** * @brief Read Gyro Sensitivity * @param pfData the pointer where the gyroscope sensitivity is stored * @retval IMU_6AXES_OK in case of success, an error code otherwise */ IMU_6AXES_StatusTypeDef LSM6DS3::LSM6DS3_G_GetSensitivity( float *pfData ) { /*Here we have to add the check if the parameters are valid*/ uint8_t tempReg = 0x00; if(LSM6DS3_IO_Read( &tempReg, LSM6DS3_XG_CTRL2_G, 1 ) != IMU_6AXES_OK) { return IMU_6AXES_ERROR; } tempReg &= LSM6DS3_G_FS_125_MASK; if(tempReg == LSM6DS3_G_FS_125_ENABLE) { *pfData = 4.375f; } else { if(LSM6DS3_IO_Read( &tempReg, LSM6DS3_XG_CTRL2_G, 1 ) != IMU_6AXES_OK) { return IMU_6AXES_ERROR; } tempReg &= LSM6DS3_G_FS_MASK; switch( tempReg ) { case LSM6DS3_G_FS_245: *pfData = 8.75f; break; case LSM6DS3_G_FS_500: *pfData = 17.50f; break; case LSM6DS3_G_FS_1000: *pfData = 35.0f; break; case LSM6DS3_G_FS_2000: *pfData = 70.0f; break; default: break; } } return IMU_6AXES_OK; } /** * @brief Read Gyro Full Scale * @param fullScale the pointer where the gyroscope full scale is stored * @retval IMU_6AXES_OK in case of success, an error code otherwise */ IMU_6AXES_StatusTypeDef LSM6DS3::LSM6DS3_G_Get_FS( float *fullScale ) { /*Here we have to add the check if the parameters are valid*/ uint8_t tempReg = 0x00; if(LSM6DS3_IO_Read( &tempReg, LSM6DS3_XG_CTRL2_G, 1 ) != IMU_6AXES_OK) { return IMU_6AXES_ERROR; } tempReg &= LSM6DS3_G_FS_125_MASK; if(tempReg == LSM6DS3_G_FS_125_ENABLE) { *fullScale = 125.0f; } else { if(LSM6DS3_IO_Read( &tempReg, LSM6DS3_XG_CTRL2_G, 1 ) != IMU_6AXES_OK) { return IMU_6AXES_ERROR; } tempReg &= LSM6DS3_G_FS_MASK; switch( tempReg ) { case LSM6DS3_G_FS_245: *fullScale = 245.0f; break; case LSM6DS3_G_FS_500: *fullScale = 500.0f; break; case LSM6DS3_G_FS_1000: *fullScale = 1000.0f; break; case LSM6DS3_G_FS_2000: *fullScale = 2000.0f; break; default: break; } } return IMU_6AXES_OK; } /** * @brief Write Gyro Full Scale * @param fullScale the gyroscope full scale to be set * @retval IMU_6AXES_OK in case of success, an error code otherwise */ IMU_6AXES_StatusTypeDef LSM6DS3::LSM6DS3_G_Set_FS( float fullScale ) { uint8_t new_fs = 0x00; uint8_t tempReg = 0x00; if(fullScale <= 125.0f) { new_fs = LSM6DS3_G_FS_125_ENABLE; if(LSM6DS3_IO_Read( &tempReg, LSM6DS3_XG_CTRL2_G, 1 ) != IMU_6AXES_OK) { return IMU_6AXES_ERROR; } tempReg &= ~(LSM6DS3_G_FS_125_MASK); tempReg |= new_fs; if(LSM6DS3_IO_Write(&tempReg, LSM6DS3_XG_CTRL2_G, 1) != IMU_6AXES_OK) { return IMU_6AXES_ERROR; } } else { /* Disable G FS 125dpp */ if(LSM6DS3_IO_Read( &tempReg, LSM6DS3_XG_CTRL2_G, 1 ) != IMU_6AXES_OK) { return IMU_6AXES_ERROR; } tempReg &= ~(LSM6DS3_G_FS_125_MASK); tempReg |= LSM6DS3_G_FS_125_DISABLE; if(LSM6DS3_IO_Write(&tempReg, LSM6DS3_XG_CTRL2_G, 1) != IMU_6AXES_OK) { return IMU_6AXES_ERROR; } new_fs = ( fullScale <= 245.0f ) ? LSM6DS3_G_FS_245 : ( fullScale <= 500.0f ) ? LSM6DS3_G_FS_500 : ( fullScale <= 1000.0f ) ? LSM6DS3_G_FS_1000 : LSM6DS3_G_FS_2000; if(LSM6DS3_IO_Read( &tempReg, LSM6DS3_XG_CTRL2_G, 1 ) != IMU_6AXES_OK) { return IMU_6AXES_ERROR; } tempReg &= ~(LSM6DS3_G_FS_MASK); tempReg |= new_fs; if(LSM6DS3_IO_Write(&tempReg, LSM6DS3_XG_CTRL2_G, 1) != IMU_6AXES_OK) { return IMU_6AXES_ERROR; } } return IMU_6AXES_OK; } /** * @brief Enable free fall detection * @retval IMU_6AXES_OK in case of success, an error code otherwise */ IMU_6AXES_StatusTypeDef LSM6DS3::LSM6DS3_Enable_Free_Fall_Detection( void ) { uint8_t tmp1 = 0x00; if(LSM6DS3_IO_Read(&tmp1, LSM6DS3_XG_CTRL1_XL, 1) != IMU_6AXES_OK) { return IMU_6AXES_ERROR; } /* Output Data Rate selection */ tmp1 &= ~(LSM6DS3_XL_ODR_MASK); tmp1 |= LSM6DS3_XL_ODR_416HZ; /* Full scale selection */ tmp1 &= ~(LSM6DS3_XL_FS_MASK); tmp1 |= LSM6DS3_XL_FS_2G; if(LSM6DS3_IO_Write(&tmp1, LSM6DS3_XG_CTRL1_XL, 1) != IMU_6AXES_OK) { return IMU_6AXES_ERROR; } if(LSM6DS3_IO_Read(&tmp1, LSM6DS3_XG_WAKE_UP_DUR, 1) != IMU_6AXES_OK) { return IMU_6AXES_ERROR; } /* FF_DUR5 setting */ tmp1 &= ~(LSM6DS3_XG_WAKE_UP_DUR_FF_DUR5_MASK); tmp1 |= LSM6DS3_XG_WAKE_UP_DUR_FF_DUR5_DEFAULT; /* WAKE_DUR setting */ tmp1 &= ~(LSM6DS3_XG_WAKE_UP_DUR_WAKE_DUR_MASK); tmp1 |= LSM6DS3_XG_WAKE_UP_DUR_WAKE_DUR_DEFAULT; /* TIMER_HR setting */ tmp1 &= ~(LSM6DS3_XG_WAKE_UP_DUR_TIMER_HR_MASK); tmp1 |= LSM6DS3_XG_WAKE_UP_DUR_TIMER_HR_DEFAULT; /* SLEEP_DUR setting */ tmp1 &= ~(LSM6DS3_XG_WAKE_UP_DUR_SLEEP_DUR_MASK); tmp1 |= LSM6DS3_XG_WAKE_UP_DUR_SLEEP_DUR_DEFAULT; if(LSM6DS3_IO_Write(&tmp1, LSM6DS3_XG_WAKE_UP_DUR, 1) != IMU_6AXES_OK) { return IMU_6AXES_ERROR; } if(LSM6DS3_IO_Read(&tmp1, LSM6DS3_XG_WAKE_FREE_FALL, 1) != IMU_6AXES_OK) { return IMU_6AXES_ERROR; } /* FF_DUR setting */ tmp1 &= ~(LSM6DS3_XG_WAKE_FREE_FALL_FF_DUR_MASK); tmp1 |= LSM6DS3_XG_WAKE_FREE_FALL_FF_DUR_TYPICAL; /* FF_THS setting */ tmp1 &= ~(LSM6DS3_XG_WAKE_FREE_FALL_FF_THS_MASK); tmp1 |= LSM6DS3_XG_WAKE_FREE_FALL_FF_THS_312MG; if(LSM6DS3_IO_Write(&tmp1, LSM6DS3_XG_WAKE_FREE_FALL, 1) != IMU_6AXES_OK) { return IMU_6AXES_ERROR; } if(LSM6DS3_IO_Read(&tmp1, LSM6DS3_XG_MD1_CFG, 1) != IMU_6AXES_OK) { return IMU_6AXES_ERROR; } /* INT1_FF setting */ tmp1 &= ~(LSM6DS3_XG_MD1_CFG_INT1_FF_MASK); tmp1 |= LSM6DS3_XG_MD1_CFG_INT1_FF_ENABLE; if(LSM6DS3_IO_Write(&tmp1, LSM6DS3_XG_MD1_CFG, 1) != IMU_6AXES_OK) { return IMU_6AXES_ERROR; } return IMU_6AXES_OK; } /** * @brief Disable free fall detection * @retval IMU_6AXES_OK in case of success, an error code otherwise */ IMU_6AXES_StatusTypeDef LSM6DS3::LSM6DS3_Disable_Free_Fall_Detection( void ) { uint8_t tmp1 = 0x00; if(LSM6DS3_IO_Read(&tmp1, LSM6DS3_XG_MD1_CFG, 1) != IMU_6AXES_OK) { return IMU_6AXES_ERROR; } /* INT1_FF setting */ tmp1 &= ~(LSM6DS3_XG_MD1_CFG_INT1_FF_MASK); tmp1 |= LSM6DS3_XG_MD1_CFG_INT1_FF_DISABLE; if(LSM6DS3_IO_Write(&tmp1, LSM6DS3_XG_MD1_CFG, 1) != IMU_6AXES_OK) { return IMU_6AXES_ERROR; } if(LSM6DS3_IO_Read(&tmp1, LSM6DS3_XG_WAKE_FREE_FALL, 1) != IMU_6AXES_OK) { return IMU_6AXES_ERROR; } /* FF_DUR setting */ tmp1 &= ~(LSM6DS3_XG_WAKE_FREE_FALL_FF_DUR_MASK); tmp1 |= LSM6DS3_XG_WAKE_FREE_FALL_FF_DUR_DEFAULT; /* FF_THS setting */ tmp1 &= ~(LSM6DS3_XG_WAKE_FREE_FALL_FF_THS_MASK); tmp1 |= LSM6DS3_XG_WAKE_FREE_FALL_FF_THS_156MG; if(LSM6DS3_IO_Write(&tmp1, LSM6DS3_XG_WAKE_FREE_FALL, 1) != IMU_6AXES_OK) { return IMU_6AXES_ERROR; } return IMU_6AXES_OK; } /** * @brief Get status of free fall detection * @param status the pointer where the status of free fall detection is stored; 0 means no detection, 1 means detection happened * @retval IMU_6AXES_OK in case of success, an error code otherwise */ IMU_6AXES_StatusTypeDef LSM6DS3::LSM6DS3_Get_Status_Free_Fall_Detection( uint8_t *status ) { uint8_t tmp1 = 0x00; if(LSM6DS3_IO_Read(&tmp1, LSM6DS3_XG_WAKE_UP_SRC, 1) != IMU_6AXES_OK) { return IMU_6AXES_ERROR; } tmp1 &= LSM6DS3_XG_WAKE_UP_SRC_FF_IA_MASK; switch( tmp1 ) { case LSM6DS3_XG_WAKE_UP_SRC_FF_IA_ENABLE: *status = 1; break; case LSM6DS3_XG_WAKE_UP_SRC_FF_IA_DISABLE: default: *status = 0; break; } return IMU_6AXES_OK; } /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/