Ant Robinson
Embed:
(wiki syntax)
Show/hide line numbers
lsm6ds0_class.cpp
Go to the documentation of this file.
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>© 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****/
Generated on Tue Jul 12 2022 20:24:46 by
1.7.2