lsm6ds0.cpp

00001 /**
00002 ******************************************************************************
00003 * @file    x_cube_mems_lsm6ds0.h
00004 * @author  AST / EST
00005 * @version V0.0.1
00006 * @date    9-December-2014
00007 * @brief   Header file for component 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 
00038 /* Includes ------------------------------------------------------------------*/
00039 #include "mbed.h"
00040 #include "lsm6ds0.h"
00041 #include "lsm6ds0_platform.h"
00042 #include <math.h>
00043 
00044 /* Methods -------------------------------------------------------------------*/
00045 
00046 /**
00047  * @brief Read data from LSM6DS0 Gyroscope and calculate angular rate in mdps.
00048  * @param float *pfData
00049  * @retval None.
00050  */
00051 void LSM6DS0::Gyro_GetAxes(AxesRaw_TypeDef *pData)
00052 {
00053 
00054   uint8_t tempReg = 0x00;
00055   int16_t pDataRaw[3];
00056   float sensitivity = 0;
00057   int ret;
00058 
00059   LSM6DS0::Gyro_GetAxesRaw(pDataRaw);
00060 
00061   //IMU_6AXES_IO_Read(&tempReg, LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_CTRL_REG1_G, 1);
00062   ret = dev_i2c.i2c_read(&tempReg, LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_CTRL_REG1_G, 1);
00063 
00064   if (ret == 0)
00065   {
00066     tempReg &= LSM6DS0_G_FS_MASK;
00067 
00068     switch(tempReg)
00069     {
00070       case LSM6DS0_G_FS_245:
00071         sensitivity = 8.75;
00072         break;
00073       case LSM6DS0_G_FS_500:
00074         sensitivity = 17.50;
00075         break;
00076       case LSM6DS0_G_FS_2000:
00077         sensitivity = 70;
00078         break;
00079     }
00080   }
00081 
00082   pData->AXIS_X = (int32_t)(pDataRaw[0] * sensitivity);
00083   pData->AXIS_Y = (int32_t)(pDataRaw[1] * sensitivity);
00084   pData->AXIS_Z = (int32_t)(pDataRaw[2] * sensitivity);
00085 }
00086 
00087 
00088 /**
00089  * @brief Read raw data from LSM6DS0 Gyroscope output register.
00090  * @param float *pfData
00091  * @retval None.
00092  */
00093 void LSM6DS0::Gyro_GetAxesRaw(int16_t *pData)
00094 {
00095     uint8_t tempReg[2] = {0,0};
00096     int ret;
00097 
00098     pData[0] = pData[1] = pData[2] = 0;
00099 
00100     //IMU_6AXES_IO_Read(&tempReg[0], LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_OUT_X_L_G + 0x80, 2);
00101     ret = dev_i2c.i2c_read(&tempReg[0], LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_OUT_X_L_G + 0x80, 2);
00102 
00103     if (ret == 0)
00104     {
00105       pData[0] = ((((int16_t)tempReg[1]) << 8)+(int16_t)tempReg[0]);
00106 
00107       //IMU_6AXES_IO_Read(&tempReg[0], LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_OUT_Y_L_G + 0x80, 2);
00108       ret = dev_i2c.i2c_read(&tempReg[0], LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_OUT_Y_L_G + 0x80, 2);
00109     }
00110     
00111     if (ret == 0)
00112     {
00113       pData[1] = ((((int16_t)tempReg[1]) << 8)+(int16_t)tempReg[0]);
00114 
00115       //IMU_6AXES_IO_Read(&tempReg[0], LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_OUT_Z_L_G + 0x80, 2);
00116       ret = dev_i2c.i2c_read(&tempReg[0], LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_OUT_Z_L_G + 0x80, 2);
00117     }
00118 
00119     if (ret == 0)
00120     {
00121       pData[2] = ((((int16_t)tempReg[1]) << 8)+(int16_t)tempReg[0]);
00122     }
00123 }
00124 
00125 
00126 /**
00127  * @brief Read data from LSM6DS0 Accelerometer and calculate linear acceleration in mg.
00128  * @param float *pfData
00129  * @retval None.
00130  */
00131 void LSM6DS0::Acc_GetAxes(AxesRaw_TypeDef *pData)
00132 {
00133   uint8_t tempReg = 0x00;
00134   int16_t pDataRaw[3];
00135   float sensitivity = 0;
00136   int ret;
00137 
00138   Acc_GetAxesRaw(pDataRaw);
00139 
00140   //IMU_6AXES_IO_Read(&tempReg, LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_CTRL_REG6_XL, 1);
00141   ret = dev_i2c.i2c_read(&tempReg, LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_CTRL_REG6_XL, 1);
00142 
00143   if (ret == 0)
00144   {
00145     tempReg &= LSM6DS0_XL_FS_MASK;
00146 
00147     switch(tempReg)
00148     {
00149       case LSM6DS0_XL_FS_2G:
00150         sensitivity = 0.061;
00151         break;
00152       case LSM6DS0_XL_FS_4G:
00153         sensitivity = 0.122;
00154         break;
00155       case LSM6DS0_XL_FS_8G:
00156         sensitivity = 0.244;
00157         break;
00158     }
00159   }
00160 
00161   pData->AXIS_X = (int32_t)(pDataRaw[0] * sensitivity);
00162   pData->AXIS_Y = (int32_t)(pDataRaw[1] * sensitivity);
00163   pData->AXIS_Z = (int32_t)(pDataRaw[2] * sensitivity);
00164   
00165 }
00166 
00167 /**
00168  * @brief Read raw data from LSM6DS0 Accelerometer output register.
00169  * @param float *pfData
00170  * @retval None.
00171  */
00172 void LSM6DS0::Acc_GetAxesRaw(int16_t *pData)
00173 {
00174     uint8_t tempReg[2] = {0,0};
00175     int ret;
00176 
00177     pData[0] = pData[1] = pData[2] = 0;
00178 
00179     //IMU_6AXES_IO_Read(&tempReg[0], LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_OUT_X_L_XL + 0x80, 2);
00180     ret = dev_i2c.i2c_read(&tempReg[0], LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_OUT_X_L_XL + 0x80, 2);
00181 
00182     if (ret == 0)
00183     {
00184       pData[0] = ((((int16_t)tempReg[1]) << 8)+(int16_t)tempReg[0]);
00185 
00186       //IMU_6AXES_IO_Read(&tempReg[0], LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_OUT_Y_L_XL + 0x80, 2);
00187       ret = dev_i2c.i2c_read(&tempReg[0], LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_OUT_Y_L_XL + 0x80, 2);
00188     }
00189     
00190     if (ret == 0)
00191     {
00192       pData[1] = ((((int16_t)tempReg[1]) << 8)+(int16_t)tempReg[0]);
00193       
00194       //IMU_6AXES_IO_Read(&tempReg[0], LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_OUT_Z_L_XL + 0x80, 2);
00195       ret = dev_i2c.i2c_read(&tempReg[0], LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_OUT_Z_L_XL + 0x80, 2);
00196     }
00197     
00198     if (ret == 0)
00199     {
00200       pData[2] = ((((int16_t)tempReg[1]) << 8)+(int16_t)tempReg[0]);
00201     }
00202 }
00203 
00204 /**
00205  * @brief  Read ID address of HTS221
00206  * @param  Device ID address
00207  * @retval ID name
00208  */
00209 uint8_t LSM6DS0::ReadID(void)
00210 {
00211     uint8_t tmp=0x00;
00212     int ret;
00213     
00214     /* Read WHO I AM register */
00215     //IMU_6AXES_IO_Read(&tmp, LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_WHO_AM_I_ADDR, 1);
00216     ret = dev_i2c.i2c_read(&tmp, LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_WHO_AM_I_ADDR, 1);
00217 
00218     /* Return the ID */
00219     return ((ret == 0) ? (uint8_t)tmp : 0);
00220 }
00221 
00222 /**
00223  * @brief  Set LSM6DS0 Initialization.
00224  * @param  InitStruct: it contains the configuration setting for the LSM6DS0.
00225  * @retval None
00226  */
00227 void LSM6DS0::Init() {
00228     
00229     uint8_t tmp1 = 0x00;
00230     int ret;
00231 
00232 /******* Gyroscope init *******/
00233 
00234     //IMU_6AXES_IO_Read(&tmp1, LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_CTRL_REG1_G, 1);
00235     ret = dev_i2c.i2c_read(&tmp1, LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_CTRL_REG1_G, 1);
00236 
00237     if (ret == 0)
00238     {
00239       /* Output Data Rate selection */
00240       tmp1 &= ~(LSM6DS0_G_ODR_MASK);
00241       tmp1 |= LSM6DS0_G_ODR_119HZ;
00242 
00243       /* Full scale selection */
00244       tmp1 &= ~(LSM6DS0_G_FS_MASK);
00245       tmp1 |= LSM6DS0_G_FS_2000;
00246 
00247       //IMU_6AXES_IO_Write(&tmp1, LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_CTRL_REG1_G, 1);
00248       ret = dev_i2c.i2c_write(&tmp1, LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_CTRL_REG1_G, 1);
00249     }
00250     
00251     if (ret == 0)
00252     {
00253       //IMU_6AXES_IO_Read(&tmp1, LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_CTRL_REG4, 1);
00254       ret = dev_i2c.i2c_read(&tmp1, LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_CTRL_REG4, 1);
00255     }
00256 
00257     if (ret == 0)
00258     {
00259       /* Enable X axis selection */
00260       tmp1 &= ~(LSM6DS0_G_XEN_MASK);
00261       tmp1 |= LSM6DS0_G_XEN_ENABLE;
00262 
00263       /* Enable Y axis selection */
00264       tmp1 &= ~(LSM6DS0_G_YEN_MASK);
00265       tmp1 |= LSM6DS0_G_YEN_ENABLE;
00266 
00267       /* Enable Z axis selection */
00268       tmp1 &= ~(LSM6DS0_G_ZEN_MASK);
00269       tmp1 |= LSM6DS0_G_ZEN_ENABLE;
00270 
00271       //IMU_6AXES_IO_Write(&tmp1, LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_CTRL_REG4, 1);
00272       ret = dev_i2c.i2c_write(&tmp1, LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_CTRL_REG4, 1);
00273     }
00274 
00275 /******************************/
00276 /***** Accelerometer init *****/
00277     if (ret == 0)
00278     {
00279       //IMU_6AXES_IO_Read(&tmp1, LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_CTRL_REG6_XL, 1);
00280       ret = dev_i2c.i2c_read(&tmp1, LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_CTRL_REG6_XL, 1);
00281     }
00282     
00283     if (ret == 0)
00284     {
00285       /* Output Data Rate selection */
00286       tmp1 &= ~(LSM6DS0_XL_ODR_MASK);
00287       tmp1 |= LSM6DS0_XL_ODR_119HZ;
00288 
00289       /* Full scale selection */
00290       tmp1 &= ~(LSM6DS0_XL_FS_MASK);
00291       tmp1 |= LSM6DS0_XL_FS_2G;
00292 
00293       //IMU_6AXES_IO_Write(&tmp1, LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_CTRL_REG6_XL, 1);
00294       ret = dev_i2c.i2c_write(&tmp1, LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_CTRL_REG6_XL, 1);
00295     }
00296     
00297     if (ret == 0)
00298     {
00299       //IMU_6AXES_IO_Read(&tmp1, LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_CTRL_REG5_XL, 1);
00300       ret = dev_i2c.i2c_read(&tmp1, LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_CTRL_REG5_XL, 1);
00301     }
00302     
00303     if (ret == 0)
00304     {
00305       /* Enable X axis selection */
00306       tmp1 &= ~(LSM6DS0_XL_XEN_MASK);
00307       tmp1 |= LSM6DS0_XL_XEN_ENABLE;
00308 
00309       /* Enable Y axis selection */
00310       tmp1 &= ~(LSM6DS0_XL_YEN_MASK);
00311       tmp1 |= LSM6DS0_XL_YEN_ENABLE;
00312 
00313       /* Enable Z axis selection */
00314       tmp1 &= ~(LSM6DS0_XL_ZEN_MASK);
00315       tmp1 |= LSM6DS0_XL_ZEN_ENABLE;
00316 
00317       //IMU_6AXES_IO_Write(&tmp1, LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_CTRL_REG5_XL, 1);
00318       ret = dev_i2c.i2c_write(&tmp1, LSM6DS0_XG_MEMS_ADDRESS, LSM6DS0_XG_CTRL_REG5_XL, 1);
00319     }
00320 /******************************/
00321 
00322     if (ret == 0)
00323     {        
00324       if(ReadID() == I_AM_LSM6DS0_XG)
00325       {
00326         LSM6DS0Initialized = 1;
00327         //ret = HUM_TEMP_OK;
00328       }
00329     }
00330     
00331     return;
00332 }