ST
iNEMO inertial module: 3D accelerometer and 3D gyroscope.
Dependencies: X_NUCLEO_COMMON ST_INTERFACES
Dependents: HelloWorld_ST_Sensors MOTENV_Mbed mbed-os-mqtt-client LSM6DSL_JS ... more
Embed:
(wiki syntax)
Show/hide line numbers
LSM6DSLSensor.cpp
Go to the documentation of this file.
00001 /** 00002 ****************************************************************************** 00003 * @file LSM6DSLSensor.cpp 00004 * @author CLab 00005 * @version V1.0.0 00006 * @date 5 August 2016 00007 * @brief Implementation of an LSM6DSL Inertial Measurement Unit (IMU) 6 axes 00008 * sensor. 00009 ****************************************************************************** 00010 * @attention 00011 * 00012 * <h2><center>© COPYRIGHT(c) 2016 STMicroelectronics</center></h2> 00013 * 00014 * Redistribution and use in source and binary forms, with or without modification, 00015 * are permitted provided that the following conditions are met: 00016 * 1. Redistributions of source code must retain the above copyright notice, 00017 * this list of conditions and the following disclaimer. 00018 * 2. Redistributions in binary form must reproduce the above copyright notice, 00019 * this list of conditions and the following disclaimer in the documentation 00020 * and/or other materials provided with the distribution. 00021 * 3. Neither the name of STMicroelectronics nor the names of its contributors 00022 * may be used to endorse or promote products derived from this software 00023 * without specific prior written permission. 00024 * 00025 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 00026 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 00027 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 00028 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 00029 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 00030 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 00031 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00032 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 00033 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 00034 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 00035 * 00036 ****************************************************************************** 00037 */ 00038 00039 00040 /* Includes ------------------------------------------------------------------*/ 00041 00042 #include "LSM6DSLSensor.h" 00043 00044 /* Class Implementation ------------------------------------------------------*/ 00045 00046 LSM6DSLSensor::LSM6DSLSensor(SPI *spi, PinName cs_pin, PinName int1_pin, PinName int2_pin, SPI_type_t spi_type ) : 00047 _dev_spi(spi), _cs_pin(cs_pin), _int1_irq(int1_pin), _int2_irq(int2_pin), _spi_type(spi_type) 00048 { 00049 assert (spi); 00050 if (cs_pin == NC) 00051 { 00052 printf ("ERROR LPS22HBSensor CS MUST NOT BE NC\n\r"); 00053 _dev_spi = NULL; 00054 _dev_i2c=NULL; 00055 return; 00056 } 00057 _cs_pin = 1; 00058 _dev_i2c=NULL; 00059 00060 if (_spi_type == SPI3W) LSM6DSL_ACC_GYRO_W_SPI_Mode((void *)this, LSM6DSL_ACC_GYRO_SIM_3_WIRE); 00061 else LSM6DSL_ACC_GYRO_W_SPI_Mode((void *)this, LSM6DSL_ACC_GYRO_SIM_4_WIRE); 00062 00063 LSM6DSL_ACC_GYRO_W_I2C_MASTER_Enable((void *)this, LSM6DSL_ACC_GYRO_MASTER_ON_DISABLED); 00064 } 00065 00066 /** Constructor 00067 * @param i2c object of an helper class which handles the I2C peripheral 00068 * @param address the address of the component's instance 00069 */ 00070 LSM6DSLSensor::LSM6DSLSensor(DevI2C *i2c, uint8_t address, PinName int1_pin, PinName int2_pin) : 00071 _dev_i2c(i2c), _address(address), _cs_pin(NC), _int1_irq(int1_pin), _int2_irq(int2_pin) 00072 { 00073 assert (i2c); 00074 _dev_spi = NULL; 00075 } 00076 00077 /** 00078 * @brief Initializing the component. 00079 * @param[in] init pointer to device specific initalization structure. 00080 * @retval "0" in case of success, an error code otherwise. 00081 */ 00082 int LSM6DSLSensor::init(void *init) 00083 { 00084 /* Enable register address automatically incremented during a multiple byte 00085 access with a serial interface. */ 00086 if ( LSM6DSL_ACC_GYRO_W_IF_Addr_Incr( (void *)this, LSM6DSL_ACC_GYRO_IF_INC_ENABLED ) == MEMS_ERROR ) 00087 { 00088 return 1; 00089 } 00090 00091 /* Enable BDU */ 00092 if ( LSM6DSL_ACC_GYRO_W_BDU( (void *)this, LSM6DSL_ACC_GYRO_BDU_BLOCK_UPDATE ) == MEMS_ERROR ) 00093 { 00094 return 1; 00095 } 00096 00097 /* FIFO mode selection */ 00098 if ( LSM6DSL_ACC_GYRO_W_FIFO_MODE( (void *)this, LSM6DSL_ACC_GYRO_FIFO_MODE_BYPASS ) == MEMS_ERROR ) 00099 { 00100 return 1; 00101 } 00102 00103 /* Output data rate selection - power down. */ 00104 if ( LSM6DSL_ACC_GYRO_W_ODR_XL( (void *)this, LSM6DSL_ACC_GYRO_ODR_XL_POWER_DOWN ) == MEMS_ERROR ) 00105 { 00106 return 1; 00107 } 00108 00109 /* Full scale selection. */ 00110 if ( set_x_fs( 2.0f ) == 1 ) 00111 { 00112 return 1; 00113 } 00114 00115 /* Output data rate selection - power down */ 00116 if ( LSM6DSL_ACC_GYRO_W_ODR_G( (void *)this, LSM6DSL_ACC_GYRO_ODR_G_POWER_DOWN ) == MEMS_ERROR ) 00117 { 00118 return 1; 00119 } 00120 00121 /* Full scale selection. */ 00122 if ( set_g_fs( 2000.0f ) == 1 ) 00123 { 00124 return 1; 00125 } 00126 00127 _x_last_odr = 104.0f; 00128 00129 _x_is_enabled = 0; 00130 00131 _g_last_odr = 104.0f; 00132 00133 _g_is_enabled = 0; 00134 00135 return 0; 00136 } 00137 00138 /** 00139 * @brief Enable LSM6DSL Accelerator 00140 * @retval 0 in case of success, an error code otherwise 00141 */ 00142 int LSM6DSLSensor::enable_x(void) 00143 { 00144 /* Check if the component is already enabled */ 00145 if ( _x_is_enabled == 1 ) 00146 { 00147 return 0; 00148 } 00149 00150 /* Output data rate selection. */ 00151 if ( set_x_odr_when_enabled( _x_last_odr ) == 1 ) 00152 { 00153 return 1; 00154 } 00155 00156 _x_is_enabled = 1; 00157 00158 return 0; 00159 } 00160 00161 /** 00162 * @brief Enable LSM6DSL Gyroscope 00163 * @retval 0 in case of success, an error code otherwise 00164 */ 00165 int LSM6DSLSensor::enable_g(void) 00166 { 00167 /* Check if the component is already enabled */ 00168 if ( _g_is_enabled == 1 ) 00169 { 00170 return 0; 00171 } 00172 00173 /* Output data rate selection. */ 00174 if ( set_g_odr_when_enabled( _g_last_odr ) == 1 ) 00175 { 00176 return 1; 00177 } 00178 00179 _g_is_enabled = 1; 00180 00181 return 0; 00182 } 00183 00184 /** 00185 * @brief Disable LSM6DSL Accelerator 00186 * @retval 0 in case of success, an error code otherwise 00187 */ 00188 int LSM6DSLSensor::disable_x(void) 00189 { 00190 /* Check if the component is already disabled */ 00191 if ( _x_is_enabled == 0 ) 00192 { 00193 return 0; 00194 } 00195 00196 /* Store actual output data rate. */ 00197 if ( get_x_odr( &_x_last_odr ) == 1 ) 00198 { 00199 return 1; 00200 } 00201 00202 /* Output data rate selection - power down. */ 00203 if ( LSM6DSL_ACC_GYRO_W_ODR_XL( (void *)this, LSM6DSL_ACC_GYRO_ODR_XL_POWER_DOWN ) == MEMS_ERROR ) 00204 { 00205 return 1; 00206 } 00207 00208 _x_is_enabled = 0; 00209 00210 return 0; 00211 } 00212 00213 /** 00214 * @brief Disable LSM6DSL Gyroscope 00215 * @retval 0 in case of success, an error code otherwise 00216 */ 00217 int LSM6DSLSensor::disable_g(void) 00218 { 00219 /* Check if the component is already disabled */ 00220 if ( _g_is_enabled == 0 ) 00221 { 00222 return 0; 00223 } 00224 00225 /* Store actual output data rate. */ 00226 if ( get_g_odr( &_g_last_odr ) == 1 ) 00227 { 00228 return 1; 00229 } 00230 00231 /* Output data rate selection - power down */ 00232 if ( LSM6DSL_ACC_GYRO_W_ODR_G( (void *)this, LSM6DSL_ACC_GYRO_ODR_G_POWER_DOWN ) == MEMS_ERROR ) 00233 { 00234 return 1; 00235 } 00236 00237 _g_is_enabled = 0; 00238 00239 return 0; 00240 } 00241 00242 /** 00243 * @brief Read ID of LSM6DSL Accelerometer and Gyroscope 00244 * @param p_id the pointer where the ID of the device is stored 00245 * @retval 0 in case of success, an error code otherwise 00246 */ 00247 int LSM6DSLSensor::read_id(uint8_t *id) 00248 { 00249 if(!id) 00250 { 00251 return 1; 00252 } 00253 00254 /* Read WHO AM I register */ 00255 if ( LSM6DSL_ACC_GYRO_R_WHO_AM_I( (void *)this, id ) == MEMS_ERROR ) 00256 { 00257 return 1; 00258 } 00259 00260 return 0; 00261 } 00262 00263 /** 00264 * @brief Read data from LSM6DSL Accelerometer 00265 * @param pData the pointer where the accelerometer data are stored 00266 * @retval 0 in case of success, an error code otherwise 00267 */ 00268 int LSM6DSLSensor::get_x_axes(int32_t *pData) 00269 { 00270 int16_t dataRaw[3]; 00271 float sensitivity = 0; 00272 00273 /* Read raw data from LSM6DSL output register. */ 00274 if ( get_x_axes_raw( dataRaw ) == 1 ) 00275 { 00276 return 1; 00277 } 00278 00279 /* Get LSM6DSL actual sensitivity. */ 00280 if ( get_x_sensitivity( &sensitivity ) == 1 ) 00281 { 00282 return 1; 00283 } 00284 00285 /* Calculate the data. */ 00286 pData[0] = ( int32_t )( dataRaw[0] * sensitivity ); 00287 pData[1] = ( int32_t )( dataRaw[1] * sensitivity ); 00288 pData[2] = ( int32_t )( dataRaw[2] * sensitivity ); 00289 00290 return 0; 00291 } 00292 00293 /** 00294 * @brief Read data from LSM6DSL Gyroscope 00295 * @param pData the pointer where the gyroscope data are stored 00296 * @retval 0 in case of success, an error code otherwise 00297 */ 00298 int LSM6DSLSensor::get_g_axes(int32_t *pData) 00299 { 00300 int16_t dataRaw[3]; 00301 float sensitivity = 0; 00302 00303 /* Read raw data from LSM6DSL output register. */ 00304 if ( get_g_axes_raw( dataRaw ) == 1 ) 00305 { 00306 return 1; 00307 } 00308 00309 /* Get LSM6DSL actual sensitivity. */ 00310 if ( get_g_sensitivity( &sensitivity ) == 1 ) 00311 { 00312 return 1; 00313 } 00314 00315 /* Calculate the data. */ 00316 pData[0] = ( int32_t )( dataRaw[0] * sensitivity ); 00317 pData[1] = ( int32_t )( dataRaw[1] * sensitivity ); 00318 pData[2] = ( int32_t )( dataRaw[2] * sensitivity ); 00319 00320 return 0; 00321 } 00322 00323 /** 00324 * @brief Read Accelerometer Sensitivity 00325 * @param pfData the pointer where the accelerometer sensitivity is stored 00326 * @retval 0 in case of success, an error code otherwise 00327 */ 00328 int LSM6DSLSensor::get_x_sensitivity(float *pfData) 00329 { 00330 LSM6DSL_ACC_GYRO_FS_XL_t fullScale; 00331 00332 /* Read actual full scale selection from sensor. */ 00333 if ( LSM6DSL_ACC_GYRO_R_FS_XL( (void *)this, &fullScale ) == MEMS_ERROR ) 00334 { 00335 return 1; 00336 } 00337 00338 /* Store the sensitivity based on actual full scale. */ 00339 switch( fullScale ) 00340 { 00341 case LSM6DSL_ACC_GYRO_FS_XL_2g: 00342 *pfData = ( float )LSM6DSL_ACC_SENSITIVITY_FOR_FS_2G; 00343 break; 00344 case LSM6DSL_ACC_GYRO_FS_XL_4g: 00345 *pfData = ( float )LSM6DSL_ACC_SENSITIVITY_FOR_FS_4G; 00346 break; 00347 case LSM6DSL_ACC_GYRO_FS_XL_8g: 00348 *pfData = ( float )LSM6DSL_ACC_SENSITIVITY_FOR_FS_8G; 00349 break; 00350 case LSM6DSL_ACC_GYRO_FS_XL_16g: 00351 *pfData = ( float )LSM6DSL_ACC_SENSITIVITY_FOR_FS_16G; 00352 break; 00353 default: 00354 *pfData = -1.0f; 00355 return 1; 00356 } 00357 00358 return 0; 00359 } 00360 00361 /** 00362 * @brief Read Gyroscope Sensitivity 00363 * @param pfData the pointer where the gyroscope sensitivity is stored 00364 * @retval 0 in case of success, an error code otherwise 00365 */ 00366 int LSM6DSLSensor::get_g_sensitivity(float *pfData) 00367 { 00368 LSM6DSL_ACC_GYRO_FS_125_t fullScale125; 00369 LSM6DSL_ACC_GYRO_FS_G_t fullScale; 00370 00371 /* Read full scale 125 selection from sensor. */ 00372 if ( LSM6DSL_ACC_GYRO_R_FS_125( (void *)this, &fullScale125 ) == MEMS_ERROR ) 00373 { 00374 return 1; 00375 } 00376 00377 if ( fullScale125 == LSM6DSL_ACC_GYRO_FS_125_ENABLED ) 00378 { 00379 *pfData = ( float )LSM6DSL_GYRO_SENSITIVITY_FOR_FS_125DPS; 00380 } 00381 00382 else 00383 { 00384 00385 /* Read actual full scale selection from sensor. */ 00386 if ( LSM6DSL_ACC_GYRO_R_FS_G( (void *)this, &fullScale ) == MEMS_ERROR ) 00387 { 00388 return 1; 00389 } 00390 00391 /* Store the sensitivity based on actual full scale. */ 00392 switch( fullScale ) 00393 { 00394 case LSM6DSL_ACC_GYRO_FS_G_245dps: 00395 *pfData = ( float )LSM6DSL_GYRO_SENSITIVITY_FOR_FS_245DPS; 00396 break; 00397 case LSM6DSL_ACC_GYRO_FS_G_500dps: 00398 *pfData = ( float )LSM6DSL_GYRO_SENSITIVITY_FOR_FS_500DPS; 00399 break; 00400 case LSM6DSL_ACC_GYRO_FS_G_1000dps: 00401 *pfData = ( float )LSM6DSL_GYRO_SENSITIVITY_FOR_FS_1000DPS; 00402 break; 00403 case LSM6DSL_ACC_GYRO_FS_G_2000dps: 00404 *pfData = ( float )LSM6DSL_GYRO_SENSITIVITY_FOR_FS_2000DPS; 00405 break; 00406 default: 00407 *pfData = -1.0f; 00408 return 1; 00409 } 00410 } 00411 00412 return 0; 00413 } 00414 00415 /** 00416 * @brief Read raw data from LSM6DSL Accelerometer 00417 * @param pData the pointer where the accelerometer raw data are stored 00418 * @retval 0 in case of success, an error code otherwise 00419 */ 00420 int LSM6DSLSensor::get_x_axes_raw(int16_t *pData) 00421 { 00422 uint8_t regValue[6] = {0, 0, 0, 0, 0, 0}; 00423 00424 /* Read output registers from LSM6DSL_ACC_GYRO_OUTX_L_XL to LSM6DSL_ACC_GYRO_OUTZ_H_XL. */ 00425 if ( LSM6DSL_ACC_GYRO_GetRawAccData( (void *)this, regValue ) == MEMS_ERROR ) 00426 { 00427 return 1; 00428 } 00429 00430 /* Format the data. */ 00431 pData[0] = ( ( ( ( int16_t )regValue[1] ) << 8 ) + ( int16_t )regValue[0] ); 00432 pData[1] = ( ( ( ( int16_t )regValue[3] ) << 8 ) + ( int16_t )regValue[2] ); 00433 pData[2] = ( ( ( ( int16_t )regValue[5] ) << 8 ) + ( int16_t )regValue[4] ); 00434 00435 return 0; 00436 } 00437 00438 /** 00439 * @brief Read raw data from LSM6DSL Gyroscope 00440 * @param pData the pointer where the gyroscope raw data are stored 00441 * @retval 0 in case of success, an error code otherwise 00442 */ 00443 int LSM6DSLSensor::get_g_axes_raw(int16_t *pData) 00444 { 00445 uint8_t regValue[6] = {0, 0, 0, 0, 0, 0}; 00446 00447 /* Read output registers from LSM6DSL_ACC_GYRO_OUTX_L_G to LSM6DSL_ACC_GYRO_OUTZ_H_G. */ 00448 if ( LSM6DSL_ACC_GYRO_GetRawGyroData( (void *)this, regValue ) == MEMS_ERROR ) 00449 { 00450 return 1; 00451 } 00452 00453 /* Format the data. */ 00454 pData[0] = ( ( ( ( int16_t )regValue[1] ) << 8 ) + ( int16_t )regValue[0] ); 00455 pData[1] = ( ( ( ( int16_t )regValue[3] ) << 8 ) + ( int16_t )regValue[2] ); 00456 pData[2] = ( ( ( ( int16_t )regValue[5] ) << 8 ) + ( int16_t )regValue[4] ); 00457 00458 return 0; 00459 } 00460 00461 /** 00462 * @brief Read LSM6DSL Accelerometer output data rate 00463 * @param odr the pointer to the output data rate 00464 * @retval 0 in case of success, an error code otherwise 00465 */ 00466 int LSM6DSLSensor::get_x_odr(float* odr) 00467 { 00468 LSM6DSL_ACC_GYRO_ODR_XL_t odr_low_level; 00469 00470 if ( LSM6DSL_ACC_GYRO_R_ODR_XL( (void *)this, &odr_low_level ) == MEMS_ERROR ) 00471 { 00472 return 1; 00473 } 00474 00475 switch( odr_low_level ) 00476 { 00477 case LSM6DSL_ACC_GYRO_ODR_XL_POWER_DOWN: 00478 *odr = 0.0f; 00479 break; 00480 case LSM6DSL_ACC_GYRO_ODR_XL_13Hz: 00481 *odr = 13.0f; 00482 break; 00483 case LSM6DSL_ACC_GYRO_ODR_XL_26Hz: 00484 *odr = 26.0f; 00485 break; 00486 case LSM6DSL_ACC_GYRO_ODR_XL_52Hz: 00487 *odr = 52.0f; 00488 break; 00489 case LSM6DSL_ACC_GYRO_ODR_XL_104Hz: 00490 *odr = 104.0f; 00491 break; 00492 case LSM6DSL_ACC_GYRO_ODR_XL_208Hz: 00493 *odr = 208.0f; 00494 break; 00495 case LSM6DSL_ACC_GYRO_ODR_XL_416Hz: 00496 *odr = 416.0f; 00497 break; 00498 case LSM6DSL_ACC_GYRO_ODR_XL_833Hz: 00499 *odr = 833.0f; 00500 break; 00501 case LSM6DSL_ACC_GYRO_ODR_XL_1660Hz: 00502 *odr = 1660.0f; 00503 break; 00504 case LSM6DSL_ACC_GYRO_ODR_XL_3330Hz: 00505 *odr = 3330.0f; 00506 break; 00507 case LSM6DSL_ACC_GYRO_ODR_XL_6660Hz: 00508 *odr = 6660.0f; 00509 break; 00510 default: 00511 *odr = -1.0f; 00512 return 1; 00513 } 00514 00515 return 0; 00516 } 00517 00518 /** 00519 * @brief Read LSM6DSL Gyroscope output data rate 00520 * @param odr the pointer to the output data rate 00521 * @retval 0 in case of success, an error code otherwise 00522 */ 00523 int LSM6DSLSensor::get_g_odr(float* odr) 00524 { 00525 LSM6DSL_ACC_GYRO_ODR_G_t odr_low_level; 00526 00527 if ( LSM6DSL_ACC_GYRO_R_ODR_G( (void *)this, &odr_low_level ) == MEMS_ERROR ) 00528 { 00529 return 1; 00530 } 00531 00532 switch( odr_low_level ) 00533 { 00534 case LSM6DSL_ACC_GYRO_ODR_G_POWER_DOWN: 00535 *odr = 0.0f; 00536 break; 00537 case LSM6DSL_ACC_GYRO_ODR_G_13Hz: 00538 *odr = 13.0f; 00539 break; 00540 case LSM6DSL_ACC_GYRO_ODR_G_26Hz: 00541 *odr = 26.0f; 00542 break; 00543 case LSM6DSL_ACC_GYRO_ODR_G_52Hz: 00544 *odr = 52.0f; 00545 break; 00546 case LSM6DSL_ACC_GYRO_ODR_G_104Hz: 00547 *odr = 104.0f; 00548 break; 00549 case LSM6DSL_ACC_GYRO_ODR_G_208Hz: 00550 *odr = 208.0f; 00551 break; 00552 case LSM6DSL_ACC_GYRO_ODR_G_416Hz: 00553 *odr = 416.0f; 00554 break; 00555 case LSM6DSL_ACC_GYRO_ODR_G_833Hz: 00556 *odr = 833.0f; 00557 break; 00558 case LSM6DSL_ACC_GYRO_ODR_G_1660Hz: 00559 *odr = 1660.0f; 00560 break; 00561 case LSM6DSL_ACC_GYRO_ODR_G_3330Hz: 00562 *odr = 3330.0f; 00563 break; 00564 case LSM6DSL_ACC_GYRO_ODR_G_6660Hz: 00565 *odr = 6660.0f; 00566 break; 00567 default: 00568 *odr = -1.0f; 00569 return 1; 00570 } 00571 00572 return 0; 00573 } 00574 00575 /** 00576 * @brief Set LSM6DSL Accelerometer output data rate 00577 * @param odr the output data rate to be set 00578 * @retval 0 in case of success, an error code otherwise 00579 */ 00580 int LSM6DSLSensor::set_x_odr(float odr) 00581 { 00582 if(_x_is_enabled == 1) 00583 { 00584 if(set_x_odr_when_enabled(odr) == 1) 00585 { 00586 return 1; 00587 } 00588 } 00589 else 00590 { 00591 if(set_x_odr_when_disabled(odr) == 1) 00592 { 00593 return 1; 00594 } 00595 } 00596 00597 return 0; 00598 } 00599 00600 /** 00601 * @brief Set LSM6DSL Accelerometer output data rate when enabled 00602 * @param odr the output data rate to be set 00603 * @retval 0 in case of success, an error code otherwise 00604 */ 00605 int LSM6DSLSensor::set_x_odr_when_enabled(float odr) 00606 { 00607 LSM6DSL_ACC_GYRO_ODR_XL_t new_odr; 00608 00609 new_odr = ( odr <= 13.0f ) ? LSM6DSL_ACC_GYRO_ODR_XL_13Hz 00610 : ( odr <= 26.0f ) ? LSM6DSL_ACC_GYRO_ODR_XL_26Hz 00611 : ( odr <= 52.0f ) ? LSM6DSL_ACC_GYRO_ODR_XL_52Hz 00612 : ( odr <= 104.0f ) ? LSM6DSL_ACC_GYRO_ODR_XL_104Hz 00613 : ( odr <= 208.0f ) ? LSM6DSL_ACC_GYRO_ODR_XL_208Hz 00614 : ( odr <= 416.0f ) ? LSM6DSL_ACC_GYRO_ODR_XL_416Hz 00615 : ( odr <= 833.0f ) ? LSM6DSL_ACC_GYRO_ODR_XL_833Hz 00616 : ( odr <= 1660.0f ) ? LSM6DSL_ACC_GYRO_ODR_XL_1660Hz 00617 : ( odr <= 3330.0f ) ? LSM6DSL_ACC_GYRO_ODR_XL_3330Hz 00618 : LSM6DSL_ACC_GYRO_ODR_XL_6660Hz; 00619 00620 if ( LSM6DSL_ACC_GYRO_W_ODR_XL( (void *)this, new_odr ) == MEMS_ERROR ) 00621 { 00622 return 1; 00623 } 00624 00625 return 0; 00626 } 00627 00628 /** 00629 * @brief Set LSM6DSL Accelerometer output data rate when disabled 00630 * @param odr the output data rate to be set 00631 * @retval 0 in case of success, an error code otherwise 00632 */ 00633 int LSM6DSLSensor::set_x_odr_when_disabled(float odr) 00634 { 00635 _x_last_odr = ( odr <= 13.0f ) ? 13.0f 00636 : ( odr <= 26.0f ) ? 26.0f 00637 : ( odr <= 52.0f ) ? 52.0f 00638 : ( odr <= 104.0f ) ? 104.0f 00639 : ( odr <= 208.0f ) ? 208.0f 00640 : ( odr <= 416.0f ) ? 416.0f 00641 : ( odr <= 833.0f ) ? 833.0f 00642 : ( odr <= 1660.0f ) ? 1660.0f 00643 : ( odr <= 3330.0f ) ? 3330.0f 00644 : 6660.0f; 00645 00646 return 0; 00647 } 00648 00649 /** 00650 * @brief Set LSM6DSL Gyroscope output data rate 00651 * @param odr the output data rate to be set 00652 * @retval 0 in case of success, an error code otherwise 00653 */ 00654 int LSM6DSLSensor::set_g_odr(float odr) 00655 { 00656 if(_g_is_enabled == 1) 00657 { 00658 if(set_g_odr_when_enabled(odr) == 1) 00659 { 00660 return 1; 00661 } 00662 } 00663 else 00664 { 00665 if(set_g_odr_when_disabled(odr) == 1) 00666 { 00667 return 1; 00668 } 00669 } 00670 00671 return 0; 00672 } 00673 00674 /** 00675 * @brief Set LSM6DSL Gyroscope output data rate when enabled 00676 * @param odr the output data rate to be set 00677 * @retval 0 in case of success, an error code otherwise 00678 */ 00679 int LSM6DSLSensor::set_g_odr_when_enabled(float odr) 00680 { 00681 LSM6DSL_ACC_GYRO_ODR_G_t new_odr; 00682 00683 new_odr = ( odr <= 13.0f ) ? LSM6DSL_ACC_GYRO_ODR_G_13Hz 00684 : ( odr <= 26.0f ) ? LSM6DSL_ACC_GYRO_ODR_G_26Hz 00685 : ( odr <= 52.0f ) ? LSM6DSL_ACC_GYRO_ODR_G_52Hz 00686 : ( odr <= 104.0f ) ? LSM6DSL_ACC_GYRO_ODR_G_104Hz 00687 : ( odr <= 208.0f ) ? LSM6DSL_ACC_GYRO_ODR_G_208Hz 00688 : ( odr <= 416.0f ) ? LSM6DSL_ACC_GYRO_ODR_G_416Hz 00689 : ( odr <= 833.0f ) ? LSM6DSL_ACC_GYRO_ODR_G_833Hz 00690 : ( odr <= 1660.0f ) ? LSM6DSL_ACC_GYRO_ODR_G_1660Hz 00691 : ( odr <= 3330.0f ) ? LSM6DSL_ACC_GYRO_ODR_G_3330Hz 00692 : LSM6DSL_ACC_GYRO_ODR_G_6660Hz; 00693 00694 if ( LSM6DSL_ACC_GYRO_W_ODR_G( (void *)this, new_odr ) == MEMS_ERROR ) 00695 { 00696 return 1; 00697 } 00698 00699 return 0; 00700 } 00701 00702 /** 00703 * @brief Set LSM6DSL Gyroscope output data rate when disabled 00704 * @param odr the output data rate to be set 00705 * @retval 0 in case of success, an error code otherwise 00706 */ 00707 int LSM6DSLSensor::set_g_odr_when_disabled(float odr) 00708 { 00709 _g_last_odr = ( odr <= 13.0f ) ? 13.0f 00710 : ( odr <= 26.0f ) ? 26.0f 00711 : ( odr <= 52.0f ) ? 52.0f 00712 : ( odr <= 104.0f ) ? 104.0f 00713 : ( odr <= 208.0f ) ? 208.0f 00714 : ( odr <= 416.0f ) ? 416.0f 00715 : ( odr <= 833.0f ) ? 833.0f 00716 : ( odr <= 1660.0f ) ? 1660.0f 00717 : ( odr <= 3330.0f ) ? 3330.0f 00718 : 6660.0f; 00719 00720 return 0; 00721 } 00722 00723 /** 00724 * @brief Read LSM6DSL Accelerometer full scale 00725 * @param fullScale the pointer to the full scale 00726 * @retval 0 in case of success, an error code otherwise 00727 */ 00728 int LSM6DSLSensor::get_x_fs(float* fullScale) 00729 { 00730 LSM6DSL_ACC_GYRO_FS_XL_t fs_low_level; 00731 00732 if ( LSM6DSL_ACC_GYRO_R_FS_XL( (void *)this, &fs_low_level ) == MEMS_ERROR ) 00733 { 00734 return 1; 00735 } 00736 00737 switch( fs_low_level ) 00738 { 00739 case LSM6DSL_ACC_GYRO_FS_XL_2g: 00740 *fullScale = 2.0f; 00741 break; 00742 case LSM6DSL_ACC_GYRO_FS_XL_4g: 00743 *fullScale = 4.0f; 00744 break; 00745 case LSM6DSL_ACC_GYRO_FS_XL_8g: 00746 *fullScale = 8.0f; 00747 break; 00748 case LSM6DSL_ACC_GYRO_FS_XL_16g: 00749 *fullScale = 16.0f; 00750 break; 00751 default: 00752 *fullScale = -1.0f; 00753 return 1; 00754 } 00755 00756 return 0; 00757 } 00758 00759 /** 00760 * @brief Read LSM6DSL Gyroscope full scale 00761 * @param fullScale the pointer to the full scale 00762 * @retval 0 in case of success, an error code otherwise 00763 */ 00764 int LSM6DSLSensor::get_g_fs(float* fullScale) 00765 { 00766 LSM6DSL_ACC_GYRO_FS_G_t fs_low_level; 00767 LSM6DSL_ACC_GYRO_FS_125_t fs_125; 00768 00769 if ( LSM6DSL_ACC_GYRO_R_FS_125( (void *)this, &fs_125 ) == MEMS_ERROR ) 00770 { 00771 return 1; 00772 } 00773 if ( LSM6DSL_ACC_GYRO_R_FS_G( (void *)this, &fs_low_level ) == MEMS_ERROR ) 00774 { 00775 return 1; 00776 } 00777 00778 if ( fs_125 == LSM6DSL_ACC_GYRO_FS_125_ENABLED ) 00779 { 00780 *fullScale = 125.0f; 00781 } 00782 00783 else 00784 { 00785 switch( fs_low_level ) 00786 { 00787 case LSM6DSL_ACC_GYRO_FS_G_245dps: 00788 *fullScale = 245.0f; 00789 break; 00790 case LSM6DSL_ACC_GYRO_FS_G_500dps: 00791 *fullScale = 500.0f; 00792 break; 00793 case LSM6DSL_ACC_GYRO_FS_G_1000dps: 00794 *fullScale = 1000.0f; 00795 break; 00796 case LSM6DSL_ACC_GYRO_FS_G_2000dps: 00797 *fullScale = 2000.0f; 00798 break; 00799 default: 00800 *fullScale = -1.0f; 00801 return 1; 00802 } 00803 } 00804 00805 return 0; 00806 } 00807 00808 /** 00809 * @brief Set LSM6DSL Accelerometer full scale 00810 * @param fullScale the full scale to be set 00811 * @retval 0 in case of success, an error code otherwise 00812 */ 00813 int LSM6DSLSensor::set_x_fs(float fullScale) 00814 { 00815 LSM6DSL_ACC_GYRO_FS_XL_t new_fs; 00816 00817 new_fs = ( fullScale <= 2.0f ) ? LSM6DSL_ACC_GYRO_FS_XL_2g 00818 : ( fullScale <= 4.0f ) ? LSM6DSL_ACC_GYRO_FS_XL_4g 00819 : ( fullScale <= 8.0f ) ? LSM6DSL_ACC_GYRO_FS_XL_8g 00820 : LSM6DSL_ACC_GYRO_FS_XL_16g; 00821 00822 if ( LSM6DSL_ACC_GYRO_W_FS_XL( (void *)this, new_fs ) == MEMS_ERROR ) 00823 { 00824 return 1; 00825 } 00826 00827 return 0; 00828 } 00829 00830 /** 00831 * @brief Set LSM6DSL Gyroscope full scale 00832 * @param fullScale the full scale to be set 00833 * @retval 0 in case of success, an error code otherwise 00834 */ 00835 int LSM6DSLSensor::set_g_fs(float fullScale) 00836 { 00837 LSM6DSL_ACC_GYRO_FS_G_t new_fs; 00838 00839 if ( fullScale <= 125.0f ) 00840 { 00841 if ( LSM6DSL_ACC_GYRO_W_FS_125( (void *)this, LSM6DSL_ACC_GYRO_FS_125_ENABLED ) == MEMS_ERROR ) 00842 { 00843 return 1; 00844 } 00845 } 00846 else 00847 { 00848 new_fs = ( fullScale <= 245.0f ) ? LSM6DSL_ACC_GYRO_FS_G_245dps 00849 : ( fullScale <= 500.0f ) ? LSM6DSL_ACC_GYRO_FS_G_500dps 00850 : ( fullScale <= 1000.0f ) ? LSM6DSL_ACC_GYRO_FS_G_1000dps 00851 : LSM6DSL_ACC_GYRO_FS_G_2000dps; 00852 00853 if ( LSM6DSL_ACC_GYRO_W_FS_125( (void *)this, LSM6DSL_ACC_GYRO_FS_125_DISABLED ) == MEMS_ERROR ) 00854 { 00855 return 1; 00856 } 00857 if ( LSM6DSL_ACC_GYRO_W_FS_G( (void *)this, new_fs ) == MEMS_ERROR ) 00858 { 00859 return 1; 00860 } 00861 } 00862 00863 return 0; 00864 } 00865 00866 /** 00867 * @brief Enable free fall detection 00868 * @param pin the interrupt pin to be used 00869 * @note This function sets the LSM6DSL accelerometer ODR to 416Hz and the LSM6DSL accelerometer full scale to 2g 00870 * @retval 0 in case of success, an error code otherwise 00871 */ 00872 int LSM6DSLSensor::enable_free_fall_detection(LSM6DSL_Interrupt_Pin_t pin) 00873 { 00874 /* Output Data Rate selection */ 00875 if(set_x_odr(416.0f) == 1) 00876 { 00877 return 1; 00878 } 00879 00880 /* Full scale selection */ 00881 if ( LSM6DSL_ACC_GYRO_W_FS_XL( (void *)this, LSM6DSL_ACC_GYRO_FS_XL_2g ) == MEMS_ERROR ) 00882 { 00883 return 1; 00884 } 00885 00886 /* FF_DUR setting */ 00887 if ( LSM6DSL_ACC_GYRO_W_FF_Duration( (void *)this, 0x06 ) == MEMS_ERROR ) 00888 { 00889 return 1; 00890 } 00891 00892 /* WAKE_DUR setting */ 00893 if ( LSM6DSL_ACC_GYRO_W_WAKE_DUR( (void *)this, 0x00 ) == MEMS_ERROR ) 00894 { 00895 return 1; 00896 } 00897 00898 /* TIMER_HR setting */ 00899 if ( LSM6DSL_ACC_GYRO_W_TIMER_HR( (void *)this, LSM6DSL_ACC_GYRO_TIMER_HR_6_4ms ) == MEMS_ERROR ) 00900 { 00901 return 1; 00902 } 00903 00904 /* SLEEP_DUR setting */ 00905 if ( LSM6DSL_ACC_GYRO_W_SLEEP_DUR( (void *)this, 0x00 ) == MEMS_ERROR ) 00906 { 00907 return 1; 00908 } 00909 00910 /* FF_THS setting */ 00911 if ( LSM6DSL_ACC_GYRO_W_FF_THS( (void *)this, LSM6DSL_ACC_GYRO_FF_THS_312mg ) == MEMS_ERROR ) 00912 { 00913 return 1; 00914 } 00915 00916 /* Enable basic Interrupts */ 00917 if ( LSM6DSL_ACC_GYRO_W_BASIC_INT( (void *)this, LSM6DSL_ACC_GYRO_BASIC_INT_ENABLED ) == MEMS_ERROR ) 00918 { 00919 return 1; 00920 } 00921 00922 /* Enable free fall event on either INT1 or INT2 pin */ 00923 switch (pin) 00924 { 00925 case LSM6DSL_INT1_PIN: 00926 if ( LSM6DSL_ACC_GYRO_W_FFEvOnInt1( (void *)this, LSM6DSL_ACC_GYRO_INT1_FF_ENABLED ) == MEMS_ERROR ) 00927 { 00928 return 1; 00929 } 00930 break; 00931 00932 case LSM6DSL_INT2_PIN: 00933 if ( LSM6DSL_ACC_GYRO_W_FFEvOnInt2( (void *)this, LSM6DSL_ACC_GYRO_INT2_FF_ENABLED ) == MEMS_ERROR ) 00934 { 00935 return 1; 00936 } 00937 break; 00938 00939 default: 00940 return 1; 00941 } 00942 00943 return 0; 00944 } 00945 00946 /** 00947 * @brief Disable free fall detection 00948 * @param None 00949 * @retval 0 in case of success, an error code otherwise 00950 */ 00951 int LSM6DSLSensor::disable_free_fall_detection(void) 00952 { 00953 /* Disable free fall event on INT1 pin */ 00954 if ( LSM6DSL_ACC_GYRO_W_FFEvOnInt1( (void *)this, LSM6DSL_ACC_GYRO_INT1_FF_DISABLED ) == MEMS_ERROR ) 00955 { 00956 return 1; 00957 } 00958 00959 /* Disable free fall event on INT2 pin */ 00960 if ( LSM6DSL_ACC_GYRO_W_FFEvOnInt2( (void *)this, LSM6DSL_ACC_GYRO_INT2_FF_DISABLED ) == MEMS_ERROR ) 00961 { 00962 return 1; 00963 } 00964 00965 /* Disable basic Interrupts */ 00966 if ( LSM6DSL_ACC_GYRO_W_BASIC_INT( (void *)this, LSM6DSL_ACC_GYRO_BASIC_INT_DISABLED ) == MEMS_ERROR ) 00967 { 00968 return 1; 00969 } 00970 00971 /* FF_DUR setting */ 00972 if ( LSM6DSL_ACC_GYRO_W_FF_Duration( (void *)this, 0x00 ) == MEMS_ERROR ) 00973 { 00974 return 1; 00975 } 00976 00977 /* FF_THS setting */ 00978 if ( LSM6DSL_ACC_GYRO_W_FF_THS( (void *)this, LSM6DSL_ACC_GYRO_FF_THS_156mg ) == MEMS_ERROR ) 00979 { 00980 return 1; 00981 } 00982 00983 return 0; 00984 } 00985 00986 /** 00987 * @brief Set the free fall detection threshold for LSM6DSL accelerometer sensor 00988 * @param thr the threshold to be set 00989 * @retval 0 in case of success, an error code otherwise 00990 */ 00991 int LSM6DSLSensor::set_free_fall_threshold(uint8_t thr) 00992 { 00993 00994 if ( LSM6DSL_ACC_GYRO_W_FF_THS( (void *)this, (LSM6DSL_ACC_GYRO_FF_THS_t)thr ) == MEMS_ERROR ) 00995 { 00996 return 1; 00997 } 00998 00999 return 0; 01000 } 01001 01002 /** 01003 * @brief Enable the pedometer feature for LSM6DSL accelerometer sensor 01004 * @note This function sets the LSM6DSL accelerometer ODR to 26Hz and the LSM6DSL accelerometer full scale to 2g 01005 * @retval 0 in case of success, an error code otherwise 01006 */ 01007 int LSM6DSLSensor::enable_pedometer(void) 01008 { 01009 /* Output Data Rate selection */ 01010 if( set_x_odr(26.0f) == 1 ) 01011 { 01012 return 1; 01013 } 01014 01015 /* Full scale selection. */ 01016 if( set_x_fs(2.0f) == 1 ) 01017 { 01018 return 1; 01019 } 01020 01021 /* Set pedometer threshold. */ 01022 if ( set_pedometer_threshold(LSM6DSL_PEDOMETER_THRESHOLD_MID_HIGH) == 1 ) 01023 { 01024 return 1; 01025 } 01026 01027 /* Enable embedded functionalities. */ 01028 if ( LSM6DSL_ACC_GYRO_W_FUNC_EN( (void *)this, LSM6DSL_ACC_GYRO_FUNC_EN_ENABLED ) == MEMS_ERROR ) 01029 { 01030 return 1; 01031 } 01032 01033 /* Enable pedometer algorithm. */ 01034 if ( LSM6DSL_ACC_GYRO_W_PEDO( (void *)this, LSM6DSL_ACC_GYRO_PEDO_ENABLED ) == MEMS_ERROR ) 01035 { 01036 return 1; 01037 } 01038 01039 /* Enable pedometer on INT1. */ 01040 if ( LSM6DSL_ACC_GYRO_W_STEP_DET_on_INT1( (void *)this, LSM6DSL_ACC_GYRO_INT1_PEDO_ENABLED ) == MEMS_ERROR ) 01041 { 01042 return 1; 01043 } 01044 01045 return 0; 01046 } 01047 01048 /** 01049 * @brief Disable the pedometer feature for LSM6DSL accelerometer sensor 01050 * @retval 0 in case of success, an error code otherwise 01051 */ 01052 int LSM6DSLSensor::disable_pedometer(void) 01053 { 01054 /* Disable pedometer on INT1. */ 01055 if ( LSM6DSL_ACC_GYRO_W_STEP_DET_on_INT1( (void *)this, LSM6DSL_ACC_GYRO_INT1_PEDO_DISABLED ) == MEMS_ERROR ) 01056 { 01057 return 1; 01058 } 01059 01060 /* Disable pedometer algorithm. */ 01061 if ( LSM6DSL_ACC_GYRO_W_PEDO( (void *)this, LSM6DSL_ACC_GYRO_PEDO_DISABLED ) == MEMS_ERROR ) 01062 { 01063 return 1; 01064 } 01065 01066 /* Disable embedded functionalities. */ 01067 if ( LSM6DSL_ACC_GYRO_W_FUNC_EN( (void *)this, LSM6DSL_ACC_GYRO_FUNC_EN_DISABLED ) == MEMS_ERROR ) 01068 { 01069 return 1; 01070 } 01071 01072 /* Reset pedometer threshold. */ 01073 if ( set_pedometer_threshold(0x0) == 1 ) 01074 { 01075 return 1; 01076 } 01077 01078 return 0; 01079 } 01080 01081 /** 01082 * @brief Get the step counter for LSM6DSL accelerometer sensor 01083 * @param step_count the pointer to the step counter 01084 * @retval 0 in case of success, an error code otherwise 01085 */ 01086 int LSM6DSLSensor::get_step_counter(uint16_t *step_count) 01087 { 01088 if ( LSM6DSL_ACC_GYRO_Get_GetStepCounter( (void *)this, ( uint8_t* )step_count ) == MEMS_ERROR ) 01089 { 01090 return 1; 01091 } 01092 01093 return 0; 01094 } 01095 01096 /** 01097 * @brief Reset of the step counter for LSM6DSL accelerometer sensor 01098 * @retval 0 in case of success, an error code otherwise 01099 */ 01100 int LSM6DSLSensor::reset_step_counter(void) 01101 { 01102 if ( LSM6DSL_ACC_GYRO_W_PedoStepReset( (void *)this, LSM6DSL_ACC_GYRO_PEDO_RST_STEP_ENABLED ) == MEMS_ERROR ) 01103 { 01104 return 1; 01105 } 01106 01107 #if MBED_MAJOR_VERSION == 5 01108 wait_ms(10); 01109 #else 01110 ThisThread::sleep_for(10ms); 01111 #endif 01112 01113 if ( LSM6DSL_ACC_GYRO_W_PedoStepReset( (void *)this, LSM6DSL_ACC_GYRO_PEDO_RST_STEP_DISABLED ) == MEMS_ERROR ) 01114 { 01115 return 1; 01116 } 01117 01118 return 0; 01119 } 01120 01121 /** 01122 * @brief Set the pedometer threshold for LSM6DSL accelerometer sensor 01123 * @param thr the threshold to be set 01124 * @retval 0 in case of success, an error code otherwise 01125 */ 01126 int LSM6DSLSensor::set_pedometer_threshold(uint8_t thr) 01127 { 01128 if ( LSM6DSL_ACC_GYRO_W_PedoThreshold( (void *)this, thr ) == MEMS_ERROR ) 01129 { 01130 return 1; 01131 } 01132 01133 return 0; 01134 } 01135 01136 /** 01137 * @brief Enable the tilt detection for LSM6DSL accelerometer sensor 01138 * @param pin the interrupt pin to be used 01139 * @note This function sets the LSM6DSL accelerometer ODR to 26Hz and the LSM6DSL accelerometer full scale to 2g 01140 * @retval 0 in case of success, an error code otherwise 01141 */ 01142 int LSM6DSLSensor::enable_tilt_detection(LSM6DSL_Interrupt_Pin_t pin) 01143 { 01144 /* Output Data Rate selection */ 01145 if( set_x_odr(26.0f) == 1 ) 01146 { 01147 return 1; 01148 } 01149 01150 /* Full scale selection. */ 01151 if( set_x_fs(2.0f) == 1 ) 01152 { 01153 return 1; 01154 } 01155 01156 /* Enable embedded functionalities */ 01157 if ( LSM6DSL_ACC_GYRO_W_FUNC_EN( (void *)this, LSM6DSL_ACC_GYRO_FUNC_EN_ENABLED ) == MEMS_ERROR ) 01158 { 01159 return 1; 01160 } 01161 01162 /* Enable tilt calculation. */ 01163 if ( LSM6DSL_ACC_GYRO_W_TILT( (void *)this, LSM6DSL_ACC_GYRO_TILT_ENABLED ) == MEMS_ERROR ) 01164 { 01165 return 1; 01166 } 01167 01168 /* Enable tilt detection on either INT1 or INT2 pin */ 01169 switch (pin) 01170 { 01171 case LSM6DSL_INT1_PIN: 01172 if ( LSM6DSL_ACC_GYRO_W_TiltEvOnInt1( (void *)this, LSM6DSL_ACC_GYRO_INT1_TILT_ENABLED ) == MEMS_ERROR ) 01173 { 01174 return 1; 01175 } 01176 break; 01177 01178 case LSM6DSL_INT2_PIN: 01179 if ( LSM6DSL_ACC_GYRO_W_TiltEvOnInt2( (void *)this, LSM6DSL_ACC_GYRO_INT2_TILT_ENABLED ) == MEMS_ERROR ) 01180 { 01181 return 1; 01182 } 01183 break; 01184 01185 default: 01186 return 1; 01187 } 01188 01189 return 0; 01190 } 01191 01192 /** 01193 * @brief Disable the tilt detection for LSM6DSL accelerometer sensor 01194 * @retval 0 in case of success, an error code otherwise 01195 */ 01196 int LSM6DSLSensor::disable_tilt_detection(void) 01197 { 01198 /* Disable tilt event on INT1. */ 01199 if ( LSM6DSL_ACC_GYRO_W_TiltEvOnInt1( (void *)this, LSM6DSL_ACC_GYRO_INT1_TILT_DISABLED ) == MEMS_ERROR ) 01200 { 01201 return 1; 01202 } 01203 01204 /* Disable tilt event on INT2. */ 01205 if ( LSM6DSL_ACC_GYRO_W_TiltEvOnInt2( (void *)this, LSM6DSL_ACC_GYRO_INT2_TILT_DISABLED ) == MEMS_ERROR ) 01206 { 01207 return 1; 01208 } 01209 01210 /* Disable tilt calculation. */ 01211 if ( LSM6DSL_ACC_GYRO_W_TILT( (void *)this, LSM6DSL_ACC_GYRO_TILT_DISABLED ) == MEMS_ERROR ) 01212 { 01213 return 1; 01214 } 01215 01216 /* Disable embedded functionalities */ 01217 if ( LSM6DSL_ACC_GYRO_W_FUNC_EN( (void *)this, LSM6DSL_ACC_GYRO_FUNC_EN_DISABLED ) == MEMS_ERROR ) 01218 { 01219 return 1; 01220 } 01221 01222 return 0; 01223 } 01224 01225 /** 01226 * @brief Enable the wake up detection for LSM6DSL accelerometer sensor 01227 * @param pin the interrupt pin to be used 01228 * @note This function sets the LSM6DSL accelerometer ODR to 416Hz and the LSM6DSL accelerometer full scale to 2g 01229 * @retval 0 in case of success, an error code otherwise 01230 */ 01231 int LSM6DSLSensor::enable_wake_up_detection(LSM6DSL_Interrupt_Pin_t pin) 01232 { 01233 /* Output Data Rate selection */ 01234 if( set_x_odr(416.0f) == 1 ) 01235 { 01236 return 1; 01237 } 01238 01239 /* Full scale selection. */ 01240 if( set_x_fs(2.0f) == 1 ) 01241 { 01242 return 1; 01243 } 01244 01245 /* WAKE_DUR setting */ 01246 if ( LSM6DSL_ACC_GYRO_W_WAKE_DUR( (void *)this, 0x00 ) == MEMS_ERROR ) 01247 { 01248 return 1; 01249 } 01250 01251 /* Set wake up threshold. */ 01252 if ( LSM6DSL_ACC_GYRO_W_WK_THS( (void *)this, 0x02 ) == MEMS_ERROR ) 01253 { 01254 return 1; 01255 } 01256 01257 /* Enable basic Interrupts */ 01258 if ( LSM6DSL_ACC_GYRO_W_BASIC_INT( (void *)this, LSM6DSL_ACC_GYRO_BASIC_INT_ENABLED ) == MEMS_ERROR ) 01259 { 01260 return 1; 01261 } 01262 01263 /* Enable wake up detection on either INT1 or INT2 pin */ 01264 switch (pin) 01265 { 01266 case LSM6DSL_INT1_PIN: 01267 if ( LSM6DSL_ACC_GYRO_W_WUEvOnInt1( (void *)this, LSM6DSL_ACC_GYRO_INT1_WU_ENABLED ) == MEMS_ERROR ) 01268 { 01269 return 1; 01270 } 01271 break; 01272 01273 case LSM6DSL_INT2_PIN: 01274 if ( LSM6DSL_ACC_GYRO_W_WUEvOnInt2( (void *)this, LSM6DSL_ACC_GYRO_INT2_WU_ENABLED ) == MEMS_ERROR ) 01275 { 01276 return 1; 01277 } 01278 break; 01279 01280 default: 01281 return 1; 01282 } 01283 01284 return 0; 01285 } 01286 01287 /** 01288 * @brief Disable the wake up detection for LSM6DSL accelerometer sensor 01289 * @retval 0 in case of success, an error code otherwise 01290 */ 01291 int LSM6DSLSensor::disable_wake_up_detection(void) 01292 { 01293 /* Disable wake up event on INT1 */ 01294 if ( LSM6DSL_ACC_GYRO_W_WUEvOnInt1( (void *)this, LSM6DSL_ACC_GYRO_INT1_WU_DISABLED ) == MEMS_ERROR ) 01295 { 01296 return 1; 01297 } 01298 01299 /* Disable wake up event on INT2 */ 01300 if ( LSM6DSL_ACC_GYRO_W_WUEvOnInt2( (void *)this, LSM6DSL_ACC_GYRO_INT2_WU_DISABLED ) == MEMS_ERROR ) 01301 { 01302 return 1; 01303 } 01304 01305 /* Disable basic Interrupts */ 01306 if ( LSM6DSL_ACC_GYRO_W_BASIC_INT( (void *)this, LSM6DSL_ACC_GYRO_BASIC_INT_DISABLED ) == MEMS_ERROR ) 01307 { 01308 return 1; 01309 } 01310 01311 /* WU_DUR setting */ 01312 if ( LSM6DSL_ACC_GYRO_W_WAKE_DUR( (void *)this, 0x00 ) == MEMS_ERROR ) 01313 { 01314 return 1; 01315 } 01316 01317 /* WU_THS setting */ 01318 if ( LSM6DSL_ACC_GYRO_W_WK_THS( (void *)this, 0x00 ) == MEMS_ERROR ) 01319 { 01320 return 1; 01321 } 01322 01323 return 0; 01324 } 01325 01326 /** 01327 * @brief Set the wake up threshold for LSM6DSL accelerometer sensor 01328 * @param thr the threshold to be set 01329 * @retval 0 in case of success, an error code otherwise 01330 */ 01331 int LSM6DSLSensor::set_wake_up_threshold(uint8_t thr) 01332 { 01333 if ( LSM6DSL_ACC_GYRO_W_WK_THS( (void *)this, thr ) == MEMS_ERROR ) 01334 { 01335 return 1; 01336 } 01337 01338 return 0; 01339 } 01340 01341 /** 01342 * @brief Enable the single tap detection for LSM6DSL accelerometer sensor 01343 * @param pin the interrupt pin to be used 01344 * @note This function sets the LSM6DSL accelerometer ODR to 416Hz and the LSM6DSL accelerometer full scale to 2g 01345 * @retval 0 in case of success, an error code otherwise 01346 */ 01347 int LSM6DSLSensor::enable_single_tap_detection(LSM6DSL_Interrupt_Pin_t pin) 01348 { 01349 /* Output Data Rate selection */ 01350 if( set_x_odr(416.0f) == 1 ) 01351 { 01352 return 1; 01353 } 01354 01355 /* Full scale selection. */ 01356 if( set_x_fs(2.0f) == 1 ) 01357 { 01358 return 1; 01359 } 01360 01361 /* Enable X direction in tap recognition. */ 01362 if ( LSM6DSL_ACC_GYRO_W_TAP_X_EN( (void *)this, LSM6DSL_ACC_GYRO_TAP_X_EN_ENABLED ) == MEMS_ERROR ) 01363 { 01364 return 1; 01365 } 01366 01367 /* Enable Y direction in tap recognition. */ 01368 if ( LSM6DSL_ACC_GYRO_W_TAP_Y_EN( (void *)this, LSM6DSL_ACC_GYRO_TAP_Y_EN_ENABLED ) == MEMS_ERROR ) 01369 { 01370 return 1; 01371 } 01372 01373 /* Enable Z direction in tap recognition. */ 01374 if ( LSM6DSL_ACC_GYRO_W_TAP_Z_EN( (void *)this, LSM6DSL_ACC_GYRO_TAP_Z_EN_ENABLED ) == MEMS_ERROR ) 01375 { 01376 return 1; 01377 } 01378 01379 /* Set tap threshold. */ 01380 if ( set_tap_threshold( LSM6DSL_TAP_THRESHOLD_MID_LOW ) == 1 ) 01381 { 01382 return 1; 01383 } 01384 01385 /* Set tap shock time window. */ 01386 if ( set_tap_shock_time( LSM6DSL_TAP_SHOCK_TIME_MID_HIGH ) == 1 ) 01387 { 01388 return 1; 01389 } 01390 01391 /* Set tap quiet time window. */ 01392 if ( set_tap_quiet_time( LSM6DSL_TAP_QUIET_TIME_MID_LOW ) == 1 ) 01393 { 01394 return 1; 01395 } 01396 01397 /* _NOTE_: Tap duration time window - don't care for single tap. */ 01398 01399 /* _NOTE_: Single/Double Tap event - don't care of this flag for single tap. */ 01400 01401 /* Enable basic Interrupts */ 01402 if ( LSM6DSL_ACC_GYRO_W_BASIC_INT( (void *)this, LSM6DSL_ACC_GYRO_BASIC_INT_ENABLED ) == MEMS_ERROR ) 01403 { 01404 return 1; 01405 } 01406 01407 /* Enable single tap on either INT1 or INT2 pin */ 01408 switch (pin) 01409 { 01410 case LSM6DSL_INT1_PIN: 01411 if ( LSM6DSL_ACC_GYRO_W_SingleTapOnInt1( (void *)this, LSM6DSL_ACC_GYRO_INT1_SINGLE_TAP_ENABLED ) == MEMS_ERROR ) 01412 { 01413 return 1; 01414 } 01415 break; 01416 01417 case LSM6DSL_INT2_PIN: 01418 if ( LSM6DSL_ACC_GYRO_W_SingleTapOnInt2( (void *)this, LSM6DSL_ACC_GYRO_INT2_SINGLE_TAP_ENABLED ) == MEMS_ERROR ) 01419 { 01420 return 1; 01421 } 01422 break; 01423 01424 default: 01425 return 1; 01426 } 01427 01428 return 0; 01429 } 01430 01431 /** 01432 * @brief Disable the single tap detection for LSM6DSL accelerometer sensor 01433 * @retval 0 in case of success, an error code otherwise 01434 */ 01435 int LSM6DSLSensor::disable_single_tap_detection(void) 01436 { 01437 /* Disable single tap interrupt on INT1 pin. */ 01438 if ( LSM6DSL_ACC_GYRO_W_SingleTapOnInt1( (void *)this, LSM6DSL_ACC_GYRO_INT1_SINGLE_TAP_DISABLED ) == MEMS_ERROR ) 01439 { 01440 return 1; 01441 } 01442 01443 /* Disable single tap interrupt on INT2 pin. */ 01444 if ( LSM6DSL_ACC_GYRO_W_SingleTapOnInt2( (void *)this, LSM6DSL_ACC_GYRO_INT2_SINGLE_TAP_DISABLED ) == MEMS_ERROR ) 01445 { 01446 return 1; 01447 } 01448 01449 /* Disable basic Interrupts */ 01450 if ( LSM6DSL_ACC_GYRO_W_BASIC_INT( (void *)this, LSM6DSL_ACC_GYRO_BASIC_INT_DISABLED ) == MEMS_ERROR ) 01451 { 01452 return 1; 01453 } 01454 01455 /* Reset tap threshold. */ 01456 if ( set_tap_threshold( 0x0 ) == 1 ) 01457 { 01458 return 1; 01459 } 01460 01461 /* Reset tap shock time window. */ 01462 if ( set_tap_shock_time( 0x0 ) == 1 ) 01463 { 01464 return 1; 01465 } 01466 01467 /* Reset tap quiet time window. */ 01468 if ( set_tap_quiet_time( 0x0 ) == 1 ) 01469 { 01470 return 1; 01471 } 01472 01473 /* _NOTE_: Tap duration time window - don't care for single tap. */ 01474 01475 /* _NOTE_: Single/Double Tap event - don't care of this flag for single tap. */ 01476 01477 /* Disable Z direction in tap recognition. */ 01478 if ( LSM6DSL_ACC_GYRO_W_TAP_Z_EN( (void *)this, LSM6DSL_ACC_GYRO_TAP_Z_EN_DISABLED ) == MEMS_ERROR ) 01479 { 01480 return 1; 01481 } 01482 01483 /* Disable Y direction in tap recognition. */ 01484 if ( LSM6DSL_ACC_GYRO_W_TAP_Y_EN( (void *)this, LSM6DSL_ACC_GYRO_TAP_Y_EN_DISABLED ) == MEMS_ERROR ) 01485 { 01486 return 1; 01487 } 01488 01489 /* Disable X direction in tap recognition. */ 01490 if ( LSM6DSL_ACC_GYRO_W_TAP_X_EN( (void *)this, LSM6DSL_ACC_GYRO_TAP_X_EN_DISABLED ) == MEMS_ERROR ) 01491 { 01492 return 1; 01493 } 01494 01495 return 0; 01496 } 01497 01498 /** 01499 * @brief Enable the double tap detection for LSM6DSL accelerometer sensor 01500 * @param pin the interrupt pin to be used 01501 * @note This function sets the LSM6DSL accelerometer ODR to 416Hz and the LSM6DSL accelerometer full scale to 2g 01502 * @retval 0 in case of success, an error code otherwise 01503 */ 01504 int LSM6DSLSensor::enable_double_tap_detection(LSM6DSL_Interrupt_Pin_t pin) 01505 { 01506 /* Output Data Rate selection */ 01507 if( set_x_odr(416.0f) == 1 ) 01508 { 01509 return 1; 01510 } 01511 01512 /* Full scale selection. */ 01513 if( set_x_fs(2.0f) == 1 ) 01514 { 01515 return 1; 01516 } 01517 01518 /* Enable X direction in tap recognition. */ 01519 if ( LSM6DSL_ACC_GYRO_W_TAP_X_EN( (void *)this, LSM6DSL_ACC_GYRO_TAP_X_EN_ENABLED ) == MEMS_ERROR ) 01520 { 01521 return 1; 01522 } 01523 01524 /* Enable Y direction in tap recognition. */ 01525 if ( LSM6DSL_ACC_GYRO_W_TAP_Y_EN( (void *)this, LSM6DSL_ACC_GYRO_TAP_Y_EN_ENABLED ) == MEMS_ERROR ) 01526 { 01527 return 1; 01528 } 01529 01530 /* Enable Z direction in tap recognition. */ 01531 if ( LSM6DSL_ACC_GYRO_W_TAP_Z_EN( (void *)this, LSM6DSL_ACC_GYRO_TAP_Z_EN_ENABLED ) == MEMS_ERROR ) 01532 { 01533 return 1; 01534 } 01535 01536 /* Set tap threshold. */ 01537 if ( set_tap_threshold( LSM6DSL_TAP_THRESHOLD_MID_LOW ) == 1 ) 01538 { 01539 return 1; 01540 } 01541 01542 /* Set tap shock time window. */ 01543 if ( set_tap_shock_time( LSM6DSL_TAP_SHOCK_TIME_HIGH ) == 1 ) 01544 { 01545 return 1; 01546 } 01547 01548 /* Set tap quiet time window. */ 01549 if ( set_tap_quiet_time( LSM6DSL_TAP_QUIET_TIME_HIGH ) == 1 ) 01550 { 01551 return 1; 01552 } 01553 01554 /* Set tap duration time window. */ 01555 if ( set_tap_duration_time( LSM6DSL_TAP_DURATION_TIME_MID ) == 1 ) 01556 { 01557 return 1; 01558 } 01559 01560 /* Single and double tap enabled. */ 01561 if ( LSM6DSL_ACC_GYRO_W_SINGLE_DOUBLE_TAP_EV( (void *)this, LSM6DSL_ACC_GYRO_SINGLE_DOUBLE_TAP_DOUBLE_TAP ) == MEMS_ERROR ) 01562 { 01563 return 1; 01564 } 01565 01566 /* Enable basic Interrupts */ 01567 if ( LSM6DSL_ACC_GYRO_W_BASIC_INT( (void *)this, LSM6DSL_ACC_GYRO_BASIC_INT_ENABLED ) == MEMS_ERROR ) 01568 { 01569 return 1; 01570 } 01571 01572 /* Enable double tap on either INT1 or INT2 pin */ 01573 switch (pin) 01574 { 01575 case LSM6DSL_INT1_PIN: 01576 if ( LSM6DSL_ACC_GYRO_W_TapEvOnInt1( (void *)this, LSM6DSL_ACC_GYRO_INT1_TAP_ENABLED ) == MEMS_ERROR ) 01577 { 01578 return 1; 01579 } 01580 break; 01581 01582 case LSM6DSL_INT2_PIN: 01583 if ( LSM6DSL_ACC_GYRO_W_TapEvOnInt2( (void *)this, LSM6DSL_ACC_GYRO_INT2_TAP_ENABLED ) == MEMS_ERROR ) 01584 { 01585 return 1; 01586 } 01587 break; 01588 01589 default: 01590 return 1; 01591 } 01592 01593 return 0; 01594 } 01595 01596 /** 01597 * @brief Disable the double tap detection for LSM6DSL accelerometer sensor 01598 * @retval 0 in case of success, an error code otherwise 01599 */ 01600 int LSM6DSLSensor::disable_double_tap_detection(void) 01601 { 01602 /* Disable double tap interrupt on INT1 pin. */ 01603 if ( LSM6DSL_ACC_GYRO_W_TapEvOnInt1( (void *)this, LSM6DSL_ACC_GYRO_INT1_TAP_DISABLED ) == MEMS_ERROR ) 01604 { 01605 return 1; 01606 } 01607 01608 /* Disable double tap interrupt on INT2 pin. */ 01609 if ( LSM6DSL_ACC_GYRO_W_TapEvOnInt2( (void *)this, LSM6DSL_ACC_GYRO_INT2_TAP_DISABLED ) == MEMS_ERROR ) 01610 { 01611 return 1; 01612 } 01613 01614 /* Disable basic Interrupts */ 01615 if ( LSM6DSL_ACC_GYRO_W_BASIC_INT( (void *)this, LSM6DSL_ACC_GYRO_BASIC_INT_DISABLED ) == MEMS_ERROR ) 01616 { 01617 return 1; 01618 } 01619 01620 /* Reset tap threshold. */ 01621 if ( set_tap_threshold( 0x0 ) == 1 ) 01622 { 01623 return 1; 01624 } 01625 01626 /* Reset tap shock time window. */ 01627 if ( set_tap_shock_time( 0x0 ) == 1 ) 01628 { 01629 return 1; 01630 } 01631 01632 /* Reset tap quiet time window. */ 01633 if ( set_tap_quiet_time( 0x0 ) == 1 ) 01634 { 01635 return 1; 01636 } 01637 01638 /* Reset tap duration time window. */ 01639 if ( set_tap_duration_time( 0x0 ) == 1 ) 01640 { 01641 return 1; 01642 } 01643 01644 /* Only single tap enabled. */ 01645 if ( LSM6DSL_ACC_GYRO_W_SINGLE_DOUBLE_TAP_EV( (void *)this, LSM6DSL_ACC_GYRO_SINGLE_DOUBLE_TAP_SINGLE_TAP ) == MEMS_ERROR ) 01646 { 01647 return 1; 01648 } 01649 01650 /* Disable Z direction in tap recognition. */ 01651 if ( LSM6DSL_ACC_GYRO_W_TAP_Z_EN( (void *)this, LSM6DSL_ACC_GYRO_TAP_Z_EN_DISABLED ) == MEMS_ERROR ) 01652 { 01653 return 1; 01654 } 01655 01656 /* Disable Y direction in tap recognition. */ 01657 if ( LSM6DSL_ACC_GYRO_W_TAP_Y_EN( (void *)this, LSM6DSL_ACC_GYRO_TAP_Y_EN_DISABLED ) == MEMS_ERROR ) 01658 { 01659 return 1; 01660 } 01661 01662 /* Disable X direction in tap recognition. */ 01663 if ( LSM6DSL_ACC_GYRO_W_TAP_X_EN( (void *)this, LSM6DSL_ACC_GYRO_TAP_X_EN_DISABLED ) == MEMS_ERROR ) 01664 { 01665 return 1; 01666 } 01667 01668 return 0; 01669 } 01670 01671 /** 01672 * @brief Set the tap threshold for LSM6DSL accelerometer sensor 01673 * @param thr the threshold to be set 01674 * @retval 0 in case of success, an error code otherwise 01675 */ 01676 int LSM6DSLSensor::set_tap_threshold(uint8_t thr) 01677 { 01678 if ( LSM6DSL_ACC_GYRO_W_TAP_THS( (void *)this, thr ) == MEMS_ERROR ) 01679 { 01680 return 1; 01681 } 01682 01683 return 0; 01684 } 01685 01686 /** 01687 * @brief Set the tap shock time window for LSM6DSL accelerometer sensor 01688 * @param time the shock time window to be set 01689 * @retval 0 in case of success, an error code otherwise 01690 */ 01691 int LSM6DSLSensor::set_tap_shock_time(uint8_t time) 01692 { 01693 if ( LSM6DSL_ACC_GYRO_W_SHOCK_Duration( (void *)this, time ) == MEMS_ERROR ) 01694 { 01695 return 1; 01696 } 01697 01698 return 0; 01699 } 01700 01701 /** 01702 * @brief Set the tap quiet time window for LSM6DSL accelerometer sensor 01703 * @param time the quiet time window to be set 01704 * @retval 0 in case of success, an error code otherwise 01705 */ 01706 int LSM6DSLSensor::set_tap_quiet_time(uint8_t time) 01707 { 01708 if ( LSM6DSL_ACC_GYRO_W_QUIET_Duration( (void *)this, time ) == MEMS_ERROR ) 01709 { 01710 return 1; 01711 } 01712 01713 return 0; 01714 } 01715 01716 /** 01717 * @brief Set the tap duration of the time window for LSM6DSL accelerometer sensor 01718 * @param time the duration of the time window to be set 01719 * @retval 0 in case of success, an error code otherwise 01720 */ 01721 int LSM6DSLSensor::set_tap_duration_time(uint8_t time) 01722 { 01723 if ( LSM6DSL_ACC_GYRO_W_DUR( (void *)this, time ) == MEMS_ERROR ) 01724 { 01725 return 1; 01726 } 01727 01728 return 0; 01729 } 01730 01731 /** 01732 * @brief Enable the 6D orientation detection for LSM6DSL accelerometer sensor 01733 * @param pin the interrupt pin to be used 01734 * @note This function sets the LSM6DSL accelerometer ODR to 416Hz and the LSM6DSL accelerometer full scale to 2g 01735 * @retval 0 in case of success, an error code otherwise 01736 */ 01737 int LSM6DSLSensor::enable_6d_orientation(LSM6DSL_Interrupt_Pin_t pin) 01738 { 01739 /* Output Data Rate selection */ 01740 if( set_x_odr(416.0f) == 1 ) 01741 { 01742 return 1; 01743 } 01744 01745 /* Full scale selection. */ 01746 if( set_x_fs(2.0f) == 1 ) 01747 { 01748 return 1; 01749 } 01750 01751 /* Set 6D threshold. */ 01752 if ( LSM6DSL_ACC_GYRO_W_SIXD_THS( (void *)this, LSM6DSL_ACC_GYRO_SIXD_THS_60_degree ) == MEMS_ERROR ) 01753 { 01754 return 1; 01755 } 01756 01757 /* Enable basic Interrupts */ 01758 if ( LSM6DSL_ACC_GYRO_W_BASIC_INT( (void *)this, LSM6DSL_ACC_GYRO_BASIC_INT_ENABLED ) == MEMS_ERROR ) 01759 { 01760 return 1; 01761 } 01762 01763 /* Enable 6D orientation on either INT1 or INT2 pin */ 01764 switch (pin) 01765 { 01766 case LSM6DSL_INT1_PIN: 01767 if ( LSM6DSL_ACC_GYRO_W_6DEvOnInt1( (void *)this, LSM6DSL_ACC_GYRO_INT1_6D_ENABLED ) == MEMS_ERROR ) 01768 { 01769 return 1; 01770 } 01771 break; 01772 01773 case LSM6DSL_INT2_PIN: 01774 if ( LSM6DSL_ACC_GYRO_W_6DEvOnInt2( (void *)this, LSM6DSL_ACC_GYRO_INT2_6D_ENABLED ) == MEMS_ERROR ) 01775 { 01776 return 1; 01777 } 01778 break; 01779 01780 default: 01781 return 1; 01782 } 01783 01784 return 0; 01785 } 01786 01787 /** 01788 * @brief Disable the 6D orientation detection for LSM6DSL accelerometer sensor 01789 * @retval 0 in case of success, an error code otherwise 01790 */ 01791 int LSM6DSLSensor::disable_6d_orientation(void) 01792 { 01793 /* Disable 6D orientation interrupt on INT1 pin. */ 01794 if ( LSM6DSL_ACC_GYRO_W_6DEvOnInt1( (void *)this, LSM6DSL_ACC_GYRO_INT1_6D_DISABLED ) == MEMS_ERROR ) 01795 { 01796 return 1; 01797 } 01798 01799 /* Disable 6D orientation interrupt on INT2 pin. */ 01800 if ( LSM6DSL_ACC_GYRO_W_6DEvOnInt2( (void *)this, LSM6DSL_ACC_GYRO_INT2_6D_DISABLED ) == MEMS_ERROR ) 01801 { 01802 return 1; 01803 } 01804 01805 /* Disable basic Interrupts */ 01806 if ( LSM6DSL_ACC_GYRO_W_BASIC_INT( (void *)this, LSM6DSL_ACC_GYRO_BASIC_INT_DISABLED ) == MEMS_ERROR ) 01807 { 01808 return 1; 01809 } 01810 01811 /* Reset 6D threshold. */ 01812 if ( LSM6DSL_ACC_GYRO_W_SIXD_THS( (void *)this, LSM6DSL_ACC_GYRO_SIXD_THS_80_degree ) == MEMS_ERROR ) 01813 { 01814 return 1; 01815 } 01816 01817 return 0; 01818 } 01819 01820 /** 01821 * @brief Get the 6D orientation XL axis for LSM6DSL accelerometer sensor 01822 * @param xl the pointer to the 6D orientation XL axis 01823 * @retval 0 in case of success, an error code otherwise 01824 */ 01825 int LSM6DSLSensor::get_6d_orientation_xl(uint8_t *xl) 01826 { 01827 LSM6DSL_ACC_GYRO_DSD_XL_t xl_raw; 01828 01829 if ( LSM6DSL_ACC_GYRO_R_DSD_XL( (void *)this, &xl_raw ) == MEMS_ERROR ) 01830 { 01831 return 1; 01832 } 01833 01834 switch( xl_raw ) 01835 { 01836 case LSM6DSL_ACC_GYRO_DSD_XL_DETECTED: 01837 *xl = 1; 01838 break; 01839 case LSM6DSL_ACC_GYRO_DSD_XL_NOT_DETECTED: 01840 *xl = 0; 01841 break; 01842 default: 01843 return 1; 01844 } 01845 01846 return 0; 01847 } 01848 01849 /** 01850 * @brief Get the 6D orientation XH axis for LSM6DSL accelerometer sensor 01851 * @param xh the pointer to the 6D orientation XH axis 01852 * @retval 0 in case of success, an error code otherwise 01853 */ 01854 int LSM6DSLSensor::get_6d_orientation_xh(uint8_t *xh) 01855 { 01856 LSM6DSL_ACC_GYRO_DSD_XH_t xh_raw; 01857 01858 if ( LSM6DSL_ACC_GYRO_R_DSD_XH( (void *)this, &xh_raw ) == MEMS_ERROR ) 01859 { 01860 return 1; 01861 } 01862 01863 switch( xh_raw ) 01864 { 01865 case LSM6DSL_ACC_GYRO_DSD_XH_DETECTED: 01866 *xh = 1; 01867 break; 01868 case LSM6DSL_ACC_GYRO_DSD_XH_NOT_DETECTED: 01869 *xh = 0; 01870 break; 01871 default: 01872 return 1; 01873 } 01874 01875 return 0; 01876 } 01877 01878 /** 01879 * @brief Get the 6D orientation YL axis for LSM6DSL accelerometer sensor 01880 * @param yl the pointer to the 6D orientation YL axis 01881 * @retval 0 in case of success, an error code otherwise 01882 */ 01883 int LSM6DSLSensor::get_6d_orientation_yl(uint8_t *yl) 01884 { 01885 LSM6DSL_ACC_GYRO_DSD_YL_t yl_raw; 01886 01887 if ( LSM6DSL_ACC_GYRO_R_DSD_YL( (void *)this, &yl_raw ) == MEMS_ERROR ) 01888 { 01889 return 1; 01890 } 01891 01892 switch( yl_raw ) 01893 { 01894 case LSM6DSL_ACC_GYRO_DSD_YL_DETECTED: 01895 *yl = 1; 01896 break; 01897 case LSM6DSL_ACC_GYRO_DSD_YL_NOT_DETECTED: 01898 *yl = 0; 01899 break; 01900 default: 01901 return 1; 01902 } 01903 01904 return 0; 01905 } 01906 01907 /** 01908 * @brief Get the 6D orientation YH axis for LSM6DSL accelerometer sensor 01909 * @param yh the pointer to the 6D orientation YH axis 01910 * @retval 0 in case of success, an error code otherwise 01911 */ 01912 int LSM6DSLSensor::get_6d_orientation_yh(uint8_t *yh) 01913 { 01914 LSM6DSL_ACC_GYRO_DSD_YH_t yh_raw; 01915 01916 if ( LSM6DSL_ACC_GYRO_R_DSD_YH( (void *)this, &yh_raw ) == MEMS_ERROR ) 01917 { 01918 return 1; 01919 } 01920 01921 switch( yh_raw ) 01922 { 01923 case LSM6DSL_ACC_GYRO_DSD_YH_DETECTED: 01924 *yh = 1; 01925 break; 01926 case LSM6DSL_ACC_GYRO_DSD_YH_NOT_DETECTED: 01927 *yh = 0; 01928 break; 01929 default: 01930 return 1; 01931 } 01932 01933 return 0; 01934 } 01935 01936 /** 01937 * @brief Get the 6D orientation ZL axis for LSM6DSL accelerometer sensor 01938 * @param zl the pointer to the 6D orientation ZL axis 01939 * @retval 0 in case of success, an error code otherwise 01940 */ 01941 int LSM6DSLSensor::get_6d_orientation_zl(uint8_t *zl) 01942 { 01943 LSM6DSL_ACC_GYRO_DSD_ZL_t zl_raw; 01944 01945 if ( LSM6DSL_ACC_GYRO_R_DSD_ZL( (void *)this, &zl_raw ) == MEMS_ERROR ) 01946 { 01947 return 1; 01948 } 01949 01950 switch( zl_raw ) 01951 { 01952 case LSM6DSL_ACC_GYRO_DSD_ZL_DETECTED: 01953 *zl = 1; 01954 break; 01955 case LSM6DSL_ACC_GYRO_DSD_ZL_NOT_DETECTED: 01956 *zl = 0; 01957 break; 01958 default: 01959 return 1; 01960 } 01961 01962 return 0; 01963 } 01964 01965 /** 01966 * @brief Get the 6D orientation ZH axis for LSM6DSL accelerometer sensor 01967 * @param zh the pointer to the 6D orientation ZH axis 01968 * @retval 0 in case of success, an error code otherwise 01969 */ 01970 int LSM6DSLSensor::get_6d_orientation_zh(uint8_t *zh) 01971 { 01972 LSM6DSL_ACC_GYRO_DSD_ZH_t zh_raw; 01973 01974 if ( LSM6DSL_ACC_GYRO_R_DSD_ZH( (void *)this, &zh_raw ) == MEMS_ERROR ) 01975 { 01976 return 1; 01977 } 01978 01979 switch( zh_raw ) 01980 { 01981 case LSM6DSL_ACC_GYRO_DSD_ZH_DETECTED: 01982 *zh = 1; 01983 break; 01984 case LSM6DSL_ACC_GYRO_DSD_ZH_NOT_DETECTED: 01985 *zh = 0; 01986 break; 01987 default: 01988 return 1; 01989 } 01990 01991 return 0; 01992 } 01993 01994 /** 01995 * @brief Get the status of all hardware events for LSM6DSL accelerometer sensor 01996 * @param status the pointer to the status of all hardware events 01997 * @retval 0 in case of success, an error code otherwise 01998 */ 01999 int LSM6DSLSensor::get_event_status(LSM6DSL_Event_Status_t *status) 02000 { 02001 uint8_t Wake_Up_Src = 0, Tap_Src = 0, D6D_Src = 0, Func_Src = 0, Md1_Cfg = 0, Md2_Cfg = 0, Int1_Ctrl = 0; 02002 02003 memset((void *)status, 0x0, sizeof(LSM6DSL_Event_Status_t)); 02004 02005 if(read_reg(LSM6DSL_ACC_GYRO_WAKE_UP_SRC, &Wake_Up_Src) != 0) 02006 { 02007 return 1; 02008 } 02009 02010 if(read_reg(LSM6DSL_ACC_GYRO_TAP_SRC, &Tap_Src) != 0) 02011 { 02012 return 1; 02013 } 02014 02015 if(read_reg(LSM6DSL_ACC_GYRO_D6D_SRC, &D6D_Src) != 0) 02016 { 02017 return 1; 02018 } 02019 02020 if(read_reg(LSM6DSL_ACC_GYRO_FUNC_SRC, &Func_Src) != 0) 02021 { 02022 return 1; 02023 } 02024 02025 if(read_reg(LSM6DSL_ACC_GYRO_MD1_CFG, &Md1_Cfg ) != 0 ) 02026 { 02027 return 1; 02028 } 02029 02030 if(read_reg(LSM6DSL_ACC_GYRO_MD2_CFG, &Md2_Cfg ) != 0) 02031 { 02032 return 1; 02033 } 02034 02035 if(read_reg(LSM6DSL_ACC_GYRO_INT1_CTRL, &Int1_Ctrl ) != 0) 02036 { 02037 return 1; 02038 } 02039 02040 if((Md1_Cfg & LSM6DSL_ACC_GYRO_INT1_FF_MASK) || (Md2_Cfg & LSM6DSL_ACC_GYRO_INT2_FF_MASK)) 02041 { 02042 if((Wake_Up_Src & LSM6DSL_ACC_GYRO_FF_EV_STATUS_MASK)) 02043 { 02044 status->FreeFallStatus = 1; 02045 } 02046 } 02047 02048 if((Md1_Cfg & LSM6DSL_ACC_GYRO_INT1_WU_MASK) || (Md2_Cfg & LSM6DSL_ACC_GYRO_INT2_WU_MASK)) 02049 { 02050 if((Wake_Up_Src & LSM6DSL_ACC_GYRO_WU_EV_STATUS_MASK)) 02051 { 02052 status->WakeUpStatus = 1; 02053 } 02054 } 02055 02056 if((Md1_Cfg & LSM6DSL_ACC_GYRO_INT1_SINGLE_TAP_MASK) || (Md2_Cfg & LSM6DSL_ACC_GYRO_INT2_SINGLE_TAP_MASK)) 02057 { 02058 if((Tap_Src & LSM6DSL_ACC_GYRO_SINGLE_TAP_EV_STATUS_MASK)) 02059 { 02060 status->TapStatus = 1; 02061 } 02062 } 02063 02064 if((Md1_Cfg & LSM6DSL_ACC_GYRO_INT1_TAP_MASK) || (Md2_Cfg & LSM6DSL_ACC_GYRO_INT2_TAP_MASK)) 02065 { 02066 if((Tap_Src & LSM6DSL_ACC_GYRO_DOUBLE_TAP_EV_STATUS_MASK)) 02067 { 02068 status->DoubleTapStatus = 1; 02069 } 02070 } 02071 02072 if((Md1_Cfg & LSM6DSL_ACC_GYRO_INT1_6D_MASK) || (Md2_Cfg & LSM6DSL_ACC_GYRO_INT2_6D_MASK)) 02073 { 02074 if((D6D_Src & LSM6DSL_ACC_GYRO_D6D_EV_STATUS_MASK)) 02075 { 02076 status->D6DOrientationStatus = 1; 02077 } 02078 } 02079 02080 if((Int1_Ctrl & LSM6DSL_ACC_GYRO_INT1_PEDO_MASK)) 02081 { 02082 if((Func_Src & LSM6DSL_ACC_GYRO_PEDO_EV_STATUS_MASK)) 02083 { 02084 status->StepStatus = 1; 02085 } 02086 } 02087 02088 if((Md1_Cfg & LSM6DSL_ACC_GYRO_INT1_TILT_MASK) || (Md2_Cfg & LSM6DSL_ACC_GYRO_INT2_TILT_MASK)) 02089 { 02090 if((Func_Src & LSM6DSL_ACC_GYRO_TILT_EV_STATUS_MASK)) 02091 { 02092 status->TiltStatus = 1; 02093 } 02094 } 02095 02096 return 0; 02097 } 02098 02099 /** 02100 * @brief Read the data from register 02101 * @param reg register address 02102 * @param data register data 02103 * @retval 0 in case of success, an error code otherwise 02104 */ 02105 int LSM6DSLSensor::read_reg( uint8_t reg, uint8_t *data ) 02106 { 02107 02108 if ( LSM6DSL_ACC_GYRO_read_reg( (void *)this, reg, data, 1 ) == MEMS_ERROR ) 02109 { 02110 return 1; 02111 } 02112 02113 return 0; 02114 } 02115 02116 /** 02117 * @brief Write the data to register 02118 * @param reg register address 02119 * @param data register data 02120 * @retval 0 in case of success, an error code otherwise 02121 */ 02122 int LSM6DSLSensor::write_reg( uint8_t reg, uint8_t data ) 02123 { 02124 02125 if ( LSM6DSL_ACC_GYRO_write_reg( (void *)this, reg, &data, 1 ) == MEMS_ERROR ) 02126 { 02127 return 1; 02128 } 02129 02130 return 0; 02131 } 02132 02133 02134 uint8_t LSM6DSL_io_write( void *handle, uint8_t WriteAddr, uint8_t *pBuffer, uint16_t nBytesToWrite ) 02135 { 02136 return ((LSM6DSLSensor *)handle)->io_write(pBuffer, WriteAddr, nBytesToWrite); 02137 } 02138 02139 uint8_t LSM6DSL_io_read( void *handle, uint8_t ReadAddr, uint8_t *pBuffer, uint16_t nBytesToRead ) 02140 { 02141 return ((LSM6DSLSensor *)handle)->io_read(pBuffer, ReadAddr, nBytesToRead); 02142 }
Generated on Tue Jul 12 2022 11:38:52 by
1.7.2