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