lsm6ds0_class.cpp

00001 /**
00002  ******************************************************************************
00003  * @file    lsm6ds0_class.cpp
00004  * @author  AST / EST
00005  * @version V0.0.1
00006  * @date    14-April-2015
00007  * @brief   Implementation file for the LSM6DS0 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 "lsm6ds0_class.h"
00040 #include "lsm6ds0.h"
00041 
00042 /* Methods -------------------------------------------------------------------*/
00043 /* betzw - based on:
00044            X-CUBE-MEMS1/trunk/Drivers/BSP/Components/lsm6ds0/lsm6ds0.c: revision #400,
00045            X-CUBE-MEMS1/trunk: revision #416
00046 */
00047 
00048 /**
00049  * @brief  Set LSM6DS0 Initialization
00050  * @param  LSM6DS0_Init the configuration setting for the LSM6DS0
00051  * @retval IMU_6AXES_OK in case of success, an error code otherwise
00052  */
00053 IMU_6AXES_StatusTypeDef LSM6DS0::LSM6DS0_Init(IMU_6AXES_InitTypeDef *LSM6DS0_Init)
00054 {
00055   /* Configure the low level interface ---------------------------------------*/
00056   if(LSM6DS0_IO_Init() != IMU_6AXES_OK)
00057   {
00058     return IMU_6AXES_ERROR;
00059   }
00060   
00061   /******* Gyroscope init *******/
00062   
00063   if(LSM6DS0_G_Set_ODR( LSM6DS0_Init->G_OutputDataRate ) != IMU_6AXES_OK)
00064   {
00065     return IMU_6AXES_ERROR;
00066   }
00067   
00068   if(LSM6DS0_G_Set_FS( LSM6DS0_Init->G_FullScale ) != IMU_6AXES_OK)
00069   {
00070     return IMU_6AXES_ERROR;
00071   }
00072   
00073   if(LSM6DS0_G_Set_Axes_Status(LSM6DS0_Init->G_X_Axis, LSM6DS0_Init->G_Y_Axis, LSM6DS0_Init->G_Z_Axis) != IMU_6AXES_OK)
00074   {
00075     return IMU_6AXES_ERROR;
00076   }
00077   
00078   /******************************/
00079   
00080   /***** Accelerometer init *****/
00081   
00082   if(LSM6DS0_X_Set_ODR( LSM6DS0_Init->X_OutputDataRate ) != IMU_6AXES_OK)
00083   {
00084     return IMU_6AXES_ERROR;
00085   }
00086   
00087   if(LSM6DS0_X_Set_FS( LSM6DS0_Init->X_FullScale ) != IMU_6AXES_OK)
00088   {
00089     return IMU_6AXES_ERROR;
00090   }
00091   
00092   if(LSM6DS0_X_Set_Axes_Status(LSM6DS0_Init->X_X_Axis, LSM6DS0_Init->X_Y_Axis, LSM6DS0_Init->X_Z_Axis) != IMU_6AXES_OK)
00093   {
00094     return IMU_6AXES_ERROR;
00095   }
00096   
00097   /* Configure interrupt lines */
00098   LSM6DS0_IO_ITConfig();
00099   
00100   return IMU_6AXES_OK;
00101   
00102   /******************************/
00103 }
00104 
00105 
00106 /**
00107  * @brief  Read ID of LSM6DS0 Accelerometer and Gyroscope
00108  * @param  xg_id the pointer where the ID of the device is stored
00109  * @retval IMU_6AXES_OK in case of success, an error code otherwise
00110  */
00111 IMU_6AXES_StatusTypeDef LSM6DS0::LSM6DS0_Read_XG_ID(uint8_t *xg_id)
00112 {
00113   if(!xg_id)
00114   {
00115     return IMU_6AXES_ERROR;
00116   }
00117   
00118   return LSM6DS0_IO_Read(xg_id, LSM6DS0_XG_WHO_AM_I_ADDR, 1);
00119 }
00120 
00121 
00122 /**
00123  * @brief  Read raw data from LSM6DS0 Accelerometer output register
00124  * @param  pData the pointer where the accelerometer raw data are stored
00125  * @retval IMU_6AXES_OK in case of success, an error code otherwise
00126  */
00127 IMU_6AXES_StatusTypeDef LSM6DS0::LSM6DS0_X_GetAxesRaw(int16_t *pData)
00128 {
00129   uint8_t tempReg[2] = {0, 0};
00130   
00131   if(LSM6DS0_IO_Read(&tempReg[0], (LSM6DS0_XG_OUT_X_L_XL | LSM6DS0_I2C_MULTIPLEBYTE_CMD),
00132                      2) != IMU_6AXES_OK)
00133   {
00134     return IMU_6AXES_ERROR;
00135   }
00136   
00137   pData[0] = ((((int16_t)tempReg[1]) << 8) + (int16_t)tempReg[0]);
00138   
00139   if(LSM6DS0_IO_Read(&tempReg[0], (LSM6DS0_XG_OUT_Y_L_XL | LSM6DS0_I2C_MULTIPLEBYTE_CMD),
00140                      2) != IMU_6AXES_OK)
00141   {
00142     return IMU_6AXES_ERROR;
00143   }
00144   
00145   pData[1] = ((((int16_t)tempReg[1]) << 8) + (int16_t)tempReg[0]);
00146   
00147   if(LSM6DS0_IO_Read(&tempReg[0], (LSM6DS0_XG_OUT_Z_L_XL | LSM6DS0_I2C_MULTIPLEBYTE_CMD),
00148                      2) != IMU_6AXES_OK)
00149   {
00150     return IMU_6AXES_ERROR;
00151   }
00152   
00153   pData[2] = ((((int16_t)tempReg[1]) << 8) + (int16_t)tempReg[0]);
00154   
00155   return IMU_6AXES_OK;
00156 }
00157 
00158 
00159 /**
00160  * @brief  Read data from LSM6DS0 Accelerometer and calculate linear acceleration in mg
00161  * @param  pData the pointer where the accelerometer data are stored
00162  * @retval IMU_6AXES_OK in case of success, an error code otherwise
00163  */
00164 IMU_6AXES_StatusTypeDef LSM6DS0::LSM6DS0_X_GetAxes(int32_t *pData)
00165 {
00166   int16_t pDataRaw[3];
00167   float sensitivity = 0;
00168   
00169   if(LSM6DS0_X_GetAxesRaw(pDataRaw) != IMU_6AXES_OK)
00170   {
00171     return IMU_6AXES_ERROR;
00172   }
00173   
00174   if(LSM6DS0_X_GetSensitivity( &sensitivity ) != IMU_6AXES_OK)
00175   {
00176     return IMU_6AXES_ERROR;
00177   }
00178   
00179   pData[0] = (int32_t)(pDataRaw[0] * sensitivity);
00180   pData[1] = (int32_t)(pDataRaw[1] * sensitivity);
00181   pData[2] = (int32_t)(pDataRaw[2] * sensitivity);
00182   
00183   return IMU_6AXES_OK;
00184 }
00185 
00186 
00187 /**
00188  * @brief  Read raw data from LSM6DS0 Gyroscope output register
00189  * @param  pData the pointer where the gyroscope raw data are stored
00190  * @retval IMU_6AXES_OK in case of success, an error code otherwise
00191  */
00192 IMU_6AXES_StatusTypeDef LSM6DS0::LSM6DS0_G_GetAxesRaw(int16_t *pData)
00193 {
00194   uint8_t tempReg[2] = {0, 0};
00195   
00196   if(LSM6DS0_IO_Read(&tempReg[0], (LSM6DS0_XG_OUT_X_L_G | LSM6DS0_I2C_MULTIPLEBYTE_CMD),
00197                      2) != IMU_6AXES_OK)
00198   {
00199     return IMU_6AXES_ERROR;
00200   }
00201   
00202   pData[0] = ((((int16_t)tempReg[1]) << 8) + (int16_t)tempReg[0]);
00203   
00204   if(LSM6DS0_IO_Read(&tempReg[0], (LSM6DS0_XG_OUT_Y_L_G | LSM6DS0_I2C_MULTIPLEBYTE_CMD),
00205                      2) != IMU_6AXES_OK)
00206   {
00207     return IMU_6AXES_ERROR;
00208   }
00209   
00210   pData[1] = ((((int16_t)tempReg[1]) << 8) + (int16_t)tempReg[0]);
00211   
00212   if(LSM6DS0_IO_Read(&tempReg[0], (LSM6DS0_XG_OUT_Z_L_G | LSM6DS0_I2C_MULTIPLEBYTE_CMD),
00213                      2) != IMU_6AXES_OK)
00214   {
00215     return IMU_6AXES_ERROR;
00216   }
00217   
00218   pData[2] = ((((int16_t)tempReg[1]) << 8) + (int16_t)tempReg[0]);
00219   
00220   return IMU_6AXES_OK;
00221 }
00222 
00223 /**
00224  * @brief  Set the status of the axes for accelerometer
00225  * @param  enableX the status of the x axis to be set
00226  * @param  enableY the status of the y axis to be set
00227  * @param  enableZ the status of the z axis to be set
00228  * @retval IMU_6AXES_OK in case of success, an error code otherwise
00229  */
00230 IMU_6AXES_StatusTypeDef LSM6DS0::LSM6DS0_X_Set_Axes_Status(uint8_t enableX, uint8_t enableY, uint8_t enableZ)
00231 {
00232   uint8_t tmp1 = 0x00;
00233   uint8_t eX = 0x00;
00234   uint8_t eY = 0x00;
00235   uint8_t eZ = 0x00;
00236   
00237   eX = ( enableX == 0 ) ? LSM6DS0_XL_XEN_DISABLE : LSM6DS0_XL_XEN_ENABLE;
00238   eY = ( enableY == 0 ) ? LSM6DS0_XL_YEN_DISABLE : LSM6DS0_XL_YEN_ENABLE;
00239   eZ = ( enableZ == 0 ) ? LSM6DS0_XL_ZEN_DISABLE : LSM6DS0_XL_ZEN_ENABLE;
00240   
00241   if(LSM6DS0_IO_Read(&tmp1, LSM6DS0_XG_CTRL_REG5_XL, 1) != IMU_6AXES_OK)
00242   {
00243     return IMU_6AXES_ERROR;
00244   }
00245   
00246   /* Enable X axis selection */
00247   tmp1 &= ~(LSM6DS0_XL_XEN_MASK);
00248   tmp1 |= eX;
00249   
00250   /* Enable Y axis selection */
00251   tmp1 &= ~(LSM6DS0_XL_YEN_MASK);
00252   tmp1 |= eY;
00253   
00254   /* Enable Z axis selection */
00255   tmp1 &= ~(LSM6DS0_XL_ZEN_MASK);
00256   tmp1 |= eZ;
00257   
00258   if(LSM6DS0_IO_Write(&tmp1, LSM6DS0_XG_CTRL_REG5_XL, 1) != IMU_6AXES_OK)
00259   {
00260     return IMU_6AXES_ERROR;
00261   }
00262   
00263   return IMU_6AXES_OK;
00264 }
00265 
00266 /**
00267  * @brief  Set the status of the axes for gyroscope
00268  * @param  enableX the status of the x axis to be set
00269  * @param  enableY the status of the y axis to be set
00270  * @param  enableZ the status of the z axis to be set
00271  * @retval IMU_6AXES_OK in case of success, an error code otherwise
00272  */
00273 IMU_6AXES_StatusTypeDef LSM6DS0::LSM6DS0_G_Set_Axes_Status(uint8_t enableX, uint8_t enableY, uint8_t enableZ)
00274 {
00275   uint8_t tmp1 = 0x00;
00276   uint8_t eX = 0x00;
00277   uint8_t eY = 0x00;
00278   uint8_t eZ = 0x00;
00279   
00280   eX = ( enableX == 0 ) ? LSM6DS0_G_XEN_DISABLE : LSM6DS0_G_XEN_ENABLE;
00281   eY = ( enableY == 0 ) ? LSM6DS0_G_YEN_DISABLE : LSM6DS0_G_YEN_ENABLE;
00282   eZ = ( enableZ == 0 ) ? LSM6DS0_G_ZEN_DISABLE : LSM6DS0_G_ZEN_ENABLE;
00283   
00284   if(LSM6DS0_IO_Read(&tmp1, LSM6DS0_XG_CTRL_REG4, 1) != IMU_6AXES_OK)
00285   {
00286     return IMU_6AXES_ERROR;
00287   }
00288   
00289   /* Enable X axis selection */
00290   tmp1 &= ~(LSM6DS0_G_XEN_MASK);
00291   tmp1 |= eX;
00292   
00293   /* Enable Y axis selection */
00294   tmp1 &= ~(LSM6DS0_G_YEN_MASK);
00295   tmp1 |= eY;
00296   
00297   /* Enable Z axis selection */
00298   tmp1 &= ~(LSM6DS0_G_ZEN_MASK);
00299   tmp1 |= eZ;
00300   
00301   if(LSM6DS0_IO_Write(&tmp1, LSM6DS0_XG_CTRL_REG4, 1) != IMU_6AXES_OK)
00302   {
00303     return IMU_6AXES_ERROR;
00304   }
00305   
00306   return IMU_6AXES_OK;
00307 }
00308 
00309 
00310 /**
00311  * @brief  Read data from LSM6DS0 Gyroscope and calculate angular rate in mdps
00312  * @param  pData the pointer where the gyroscope data are stored
00313  * @retval IMU_6AXES_OK in case of success, an error code otherwise
00314  */
00315 IMU_6AXES_StatusTypeDef LSM6DS0::LSM6DS0_G_GetAxes(int32_t *pData)
00316 {
00317   int16_t pDataRaw[3];
00318   float sensitivity = 0;
00319   
00320   if(LSM6DS0_G_GetAxesRaw(pDataRaw) != IMU_6AXES_OK)
00321   {
00322     return IMU_6AXES_ERROR;
00323   }
00324   
00325   if(LSM6DS0_G_GetSensitivity( &sensitivity ) != IMU_6AXES_OK)
00326   {
00327     return IMU_6AXES_ERROR;
00328   }
00329   
00330   pData[0] = (int32_t)(pDataRaw[0] * sensitivity);
00331   pData[1] = (int32_t)(pDataRaw[1] * sensitivity);
00332   pData[2] = (int32_t)(pDataRaw[2] * sensitivity);
00333   
00334   return IMU_6AXES_OK;
00335 }
00336 
00337 /**
00338  * @brief  Read Accelero Output Data Rate
00339  * @param  odr the pointer where the accelerometer output data rate is stored
00340  * @retval IMU_6AXES_OK in case of success, an error code otherwise
00341  */
00342 IMU_6AXES_StatusTypeDef    LSM6DS0::LSM6DS0_X_Get_ODR( float *odr )
00343 {
00344   /*Here we have to add the check if the parameters are valid*/
00345   uint8_t tempReg = 0x00;
00346   
00347   if(LSM6DS0_IO_Read( &tempReg, LSM6DS0_XG_CTRL_REG6_XL, 1 ) != IMU_6AXES_OK)
00348   {
00349     return IMU_6AXES_ERROR;
00350   }
00351   
00352   tempReg &= LSM6DS0_XL_ODR_MASK;
00353   
00354   switch( tempReg )
00355   {
00356     case LSM6DS0_XL_ODR_PD:
00357       *odr = 0.0f;
00358       break;
00359     case LSM6DS0_XL_ODR_10HZ:
00360       *odr = 10.0f;
00361       break;
00362     case LSM6DS0_XL_ODR_50HZ:
00363       *odr = 50.0f;
00364       break;
00365     case LSM6DS0_XL_ODR_119HZ:
00366       *odr = 119.0f;
00367       break;
00368     case LSM6DS0_XL_ODR_238HZ:
00369       *odr = 238.0f;
00370       break;
00371     case LSM6DS0_XL_ODR_476HZ:
00372       *odr = 476.0f;
00373       break;
00374     case LSM6DS0_XL_ODR_952HZ:
00375       *odr = 952.0f;
00376       break;
00377     default:
00378       break;
00379   }
00380   
00381   return IMU_6AXES_OK;
00382 }
00383 
00384 /**
00385  * @brief  Write Accelero Output Data Rate
00386  * @param  odr the accelerometer output data rate to be set
00387  * @retval IMU_6AXES_OK in case of success, an error code otherwise
00388  */
00389 IMU_6AXES_StatusTypeDef    LSM6DS0::LSM6DS0_X_Set_ODR( float odr )
00390 {
00391   uint8_t new_odr = 0x00;
00392   uint8_t tempReg = 0x00;
00393   
00394   new_odr = ( odr <= 0.0f   ) ? LSM6DS0_XL_ODR_PD          /* Power Down */
00395             : ( odr <= 10.0f  ) ? LSM6DS0_XL_ODR_10HZ
00396             : ( odr <= 50.0f  ) ? LSM6DS0_XL_ODR_50HZ
00397             : ( odr <= 119.0f ) ? LSM6DS0_XL_ODR_119HZ
00398             : ( odr <= 238.0f ) ? LSM6DS0_XL_ODR_238HZ
00399             : ( odr <= 476.0f ) ? LSM6DS0_XL_ODR_476HZ
00400             :                     LSM6DS0_XL_ODR_952HZ;
00401             
00402   if(LSM6DS0_IO_Read( &tempReg, LSM6DS0_XG_CTRL_REG6_XL, 1 ) != IMU_6AXES_OK)
00403   {
00404     return IMU_6AXES_ERROR;
00405   }
00406   
00407   tempReg &= ~(LSM6DS0_XL_ODR_MASK);
00408   tempReg |= new_odr;
00409   
00410   if(LSM6DS0_IO_Write(&tempReg, LSM6DS0_XG_CTRL_REG6_XL, 1) != IMU_6AXES_OK)
00411   {
00412     return IMU_6AXES_ERROR;
00413   }
00414   
00415   return IMU_6AXES_OK;
00416 }
00417 
00418 /**
00419  * @brief  Read Accelero Sensitivity
00420  * @param  pfData the pointer where the accelerometer sensitivity is stored
00421  * @retval IMU_6AXES_OK in case of success, an error code otherwise
00422  */
00423 IMU_6AXES_StatusTypeDef    LSM6DS0::LSM6DS0_X_GetSensitivity( float *pfData )
00424 {
00425   /*Here we have to add the check if the parameters are valid*/
00426   uint8_t tempReg = 0x00;
00427   
00428   if(LSM6DS0_IO_Read( &tempReg, LSM6DS0_XG_CTRL_REG6_XL, 1 ) != IMU_6AXES_OK)
00429   {
00430     return IMU_6AXES_ERROR;
00431   }
00432   
00433   tempReg &= LSM6DS0_XL_FS_MASK;
00434   
00435   switch( tempReg )
00436   {
00437     case LSM6DS0_XL_FS_2G:
00438       *pfData = 0.061f;
00439       break;
00440     case LSM6DS0_XL_FS_4G:
00441       *pfData = 0.122f;
00442       break;
00443     case LSM6DS0_XL_FS_8G:
00444       *pfData = 0.244f;
00445       break;
00446     case LSM6DS0_XL_FS_16G:
00447       *pfData = 0.732f;
00448       break;
00449     default:
00450       break;
00451   }
00452   
00453   return IMU_6AXES_OK;
00454 }
00455 
00456 /**
00457  * @brief  Read Accelero Full Scale
00458  * @param  fullScale the pointer where the accelerometer full scale is stored
00459  * @retval IMU_6AXES_OK in case of success, an error code otherwise
00460  */
00461 IMU_6AXES_StatusTypeDef    LSM6DS0::LSM6DS0_X_Get_FS( float *fullScale )
00462 {
00463   /*Here we have to add the check if the parameters are valid*/
00464   uint8_t tempReg = 0x00;
00465   
00466   if(LSM6DS0_IO_Read( &tempReg, LSM6DS0_XG_CTRL_REG6_XL, 1 ) != IMU_6AXES_OK)
00467   {
00468     return IMU_6AXES_ERROR;
00469   }
00470   
00471   tempReg &= LSM6DS0_XL_FS_MASK;
00472   
00473   switch( tempReg )
00474   {
00475     case LSM6DS0_XL_FS_2G:
00476       *fullScale = 2.0f;
00477       break;
00478     case LSM6DS0_XL_FS_4G:
00479       *fullScale = 4.0f;
00480       break;
00481     case LSM6DS0_XL_FS_8G:
00482       *fullScale = 8.0f;
00483       break;
00484     case LSM6DS0_XL_FS_16G:
00485       *fullScale = 16.0f;
00486       break;
00487     default:
00488       break;
00489   }
00490   
00491   return IMU_6AXES_OK;
00492 }
00493 
00494 /**
00495  * @brief  Write Accelero Full Scale
00496  * @param  fullScale the accelerometer full scale to be set
00497  * @retval IMU_6AXES_OK in case of success, an error code otherwise
00498  */
00499 IMU_6AXES_StatusTypeDef    LSM6DS0::LSM6DS0_X_Set_FS( float fullScale )
00500 {
00501   uint8_t new_fs = 0x00;
00502   uint8_t tempReg = 0x00;
00503   
00504   new_fs = ( fullScale <= 2.0f ) ? LSM6DS0_XL_FS_2G
00505            : ( fullScale <= 4.0f ) ? LSM6DS0_XL_FS_4G
00506            : ( fullScale <= 8.0f ) ? LSM6DS0_XL_FS_8G
00507            :                         LSM6DS0_XL_FS_16G;
00508            
00509   if(LSM6DS0_IO_Read( &tempReg, LSM6DS0_XG_CTRL_REG6_XL, 1 ) != IMU_6AXES_OK)
00510   {
00511     return IMU_6AXES_ERROR;
00512   }
00513   
00514   tempReg &= ~(LSM6DS0_XL_FS_MASK);
00515   tempReg |= new_fs;
00516   
00517   if(LSM6DS0_IO_Write(&tempReg, LSM6DS0_XG_CTRL_REG6_XL, 1) != IMU_6AXES_OK)
00518   {
00519     return IMU_6AXES_ERROR;
00520   }
00521   
00522   return IMU_6AXES_OK;
00523 }
00524 
00525 /**
00526  * @brief  Read Gyro Output Data Rate
00527  * @param  odr the pointer where the gyroscope output data rate is stored
00528  * @retval IMU_6AXES_OK in case of success, an error code otherwise
00529  */
00530 IMU_6AXES_StatusTypeDef    LSM6DS0::LSM6DS0_G_Get_ODR( float *odr )
00531 {
00532   /*Here we have to add the check if the parameters are valid*/
00533   uint8_t tempReg = 0x00;
00534   
00535   if(LSM6DS0_IO_Read( &tempReg, LSM6DS0_XG_CTRL_REG1_G, 1 ) != IMU_6AXES_OK)
00536   {
00537     return IMU_6AXES_ERROR;
00538   }
00539   
00540   tempReg &= LSM6DS0_G_ODR_MASK;
00541   
00542   switch( tempReg )
00543   {
00544     case LSM6DS0_G_ODR_PD:
00545       *odr = 0.0f;
00546       break;
00547     case LSM6DS0_G_ODR_14_9HZ:
00548       *odr = 14.9f;
00549       break;
00550     case LSM6DS0_G_ODR_59_5HZ:
00551       *odr = 59.5f;
00552       break;
00553     case LSM6DS0_G_ODR_119HZ:
00554       *odr = 119.0f;
00555       break;
00556     case LSM6DS0_G_ODR_238HZ:
00557       *odr = 238.0f;
00558       break;
00559     case LSM6DS0_G_ODR_476HZ:
00560       *odr = 476.0f;
00561       break;
00562     case LSM6DS0_G_ODR_952HZ:
00563       *odr = 952.0f;
00564       break;
00565     default:
00566       break;
00567   }
00568   
00569   return IMU_6AXES_OK;
00570 }
00571 
00572 /**
00573  * @brief  Write Gyro Output Data Rate
00574  * @param  odr the gyroscope output data rate to be set
00575  * @retval IMU_6AXES_OK in case of success, an error code otherwise
00576  */
00577 IMU_6AXES_StatusTypeDef    LSM6DS0::LSM6DS0_G_Set_ODR( float odr )
00578 {
00579   uint8_t new_odr = 0x00;
00580   uint8_t tempReg = 0x00;
00581   
00582   new_odr = ( odr <= 0.0f   ) ? LSM6DS0_G_ODR_PD          /* Power Down */
00583             : ( odr <= 14.9f  ) ? LSM6DS0_G_ODR_14_9HZ
00584             : ( odr <= 59.5f  ) ? LSM6DS0_G_ODR_59_5HZ
00585             : ( odr <= 119.0f ) ? LSM6DS0_G_ODR_119HZ
00586             : ( odr <= 238.0f ) ? LSM6DS0_G_ODR_238HZ
00587             : ( odr <= 476.0f ) ? LSM6DS0_G_ODR_476HZ
00588             :                     LSM6DS0_G_ODR_952HZ;
00589             
00590   if(LSM6DS0_IO_Read( &tempReg, LSM6DS0_XG_CTRL_REG1_G, 1 ) != IMU_6AXES_OK)
00591   {
00592     return IMU_6AXES_ERROR;
00593   }
00594   
00595   tempReg &= ~(LSM6DS0_G_ODR_MASK);
00596   tempReg |= new_odr;
00597   
00598   if(LSM6DS0_IO_Write(&tempReg, LSM6DS0_XG_CTRL_REG1_G, 1) != IMU_6AXES_OK)
00599   {
00600     return IMU_6AXES_ERROR;
00601   }
00602   
00603   return IMU_6AXES_OK;
00604 }
00605 
00606 /**
00607  * @brief  Read Gyro Sensitivity
00608  * @param  pfData the pointer where the gyroscope sensitivity is stored
00609  * @retval IMU_6AXES_OK in case of success, an error code otherwise
00610 */
00611 IMU_6AXES_StatusTypeDef    LSM6DS0::LSM6DS0_G_GetSensitivity( float *pfData )
00612 {
00613   /*Here we have to add the check if the parameters are valid*/
00614   uint8_t tempReg = 0x00;
00615   
00616   if(LSM6DS0_IO_Read( &tempReg, LSM6DS0_XG_CTRL_REG1_G, 1 ) != IMU_6AXES_OK)
00617   {
00618     return IMU_6AXES_ERROR;
00619   }
00620   
00621   tempReg &= LSM6DS0_G_FS_MASK;
00622   
00623   switch( tempReg )
00624   {
00625     case LSM6DS0_G_FS_245:
00626       *pfData = 8.75f;
00627       break;
00628     case LSM6DS0_G_FS_500:
00629       *pfData = 17.50f;
00630       break;
00631     case LSM6DS0_G_FS_2000:
00632       *pfData = 70.0f;
00633       break;
00634     default:
00635       break;
00636   }
00637   
00638   return IMU_6AXES_OK;
00639 }
00640 
00641 /**
00642  * @brief  Read Gyro Full Scale
00643  * @param  fullScale the pointer where the gyroscope full scale is stored
00644  * @retval IMU_6AXES_OK in case of success, an error code otherwise
00645 */
00646 IMU_6AXES_StatusTypeDef    LSM6DS0::LSM6DS0_G_Get_FS( float *fullScale )
00647 {
00648   /*Here we have to add the check if the parameters are valid*/
00649   uint8_t tempReg = 0x00;
00650   
00651   if(LSM6DS0_IO_Read( &tempReg, LSM6DS0_XG_CTRL_REG1_G, 1 ) != IMU_6AXES_OK)
00652   {
00653     return IMU_6AXES_ERROR;
00654   }
00655   
00656   tempReg &= LSM6DS0_G_FS_MASK;
00657   
00658   switch( tempReg )
00659   {
00660     case LSM6DS0_G_FS_245:
00661       *fullScale = 245.0f;
00662       break;
00663     case LSM6DS0_G_FS_500:
00664       *fullScale = 500.0f;
00665       break;
00666     case LSM6DS0_G_FS_2000:
00667       *fullScale = 2000.0f;
00668       break;
00669     default:
00670       break;
00671   }
00672   
00673   return IMU_6AXES_OK;
00674 }
00675 
00676 /**
00677  * @brief  Write Gyro Full Scale
00678  * @param  fullScale the gyroscope full scale to be set
00679  * @retval IMU_6AXES_OK in case of success, an error code otherwise
00680 */
00681 IMU_6AXES_StatusTypeDef    LSM6DS0::LSM6DS0_G_Set_FS( float fullScale )
00682 {
00683   uint8_t new_fs = 0x00;
00684   uint8_t tempReg = 0x00;
00685   
00686   new_fs = ( fullScale <= 245.0f ) ? LSM6DS0_G_FS_245
00687            : ( fullScale <= 500.0f ) ? LSM6DS0_G_FS_500
00688            :                           LSM6DS0_G_FS_2000;
00689            
00690   if(LSM6DS0_IO_Read( &tempReg, LSM6DS0_XG_CTRL_REG1_G, 1 ) != IMU_6AXES_OK)
00691   {
00692     return IMU_6AXES_ERROR;
00693   }
00694   
00695   tempReg &= ~(LSM6DS0_G_FS_MASK);
00696   tempReg |= new_fs;
00697   
00698   if(LSM6DS0_IO_Write(&tempReg, LSM6DS0_XG_CTRL_REG1_G, 1) != IMU_6AXES_OK)
00699   {
00700     return IMU_6AXES_ERROR;
00701   }
00702   
00703   return IMU_6AXES_OK;
00704 }
00705 
00706 /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/