lsm6ds3_class.cpp

00001 /**
00002  ******************************************************************************
00003  * @file    lsm6ds3_class.cpp
00004  * @author  AST / EST
00005  * @version V0.0.1
00006  * @date    14-April-2015
00007  * @brief   Implementation file for the LSM6DS3 driver class
00008  ******************************************************************************
00009  * @attention
00010  *
00011  * <h2><center>&copy; COPYRIGHT(c) 2015 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 "lsm6ds3_class.h"
00040 #include "lsm6ds3.h"
00041 
00042 /* Methods -------------------------------------------------------------------*/
00043 /* betzw - based on:
00044            X-CUBE-MEMS1/trunk/Drivers/BSP/Components/lsm6ds3/lsm6ds3.c: revision #400,
00045            X-CUBE-MEMS1/trunk: revision #416
00046 */
00047 
00048 /**
00049  * @brief  Set LSM6DS3 Initialization
00050  * @param  LSM6DS3_Init the configuration setting for the LSM6DS3
00051  * @retval IMU_6AXES_OK in case of success, an error code otherwise
00052  */
00053 IMU_6AXES_StatusTypeDef    LSM6DS3::LSM6DS3_Init( IMU_6AXES_InitTypeDef *LSM6DS3_Init )
00054 {
00055   /*Here we have to add the check if the parameters are valid*/
00056   
00057   /* Configure the low level interface -------------------------------------*/
00058   if(LSM6DS3_IO_Init() != IMU_6AXES_OK)
00059   {
00060     return IMU_6AXES_ERROR;
00061   }
00062   
00063   
00064   /******** Common init *********/
00065   
00066   if(LSM6DS3_Common_Sensor_Enable() != IMU_6AXES_OK)
00067   {
00068     return IMU_6AXES_ERROR;
00069   }
00070   
00071   
00072   /******* Gyroscope init *******/
00073   
00074   if(LSM6DS3_G_Set_ODR( LSM6DS3_Init->G_OutputDataRate ) != IMU_6AXES_OK)
00075   {
00076     return IMU_6AXES_ERROR;
00077   }
00078   
00079   if(LSM6DS3_G_Set_FS( LSM6DS3_Init->G_FullScale ) != IMU_6AXES_OK)
00080   {
00081     return IMU_6AXES_ERROR;
00082   }
00083   
00084   if(LSM6DS3_G_Set_Axes_Status(LSM6DS3_Init->G_X_Axis, LSM6DS3_Init->G_Y_Axis, LSM6DS3_Init->G_Z_Axis) != IMU_6AXES_OK)
00085   {
00086     return IMU_6AXES_ERROR;
00087   }
00088   
00089   
00090   /***** Accelerometer init *****/
00091   
00092   if(LSM6DS3_X_Set_ODR( LSM6DS3_Init->X_OutputDataRate ) != IMU_6AXES_OK)
00093   {
00094     return IMU_6AXES_ERROR;
00095   }
00096   
00097   if(LSM6DS3_X_Set_FS( LSM6DS3_Init->X_FullScale ) != IMU_6AXES_OK)
00098   {
00099     return IMU_6AXES_ERROR;
00100   }
00101   
00102   if(LSM6DS3_X_Set_Axes_Status(LSM6DS3_Init->X_X_Axis, LSM6DS3_Init->X_Y_Axis, LSM6DS3_Init->X_Z_Axis) != IMU_6AXES_OK)
00103   {
00104     return IMU_6AXES_ERROR;
00105   }
00106   
00107   /* Configure interrupt lines */
00108   LSM6DS3_IO_ITConfig();
00109   
00110   return IMU_6AXES_OK;
00111 }
00112 
00113 /**
00114  * @brief  Read ID of LSM6DS3 Accelerometer and Gyroscope
00115  * @param  xg_id the pointer where the ID of the device is stored
00116  * @retval IMU_6AXES_OK in case of success, an error code otherwise
00117  */
00118 IMU_6AXES_StatusTypeDef    LSM6DS3::LSM6DS3_Read_XG_ID( uint8_t *xg_id)
00119 {
00120   if(!xg_id)
00121   {
00122     return IMU_6AXES_ERROR;
00123   }
00124   
00125   return LSM6DS3_IO_Read(xg_id, LSM6DS3_XG_WHO_AM_I_ADDR, 1);
00126 }
00127 
00128 /**
00129  * @brief  Set LSM6DS3 common initialization
00130  * @retval IMU_6AXES_OK in case of success, an error code otherwise
00131  */
00132 IMU_6AXES_StatusTypeDef LSM6DS3::LSM6DS3_Common_Sensor_Enable(void)
00133 {
00134   uint8_t tmp1 = 0x00;
00135   
00136   if(LSM6DS3_IO_Read(&tmp1, LSM6DS3_XG_CTRL3_C, 1) != IMU_6AXES_OK)
00137   {
00138     return IMU_6AXES_ERROR;
00139   }
00140   
00141   /* Enable register address automatically incremented during a multiple byte
00142      access with a serial interface (I2C or SPI) */
00143   tmp1 &= ~(LSM6DS3_XG_IF_INC_MASK);
00144   tmp1 |= LSM6DS3_XG_IF_INC;
00145   
00146   if(LSM6DS3_IO_Write(&tmp1, LSM6DS3_XG_CTRL3_C, 1) != IMU_6AXES_OK)
00147   {
00148     return IMU_6AXES_ERROR;
00149   }
00150   
00151   
00152   if(LSM6DS3_IO_Read(&tmp1, LSM6DS3_XG_FIFO_CTRL5, 1) != IMU_6AXES_OK)
00153   {
00154     return IMU_6AXES_ERROR;
00155   }
00156   
00157   /* FIFO ODR selection */
00158   tmp1 &= ~(LSM6DS3_XG_FIFO_ODR_MASK);
00159   tmp1 |= LSM6DS3_XG_FIFO_ODR_NA;
00160   
00161   /* FIFO mode selection */
00162   tmp1 &= ~(LSM6DS3_XG_FIFO_MODE_MASK);
00163   tmp1 |= LSM6DS3_XG_FIFO_MODE_BYPASS;
00164   
00165   if(LSM6DS3_IO_Write(&tmp1, LSM6DS3_XG_FIFO_CTRL5, 1) != IMU_6AXES_OK)
00166   {
00167     return IMU_6AXES_ERROR;
00168   }
00169   
00170   return IMU_6AXES_OK;
00171 }
00172 
00173 /**
00174  * @brief  Read raw data from LSM6DS3 Accelerometer output register
00175  * @param  pData the pointer where the accelerometer raw data are stored
00176  * @retval IMU_6AXES_OK in case of success, an error code otherwise
00177  */
00178 IMU_6AXES_StatusTypeDef LSM6DS3::LSM6DS3_X_GetAxesRaw( int16_t *pData )
00179 {
00180   /*Here we have to add the check if the parameters are valid*/
00181   
00182   uint8_t tempReg[2] = {0, 0};
00183   
00184   
00185   if(LSM6DS3_IO_Read(&tempReg[0], LSM6DS3_XG_OUT_X_L_XL, 2) != IMU_6AXES_OK)
00186   {
00187     return IMU_6AXES_ERROR;
00188   }
00189   
00190   pData[0] = ((((int16_t)tempReg[1]) << 8) + (int16_t)tempReg[0]);
00191   
00192   if(LSM6DS3_IO_Read(&tempReg[0], LSM6DS3_XG_OUT_Y_L_XL, 2) != IMU_6AXES_OK)
00193   {
00194     return IMU_6AXES_ERROR;
00195   }
00196   
00197   pData[1] = ((((int16_t)tempReg[1]) << 8) + (int16_t)tempReg[0]);
00198   
00199   if(LSM6DS3_IO_Read(&tempReg[0], LSM6DS3_XG_OUT_Z_L_XL, 2) != IMU_6AXES_OK)
00200   {
00201     return IMU_6AXES_ERROR;
00202   }
00203   
00204   pData[2] = ((((int16_t)tempReg[1]) << 8) + (int16_t)tempReg[0]);
00205   
00206   return IMU_6AXES_OK;
00207 }
00208 
00209 
00210 
00211 /**
00212  * @brief  Read data from LSM6DS3 Accelerometer and calculate linear acceleration in mg
00213  * @param  pData the pointer where the accelerometer data are stored
00214  * @retval IMU_6AXES_OK in case of success, an error code otherwise
00215  */
00216 IMU_6AXES_StatusTypeDef    LSM6DS3::LSM6DS3_X_GetAxes( int32_t *pData )
00217 {
00218   /*Here we have to add the check if the parameters are valid*/
00219   int16_t pDataRaw[3];
00220   float sensitivity = 0.0f;
00221   
00222   if(LSM6DS3_X_GetAxesRaw(pDataRaw) != IMU_6AXES_OK)
00223   {
00224     return IMU_6AXES_ERROR;
00225   }
00226   
00227   if(LSM6DS3_X_GetSensitivity( &sensitivity ) != IMU_6AXES_OK)
00228   {
00229     return IMU_6AXES_ERROR;
00230   }
00231   
00232   pData[0] = (int32_t)(pDataRaw[0] * sensitivity);
00233   pData[1] = (int32_t)(pDataRaw[1] * sensitivity);
00234   pData[2] = (int32_t)(pDataRaw[2] * sensitivity);
00235   
00236   return IMU_6AXES_OK;
00237 }
00238 
00239 
00240 
00241 /**
00242  * @brief  Read raw data from LSM6DS3 Gyroscope output register
00243  * @param  pData the pointer where the gyroscope raw data are stored
00244  * @retval IMU_6AXES_OK in case of success, an error code otherwise
00245  */
00246 IMU_6AXES_StatusTypeDef LSM6DS3::LSM6DS3_G_GetAxesRaw( int16_t *pData )
00247 {
00248   /*Here we have to add the check if the parameters are valid*/
00249   
00250   uint8_t tempReg[2] = {0, 0};
00251   
00252   
00253   if(LSM6DS3_IO_Read(&tempReg[0], LSM6DS3_XG_OUT_X_L_G, 2) != IMU_6AXES_OK)
00254   {
00255     return IMU_6AXES_ERROR;
00256   }
00257   
00258   pData[0] = ((((int16_t)tempReg[1]) << 8) + (int16_t)tempReg[0]);
00259   
00260   if(LSM6DS3_IO_Read(&tempReg[0], LSM6DS3_XG_OUT_Y_L_G, 2) != IMU_6AXES_OK)
00261   {
00262     return IMU_6AXES_ERROR;
00263   }
00264   
00265   pData[1] = ((((int16_t)tempReg[1]) << 8) + (int16_t)tempReg[0]);
00266   
00267   if(LSM6DS3_IO_Read(&tempReg[0], LSM6DS3_XG_OUT_Z_L_G, 2) != IMU_6AXES_OK)
00268   {
00269     return IMU_6AXES_ERROR;
00270   }
00271   
00272   pData[2] = ((((int16_t)tempReg[1]) << 8) + (int16_t)tempReg[0]);
00273   
00274   return IMU_6AXES_OK;
00275 }
00276 
00277 /**
00278  * @brief  Set the status of the axes for accelerometer
00279  * @param  enableX the status of the x axis to be set
00280  * @param  enableY the status of the y axis to be set
00281  * @param  enableZ the status of the z axis to be set
00282  * @retval IMU_6AXES_OK in case of success, an error code otherwise
00283  */
00284 IMU_6AXES_StatusTypeDef LSM6DS3::LSM6DS3_X_Set_Axes_Status(uint8_t enableX, uint8_t enableY, uint8_t enableZ)
00285 {
00286   uint8_t tmp1 = 0x00;
00287   uint8_t eX = 0x00;
00288   uint8_t eY = 0x00;
00289   uint8_t eZ = 0x00;
00290   
00291   eX = ( enableX == 0 ) ? LSM6DS3_XL_XEN_DISABLE : LSM6DS3_XL_XEN_ENABLE;
00292   eY = ( enableY == 0 ) ? LSM6DS3_XL_YEN_DISABLE : LSM6DS3_XL_YEN_ENABLE;
00293   eZ = ( enableZ == 0 ) ? LSM6DS3_XL_ZEN_DISABLE : LSM6DS3_XL_ZEN_ENABLE;
00294   
00295   if(LSM6DS3_IO_Read(&tmp1, LSM6DS3_XG_CTRL9_XL, 1) != IMU_6AXES_OK)
00296   {
00297     return IMU_6AXES_ERROR;
00298   }
00299   
00300   /* Enable X axis selection */
00301   tmp1 &= ~(LSM6DS3_XL_XEN_MASK);
00302   tmp1 |= eX;
00303   
00304   /* Enable Y axis selection */
00305   tmp1 &= ~(LSM6DS3_XL_YEN_MASK);
00306   tmp1 |= eY;
00307   
00308   /* Enable Z axis selection */
00309   tmp1 &= ~(LSM6DS3_XL_ZEN_MASK);
00310   tmp1 |= eZ;
00311   
00312   if(LSM6DS3_IO_Write(&tmp1, LSM6DS3_XG_CTRL9_XL, 1) != IMU_6AXES_OK)
00313   {
00314     return IMU_6AXES_ERROR;
00315   }
00316   
00317   return IMU_6AXES_OK;
00318 }
00319 
00320 /**
00321  * @brief  Set the status of the axes for gyroscope
00322  * @param  enableX the status of the x axis to be set
00323  * @param  enableY the status of the y axis to be set
00324  * @param  enableZ the status of the z axis to be set
00325  * @retval IMU_6AXES_OK in case of success, an error code otherwise
00326  */
00327 IMU_6AXES_StatusTypeDef LSM6DS3::LSM6DS3_G_Set_Axes_Status(uint8_t enableX, uint8_t enableY, uint8_t enableZ)
00328 {
00329   uint8_t tmp1 = 0x00;
00330   uint8_t eX = 0x00;
00331   uint8_t eY = 0x00;
00332   uint8_t eZ = 0x00;
00333   
00334   eX = ( enableX == 0 ) ? LSM6DS3_G_XEN_DISABLE : LSM6DS3_G_XEN_ENABLE;
00335   eY = ( enableY == 0 ) ? LSM6DS3_G_YEN_DISABLE : LSM6DS3_G_YEN_ENABLE;
00336   eZ = ( enableZ == 0 ) ? LSM6DS3_G_ZEN_DISABLE : LSM6DS3_G_ZEN_ENABLE;
00337   
00338   if(LSM6DS3_IO_Read(&tmp1, LSM6DS3_XG_CTRL10_C, 1) != IMU_6AXES_OK)
00339   {
00340     return IMU_6AXES_ERROR;
00341   }
00342   
00343   /* Enable X axis selection */
00344   tmp1 &= ~(LSM6DS3_G_XEN_MASK);
00345   tmp1 |= eX;
00346   
00347   /* Enable Y axis selection */
00348   tmp1 &= ~(LSM6DS3_G_YEN_MASK);
00349   tmp1 |= eY;
00350   
00351   /* Enable Z axis selection */
00352   tmp1 &= ~(LSM6DS3_G_ZEN_MASK);
00353   tmp1 |= eZ;
00354   
00355   if(LSM6DS3_IO_Write(&tmp1, LSM6DS3_XG_CTRL10_C, 1) != IMU_6AXES_OK)
00356   {
00357     return IMU_6AXES_ERROR;
00358   }
00359   
00360   return IMU_6AXES_OK;
00361 }
00362 
00363 /**
00364  * @brief  Read data from LSM6DS3 Gyroscope and calculate angular rate in mdps
00365  * @param  pData the pointer where the gyroscope data are stored
00366  * @retval IMU_6AXES_OK in case of success, an error code otherwise
00367  */
00368 IMU_6AXES_StatusTypeDef    LSM6DS3::LSM6DS3_G_GetAxes( int32_t *pData )
00369 {
00370   /*Here we have to add the check if the parameters are valid*/
00371   int16_t pDataRaw[3];
00372   float sensitivity = 0.0f;
00373   
00374   if(LSM6DS3_G_GetAxesRaw(pDataRaw) != IMU_6AXES_OK)
00375   {
00376     return IMU_6AXES_ERROR;
00377   }
00378   
00379   if(LSM6DS3_G_GetSensitivity( &sensitivity ) != IMU_6AXES_OK)
00380   {
00381     return IMU_6AXES_ERROR;
00382   }
00383   
00384   pData[0] = (int32_t)(pDataRaw[0] * sensitivity);
00385   pData[1] = (int32_t)(pDataRaw[1] * sensitivity);
00386   pData[2] = (int32_t)(pDataRaw[2] * sensitivity);
00387   
00388   return IMU_6AXES_OK;
00389 }
00390 
00391 /**
00392  * @brief  Read Accelero Output Data Rate
00393  * @param  odr the pointer where the accelerometer output data rate is stored
00394  * @retval IMU_6AXES_OK in case of success, an error code otherwise
00395  */
00396 IMU_6AXES_StatusTypeDef    LSM6DS3::LSM6DS3_X_Get_ODR( float *odr )
00397 {
00398   /*Here we have to add the check if the parameters are valid*/
00399   uint8_t tempReg = 0x00;
00400   
00401   if(LSM6DS3_IO_Read( &tempReg, LSM6DS3_XG_CTRL1_XL, 1 ) != IMU_6AXES_OK)
00402   {
00403     return IMU_6AXES_ERROR;
00404   }
00405   
00406   tempReg &= LSM6DS3_XL_ODR_MASK;
00407   
00408   switch( tempReg )
00409   {
00410     case LSM6DS3_XL_ODR_PD:
00411       *odr = 0.0f;
00412       break;
00413     case LSM6DS3_XL_ODR_13HZ:
00414       *odr = 13.0f;
00415       break;
00416     case LSM6DS3_XL_ODR_26HZ:
00417       *odr = 26.0f;
00418       break;
00419     case LSM6DS3_XL_ODR_52HZ:
00420       *odr = 52.0f;
00421       break;
00422     case LSM6DS3_XL_ODR_104HZ:
00423       *odr = 104.0f;
00424       break;
00425     case LSM6DS3_XL_ODR_208HZ:
00426       *odr = 208.0f;
00427       break;
00428     case LSM6DS3_XL_ODR_416HZ:
00429       *odr = 416.0f;
00430       break;
00431     case LSM6DS3_XL_ODR_833HZ:
00432       *odr = 833.0f;
00433       break;
00434     case LSM6DS3_XL_ODR_1660HZ:
00435       *odr = 1660.0f;
00436       break;
00437     case LSM6DS3_XL_ODR_3330HZ:
00438       *odr = 3330.0f;
00439       break;
00440     case LSM6DS3_XL_ODR_6660HZ:
00441       *odr = 6660.0f;
00442       break;
00443     default:
00444       break;
00445   }
00446   
00447   return IMU_6AXES_OK;
00448 }
00449 
00450 /**
00451  * @brief  Write Accelero Output Data Rate
00452  * @param  odr the accelerometer output data rate to be set
00453  * @retval IMU_6AXES_OK in case of success, an error code otherwise
00454  */
00455 IMU_6AXES_StatusTypeDef    LSM6DS3::LSM6DS3_X_Set_ODR( float odr )
00456 {
00457   uint8_t new_odr = 0x00;
00458   uint8_t tempReg = 0x00;
00459   
00460   new_odr = ( odr <= 0.0f    ) ? LSM6DS3_XL_ODR_PD          /* Power Down */
00461             : ( odr <= 13.0f   ) ? LSM6DS3_XL_ODR_13HZ
00462             : ( odr <= 26.0f   ) ? LSM6DS3_XL_ODR_26HZ
00463             : ( odr <= 52.0f   ) ? LSM6DS3_XL_ODR_52HZ
00464             : ( odr <= 104.0f  ) ? LSM6DS3_XL_ODR_104HZ
00465             : ( odr <= 208.0f  ) ? LSM6DS3_XL_ODR_208HZ
00466             : ( odr <= 416.0f  ) ? LSM6DS3_XL_ODR_416HZ
00467             : ( odr <= 833.0f  ) ? LSM6DS3_XL_ODR_833HZ
00468             : ( odr <= 1660.0f ) ? LSM6DS3_XL_ODR_1660HZ
00469             : ( odr <= 3330.0f ) ? LSM6DS3_XL_ODR_3330HZ
00470             :                      LSM6DS3_XL_ODR_6660HZ;
00471             
00472   if(LSM6DS3_IO_Read( &tempReg, LSM6DS3_XG_CTRL1_XL, 1 ) != IMU_6AXES_OK)
00473   {
00474     return IMU_6AXES_ERROR;
00475   }
00476   
00477   tempReg &= ~(LSM6DS3_XL_ODR_MASK);
00478   tempReg |= new_odr;
00479   
00480   if(LSM6DS3_IO_Write(&tempReg, LSM6DS3_XG_CTRL1_XL, 1) != IMU_6AXES_OK)
00481   {
00482     return IMU_6AXES_ERROR;
00483   }
00484   
00485   return IMU_6AXES_OK;
00486 }
00487 
00488 /**
00489  * @brief  Read Accelero Sensitivity
00490  * @param  pfData the pointer where the accelerometer sensitivity is stored
00491  * @retval IMU_6AXES_OK in case of success, an error code otherwise
00492  */
00493 IMU_6AXES_StatusTypeDef    LSM6DS3::LSM6DS3_X_GetSensitivity( float *pfData )
00494 {
00495   /*Here we have to add the check if the parameters are valid*/
00496   
00497   uint8_t tempReg = 0x00;
00498   
00499   
00500   if(LSM6DS3_IO_Read( &tempReg, LSM6DS3_XG_CTRL1_XL, 1 ) != IMU_6AXES_OK)
00501   {
00502     return IMU_6AXES_ERROR;
00503   }
00504   
00505   tempReg &= LSM6DS3_XL_FS_MASK;
00506   
00507   switch( tempReg )
00508   {
00509     case LSM6DS3_XL_FS_2G:
00510       *pfData = 0.061f;
00511       break;
00512     case LSM6DS3_XL_FS_4G:
00513       *pfData = 0.122f;
00514       break;
00515     case LSM6DS3_XL_FS_8G:
00516       *pfData = 0.244f;
00517       break;
00518     case LSM6DS3_XL_FS_16G:
00519       *pfData = 0.488f;
00520       break;
00521     default:
00522       break;
00523   }
00524   
00525   return IMU_6AXES_OK;
00526 }
00527 
00528 /**
00529  * @brief  Read Accelero Full Scale
00530  * @param  fullScale the pointer where the accelerometer full scale is stored
00531  * @retval IMU_6AXES_OK in case of success, an error code otherwise
00532  */
00533 IMU_6AXES_StatusTypeDef    LSM6DS3::LSM6DS3_X_Get_FS( float *fullScale )
00534 {
00535   /*Here we have to add the check if the parameters are valid*/
00536   
00537   uint8_t tempReg = 0x00;
00538   
00539   
00540   if(LSM6DS3_IO_Read( &tempReg, LSM6DS3_XG_CTRL1_XL, 1 ) != IMU_6AXES_OK)
00541   {
00542     return IMU_6AXES_ERROR;
00543   }
00544   
00545   tempReg &= LSM6DS3_XL_FS_MASK;
00546   
00547   switch( tempReg )
00548   {
00549     case LSM6DS3_XL_FS_2G:
00550       *fullScale = 2.0f;
00551       break;
00552     case LSM6DS3_XL_FS_4G:
00553       *fullScale = 4.0f;
00554       break;
00555     case LSM6DS3_XL_FS_8G:
00556       *fullScale = 8.0f;
00557       break;
00558     case LSM6DS3_XL_FS_16G:
00559       *fullScale = 16.0f;
00560       break;
00561     default:
00562       break;
00563   }
00564   
00565   return IMU_6AXES_OK;
00566 }
00567 
00568 /**
00569  * @brief  Write Accelero Full Scale
00570  * @param  fullScale the accelerometer full scale to be set
00571  * @retval IMU_6AXES_OK in case of success, an error code otherwise
00572  */
00573 IMU_6AXES_StatusTypeDef    LSM6DS3::LSM6DS3_X_Set_FS( float fullScale )
00574 {
00575   uint8_t new_fs = 0x00;
00576   uint8_t tempReg = 0x00;
00577   
00578   new_fs = ( fullScale <= 2.0f ) ? LSM6DS3_XL_FS_2G
00579            : ( fullScale <= 4.0f ) ? LSM6DS3_XL_FS_4G
00580            : ( fullScale <= 8.0f ) ? LSM6DS3_XL_FS_8G
00581            :                         LSM6DS3_XL_FS_16G;
00582            
00583   if(LSM6DS3_IO_Read( &tempReg, LSM6DS3_XG_CTRL1_XL, 1 ) != IMU_6AXES_OK)
00584   {
00585     return IMU_6AXES_ERROR;
00586   }
00587   
00588   tempReg &= ~(LSM6DS3_XL_FS_MASK);
00589   tempReg |= new_fs;
00590   
00591   if(LSM6DS3_IO_Write(&tempReg, LSM6DS3_XG_CTRL1_XL, 1) != IMU_6AXES_OK)
00592   {
00593     return IMU_6AXES_ERROR;
00594   }
00595   
00596   return IMU_6AXES_OK;
00597 }
00598 
00599 /**
00600  * @brief  Read Gyro Output Data Rate
00601  * @param  odr the pointer where the gyroscope output data rate is stored
00602  * @retval IMU_6AXES_OK in case of success, an error code otherwise
00603  */
00604 IMU_6AXES_StatusTypeDef    LSM6DS3::LSM6DS3_G_Get_ODR( float *odr )
00605 {
00606   /*Here we have to add the check if the parameters are valid*/
00607   uint8_t tempReg = 0x00;
00608   
00609   if(LSM6DS3_IO_Read( &tempReg, LSM6DS3_XG_CTRL2_G, 1 ) != IMU_6AXES_OK)
00610   {
00611     return IMU_6AXES_ERROR;
00612   }
00613   
00614   tempReg &= LSM6DS3_G_ODR_MASK;
00615   
00616   switch( tempReg )
00617   {
00618     case LSM6DS3_G_ODR_PD:
00619       *odr = 0.0f;
00620       break;
00621     case LSM6DS3_G_ODR_13HZ:
00622       *odr = 13.0f;
00623       break;
00624     case LSM6DS3_G_ODR_26HZ:
00625       *odr = 26.0f;
00626       break;
00627     case LSM6DS3_G_ODR_52HZ:
00628       *odr = 52.0f;
00629       break;
00630     case LSM6DS3_G_ODR_104HZ:
00631       *odr = 104.0f;
00632       break;
00633     case LSM6DS3_G_ODR_208HZ:
00634       *odr = 208.0f;
00635       break;
00636     case LSM6DS3_G_ODR_416HZ:
00637       *odr = 416.0f;
00638       break;
00639     case LSM6DS3_G_ODR_833HZ:
00640       *odr = 833.0f;
00641       break;
00642     case LSM6DS3_G_ODR_1660HZ:
00643       *odr = 1660.0f;
00644       break;
00645     default:
00646       break;
00647   }
00648   
00649   return IMU_6AXES_OK;
00650 }
00651 
00652 /**
00653  * @brief  Write Gyro Output Data Rate
00654  * @param  odr the gyroscope output data rate to be set
00655  * @retval IMU_6AXES_OK in case of success, an error code otherwise
00656  */
00657 IMU_6AXES_StatusTypeDef    LSM6DS3::LSM6DS3_G_Set_ODR( float odr )
00658 {
00659   uint8_t new_odr = 0x00;
00660   uint8_t tempReg = 0x00;
00661   
00662   new_odr = ( odr <= 0.0f   ) ? LSM6DS3_G_ODR_PD          /* Power Down */
00663             : ( odr <= 13.0f  ) ? LSM6DS3_G_ODR_13HZ
00664             : ( odr <= 26.0f  ) ? LSM6DS3_G_ODR_26HZ
00665             : ( odr <= 52.0f  ) ? LSM6DS3_G_ODR_52HZ
00666             : ( odr <= 104.0f ) ? LSM6DS3_G_ODR_104HZ
00667             : ( odr <= 208.0f ) ? LSM6DS3_G_ODR_208HZ
00668             : ( odr <= 416.0f ) ? LSM6DS3_G_ODR_416HZ
00669             : ( odr <= 833.0f ) ? LSM6DS3_G_ODR_833HZ
00670             :                     LSM6DS3_G_ODR_1660HZ;
00671             
00672   if(LSM6DS3_IO_Read( &tempReg, LSM6DS3_XG_CTRL2_G, 1 ) != IMU_6AXES_OK)
00673   {
00674     return IMU_6AXES_ERROR;
00675   }
00676   
00677   tempReg &= ~(LSM6DS3_G_ODR_MASK);
00678   tempReg |= new_odr;
00679   
00680   if(LSM6DS3_IO_Write(&tempReg, LSM6DS3_XG_CTRL2_G, 1) != IMU_6AXES_OK)
00681   {
00682     return IMU_6AXES_ERROR;
00683   }
00684   
00685   return IMU_6AXES_OK;
00686 }
00687 
00688 /**
00689  * @brief  Read Gyro Sensitivity
00690  * @param  pfData the pointer where the gyroscope sensitivity is stored
00691  * @retval IMU_6AXES_OK in case of success, an error code otherwise
00692 */
00693 IMU_6AXES_StatusTypeDef    LSM6DS3::LSM6DS3_G_GetSensitivity( float *pfData )
00694 {
00695   /*Here we have to add the check if the parameters are valid*/
00696   
00697   uint8_t tempReg = 0x00;
00698   
00699   if(LSM6DS3_IO_Read( &tempReg, LSM6DS3_XG_CTRL2_G, 1 ) != IMU_6AXES_OK)
00700   {
00701     return IMU_6AXES_ERROR;
00702   }
00703   
00704   tempReg &= LSM6DS3_G_FS_125_MASK;
00705   
00706   if(tempReg == LSM6DS3_G_FS_125_ENABLE)
00707   {
00708     *pfData = 4.375f;
00709   }
00710   else
00711   {
00712     if(LSM6DS3_IO_Read( &tempReg, LSM6DS3_XG_CTRL2_G, 1 ) != IMU_6AXES_OK)
00713     {
00714       return IMU_6AXES_ERROR;
00715     }
00716     
00717     tempReg &= LSM6DS3_G_FS_MASK;
00718     
00719     switch( tempReg )
00720     {
00721       case LSM6DS3_G_FS_245:
00722         *pfData = 8.75f;
00723         break;
00724       case LSM6DS3_G_FS_500:
00725         *pfData = 17.50f;
00726         break;
00727       case LSM6DS3_G_FS_1000:
00728         *pfData = 35.0f;
00729         break;
00730       case LSM6DS3_G_FS_2000:
00731         *pfData = 70.0f;
00732         break;
00733       default:
00734         break;
00735     }
00736   }
00737   
00738   return IMU_6AXES_OK;
00739 }
00740 
00741 /**
00742  * @brief  Read Gyro Full Scale
00743  * @param  fullScale the pointer where the gyroscope full scale is stored
00744  * @retval IMU_6AXES_OK in case of success, an error code otherwise
00745 */
00746 IMU_6AXES_StatusTypeDef    LSM6DS3::LSM6DS3_G_Get_FS( float *fullScale )
00747 {
00748   /*Here we have to add the check if the parameters are valid*/
00749   
00750   uint8_t tempReg = 0x00;
00751   
00752   if(LSM6DS3_IO_Read( &tempReg, LSM6DS3_XG_CTRL2_G, 1 ) != IMU_6AXES_OK)
00753   {
00754     return IMU_6AXES_ERROR;
00755   }
00756   
00757   tempReg &= LSM6DS3_G_FS_125_MASK;
00758   
00759   if(tempReg == LSM6DS3_G_FS_125_ENABLE)
00760   {
00761     *fullScale = 125.0f;
00762   }
00763   else
00764   {
00765     if(LSM6DS3_IO_Read( &tempReg, LSM6DS3_XG_CTRL2_G, 1 ) != IMU_6AXES_OK)
00766     {
00767       return IMU_6AXES_ERROR;
00768     }
00769     
00770     tempReg &= LSM6DS3_G_FS_MASK;
00771     
00772     switch( tempReg )
00773     {
00774       case LSM6DS3_G_FS_245:
00775         *fullScale = 245.0f;
00776         break;
00777       case LSM6DS3_G_FS_500:
00778         *fullScale = 500.0f;
00779         break;
00780       case LSM6DS3_G_FS_1000:
00781         *fullScale = 1000.0f;
00782         break;
00783       case LSM6DS3_G_FS_2000:
00784         *fullScale = 2000.0f;
00785         break;
00786       default:
00787         break;
00788     }
00789   }
00790   
00791   return IMU_6AXES_OK;
00792 }
00793 
00794 /**
00795  * @brief  Write Gyro Full Scale
00796  * @param  fullScale the gyroscope full scale to be set
00797  * @retval IMU_6AXES_OK in case of success, an error code otherwise
00798 */
00799 IMU_6AXES_StatusTypeDef    LSM6DS3::LSM6DS3_G_Set_FS( float fullScale )
00800 {
00801   uint8_t new_fs = 0x00;
00802   uint8_t tempReg = 0x00;
00803   
00804   if(fullScale <= 125.0f)
00805   {
00806     new_fs = LSM6DS3_G_FS_125_ENABLE;
00807     
00808     if(LSM6DS3_IO_Read( &tempReg, LSM6DS3_XG_CTRL2_G, 1 ) != IMU_6AXES_OK)
00809     {
00810       return IMU_6AXES_ERROR;
00811     }
00812     
00813     tempReg &= ~(LSM6DS3_G_FS_125_MASK);
00814     tempReg |= new_fs;
00815     
00816     if(LSM6DS3_IO_Write(&tempReg, LSM6DS3_XG_CTRL2_G, 1) != IMU_6AXES_OK)
00817     {
00818       return IMU_6AXES_ERROR;
00819     }
00820   }
00821   else
00822   {
00823     /* Disable G FS 125dpp  */
00824     if(LSM6DS3_IO_Read( &tempReg, LSM6DS3_XG_CTRL2_G, 1 ) != IMU_6AXES_OK)
00825     {
00826       return IMU_6AXES_ERROR;
00827     }
00828     
00829     tempReg &= ~(LSM6DS3_G_FS_125_MASK);
00830     tempReg |= LSM6DS3_G_FS_125_DISABLE;
00831     
00832     if(LSM6DS3_IO_Write(&tempReg, LSM6DS3_XG_CTRL2_G, 1) != IMU_6AXES_OK)
00833     {
00834       return IMU_6AXES_ERROR;
00835     }
00836     
00837     new_fs = ( fullScale <= 245.0f )  ? LSM6DS3_G_FS_245
00838              : ( fullScale <= 500.0f )  ? LSM6DS3_G_FS_500
00839              : ( fullScale <= 1000.0f ) ? LSM6DS3_G_FS_1000
00840              :                            LSM6DS3_G_FS_2000;
00841              
00842     if(LSM6DS3_IO_Read( &tempReg, LSM6DS3_XG_CTRL2_G, 1 ) != IMU_6AXES_OK)
00843     {
00844       return IMU_6AXES_ERROR;
00845     }
00846     
00847     tempReg &= ~(LSM6DS3_G_FS_MASK);
00848     tempReg |= new_fs;
00849     
00850     if(LSM6DS3_IO_Write(&tempReg, LSM6DS3_XG_CTRL2_G, 1) != IMU_6AXES_OK)
00851     {
00852       return IMU_6AXES_ERROR;
00853     }
00854   }
00855   
00856   return IMU_6AXES_OK;
00857 }
00858 
00859 /**
00860  * @brief  Enable free fall detection
00861  * @retval IMU_6AXES_OK in case of success, an error code otherwise
00862 */
00863 IMU_6AXES_StatusTypeDef    LSM6DS3::LSM6DS3_Enable_Free_Fall_Detection( void )
00864 {
00865   uint8_t tmp1 = 0x00;
00866   
00867   if(LSM6DS3_IO_Read(&tmp1, LSM6DS3_XG_CTRL1_XL, 1) != IMU_6AXES_OK)
00868   {
00869     return IMU_6AXES_ERROR;
00870   }
00871   
00872   /* Output Data Rate selection */
00873   tmp1 &= ~(LSM6DS3_XL_ODR_MASK);
00874   tmp1 |= LSM6DS3_XL_ODR_416HZ;
00875   
00876   /* Full scale selection */
00877   tmp1 &= ~(LSM6DS3_XL_FS_MASK);
00878   tmp1 |= LSM6DS3_XL_FS_2G;
00879   
00880   if(LSM6DS3_IO_Write(&tmp1, LSM6DS3_XG_CTRL1_XL, 1) != IMU_6AXES_OK)
00881   {
00882     return IMU_6AXES_ERROR;
00883   }
00884   
00885   if(LSM6DS3_IO_Read(&tmp1, LSM6DS3_XG_WAKE_UP_DUR, 1) != IMU_6AXES_OK)
00886   {
00887     return IMU_6AXES_ERROR;
00888   }
00889   
00890   /* FF_DUR5 setting */
00891   tmp1 &= ~(LSM6DS3_XG_WAKE_UP_DUR_FF_DUR5_MASK);
00892   tmp1 |= LSM6DS3_XG_WAKE_UP_DUR_FF_DUR5_DEFAULT;
00893   
00894   /* WAKE_DUR setting */
00895   tmp1 &= ~(LSM6DS3_XG_WAKE_UP_DUR_WAKE_DUR_MASK);
00896   tmp1 |= LSM6DS3_XG_WAKE_UP_DUR_WAKE_DUR_DEFAULT;
00897   
00898   /* TIMER_HR setting */
00899   tmp1 &= ~(LSM6DS3_XG_WAKE_UP_DUR_TIMER_HR_MASK);
00900   tmp1 |= LSM6DS3_XG_WAKE_UP_DUR_TIMER_HR_DEFAULT;
00901   
00902   /* SLEEP_DUR setting */
00903   tmp1 &= ~(LSM6DS3_XG_WAKE_UP_DUR_SLEEP_DUR_MASK);
00904   tmp1 |= LSM6DS3_XG_WAKE_UP_DUR_SLEEP_DUR_DEFAULT;
00905   
00906   if(LSM6DS3_IO_Write(&tmp1, LSM6DS3_XG_WAKE_UP_DUR, 1) != IMU_6AXES_OK)
00907   {
00908     return IMU_6AXES_ERROR;
00909   }
00910   
00911   if(LSM6DS3_IO_Read(&tmp1, LSM6DS3_XG_WAKE_FREE_FALL, 1) != IMU_6AXES_OK)
00912   {
00913     return IMU_6AXES_ERROR;
00914   }
00915   
00916   /* FF_DUR setting */
00917   tmp1 &= ~(LSM6DS3_XG_WAKE_FREE_FALL_FF_DUR_MASK);
00918   tmp1 |= LSM6DS3_XG_WAKE_FREE_FALL_FF_DUR_TYPICAL;
00919   
00920   /* FF_THS setting */
00921   tmp1 &= ~(LSM6DS3_XG_WAKE_FREE_FALL_FF_THS_MASK);
00922   tmp1 |= LSM6DS3_XG_WAKE_FREE_FALL_FF_THS_312MG;
00923   
00924   if(LSM6DS3_IO_Write(&tmp1, LSM6DS3_XG_WAKE_FREE_FALL, 1) != IMU_6AXES_OK)
00925   {
00926     return IMU_6AXES_ERROR;
00927   }
00928   
00929   if(LSM6DS3_IO_Read(&tmp1, LSM6DS3_XG_MD1_CFG, 1) != IMU_6AXES_OK)
00930   {
00931     return IMU_6AXES_ERROR;
00932   }
00933   
00934   /* INT1_FF setting */
00935   tmp1 &= ~(LSM6DS3_XG_MD1_CFG_INT1_FF_MASK);
00936   tmp1 |= LSM6DS3_XG_MD1_CFG_INT1_FF_ENABLE;
00937   
00938   if(LSM6DS3_IO_Write(&tmp1, LSM6DS3_XG_MD1_CFG, 1) != IMU_6AXES_OK)
00939   {
00940     return IMU_6AXES_ERROR;
00941   }
00942   
00943   return IMU_6AXES_OK;
00944 }
00945 
00946 /**
00947  * @brief  Disable free fall detection
00948  * @retval IMU_6AXES_OK in case of success, an error code otherwise
00949 */
00950 IMU_6AXES_StatusTypeDef    LSM6DS3::LSM6DS3_Disable_Free_Fall_Detection( void )
00951 {
00952   uint8_t tmp1 = 0x00;
00953   
00954   if(LSM6DS3_IO_Read(&tmp1, LSM6DS3_XG_MD1_CFG, 1) != IMU_6AXES_OK)
00955   {
00956     return IMU_6AXES_ERROR;
00957   }
00958   
00959   /* INT1_FF setting */
00960   tmp1 &= ~(LSM6DS3_XG_MD1_CFG_INT1_FF_MASK);
00961   tmp1 |= LSM6DS3_XG_MD1_CFG_INT1_FF_DISABLE;
00962   
00963   if(LSM6DS3_IO_Write(&tmp1, LSM6DS3_XG_MD1_CFG, 1) != IMU_6AXES_OK)
00964   {
00965     return IMU_6AXES_ERROR;
00966   }
00967   
00968   if(LSM6DS3_IO_Read(&tmp1, LSM6DS3_XG_WAKE_FREE_FALL, 1) != IMU_6AXES_OK)
00969   {
00970     return IMU_6AXES_ERROR;
00971   }
00972   
00973   /* FF_DUR setting */
00974   tmp1 &= ~(LSM6DS3_XG_WAKE_FREE_FALL_FF_DUR_MASK);
00975   tmp1 |= LSM6DS3_XG_WAKE_FREE_FALL_FF_DUR_DEFAULT;
00976   
00977   /* FF_THS setting */
00978   tmp1 &= ~(LSM6DS3_XG_WAKE_FREE_FALL_FF_THS_MASK);
00979   tmp1 |= LSM6DS3_XG_WAKE_FREE_FALL_FF_THS_156MG;
00980   
00981   if(LSM6DS3_IO_Write(&tmp1, LSM6DS3_XG_WAKE_FREE_FALL, 1) != IMU_6AXES_OK)
00982   {
00983     return IMU_6AXES_ERROR;
00984   }
00985   
00986   return IMU_6AXES_OK;
00987 }
00988 
00989 /**
00990  * @brief  Get status of free fall detection
00991  * @param  status the pointer where the status of free fall detection is stored; 0 means no detection, 1 means detection happened
00992  * @retval IMU_6AXES_OK in case of success, an error code otherwise
00993 */
00994 IMU_6AXES_StatusTypeDef    LSM6DS3::LSM6DS3_Get_Status_Free_Fall_Detection( uint8_t *status )
00995 {
00996   uint8_t tmp1 = 0x00;
00997   
00998   if(LSM6DS3_IO_Read(&tmp1, LSM6DS3_XG_WAKE_UP_SRC, 1) != IMU_6AXES_OK)
00999   {
01000     return IMU_6AXES_ERROR;
01001   }
01002   
01003   tmp1 &= LSM6DS3_XG_WAKE_UP_SRC_FF_IA_MASK;
01004   
01005   switch( tmp1 )
01006   {
01007     case LSM6DS3_XG_WAKE_UP_SRC_FF_IA_ENABLE:
01008       *status = 1;
01009       break;
01010     case LSM6DS3_XG_WAKE_UP_SRC_FF_IA_DISABLE:
01011     default:
01012       *status = 0;
01013       break;
01014   }
01015   
01016   return IMU_6AXES_OK;
01017 }
01018 
01019 /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/