Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Fork of ReferredCoursework2016 by
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>© 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 Sat Jul 16 2022 05:47:21 by
