ST
/
LSM6DSOX
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.
Dependencies: X_NUCLEO_COMMON ST_INTERFACES
Embed:
(wiki syntax)
Show/hide line numbers
LSM6DSOXSensor.cpp
Go to the documentation of this file.
00001 /** 00002 ****************************************************************************** 00003 * @file LSM6DSOXSensor.cpp 00004 * @author SRA 00005 * @version V1.0.0 00006 * @date February 2019 00007 * @brief Implementation of an LSM6DSOX Inertial Measurement Unit (IMU) 6 axes 00008 * sensor. 00009 ****************************************************************************** 00010 * @attention 00011 * 00012 * <h2><center>© COPYRIGHT(c) 2019 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 "LSM6DSOXSensor.h" 00043 00044 00045 /* Class Implementation ------------------------------------------------------*/ 00046 00047 /** Constructor 00048 * @param spi object of an helper class which handles the SPI peripheral 00049 * @param cs_pin the chip select pin 00050 * @param int1_pin the interrupt 1 pin 00051 * @param int2_pin the interrupt 2 pin 00052 * @param spi_type the SPI type 00053 */ 00054 LSM6DSOXSensor::LSM6DSOXSensor(SPI *spi, PinName cs_pin, PinName int1_pin, PinName int2_pin, SPI_type_t spi_type) : _dev_spi(spi), _cs_pin(cs_pin), _int1_irq(int1_pin), _int2_irq(int2_pin), _spi_type(spi_type) 00055 { 00056 assert (spi); 00057 if (cs_pin == NC) 00058 { 00059 printf ("ERROR LSM6DSOX CS MUST NOT BE NC\n\r"); 00060 _dev_spi = NULL; 00061 _dev_i2c = NULL; 00062 return; 00063 } 00064 00065 _reg_ctx.write_reg = LSM6DSOX_io_write; 00066 _reg_ctx.read_reg = LSM6DSOX_io_read; 00067 _reg_ctx.handle = (void *)this; 00068 _cs_pin = 1; 00069 _dev_i2c = NULL; 00070 _address = 0; 00071 00072 if (_spi_type == SPI3W) 00073 { 00074 /* Enable SPI 3-Wires on the component */ 00075 uint8_t data = 0x0C; 00076 lsm6dsox_write_reg(&_reg_ctx, LSM6DSOX_CTRL3_C, &data, 1); 00077 } 00078 } 00079 00080 00081 /** Constructor 00082 * @param i2c object of an helper class which handles the I2C peripheral 00083 * @param address the address of the component's instance 00084 * @param int1_pin the interrupt 1 pin 00085 * @param int2_pin the interrupt 2 pin 00086 */ 00087 LSM6DSOXSensor::LSM6DSOXSensor(DevI2C *i2c, uint8_t address, PinName int1_pin, PinName int2_pin) : _dev_i2c(i2c), _address(address), _cs_pin(NC), _int1_irq(int1_pin), _int2_irq(int2_pin) 00088 { 00089 assert (i2c); 00090 _dev_spi = NULL; 00091 _reg_ctx.write_reg = LSM6DSOX_io_write; 00092 _reg_ctx.read_reg = LSM6DSOX_io_read; 00093 _reg_ctx.handle = (void *)this; 00094 } 00095 00096 /** 00097 * @brief Initializing the component 00098 * @param init pointer to device specific initalization structure 00099 * @retval 0 in case of success, an error code otherwise 00100 */ 00101 int LSM6DSOXSensor::init(void *init) 00102 { 00103 /* Enable register address automatically incremented during a multiple byte 00104 access with a serial interface. */ 00105 if (lsm6dsox_auto_increment_set(&_reg_ctx, PROPERTY_ENABLE) != 0) 00106 { 00107 return 1; 00108 } 00109 00110 /* Enable BDU */ 00111 if (lsm6dsox_block_data_update_set(&_reg_ctx, PROPERTY_ENABLE) != 0) 00112 { 00113 return 1; 00114 } 00115 00116 /* FIFO mode selection */ 00117 if (lsm6dsox_fifo_mode_set(&_reg_ctx, LSM6DSOX_BYPASS_MODE) != 0) 00118 { 00119 return 1; 00120 } 00121 00122 /* Output data rate selection - power down. */ 00123 if (lsm6dsox_xl_data_rate_set(&_reg_ctx, LSM6DSOX_XL_ODR_OFF) != 0) 00124 { 00125 return 1; 00126 } 00127 00128 /* Full scale selection. */ 00129 if (lsm6dsox_xl_full_scale_set(&_reg_ctx, LSM6DSOX_2g) != 0) 00130 { 00131 return 1; 00132 } 00133 00134 /* Output data rate selection - power down. */ 00135 if (lsm6dsox_gy_data_rate_set(&_reg_ctx, LSM6DSOX_GY_ODR_OFF) != 0) 00136 { 00137 return 1; 00138 } 00139 00140 /* Full scale selection. */ 00141 if (lsm6dsox_gy_full_scale_set(&_reg_ctx, LSM6DSOX_2000dps) != 0) 00142 { 00143 return 1; 00144 } 00145 00146 /* Select default output data rate. */ 00147 _x_last_odr = LSM6DSOX_XL_ODR_104Hz; 00148 00149 /* Select default output data rate. */ 00150 _g_last_odr = LSM6DSOX_GY_ODR_104Hz; 00151 00152 _x_is_enabled = 0; 00153 00154 _g_is_enabled = 0; 00155 00156 return 0; 00157 } 00158 00159 /** 00160 * @brief Read component ID 00161 * @param id the WHO_AM_I value 00162 * @retval 0 in case of success, an error code otherwise 00163 */ 00164 int LSM6DSOXSensor::read_id(uint8_t *id) 00165 { 00166 if (lsm6dsox_device_id_get(&_reg_ctx, id) != 0) 00167 { 00168 return 1; 00169 } 00170 00171 return 0; 00172 } 00173 00174 /** 00175 * @brief Enable the LSM6DSOX accelerometer sensor 00176 * @retval 0 in case of success, an error code otherwise 00177 */ 00178 int LSM6DSOXSensor::enable_x() 00179 { 00180 /* Check if the component is already enabled */ 00181 if (_x_is_enabled == 1U) 00182 { 00183 return 0; 00184 } 00185 00186 /* Output data rate selection. */ 00187 if (lsm6dsox_xl_data_rate_set(&_reg_ctx, _x_last_odr) != 0) 00188 { 00189 return 1; 00190 } 00191 00192 _x_is_enabled = 1; 00193 00194 return 0; 00195 } 00196 00197 /** 00198 * @brief Disable the LSM6DSOX accelerometer sensor 00199 * @retval 0 in case of success, an error code otherwise 00200 */ 00201 int LSM6DSOXSensor::disable_x() 00202 { 00203 /* Check if the component is already disabled */ 00204 if (_x_is_enabled == 0U) 00205 { 00206 return 0; 00207 } 00208 00209 /* Get current output data rate. */ 00210 if (lsm6dsox_xl_data_rate_get(&_reg_ctx, &_x_last_odr) != 0) 00211 { 00212 return 1; 00213 } 00214 00215 /* Output data rate selection - power down. */ 00216 if (lsm6dsox_xl_data_rate_set(&_reg_ctx, LSM6DSOX_XL_ODR_OFF) != 0) 00217 { 00218 return 1; 00219 } 00220 00221 _x_is_enabled = 0; 00222 00223 return 0; 00224 } 00225 00226 /** 00227 * @brief Get the LSM6DSOX accelerometer sensor sensitivity 00228 * @param sensitivity pointer where the sensitivity is written 00229 * @retval 0 in case of success, an error code otherwise 00230 */ 00231 int LSM6DSOXSensor::get_x_sensitivity(float *sensitivity) 00232 { 00233 int ret = 0; 00234 lsm6dsox_fs_xl_t full_scale; 00235 00236 /* Read actual full scale selection from sensor. */ 00237 if (lsm6dsox_xl_full_scale_get(&_reg_ctx, &full_scale) != 0) 00238 { 00239 return 1; 00240 } 00241 00242 /* Store the sensitivity based on actual full scale. */ 00243 switch (full_scale) 00244 { 00245 case LSM6DSOX_2g: 00246 *sensitivity = LSM6DSOX_ACC_SENSITIVITY_FS_2G; 00247 break; 00248 00249 case LSM6DSOX_4g: 00250 *sensitivity = LSM6DSOX_ACC_SENSITIVITY_FS_4G; 00251 break; 00252 00253 case LSM6DSOX_8g: 00254 *sensitivity = LSM6DSOX_ACC_SENSITIVITY_FS_8G; 00255 break; 00256 00257 case LSM6DSOX_16g: 00258 *sensitivity = LSM6DSOX_ACC_SENSITIVITY_FS_16G; 00259 break; 00260 00261 default: 00262 ret = 1; 00263 break; 00264 } 00265 00266 return ret; 00267 } 00268 00269 /** 00270 * @brief Get the LSM6DSOX accelerometer sensor output data rate 00271 * @param odr pointer where the output data rate is written 00272 * @retval 0 in case of success, an error code otherwise 00273 */ 00274 int LSM6DSOXSensor::get_x_odr(float *odr) 00275 { 00276 int ret = 0; 00277 lsm6dsox_odr_xl_t odr_low_level; 00278 00279 /* Get current output data rate. */ 00280 if (lsm6dsox_xl_data_rate_get(&_reg_ctx, &odr_low_level) != 0) 00281 { 00282 return 1; 00283 } 00284 00285 switch (odr_low_level) 00286 { 00287 case LSM6DSOX_XL_ODR_OFF: 00288 *odr = 0.0f; 00289 break; 00290 00291 case LSM6DSOX_XL_ODR_1Hz6: 00292 *odr = 1.6f; 00293 break; 00294 00295 case LSM6DSOX_XL_ODR_12Hz5: 00296 *odr = 12.5f; 00297 break; 00298 00299 case LSM6DSOX_XL_ODR_26Hz: 00300 *odr = 26.0f; 00301 break; 00302 00303 case LSM6DSOX_XL_ODR_52Hz: 00304 *odr = 52.0f; 00305 break; 00306 00307 case LSM6DSOX_XL_ODR_104Hz: 00308 *odr = 104.0f; 00309 break; 00310 00311 case LSM6DSOX_XL_ODR_208Hz: 00312 *odr = 208.0f; 00313 break; 00314 00315 case LSM6DSOX_XL_ODR_417Hz: 00316 *odr = 417.0f; 00317 break; 00318 00319 case LSM6DSOX_XL_ODR_833Hz: 00320 *odr = 833.0f; 00321 break; 00322 00323 case LSM6DSOX_XL_ODR_1667Hz: 00324 *odr = 1667.0f; 00325 break; 00326 00327 case LSM6DSOX_XL_ODR_3333Hz: 00328 *odr = 3333.0f; 00329 break; 00330 00331 case LSM6DSOX_XL_ODR_6667Hz: 00332 *odr = 6667.0f; 00333 break; 00334 00335 default: 00336 ret = 1; 00337 break; 00338 } 00339 00340 return ret; 00341 } 00342 00343 /** 00344 * @brief Set the LSM6DSOX accelerometer sensor output data rate 00345 * @param odr the output data rate value to be set 00346 * @retval 0 in case of success, an error code otherwise 00347 */ 00348 int LSM6DSOXSensor::set_x_odr(float odr) 00349 { 00350 return set_x_odr_with_mode(odr, LSM6DSOX_ACC_HIGH_PERFORMANCE_MODE); 00351 } 00352 00353 /** 00354 * @brief Set the LSM6DSOX accelerometer sensor output data rate with operating mode 00355 * @param odr the output data rate value to be set 00356 * @param mode the accelerometer operating mode 00357 * @note This function switches off the gyroscope if Ultra Low Power Mode is set 00358 * @retval 0 in case of success, an error code otherwise 00359 */ 00360 int LSM6DSOXSensor::set_x_odr_with_mode(float odr, LSM6DSOX_ACC_Operating_Mode_t mode) 00361 { 00362 int ret = 0; 00363 00364 switch (mode) 00365 { 00366 case LSM6DSOX_ACC_HIGH_PERFORMANCE_MODE: 00367 { 00368 /* We must uncheck Low Power and Ultra Low Power bits if they are enabled */ 00369 lsm6dsox_ctrl5_c_t val1; 00370 lsm6dsox_ctrl6_c_t val2; 00371 00372 if (lsm6dsox_read_reg(&_reg_ctx, LSM6DSOX_CTRL5_C, (uint8_t *)&val1, 1) != 0) 00373 { 00374 return 1; 00375 } 00376 00377 if (val1.xl_ulp_en) 00378 { 00379 /* Power off the accelerometer */ 00380 if (_x_is_enabled == 1U) 00381 { 00382 if (lsm6dsox_xl_data_rate_set(&_reg_ctx, LSM6DSOX_XL_ODR_OFF) != 0) 00383 { 00384 return 1; 00385 } 00386 } 00387 00388 val1.xl_ulp_en = 0; 00389 if (lsm6dsox_write_reg(&_reg_ctx, LSM6DSOX_CTRL5_C, (uint8_t *)&val1, 1) != 0) 00390 { 00391 return 1; 00392 } 00393 } 00394 00395 if (lsm6dsox_read_reg(&_reg_ctx, LSM6DSOX_CTRL6_C, (uint8_t *)&val2, 1) != 0) 00396 { 00397 return 1; 00398 } 00399 00400 if (val2.xl_hm_mode) 00401 { 00402 val2.xl_hm_mode = 0; 00403 if (lsm6dsox_write_reg(&_reg_ctx, LSM6DSOX_CTRL6_C, (uint8_t *)&val2, 1) != 0) 00404 { 00405 return 1; 00406 } 00407 } 00408 00409 /* ODR should be at least 12.5Hz */ 00410 if (odr < 12.5f) 00411 { 00412 odr = 12.5f; 00413 } 00414 break; 00415 } 00416 case LSM6DSOX_ACC_LOW_POWER_NORMAL_MODE: 00417 { 00418 /* We must uncheck Ultra Low Power bit if it is enabled */ 00419 /* and check the Low Power bit if it is unchecked */ 00420 lsm6dsox_ctrl5_c_t val1; 00421 lsm6dsox_ctrl6_c_t val2; 00422 00423 if (lsm6dsox_read_reg(&_reg_ctx, LSM6DSOX_CTRL5_C, (uint8_t *)&val1, 1) != 0) 00424 { 00425 return 1; 00426 } 00427 00428 if (val1.xl_ulp_en) 00429 { 00430 /* Power off the accelerometer */ 00431 if (_x_is_enabled == 1U) 00432 { 00433 if (lsm6dsox_xl_data_rate_set(&_reg_ctx, LSM6DSOX_XL_ODR_OFF) != 0) 00434 { 00435 return 1; 00436 } 00437 } 00438 00439 val1.xl_ulp_en = 0; 00440 if (lsm6dsox_write_reg(&_reg_ctx, LSM6DSOX_CTRL5_C, (uint8_t *)&val1, 1) != 0) 00441 { 00442 return 1; 00443 } 00444 } 00445 00446 if (lsm6dsox_read_reg(&_reg_ctx, LSM6DSOX_CTRL6_C, (uint8_t *)&val2, 1) != 0) 00447 { 00448 return 1; 00449 } 00450 00451 if (!val2.xl_hm_mode) 00452 { 00453 val2.xl_hm_mode = 1U; 00454 if (lsm6dsox_write_reg(&_reg_ctx, LSM6DSOX_CTRL6_C, (uint8_t *)&val2, 1) != 0) 00455 { 00456 return 1; 00457 } 00458 } 00459 00460 /* Now we need to limit the ODR to 208 Hz if it is higher */ 00461 if (odr > 208.0f) 00462 { 00463 odr = 208.0f; 00464 } 00465 break; 00466 } 00467 case LSM6DSOX_ACC_ULTRA_LOW_POWER_MODE: 00468 { 00469 /* We must uncheck Low Power bit if it is enabled */ 00470 /* and check the Ultra Low Power bit if it is unchecked */ 00471 /* We must switch off gyro otherwise Ultra Low Power does not work */ 00472 lsm6dsox_ctrl5_c_t val1; 00473 lsm6dsox_ctrl6_c_t val2; 00474 00475 if (lsm6dsox_read_reg(&_reg_ctx, LSM6DSOX_CTRL6_C, (uint8_t *)&val2, 1) != 0) 00476 { 00477 return 1; 00478 } 00479 00480 if (val2.xl_hm_mode) 00481 { 00482 val2.xl_hm_mode = 0; 00483 if (lsm6dsox_write_reg(&_reg_ctx, LSM6DSOX_CTRL6_C, (uint8_t *)&val2, 1) != 0) 00484 { 00485 return 1; 00486 } 00487 } 00488 00489 /* Disable Gyro */ 00490 if (_g_is_enabled == 1U) 00491 { 00492 if (disable_g() != 0) 00493 { 00494 return 1; 00495 } 00496 } 00497 00498 if (lsm6dsox_read_reg(&_reg_ctx, LSM6DSOX_CTRL5_C, (uint8_t *)&val1, 1) != 0) 00499 { 00500 return 1; 00501 } 00502 00503 if (!val1.xl_ulp_en) 00504 { 00505 /* Power off the accelerometer */ 00506 if (_x_is_enabled == 1U) 00507 { 00508 if (lsm6dsox_xl_data_rate_set(&_reg_ctx, LSM6DSOX_XL_ODR_OFF) != 0) 00509 { 00510 return 1; 00511 } 00512 } 00513 00514 val1.xl_ulp_en = 1U; 00515 if (lsm6dsox_write_reg(&_reg_ctx, LSM6DSOX_CTRL5_C, (uint8_t *)&val1, 1) != 0) 00516 { 00517 return 1; 00518 } 00519 } 00520 00521 /* Now we need to limit the ODR to 208 Hz if it is higher */ 00522 if (odr > 208.0f) 00523 { 00524 odr = 208.0f; 00525 } 00526 break; 00527 } 00528 default: 00529 ret = 1; 00530 break; 00531 } 00532 00533 /* Check if the component is enabled */ 00534 if (_x_is_enabled == 1U) 00535 { 00536 ret = set_x_odr_when_enabled(odr); 00537 } 00538 else 00539 { 00540 ret = set_x_odr_when_disabled(odr); 00541 } 00542 00543 return ret; 00544 } 00545 00546 /** 00547 * @brief Set the LSM6DSOX accelerometer sensor output data rate when enabled 00548 * @param odr the functional output data rate to be set 00549 * @retval 0 in case of success, an error code otherwise 00550 */ 00551 int LSM6DSOXSensor::set_x_odr_when_enabled(float odr) 00552 { 00553 lsm6dsox_odr_xl_t new_odr; 00554 00555 new_odr = (odr <= 1.6f) ? LSM6DSOX_XL_ODR_1Hz6 00556 : (odr <= 12.5f) ? LSM6DSOX_XL_ODR_12Hz5 00557 : (odr <= 26.0f) ? LSM6DSOX_XL_ODR_26Hz 00558 : (odr <= 52.0f) ? LSM6DSOX_XL_ODR_52Hz 00559 : (odr <= 104.0f) ? LSM6DSOX_XL_ODR_104Hz 00560 : (odr <= 208.0f) ? LSM6DSOX_XL_ODR_208Hz 00561 : (odr <= 417.0f) ? LSM6DSOX_XL_ODR_417Hz 00562 : (odr <= 833.0f) ? LSM6DSOX_XL_ODR_833Hz 00563 : (odr <= 1667.0f) ? LSM6DSOX_XL_ODR_1667Hz 00564 : (odr <= 3333.0f) ? LSM6DSOX_XL_ODR_3333Hz 00565 : LSM6DSOX_XL_ODR_6667Hz; 00566 00567 /* Output data rate selection. */ 00568 if (lsm6dsox_xl_data_rate_set(&_reg_ctx, new_odr) != 0) 00569 { 00570 return 1; 00571 } 00572 00573 return 0; 00574 } 00575 00576 /** 00577 * @brief Set the LSM6DSOX accelerometer sensor output data rate when disabled 00578 * @param odr the functional output data rate to be set 00579 * @retval 0 in case of success, an error code otherwise 00580 */ 00581 int LSM6DSOXSensor::set_x_odr_when_disabled(float odr) 00582 { 00583 _x_last_odr = (odr <= 1.6f) ? LSM6DSOX_XL_ODR_1Hz6 00584 : (odr <= 12.5f) ? LSM6DSOX_XL_ODR_12Hz5 00585 : (odr <= 26.0f) ? LSM6DSOX_XL_ODR_26Hz 00586 : (odr <= 52.0f) ? LSM6DSOX_XL_ODR_52Hz 00587 : (odr <= 104.0f) ? LSM6DSOX_XL_ODR_104Hz 00588 : (odr <= 208.0f) ? LSM6DSOX_XL_ODR_208Hz 00589 : (odr <= 417.0f) ? LSM6DSOX_XL_ODR_417Hz 00590 : (odr <= 833.0f) ? LSM6DSOX_XL_ODR_833Hz 00591 : (odr <= 1667.0f) ? LSM6DSOX_XL_ODR_1667Hz 00592 : (odr <= 3333.0f) ? LSM6DSOX_XL_ODR_3333Hz 00593 : LSM6DSOX_XL_ODR_6667Hz; 00594 00595 return 0; 00596 } 00597 00598 00599 /** 00600 * @brief Get the LSM6DSOX accelerometer sensor full scale 00601 * @param full_scale pointer where the full scale is written 00602 * @retval 0 in case of success, an error code otherwise 00603 */ 00604 int LSM6DSOXSensor::get_x_fs(float *full_scale) 00605 { 00606 int ret = 0; 00607 lsm6dsox_fs_xl_t fs_low_level; 00608 00609 /* Read actual full scale selection from sensor. */ 00610 if (lsm6dsox_xl_full_scale_get(&_reg_ctx, &fs_low_level) != 0) 00611 { 00612 return 1; 00613 } 00614 00615 switch (fs_low_level) 00616 { 00617 case LSM6DSOX_2g: 00618 *full_scale = 2.0f; 00619 break; 00620 00621 case LSM6DSOX_4g: 00622 *full_scale = 4.0f; 00623 break; 00624 00625 case LSM6DSOX_8g: 00626 *full_scale = 8.0f; 00627 break; 00628 00629 case LSM6DSOX_16g: 00630 *full_scale = 16.0f; 00631 break; 00632 00633 default: 00634 ret = 1; 00635 break; 00636 } 00637 00638 return ret; 00639 } 00640 00641 /** 00642 * @brief Set the LSM6DSOX accelerometer sensor full scale 00643 * @param full_scale the functional full scale to be set 00644 * @retval 0 in case of success, an error code otherwise 00645 */ 00646 int LSM6DSOXSensor::set_x_fs(float full_scale) 00647 { 00648 lsm6dsox_fs_xl_t new_fs; 00649 00650 /* Seems like MISRA C-2012 rule 14.3a violation but only from single file statical analysis point of view because 00651 the parameter passed to the function is not known at the moment of analysis */ 00652 new_fs = (full_scale <= 2.0f) ? LSM6DSOX_2g 00653 : (full_scale <= 4.0f) ? LSM6DSOX_4g 00654 : (full_scale <= 8.0f) ? LSM6DSOX_8g 00655 : LSM6DSOX_16g; 00656 00657 if (lsm6dsox_xl_full_scale_set(&_reg_ctx, new_fs) != 0) 00658 { 00659 return 1; 00660 } 00661 00662 return 0; 00663 } 00664 00665 /** 00666 * @brief Get the LSM6DSOX accelerometer sensor raw axes 00667 * @param value pointer where the raw values of the axes are written 00668 * @retval 0 in case of success, an error code otherwise 00669 */ 00670 int LSM6DSOXSensor::get_x_axes_raw(int16_t *value) 00671 { 00672 axis3bit16_t data_raw; 00673 00674 /* Read raw data values. */ 00675 if (lsm6dsox_acceleration_raw_get(&_reg_ctx, data_raw.u8bit) != 0) 00676 { 00677 return 1; 00678 } 00679 00680 /* Format the data. */ 00681 value[0] = data_raw.i16bit[0]; 00682 value[1] = data_raw.i16bit[1]; 00683 value[2] = data_raw.i16bit[2]; 00684 00685 return 0; 00686 } 00687 00688 00689 /** 00690 * @brief Get the LSM6DSOX accelerometer sensor axes 00691 * @param acceleration pointer where the values of the axes are written 00692 * @retval 0 in case of success, an error code otherwise 00693 */ 00694 int LSM6DSOXSensor::get_x_axes(int32_t *acceleration) 00695 { 00696 axis3bit16_t data_raw; 00697 float sensitivity = 0.0f; 00698 00699 /* Read raw data values. */ 00700 if (lsm6dsox_acceleration_raw_get(&_reg_ctx, data_raw.u8bit) != 0) 00701 { 00702 return 1; 00703 } 00704 00705 /* Get LSM6DSOX actual sensitivity. */ 00706 if (get_x_sensitivity(&sensitivity) != 0) 00707 { 00708 return 1; 00709 } 00710 00711 /* Calculate the data. */ 00712 acceleration[0] = (int32_t)((float)((float)data_raw.i16bit[0] * sensitivity)); 00713 acceleration[1] = (int32_t)((float)((float)data_raw.i16bit[1] * sensitivity)); 00714 acceleration[2] = (int32_t)((float)((float)data_raw.i16bit[2] * sensitivity)); 00715 00716 return 0; 00717 } 00718 00719 00720 /** 00721 * @brief Enable the LSM6DSOX gyroscope sensor 00722 * @retval 0 in case of success, an error code otherwise 00723 */ 00724 int LSM6DSOXSensor::enable_g() 00725 { 00726 /* Check if the component is already enabled */ 00727 if (_g_is_enabled == 1U) 00728 { 00729 return 0; 00730 } 00731 00732 /* Output data rate selection. */ 00733 if (lsm6dsox_gy_data_rate_set(&_reg_ctx, _g_last_odr) != 0) 00734 { 00735 return 1; 00736 } 00737 00738 _g_is_enabled = 1; 00739 00740 return 0; 00741 } 00742 00743 00744 /** 00745 * @brief Disable the LSM6DSOX gyroscope sensor 00746 * @retval 0 in case of success, an error code otherwise 00747 */ 00748 int LSM6DSOXSensor::disable_g() 00749 { 00750 /* Check if the component is already disabled */ 00751 if (_g_is_enabled == 0U) 00752 { 00753 return 0; 00754 } 00755 00756 /* Get current output data rate. */ 00757 if (lsm6dsox_gy_data_rate_get(&_reg_ctx, &_g_last_odr) != 0) 00758 { 00759 return 1; 00760 } 00761 00762 /* Output data rate selection - power down. */ 00763 if (lsm6dsox_gy_data_rate_set(&_reg_ctx, LSM6DSOX_GY_ODR_OFF) != 0) 00764 { 00765 return 1; 00766 } 00767 00768 _g_is_enabled = 0; 00769 00770 return 0; 00771 } 00772 00773 /** 00774 * @brief Get the LSM6DSOX gyroscope sensor sensitivity 00775 * @param sensitivity pointer where the sensitivity is written 00776 * @retval 0 in case of success, an error code otherwise 00777 */ 00778 int LSM6DSOXSensor::get_g_sensitivity(float *sensitivity) 00779 { 00780 int ret = 0; 00781 lsm6dsox_fs_g_t full_scale; 00782 00783 /* Read actual full scale selection from sensor. */ 00784 if (lsm6dsox_gy_full_scale_get(&_reg_ctx, &full_scale) != 0) 00785 { 00786 return 1; 00787 } 00788 00789 /* Store the sensitivity based on actual full scale. */ 00790 switch (full_scale) 00791 { 00792 case LSM6DSOX_125dps: 00793 *sensitivity = LSM6DSOX_GYRO_SENSITIVITY_FS_125DPS; 00794 break; 00795 00796 case LSM6DSOX_250dps: 00797 *sensitivity = LSM6DSOX_GYRO_SENSITIVITY_FS_250DPS; 00798 break; 00799 00800 case LSM6DSOX_500dps: 00801 *sensitivity = LSM6DSOX_GYRO_SENSITIVITY_FS_500DPS; 00802 break; 00803 00804 case LSM6DSOX_1000dps: 00805 *sensitivity = LSM6DSOX_GYRO_SENSITIVITY_FS_1000DPS; 00806 break; 00807 00808 case LSM6DSOX_2000dps: 00809 *sensitivity = LSM6DSOX_GYRO_SENSITIVITY_FS_2000DPS; 00810 break; 00811 00812 default: 00813 ret = 1; 00814 break; 00815 } 00816 00817 return ret; 00818 } 00819 00820 /** 00821 * @brief Get the LSM6DSOX gyroscope sensor output data rate 00822 * @param odr pointer where the output data rate is written 00823 * @retval 0 in case of success, an error code otherwise 00824 */ 00825 int LSM6DSOXSensor::get_g_odr(float *odr) 00826 { 00827 int ret = 0; 00828 lsm6dsox_odr_g_t odr_low_level; 00829 00830 /* Get current output data rate. */ 00831 if (lsm6dsox_gy_data_rate_get(&_reg_ctx, &odr_low_level) != 0) 00832 { 00833 return 1; 00834 } 00835 00836 switch (odr_low_level) 00837 { 00838 case LSM6DSOX_GY_ODR_OFF: 00839 *odr = 0.0f; 00840 break; 00841 00842 case LSM6DSOX_GY_ODR_12Hz5: 00843 *odr = 12.5f; 00844 break; 00845 00846 case LSM6DSOX_GY_ODR_26Hz: 00847 *odr = 26.0f; 00848 break; 00849 00850 case LSM6DSOX_GY_ODR_52Hz: 00851 *odr = 52.0f; 00852 break; 00853 00854 case LSM6DSOX_GY_ODR_104Hz: 00855 *odr = 104.0f; 00856 break; 00857 00858 case LSM6DSOX_GY_ODR_208Hz: 00859 *odr = 208.0f; 00860 break; 00861 00862 case LSM6DSOX_GY_ODR_417Hz: 00863 *odr = 417.0f; 00864 break; 00865 00866 case LSM6DSOX_GY_ODR_833Hz: 00867 *odr = 833.0f; 00868 break; 00869 00870 case LSM6DSOX_GY_ODR_1667Hz: 00871 *odr = 1667.0f; 00872 break; 00873 00874 case LSM6DSOX_GY_ODR_3333Hz: 00875 *odr = 3333.0f; 00876 break; 00877 00878 case LSM6DSOX_GY_ODR_6667Hz: 00879 *odr = 6667.0f; 00880 break; 00881 00882 default: 00883 ret = 1; 00884 break; 00885 } 00886 00887 return ret; 00888 } 00889 00890 /** 00891 * @brief Set the LSM6DSOX gyroscope sensor output data rate 00892 * @param odr the output data rate value to be set 00893 * @retval 0 in case of success, an error code otherwise 00894 */ 00895 int LSM6DSOXSensor::set_g_odr(float odr) 00896 { 00897 return set_g_odr_with_mode(odr, LSM6DSOX_GYRO_HIGH_PERFORMANCE_MODE); 00898 } 00899 00900 /** 00901 * @brief Set the LSM6DSOX gyroscope sensor output data rate with operating mode 00902 * @param odr the output data rate value to be set 00903 * @param mode the gyroscope operating mode 00904 * @retval 0 in case of success, an error code otherwise 00905 */ 00906 int LSM6DSOXSensor::set_g_odr_with_mode(float odr, LSM6DSOX_GYRO_Operating_Mode_t mode) 00907 { 00908 int ret = 0; 00909 00910 switch (mode) 00911 { 00912 case LSM6DSOX_GYRO_HIGH_PERFORMANCE_MODE: 00913 { 00914 /* We must uncheck Low Power bit if it is enabled */ 00915 lsm6dsox_ctrl7_g_t val1; 00916 00917 if (lsm6dsox_read_reg(&_reg_ctx, LSM6DSOX_CTRL7_G, (uint8_t *)&val1, 1) != 0) 00918 { 00919 return 1; 00920 } 00921 00922 if (val1.g_hm_mode) 00923 { 00924 val1.g_hm_mode = 0; 00925 if (lsm6dsox_write_reg(&_reg_ctx, LSM6DSOX_CTRL7_G, (uint8_t *)&val1, 1) != 0) 00926 { 00927 return 1; 00928 } 00929 } 00930 break; 00931 } 00932 case LSM6DSOX_GYRO_LOW_POWER_NORMAL_MODE: 00933 { 00934 /* We must check the Low Power bit if it is unchecked */ 00935 lsm6dsox_ctrl7_g_t val1; 00936 00937 if (lsm6dsox_read_reg(&_reg_ctx, LSM6DSOX_CTRL7_G, (uint8_t *)&val1, 1) != 0) 00938 { 00939 return 1; 00940 } 00941 00942 if (!val1.g_hm_mode) 00943 { 00944 val1.g_hm_mode = 1U; 00945 if (lsm6dsox_write_reg(&_reg_ctx, LSM6DSOX_CTRL7_G, (uint8_t *)&val1, 1) != 0) 00946 { 00947 return 1; 00948 } 00949 } 00950 00951 /* Now we need to limit the ODR to 208 Hz if it is higher */ 00952 if (odr > 208.0f) 00953 { 00954 odr = 208.0f; 00955 } 00956 break; 00957 } 00958 default: 00959 ret = 1; 00960 break; 00961 } 00962 00963 /* Check if the component is enabled */ 00964 if (_g_is_enabled == 1U) 00965 { 00966 ret = set_g_odr_when_enabled(odr); 00967 } 00968 else 00969 { 00970 ret = set_g_odr_when_disabled(odr); 00971 } 00972 00973 return ret; 00974 } 00975 00976 /** 00977 * @brief Set the LSM6DSOX gyroscope sensor output data rate when enabled 00978 * @param odr the functional output data rate to be set 00979 * @retval 0 in case of success, an error code otherwise 00980 */ 00981 int LSM6DSOXSensor::set_g_odr_when_enabled(float odr) 00982 { 00983 lsm6dsox_odr_g_t new_odr; 00984 00985 new_odr = (odr <= 12.5f) ? LSM6DSOX_GY_ODR_12Hz5 00986 : (odr <= 26.0f) ? LSM6DSOX_GY_ODR_26Hz 00987 : (odr <= 52.0f) ? LSM6DSOX_GY_ODR_52Hz 00988 : (odr <= 104.0f) ? LSM6DSOX_GY_ODR_104Hz 00989 : (odr <= 208.0f) ? LSM6DSOX_GY_ODR_208Hz 00990 : (odr <= 417.0f) ? LSM6DSOX_GY_ODR_417Hz 00991 : (odr <= 833.0f) ? LSM6DSOX_GY_ODR_833Hz 00992 : (odr <= 1667.0f) ? LSM6DSOX_GY_ODR_1667Hz 00993 : (odr <= 3333.0f) ? LSM6DSOX_GY_ODR_3333Hz 00994 : LSM6DSOX_GY_ODR_6667Hz; 00995 00996 /* Output data rate selection. */ 00997 if (lsm6dsox_gy_data_rate_set(&_reg_ctx, new_odr) != 0) 00998 { 00999 return 1; 01000 } 01001 01002 return 0; 01003 } 01004 01005 /** 01006 * @brief Set the LSM6DSOX gyroscope sensor output data rate when disabled 01007 * @param odr the functional output data rate to be set 01008 * @retval 0 in case of success, an error code otherwise 01009 */ 01010 int LSM6DSOXSensor::set_g_odr_when_disabled(float odr) 01011 { 01012 _g_last_odr = (odr <= 12.5f) ? LSM6DSOX_GY_ODR_12Hz5 01013 : (odr <= 26.0f) ? LSM6DSOX_GY_ODR_26Hz 01014 : (odr <= 52.0f) ? LSM6DSOX_GY_ODR_52Hz 01015 : (odr <= 104.0f) ? LSM6DSOX_GY_ODR_104Hz 01016 : (odr <= 208.0f) ? LSM6DSOX_GY_ODR_208Hz 01017 : (odr <= 417.0f) ? LSM6DSOX_GY_ODR_417Hz 01018 : (odr <= 833.0f) ? LSM6DSOX_GY_ODR_833Hz 01019 : (odr <= 1667.0f) ? LSM6DSOX_GY_ODR_1667Hz 01020 : (odr <= 3333.0f) ? LSM6DSOX_GY_ODR_3333Hz 01021 : LSM6DSOX_GY_ODR_6667Hz; 01022 01023 return 0; 01024 } 01025 01026 01027 /** 01028 * @brief Get the LSM6DSOX gyroscope sensor full scale 01029 * @param full_scale pointer where the full scale is written 01030 * @retval 0 in case of success, an error code otherwise 01031 */ 01032 int LSM6DSOXSensor::get_g_fs(float *full_scale) 01033 { 01034 int ret = 0; 01035 lsm6dsox_fs_g_t fs_low_level; 01036 01037 /* Read actual full scale selection from sensor. */ 01038 if (lsm6dsox_gy_full_scale_get(&_reg_ctx, &fs_low_level) != 0) 01039 { 01040 return 1; 01041 } 01042 01043 switch (fs_low_level) 01044 { 01045 case LSM6DSOX_125dps: 01046 *full_scale = 125.0f; 01047 break; 01048 01049 case LSM6DSOX_250dps: 01050 *full_scale = 250.0f; 01051 break; 01052 01053 case LSM6DSOX_500dps: 01054 *full_scale = 500.0f; 01055 break; 01056 01057 case LSM6DSOX_1000dps: 01058 *full_scale = 1000.0f; 01059 break; 01060 01061 case LSM6DSOX_2000dps: 01062 *full_scale = 2000.0f; 01063 break; 01064 01065 default: 01066 ret = 1; 01067 break; 01068 } 01069 01070 return ret; 01071 } 01072 01073 /** 01074 * @brief Set the LSM6DSOX gyroscope sensor full scale 01075 * @param full_scale the functional full scale to be set 01076 * @retval 0 in case of success, an error code otherwise 01077 */ 01078 int LSM6DSOXSensor::set_g_fs(float full_scale) 01079 { 01080 lsm6dsox_fs_g_t new_fs; 01081 01082 new_fs = (full_scale <= 125.0f) ? LSM6DSOX_125dps 01083 : (full_scale <= 250.0f) ? LSM6DSOX_250dps 01084 : (full_scale <= 500.0f) ? LSM6DSOX_500dps 01085 : (full_scale <= 1000.0f) ? LSM6DSOX_1000dps 01086 : LSM6DSOX_2000dps; 01087 01088 if (lsm6dsox_gy_full_scale_set(&_reg_ctx, new_fs) != 0) 01089 { 01090 return 1; 01091 } 01092 01093 return 0; 01094 } 01095 01096 /** 01097 * @brief Get the LSM6DSOX gyroscope sensor raw axes 01098 * @param value pointer where the raw values of the axes are written 01099 * @retval 0 in case of success, an error code otherwise 01100 */ 01101 int LSM6DSOXSensor::get_g_axes_raw(int16_t *value) 01102 { 01103 axis3bit16_t data_raw; 01104 01105 /* Read raw data values. */ 01106 if (lsm6dsox_angular_rate_raw_get(&_reg_ctx, data_raw.u8bit) != 0) 01107 { 01108 return 1; 01109 } 01110 01111 /* Format the data. */ 01112 value[0] = data_raw.i16bit[0]; 01113 value[1] = data_raw.i16bit[1]; 01114 value[2] = data_raw.i16bit[2]; 01115 01116 return 0; 01117 } 01118 01119 01120 /** 01121 * @brief Get the LSM6DSOX gyroscope sensor axes 01122 * @param angular_rate pointer where the values of the axes are written 01123 * @retval 0 in case of success, an error code otherwise 01124 */ 01125 int LSM6DSOXSensor::get_g_axes(int32_t *angular_rate) 01126 { 01127 axis3bit16_t data_raw; 01128 float sensitivity; 01129 01130 /* Read raw data values. */ 01131 if (lsm6dsox_angular_rate_raw_get(&_reg_ctx, data_raw.u8bit) != 0) 01132 { 01133 return 1; 01134 } 01135 01136 /* Get LSM6DSOX actual sensitivity. */ 01137 if (get_g_sensitivity(&sensitivity) != 0) 01138 { 01139 return 1; 01140 } 01141 01142 /* Calculate the data. */ 01143 angular_rate[0] = (int32_t)((float)((float)data_raw.i16bit[0] * sensitivity)); 01144 angular_rate[1] = (int32_t)((float)((float)data_raw.i16bit[1] * sensitivity)); 01145 angular_rate[2] = (int32_t)((float)((float)data_raw.i16bit[2] * sensitivity)); 01146 01147 return 0; 01148 } 01149 01150 01151 /** 01152 * @brief Get the LSM6DSOX register value 01153 * @param reg address to be read 01154 * @param data pointer where the value is written 01155 * @retval 0 in case of success, an error code otherwise 01156 */ 01157 int LSM6DSOXSensor::read_reg(uint8_t reg, uint8_t *data) 01158 { 01159 if (lsm6dsox_read_reg(&_reg_ctx, reg, data, 1) != 0) 01160 { 01161 return 1; 01162 } 01163 01164 return 0; 01165 } 01166 01167 01168 /** 01169 * @brief Set the LSM6DSOX register value 01170 * @param reg address to be written 01171 * @param data value to be written 01172 * @retval 0 in case of success, an error code otherwise 01173 */ 01174 int LSM6DSOXSensor::write_reg(uint8_t reg, uint8_t data) 01175 { 01176 if (lsm6dsox_write_reg(&_reg_ctx, reg, &data, 1) != 0) 01177 { 01178 return 1; 01179 } 01180 01181 return 0; 01182 } 01183 01184 /** 01185 * @brief Set the interrupt latch 01186 * @param status value to be written 01187 * @retval 0 in case of success, an error code otherwise 01188 */ 01189 int LSM6DSOXSensor::set_interrupt_latch(uint8_t status) 01190 { 01191 if (status > 1U) 01192 { 01193 return 1; 01194 } 01195 01196 if (lsm6dsox_int_notification_set(&_reg_ctx, (lsm6dsox_lir_t)status) != 0) 01197 { 01198 return 1; 01199 } 01200 01201 return 0; 01202 } 01203 01204 /** 01205 * @brief Enable free fall detection 01206 * @param int_pin interrupt pin line to be used 01207 * @retval 0 in case of success, an error code otherwise 01208 */ 01209 int LSM6DSOXSensor::enable_free_fall_detection(LSM6DSOX_Interrupt_Pin_t int_pin) 01210 { 01211 int ret = 0; 01212 lsm6dsox_pin_int1_route_t val1; 01213 lsm6dsox_pin_int2_route_t val2; 01214 01215 /* Output Data Rate selection */ 01216 if (set_x_odr(416.0f) != 0) 01217 { 01218 return 1; 01219 } 01220 01221 /* Full scale selection */ 01222 if (set_x_fs(2.0f) != 0) 01223 { 01224 return 1; 01225 } 01226 01227 /* FF_DUR setting */ 01228 if (lsm6dsox_ff_dur_set(&_reg_ctx, 0x06) != 0) 01229 { 01230 return 1; 01231 } 01232 01233 /* WAKE_DUR setting */ 01234 if (lsm6dsox_wkup_dur_set(&_reg_ctx, 0x00) != 0) 01235 { 01236 return 1; 01237 } 01238 01239 /* SLEEP_DUR setting */ 01240 if (lsm6dsox_act_sleep_dur_set(&_reg_ctx, 0x00) != 0) 01241 { 01242 return 1; 01243 } 01244 01245 /* FF_THS setting */ 01246 if (lsm6dsox_ff_threshold_set(&_reg_ctx, LSM6DSOX_FF_TSH_312mg) != 0) 01247 { 01248 return 1; 01249 } 01250 01251 /* Enable free fall event on either INT1 or INT2 pin */ 01252 switch (int_pin) 01253 { 01254 case LSM6DSOX_INT1_PIN: 01255 if (lsm6dsox_pin_int1_route_get(&_reg_ctx, &val1) != 0) 01256 { 01257 return 1; 01258 } 01259 01260 val1.free_fall = PROPERTY_ENABLE; 01261 01262 if (lsm6dsox_pin_int1_route_set(&_reg_ctx, val1) != 0) 01263 { 01264 return 1; 01265 } 01266 break; 01267 01268 case LSM6DSOX_INT2_PIN: 01269 if (lsm6dsox_pin_int2_route_get(&_reg_ctx, NULL, &val2) != 0) 01270 { 01271 return 1; 01272 } 01273 01274 val2.free_fall = PROPERTY_ENABLE; 01275 01276 if (lsm6dsox_pin_int2_route_set(&_reg_ctx, NULL, val2) != 0) 01277 { 01278 return 1; 01279 } 01280 break; 01281 01282 default: 01283 ret = 1; 01284 break; 01285 } 01286 01287 return ret; 01288 } 01289 01290 /** 01291 * @brief Disable free fall detection 01292 * @retval 0 in case of success, an error code otherwise 01293 */ 01294 int LSM6DSOXSensor::disable_free_fall_detection() 01295 { 01296 lsm6dsox_pin_int1_route_t val1; 01297 lsm6dsox_pin_int2_route_t val2; 01298 01299 /* Disable free fall event on both INT1 and INT2 pins */ 01300 if (lsm6dsox_pin_int1_route_get(&_reg_ctx, &val1) != 0) 01301 { 01302 return 1; 01303 } 01304 01305 val1.free_fall = PROPERTY_DISABLE; 01306 01307 if (lsm6dsox_pin_int1_route_set(&_reg_ctx, val1) != 0) 01308 { 01309 return 1; 01310 } 01311 01312 if (lsm6dsox_pin_int2_route_get(&_reg_ctx, NULL, &val2) != 0) 01313 { 01314 return 1; 01315 } 01316 01317 val2.free_fall = PROPERTY_DISABLE; 01318 01319 if (lsm6dsox_pin_int2_route_set(&_reg_ctx, NULL, val2) != 0) 01320 { 01321 return 1; 01322 } 01323 01324 /* FF_DUR setting */ 01325 if (lsm6dsox_ff_dur_set(&_reg_ctx, 0x00) != 0) 01326 { 01327 return 1; 01328 } 01329 01330 /* FF_THS setting */ 01331 if (lsm6dsox_ff_threshold_set(&_reg_ctx, LSM6DSOX_FF_TSH_156mg) != 0) 01332 { 01333 return 1; 01334 } 01335 01336 return 0; 01337 } 01338 01339 /** 01340 * @brief Set free fall threshold 01341 * @param thr free fall detection threshold 01342 * @retval 0 in case of success, an error code otherwise 01343 */ 01344 int LSM6DSOXSensor::set_free_fall_threshold(uint8_t thr) 01345 { 01346 if (lsm6dsox_ff_threshold_set(&_reg_ctx, (lsm6dsox_ff_ths_t)thr) != 0) 01347 { 01348 return 1; 01349 } 01350 01351 return 0; 01352 } 01353 01354 01355 /** 01356 * @brief Set free fall duration 01357 * @param dur free fall detection duration 01358 * @retval 0 in case of success, an error code otherwise 01359 */ 01360 int LSM6DSOXSensor::set_free_fall_duration(uint8_t dur) 01361 { 01362 if (lsm6dsox_ff_dur_set(&_reg_ctx, dur) != 0) 01363 { 01364 return 1; 01365 } 01366 01367 return 0; 01368 } 01369 01370 01371 /** 01372 * @brief Enable pedometer 01373 * @retval 0 in case of success, an error code otherwise 01374 */ 01375 int LSM6DSOXSensor::enable_pedometer() 01376 { 01377 lsm6dsox_pin_int1_route_t val; 01378 lsm6dsox_emb_sens_t emb_sens; 01379 01380 /* Output Data Rate selection */ 01381 if (set_x_odr(26.0f) != 0) 01382 { 01383 return 1; 01384 } 01385 01386 /* Full scale selection */ 01387 if (set_x_fs(2.0f) != 0) 01388 { 01389 return 1; 01390 } 01391 01392 /* Save current embedded features */ 01393 if (lsm6dsox_embedded_sens_get(&_reg_ctx, &emb_sens) != 0) 01394 { 01395 return 1; 01396 } 01397 01398 /* Turn off embedded features */ 01399 if (lsm6dsox_embedded_sens_off(&_reg_ctx) != 0) 01400 { 01401 return 1; 01402 } 01403 01404 /* Wait for 10 ms */ 01405 ThisThread::sleep_for(10); 01406 01407 /* Enable pedometer algorithm. */ 01408 emb_sens.step = PROPERTY_ENABLE; 01409 01410 if (lsm6dsox_pedo_sens_set(&_reg_ctx, LSM6DSOX_PEDO_BASE_MODE) != 0) 01411 { 01412 return 1; 01413 } 01414 01415 /* Turn on embedded features */ 01416 if (lsm6dsox_embedded_sens_set(&_reg_ctx, &emb_sens) != 0) 01417 { 01418 return 1; 01419 } 01420 01421 /* Enable step detector on INT1 pin */ 01422 if (lsm6dsox_pin_int1_route_get(&_reg_ctx, &val) != 0) 01423 { 01424 return 1; 01425 } 01426 01427 val.step_detector = PROPERTY_ENABLE; 01428 01429 if (lsm6dsox_pin_int1_route_set(&_reg_ctx, val) != 0) 01430 { 01431 return 1; 01432 } 01433 01434 return 0; 01435 } 01436 01437 /** 01438 * @brief Disable pedometer 01439 * @retval 0 in case of success, an error code otherwise 01440 */ 01441 int LSM6DSOXSensor::disable_pedometer() 01442 { 01443 lsm6dsox_pin_int1_route_t val1; 01444 lsm6dsox_emb_sens_t emb_sens; 01445 01446 /* Disable step detector on INT1 pin */ 01447 if (lsm6dsox_pin_int1_route_get(&_reg_ctx, &val1) != 0) 01448 { 01449 return 1; 01450 } 01451 01452 val1.step_detector = PROPERTY_DISABLE; 01453 01454 if (lsm6dsox_pin_int1_route_set(&_reg_ctx, val1) != 0) 01455 { 01456 return 1; 01457 } 01458 01459 /* Save current embedded features */ 01460 if (lsm6dsox_embedded_sens_get(&_reg_ctx, &emb_sens) != 0) 01461 { 01462 return 1; 01463 } 01464 01465 /* Disable pedometer algorithm. */ 01466 emb_sens.step = PROPERTY_DISABLE; 01467 01468 if (lsm6dsox_embedded_sens_set(&_reg_ctx, &emb_sens) != 0) 01469 { 01470 return 1; 01471 } 01472 01473 return 0; 01474 } 01475 01476 /** 01477 * @brief Get step count 01478 * @param step_count step counter 01479 * @retval 0 in case of success, an error code otherwise 01480 */ 01481 int LSM6DSOXSensor::get_step_counter(uint16_t *step_count) 01482 { 01483 if (lsm6dsox_number_of_steps_get(&_reg_ctx, (uint8_t *)step_count) != 0) 01484 { 01485 return 1; 01486 } 01487 01488 return 0; 01489 } 01490 01491 /** 01492 * @brief Enable step counter reset 01493 * @retval 0 in case of success, an error code otherwise 01494 */ 01495 int LSM6DSOXSensor::reset_step_counter() 01496 { 01497 if (lsm6dsox_steps_reset(&_reg_ctx) != 0) 01498 { 01499 return 1; 01500 } 01501 01502 return 0; 01503 } 01504 01505 /** 01506 * @brief Enable tilt detection 01507 * @param int_pin interrupt pin line to be used 01508 * @retval 0 in case of success, an error code otherwise 01509 */ 01510 int LSM6DSOXSensor::enable_tilt_detection(LSM6DSOX_Interrupt_Pin_t int_pin) 01511 { 01512 int ret = 0; 01513 lsm6dsox_pin_int1_route_t val1; 01514 lsm6dsox_pin_int2_route_t val2; 01515 lsm6dsox_emb_sens_t emb_sens; 01516 01517 /* Output Data Rate selection */ 01518 if (set_x_odr(26.0f) != 0) 01519 { 01520 return 1; 01521 } 01522 01523 /* Full scale selection */ 01524 if (set_x_fs(2.0f) != 0) 01525 { 01526 return 1; 01527 } 01528 01529 /* Save current embedded features */ 01530 if (lsm6dsox_embedded_sens_get(&_reg_ctx, &emb_sens) != 0) 01531 { 01532 return 1; 01533 } 01534 01535 /* Turn off embedded features */ 01536 if (lsm6dsox_embedded_sens_off(&_reg_ctx) != 0) 01537 { 01538 return 1; 01539 } 01540 01541 /* Wait for 10 ms */ 01542 ThisThread::sleep_for(10); 01543 01544 /* Enable tilt algorithm. */ 01545 emb_sens.tilt = PROPERTY_ENABLE; 01546 01547 /* Turn on embedded features */ 01548 if (lsm6dsox_embedded_sens_set(&_reg_ctx, &emb_sens) != 0) 01549 { 01550 return 1; 01551 } 01552 01553 /* Enable tilt event on either INT1 or INT2 pin */ 01554 switch (int_pin) 01555 { 01556 case LSM6DSOX_INT1_PIN: 01557 if (lsm6dsox_pin_int1_route_get(&_reg_ctx, &val1) != 0) 01558 { 01559 return 1; 01560 } 01561 01562 val1.tilt = PROPERTY_ENABLE; 01563 01564 if (lsm6dsox_pin_int1_route_set(&_reg_ctx, val1) != 0) 01565 { 01566 return 1; 01567 } 01568 break; 01569 01570 case LSM6DSOX_INT2_PIN: 01571 if (lsm6dsox_pin_int2_route_get(&_reg_ctx, NULL, &val2) != 0) 01572 { 01573 return 1; 01574 } 01575 01576 val2.tilt = PROPERTY_ENABLE; 01577 01578 if (lsm6dsox_pin_int2_route_set(&_reg_ctx, NULL, val2) != 0) 01579 { 01580 return 1; 01581 } 01582 break; 01583 01584 default: 01585 ret = 1; 01586 break; 01587 } 01588 01589 return ret; 01590 } 01591 01592 /** 01593 * @brief Disable tilt detection 01594 * @retval 0 in case of success, an error code otherwise 01595 */ 01596 int LSM6DSOXSensor::disable_tilt_detection() 01597 { 01598 lsm6dsox_pin_int1_route_t val1; 01599 lsm6dsox_pin_int2_route_t val2; 01600 lsm6dsox_emb_sens_t emb_sens; 01601 01602 /* Disable tilt event on both INT1 and INT2 pins */ 01603 if (lsm6dsox_pin_int1_route_get(&_reg_ctx, &val1) != 0) 01604 { 01605 return 1; 01606 } 01607 01608 val1.tilt = PROPERTY_DISABLE; 01609 01610 if (lsm6dsox_pin_int1_route_set(&_reg_ctx, val1) != 0) 01611 { 01612 return 1; 01613 } 01614 01615 if (lsm6dsox_pin_int2_route_get(&_reg_ctx, NULL, &val2) != 0) 01616 { 01617 return 1; 01618 } 01619 01620 val2.tilt = PROPERTY_DISABLE; 01621 01622 if (lsm6dsox_pin_int2_route_set(&_reg_ctx, NULL, val2) != 0) 01623 { 01624 return 1; 01625 } 01626 01627 /* Save current embedded features */ 01628 if (lsm6dsox_embedded_sens_get(&_reg_ctx, &emb_sens) != 0) 01629 { 01630 return 1; 01631 } 01632 01633 /* Disable tilt algorithm. */ 01634 emb_sens.tilt = PROPERTY_DISABLE; 01635 01636 if (lsm6dsox_embedded_sens_set(&_reg_ctx, &emb_sens) != 0) 01637 { 01638 return 1; 01639 } 01640 01641 return 0; 01642 } 01643 01644 /** 01645 * @brief Enable wake up detection 01646 * @param int_pin interrupt pin line to be used 01647 * @retval 0 in case of success, an error code otherwise 01648 */ 01649 int LSM6DSOXSensor::enable_wake_up_detection(LSM6DSOX_Interrupt_Pin_t int_pin) 01650 { 01651 int ret = 0; 01652 lsm6dsox_pin_int1_route_t val1; 01653 lsm6dsox_pin_int2_route_t val2; 01654 01655 /* Output Data Rate selection */ 01656 if (set_x_odr(416.0f) != 0) 01657 { 01658 return 1; 01659 } 01660 01661 /* Full scale selection */ 01662 if (set_x_fs(2.0f) != 0) 01663 { 01664 return 1; 01665 } 01666 01667 /* WAKE_DUR setting */ 01668 if (lsm6dsox_wkup_dur_set(&_reg_ctx, 0x00) != 0) 01669 { 01670 return 1; 01671 } 01672 01673 /* Set wake up threshold. */ 01674 if (lsm6dsox_wkup_threshold_set(&_reg_ctx, 0x02) != 0) 01675 { 01676 return 1; 01677 } 01678 01679 /* Enable wake up event on either INT1 or INT2 pin */ 01680 switch (int_pin) 01681 { 01682 case LSM6DSOX_INT1_PIN: 01683 if (lsm6dsox_pin_int1_route_get(&_reg_ctx, &val1) != 0) 01684 { 01685 return 1; 01686 } 01687 01688 val1.wake_up = PROPERTY_ENABLE; 01689 01690 if (lsm6dsox_pin_int1_route_set(&_reg_ctx, val1) != 0) 01691 { 01692 return 1; 01693 } 01694 break; 01695 01696 case LSM6DSOX_INT2_PIN: 01697 if (lsm6dsox_pin_int2_route_get(&_reg_ctx, NULL, &val2) != 0) 01698 { 01699 return 1; 01700 } 01701 01702 val2.wake_up = PROPERTY_ENABLE; 01703 01704 if (lsm6dsox_pin_int2_route_set(&_reg_ctx, NULL, val2) != 0) 01705 { 01706 return 1; 01707 } 01708 break; 01709 01710 default: 01711 ret = 1; 01712 break; 01713 } 01714 01715 return ret; 01716 } 01717 01718 01719 /** 01720 * @brief Disable wake up detection 01721 * @retval 0 in case of success, an error code otherwise 01722 */ 01723 int LSM6DSOXSensor::disable_wake_up_detection() 01724 { 01725 lsm6dsox_pin_int1_route_t val1; 01726 lsm6dsox_pin_int2_route_t val2; 01727 01728 /* Disable wake up event on both INT1 and INT2 pins */ 01729 if (lsm6dsox_pin_int1_route_get(&_reg_ctx, &val1) != 0) 01730 { 01731 return 1; 01732 } 01733 01734 val1.wake_up = PROPERTY_DISABLE; 01735 01736 if (lsm6dsox_pin_int1_route_set(&_reg_ctx, val1) != 0) 01737 { 01738 return 1; 01739 } 01740 01741 if (lsm6dsox_pin_int2_route_get(&_reg_ctx, NULL, &val2) != 0) 01742 { 01743 return 1; 01744 } 01745 01746 val2.wake_up = PROPERTY_DISABLE; 01747 01748 if (lsm6dsox_pin_int2_route_set(&_reg_ctx, NULL, val2) != 0) 01749 { 01750 return 1; 01751 } 01752 01753 /* Reset wake up threshold. */ 01754 if (lsm6dsox_wkup_threshold_set(&_reg_ctx, 0x00) != 0) 01755 { 01756 return 1; 01757 } 01758 01759 /* WAKE_DUR setting */ 01760 if (lsm6dsox_wkup_dur_set(&_reg_ctx, 0x00) != 0) 01761 { 01762 return 1; 01763 } 01764 01765 return 0; 01766 } 01767 01768 /** 01769 * @brief Set wake up threshold 01770 * @param thr wake up detection threshold 01771 * @retval 0 in case of success, an error code otherwise 01772 */ 01773 int LSM6DSOXSensor::set_wake_up_threshold(uint8_t thr) 01774 { 01775 /* Set wake up threshold. */ 01776 if (lsm6dsox_wkup_threshold_set(&_reg_ctx, thr) != 0) 01777 { 01778 return 1; 01779 } 01780 01781 return 0; 01782 } 01783 01784 /** 01785 * @brief Set wake up duration 01786 * @param dur wake up detection duration 01787 * @retval 0 in case of success, an error code otherwise 01788 */ 01789 int LSM6DSOXSensor::set_wake_up_duration(uint8_t dur) 01790 { 01791 /* Set wake up duration. */ 01792 if (lsm6dsox_wkup_dur_set(&_reg_ctx, dur) != 0) 01793 { 01794 return 1; 01795 } 01796 01797 return 0; 01798 } 01799 01800 /** 01801 * @brief Enable single tap detection 01802 * @param int_pin interrupt pin line to be used 01803 * @retval 0 in case of success, an error code otherwise 01804 */ 01805 int LSM6DSOXSensor::enable_single_tap_detection(LSM6DSOX_Interrupt_Pin_t int_pin) 01806 { 01807 int ret = 0; 01808 lsm6dsox_pin_int1_route_t val1; 01809 lsm6dsox_pin_int2_route_t val2; 01810 01811 /* Output Data Rate selection */ 01812 if (set_x_odr(416.0f) != 0) 01813 { 01814 return 1; 01815 } 01816 01817 /* Full scale selection */ 01818 if (set_x_fs(2.0f) != 0) 01819 { 01820 return 1; 01821 } 01822 01823 /* Enable X direction in tap recognition. */ 01824 if (lsm6dsox_tap_detection_on_x_set(&_reg_ctx, PROPERTY_ENABLE) != 0) 01825 { 01826 return 1; 01827 } 01828 01829 /* Enable Y direction in tap recognition. */ 01830 if (lsm6dsox_tap_detection_on_y_set(&_reg_ctx, PROPERTY_ENABLE) != 0) 01831 { 01832 return 1; 01833 } 01834 01835 /* Enable Z direction in tap recognition. */ 01836 if (lsm6dsox_tap_detection_on_z_set(&_reg_ctx, PROPERTY_ENABLE) != 0) 01837 { 01838 return 1; 01839 } 01840 01841 /* Set tap threshold. */ 01842 if (lsm6dsox_tap_threshold_x_set(&_reg_ctx, 0x08) != 0) 01843 { 01844 return 1; 01845 } 01846 01847 /* Set tap shock time window. */ 01848 if (lsm6dsox_tap_shock_set(&_reg_ctx, 0x02) != 0) 01849 { 01850 return 1; 01851 } 01852 01853 /* Set tap quiet time window. */ 01854 if (lsm6dsox_tap_quiet_set(&_reg_ctx, 0x01) != 0) 01855 { 01856 return 1; 01857 } 01858 01859 /* _NOTE_: Tap duration time window - don't care for single tap. */ 01860 01861 /* _NOTE_: Single/Double Tap event - don't care of this flag for single tap. */ 01862 01863 /* Enable single tap event on either INT1 or INT2 pin */ 01864 switch (int_pin) 01865 { 01866 case LSM6DSOX_INT1_PIN: 01867 if (lsm6dsox_pin_int1_route_get(&_reg_ctx, &val1) != 0) 01868 { 01869 return 1; 01870 } 01871 01872 val1.single_tap = PROPERTY_ENABLE; 01873 01874 if (lsm6dsox_pin_int1_route_set(&_reg_ctx, val1) != 0) 01875 { 01876 return 1; 01877 } 01878 break; 01879 01880 case LSM6DSOX_INT2_PIN: 01881 if (lsm6dsox_pin_int2_route_get(&_reg_ctx, NULL, &val2) != 0) 01882 { 01883 return 1; 01884 } 01885 01886 val2.single_tap = PROPERTY_ENABLE; 01887 01888 if (lsm6dsox_pin_int2_route_set(&_reg_ctx, NULL, val2) != 0) 01889 { 01890 return 1; 01891 } 01892 break; 01893 01894 default: 01895 ret = 1; 01896 break; 01897 } 01898 01899 return ret; 01900 } 01901 01902 /** 01903 * @brief Disable single tap detection 01904 * @retval 0 in case of success, an error code otherwise 01905 */ 01906 int LSM6DSOXSensor::disable_single_tap_detection() 01907 { 01908 lsm6dsox_pin_int1_route_t val1; 01909 lsm6dsox_pin_int2_route_t val2; 01910 01911 /* Disable single tap event on both INT1 and INT2 pins */ 01912 if (lsm6dsox_pin_int1_route_get(&_reg_ctx, &val1) != 0) 01913 { 01914 return 1; 01915 } 01916 01917 val1.single_tap = PROPERTY_DISABLE; 01918 01919 if (lsm6dsox_pin_int1_route_set(&_reg_ctx, val1) != 0) 01920 { 01921 return 1; 01922 } 01923 01924 if (lsm6dsox_pin_int2_route_get(&_reg_ctx, NULL, &val2) != 0) 01925 { 01926 return 1; 01927 } 01928 01929 val2.single_tap = PROPERTY_DISABLE; 01930 01931 if (lsm6dsox_pin_int2_route_set(&_reg_ctx, NULL, val2) != 0) 01932 { 01933 return 1; 01934 } 01935 01936 /* Reset tap quiet time window. */ 01937 if (lsm6dsox_tap_quiet_set(&_reg_ctx, 0x00) != 0) 01938 { 01939 return 1; 01940 } 01941 01942 /* Reset tap shock time window. */ 01943 if (lsm6dsox_tap_shock_set(&_reg_ctx, 0x00) != 0) 01944 { 01945 return 1; 01946 } 01947 01948 /* Reset tap threshold. */ 01949 if (lsm6dsox_tap_threshold_x_set(&_reg_ctx, 0x00) != 0) 01950 { 01951 return 1; 01952 } 01953 01954 /* Disable Z direction in tap recognition. */ 01955 if (lsm6dsox_tap_detection_on_z_set(&_reg_ctx, PROPERTY_DISABLE) != 0) 01956 { 01957 return 1; 01958 } 01959 01960 /* Disable Y direction in tap recognition. */ 01961 if (lsm6dsox_tap_detection_on_y_set(&_reg_ctx, PROPERTY_DISABLE) != 0) 01962 { 01963 return 1; 01964 } 01965 01966 /* Disable X direction in tap recognition. */ 01967 if (lsm6dsox_tap_detection_on_x_set(&_reg_ctx, PROPERTY_DISABLE) != 0) 01968 { 01969 return 1; 01970 } 01971 01972 return 0; 01973 } 01974 01975 /** 01976 * @brief Enable double tap detection 01977 * @param int_pin interrupt pin line to be used 01978 * @retval 0 in case of success, an error code otherwise 01979 */ 01980 int LSM6DSOXSensor::enable_double_tap_detection(LSM6DSOX_Interrupt_Pin_t int_pin) 01981 { 01982 int ret = 0; 01983 lsm6dsox_pin_int1_route_t val1; 01984 lsm6dsox_pin_int2_route_t val2; 01985 01986 /* Output Data Rate selection */ 01987 if (set_x_odr(416.0f) != 0) 01988 { 01989 return 1; 01990 } 01991 01992 /* Full scale selection */ 01993 if (set_x_fs(2.0f) != 0) 01994 { 01995 return 1; 01996 } 01997 01998 /* Enable X direction in tap recognition. */ 01999 if (lsm6dsox_tap_detection_on_x_set(&_reg_ctx, PROPERTY_ENABLE) != 0) 02000 { 02001 return 1; 02002 } 02003 02004 /* Enable Y direction in tap recognition. */ 02005 if (lsm6dsox_tap_detection_on_y_set(&_reg_ctx, PROPERTY_ENABLE) != 0) 02006 { 02007 return 1; 02008 } 02009 02010 /* Enable Z direction in tap recognition. */ 02011 if (lsm6dsox_tap_detection_on_z_set(&_reg_ctx, PROPERTY_ENABLE) != 0) 02012 { 02013 return 1; 02014 } 02015 02016 /* Set tap threshold. */ 02017 if (lsm6dsox_tap_threshold_x_set(&_reg_ctx, 0x08) != 0) 02018 { 02019 return 1; 02020 } 02021 02022 /* Set tap shock time window. */ 02023 if (lsm6dsox_tap_shock_set(&_reg_ctx, 0x03) != 0) 02024 { 02025 return 1; 02026 } 02027 02028 /* Set tap quiet time window. */ 02029 if (lsm6dsox_tap_quiet_set(&_reg_ctx, 0x03) != 0) 02030 { 02031 return 1; 02032 } 02033 02034 /* Set tap duration time window. */ 02035 if (lsm6dsox_tap_dur_set(&_reg_ctx, 0x08) != 0) 02036 { 02037 return 1; 02038 } 02039 02040 /* Single and double tap enabled. */ 02041 if (lsm6dsox_tap_mode_set(&_reg_ctx, LSM6DSOX_BOTH_SINGLE_DOUBLE) != 0) 02042 { 02043 return 1; 02044 } 02045 02046 /* Enable double tap event on either INT1 or INT2 pin */ 02047 switch (int_pin) 02048 { 02049 case LSM6DSOX_INT1_PIN: 02050 if (lsm6dsox_pin_int1_route_get(&_reg_ctx, &val1) != 0) 02051 { 02052 return 1; 02053 } 02054 02055 val1.double_tap = PROPERTY_ENABLE; 02056 02057 if (lsm6dsox_pin_int1_route_set(&_reg_ctx, val1) != 0) 02058 { 02059 return 1; 02060 } 02061 break; 02062 02063 case LSM6DSOX_INT2_PIN: 02064 if (lsm6dsox_pin_int2_route_get(&_reg_ctx, NULL, &val2) != 0) 02065 { 02066 return 1; 02067 } 02068 02069 val2.double_tap = PROPERTY_ENABLE; 02070 02071 if (lsm6dsox_pin_int2_route_set(&_reg_ctx, NULL, val2) != 0) 02072 { 02073 return 1; 02074 } 02075 break; 02076 02077 default: 02078 ret = 1; 02079 break; 02080 } 02081 02082 return ret; 02083 } 02084 02085 /** 02086 * @brief Disable double tap detection 02087 * @retval 0 in case of success, an error code otherwise 02088 */ 02089 int LSM6DSOXSensor::disable_double_tap_detection() 02090 { 02091 lsm6dsox_pin_int1_route_t val1; 02092 lsm6dsox_pin_int2_route_t val2; 02093 02094 /* Disable double tap event on both INT1 and INT2 pins */ 02095 if (lsm6dsox_pin_int1_route_get(&_reg_ctx, &val1) != 0) 02096 { 02097 return 1; 02098 } 02099 02100 val1.double_tap = PROPERTY_DISABLE; 02101 02102 if (lsm6dsox_pin_int1_route_set(&_reg_ctx, val1) != 0) 02103 { 02104 return 1; 02105 } 02106 02107 if (lsm6dsox_pin_int2_route_get(&_reg_ctx, NULL, &val2) != 0) 02108 { 02109 return 1; 02110 } 02111 02112 val2.double_tap = PROPERTY_DISABLE; 02113 02114 if (lsm6dsox_pin_int2_route_set(&_reg_ctx, NULL, val2) != 0) 02115 { 02116 return 1; 02117 } 02118 02119 /* Only single tap enabled. */ 02120 if (lsm6dsox_tap_mode_set(&_reg_ctx, LSM6DSOX_ONLY_SINGLE) != 0) 02121 { 02122 return 1; 02123 } 02124 02125 /* Reset tap duration time window. */ 02126 if (lsm6dsox_tap_dur_set(&_reg_ctx, 0x00) != 0) 02127 { 02128 return 1; 02129 } 02130 02131 /* Reset tap quiet time window. */ 02132 if (lsm6dsox_tap_quiet_set(&_reg_ctx, 0x00) != 0) 02133 { 02134 return 1; 02135 } 02136 02137 /* Reset tap shock time window. */ 02138 if (lsm6dsox_tap_shock_set(&_reg_ctx, 0x00) != 0) 02139 { 02140 return 1; 02141 } 02142 02143 /* Reset tap threshold. */ 02144 if (lsm6dsox_tap_threshold_x_set(&_reg_ctx, 0x00) != 0) 02145 { 02146 return 1; 02147 } 02148 02149 /* Disable Z direction in tap recognition. */ 02150 if (lsm6dsox_tap_detection_on_z_set(&_reg_ctx, PROPERTY_DISABLE) != 0) 02151 { 02152 return 1; 02153 } 02154 02155 /* Disable Y direction in tap recognition. */ 02156 if (lsm6dsox_tap_detection_on_y_set(&_reg_ctx, PROPERTY_DISABLE) != 0) 02157 { 02158 return 1; 02159 } 02160 02161 /* Disable X direction in tap recognition. */ 02162 if (lsm6dsox_tap_detection_on_x_set(&_reg_ctx, PROPERTY_DISABLE) != 0) 02163 { 02164 return 1; 02165 } 02166 02167 return 0; 02168 } 02169 02170 /** 02171 * @brief Set tap threshold 02172 * @param thr tap threshold 02173 * @retval 0 in case of success, an error code otherwise 02174 */ 02175 int LSM6DSOXSensor::set_tap_threshold(uint8_t thr) 02176 { 02177 /* Set tap threshold. */ 02178 if (lsm6dsox_tap_threshold_x_set(&_reg_ctx, thr) != 0) 02179 { 02180 return 1; 02181 } 02182 02183 return 0; 02184 } 02185 02186 /** 02187 * @brief Set tap shock time 02188 * @param time tap shock time 02189 * @retval 0 in case of success, an error code otherwise 02190 */ 02191 int LSM6DSOXSensor::set_tap_shock_time(uint8_t time) 02192 { 02193 /* Set tap shock time window. */ 02194 if (lsm6dsox_tap_shock_set(&_reg_ctx, time) != 0) 02195 { 02196 return 1; 02197 } 02198 02199 return 0; 02200 } 02201 02202 /** 02203 * @brief Set tap quiet time 02204 * @param time tap quiet time 02205 * @retval 0 in case of success, an error code otherwise 02206 */ 02207 int LSM6DSOXSensor::set_tap_quiet_time(uint8_t time) 02208 { 02209 /* Set tap quiet time window. */ 02210 if (lsm6dsox_tap_quiet_set(&_reg_ctx, time) != 0) 02211 { 02212 return 1; 02213 } 02214 02215 return 0; 02216 } 02217 02218 /** 02219 * @brief Set tap duration time 02220 * @param time tap duration time 02221 * @retval 0 in case of success, an error code otherwise 02222 */ 02223 int LSM6DSOXSensor::set_tap_duration_time(uint8_t time) 02224 { 02225 /* Set tap duration time window. */ 02226 if (lsm6dsox_tap_dur_set(&_reg_ctx, time) != 0) 02227 { 02228 return 1; 02229 } 02230 02231 return 0; 02232 } 02233 02234 /** 02235 * @brief Enable 6D orientation detection 02236 * @param int_pin interrupt pin line to be used 02237 * @retval 0 in case of success, an error code otherwise 02238 */ 02239 int LSM6DSOXSensor::enable_6d_orientation(LSM6DSOX_Interrupt_Pin_t int_pin) 02240 { 02241 int ret = 0; 02242 lsm6dsox_pin_int1_route_t val1; 02243 lsm6dsox_pin_int2_route_t val2; 02244 02245 /* Output Data Rate selection */ 02246 if (set_x_odr(416.0f) != 0) 02247 { 02248 return 1; 02249 } 02250 02251 /* Full scale selection */ 02252 if (set_x_fs(2.0f) != 0) 02253 { 02254 return 1; 02255 } 02256 02257 /* 6D orientation enabled. */ 02258 if (lsm6dsox_6d_threshold_set(&_reg_ctx, LSM6DSOX_DEG_60) != 0) 02259 { 02260 return 1; 02261 } 02262 02263 /* Enable 6D orientation event on either INT1 or INT2 pin */ 02264 switch (int_pin) 02265 { 02266 case LSM6DSOX_INT1_PIN: 02267 if (lsm6dsox_pin_int1_route_get(&_reg_ctx, &val1) != 0) 02268 { 02269 return 1; 02270 } 02271 02272 val1.six_d = PROPERTY_ENABLE; 02273 02274 if (lsm6dsox_pin_int1_route_set(&_reg_ctx, val1) != 0) 02275 { 02276 return 1; 02277 } 02278 break; 02279 02280 case LSM6DSOX_INT2_PIN: 02281 if (lsm6dsox_pin_int2_route_get(&_reg_ctx, NULL, &val2) != 0) 02282 { 02283 return 1; 02284 } 02285 02286 val2.six_d = PROPERTY_ENABLE; 02287 02288 if (lsm6dsox_pin_int2_route_set(&_reg_ctx, NULL, val2) != 0) 02289 { 02290 return 1; 02291 } 02292 break; 02293 02294 default: 02295 ret = 1; 02296 break; 02297 } 02298 02299 return ret; 02300 } 02301 02302 /** 02303 * @brief Disable 6D orientation detection 02304 * @retval 0 in case of success, an error code otherwise 02305 */ 02306 int LSM6DSOXSensor::disable_6d_orientation() 02307 { 02308 lsm6dsox_pin_int1_route_t val1; 02309 lsm6dsox_pin_int2_route_t val2; 02310 02311 /* Disable 6D orientation event on both INT1 and INT2 pins */ 02312 if (lsm6dsox_pin_int1_route_get(&_reg_ctx, &val1) != 0) 02313 { 02314 return 1; 02315 } 02316 02317 val1.six_d = PROPERTY_DISABLE; 02318 02319 if (lsm6dsox_pin_int1_route_set(&_reg_ctx, val1) != 0) 02320 { 02321 return 1; 02322 } 02323 02324 if (lsm6dsox_pin_int2_route_get(&_reg_ctx, NULL, &val2) != 0) 02325 { 02326 return 1; 02327 } 02328 02329 val2.six_d = PROPERTY_DISABLE; 02330 02331 if (lsm6dsox_pin_int2_route_set(&_reg_ctx, NULL, val2) != 0) 02332 { 02333 return 1; 02334 } 02335 02336 /* Reset 6D orientation. */ 02337 if (lsm6dsox_6d_threshold_set(&_reg_ctx, LSM6DSOX_DEG_80) != 0) 02338 { 02339 return 1; 02340 } 02341 02342 return 0; 02343 } 02344 02345 /** 02346 * @brief Set 6D orientation threshold 02347 * @param thr 6D Orientation detection threshold 02348 * @retval 0 in case of success, an error code otherwise 02349 */ 02350 int LSM6DSOXSensor::set_6d_orientation_threshold(uint8_t thr) 02351 { 02352 if (lsm6dsox_6d_threshold_set(&_reg_ctx, (lsm6dsox_sixd_ths_t)thr) != 0) 02353 { 02354 return 1; 02355 } 02356 02357 return 0; 02358 } 02359 02360 /** 02361 * @brief Get the status of XLow orientation 02362 * @param xl the status of XLow orientation 02363 * @retval 0 in case of success, an error code otherwise 02364 */ 02365 int LSM6DSOXSensor::get_6d_orientation_xl(uint8_t *xl) 02366 { 02367 lsm6dsox_d6d_src_t data; 02368 02369 if (lsm6dsox_read_reg(&_reg_ctx, LSM6DSOX_D6D_SRC, (uint8_t *)&data, 1) != 0) 02370 { 02371 return 1; 02372 } 02373 02374 *xl = data.xl; 02375 02376 return 0; 02377 } 02378 02379 /** 02380 * @brief Get the status of XHigh orientation 02381 * @param xh the status of XHigh orientation 02382 * @retval 0 in case of success, an error code otherwise 02383 */ 02384 int LSM6DSOXSensor::get_6d_orientation_xh(uint8_t *xh) 02385 { 02386 lsm6dsox_d6d_src_t data; 02387 02388 if (lsm6dsox_read_reg(&_reg_ctx, LSM6DSOX_D6D_SRC, (uint8_t *)&data, 1) != 0) 02389 { 02390 return 1; 02391 } 02392 02393 *xh = data.xh; 02394 02395 return 0; 02396 } 02397 02398 /** 02399 * @brief Get the status of YLow orientation 02400 * @param yl the status of YLow orientation 02401 * @retval 0 in case of success, an error code otherwise 02402 */ 02403 int LSM6DSOXSensor::get_6d_orientation_yl(uint8_t *yl) 02404 { 02405 lsm6dsox_d6d_src_t data; 02406 02407 if (lsm6dsox_read_reg(&_reg_ctx, LSM6DSOX_D6D_SRC, (uint8_t *)&data, 1) != 0) 02408 { 02409 return 1; 02410 } 02411 02412 *yl = data.yl; 02413 02414 return 0; 02415 } 02416 02417 /** 02418 * @brief Get the status of YHigh orientation 02419 * @param yh the status of YHigh orientation 02420 * @retval 0 in case of success, an error code otherwise 02421 */ 02422 int LSM6DSOXSensor::get_6d_orientation_yh(uint8_t *yh) 02423 { 02424 lsm6dsox_d6d_src_t data; 02425 02426 if (lsm6dsox_read_reg(&_reg_ctx, LSM6DSOX_D6D_SRC, (uint8_t *)&data, 1) != 0) 02427 { 02428 return 1; 02429 } 02430 02431 *yh = data.yh; 02432 02433 return 0; 02434 } 02435 02436 /** 02437 * @brief Get the status of ZLow orientation 02438 * @param zl the status of ZLow orientation 02439 * @retval 0 in case of success, an error code otherwise 02440 */ 02441 int LSM6DSOXSensor::get_6d_orientation_zl(uint8_t *zl) 02442 { 02443 lsm6dsox_d6d_src_t data; 02444 02445 if (lsm6dsox_read_reg(&_reg_ctx, LSM6DSOX_D6D_SRC, (uint8_t *)&data, 1) != 0) 02446 { 02447 return 1; 02448 } 02449 02450 *zl = data.zl; 02451 02452 return 0; 02453 } 02454 02455 /** 02456 * @brief Get the status of ZHigh orientation 02457 * @param zh the status of ZHigh orientation 02458 * @retval 0 in case of success, an error code otherwise 02459 */ 02460 int LSM6DSOXSensor::get_6d_orientation_zh(uint8_t *zh) 02461 { 02462 lsm6dsox_d6d_src_t data; 02463 02464 if (lsm6dsox_read_reg(&_reg_ctx, LSM6DSOX_D6D_SRC, (uint8_t *)&data, 1) != 0) 02465 { 02466 return 1; 02467 } 02468 02469 *zh = data.zh; 02470 02471 return 0; 02472 } 02473 02474 /** 02475 * @brief Get the LSM6DSOX ACC data ready bit value 02476 * @param status the status of data ready bit 02477 * @retval 0 in case of success, an error code otherwise 02478 */ 02479 int LSM6DSOXSensor::get_x_drdy_status(uint8_t *status) 02480 { 02481 if (lsm6dsox_xl_flag_data_ready_get(&_reg_ctx, status) != 0) 02482 { 02483 return 1; 02484 } 02485 02486 return 0; 02487 } 02488 02489 /** 02490 * @brief Get the status of all hardware events 02491 * @param status the status of all hardware events 02492 * @retval 0 in case of success, an error code otherwise 02493 */ 02494 int LSM6DSOXSensor::get_event_status(LSM6DSOX_Event_Status_t *status) 02495 { 02496 uint8_t tilt_ia = 0U; 02497 lsm6dsox_wake_up_src_t wake_up_src; 02498 lsm6dsox_tap_src_t tap_src; 02499 lsm6dsox_d6d_src_t d6d_src; 02500 lsm6dsox_emb_func_src_t func_src; 02501 lsm6dsox_md1_cfg_t md1_cfg; 02502 lsm6dsox_md2_cfg_t md2_cfg; 02503 lsm6dsox_emb_func_int1_t int1_ctrl; 02504 lsm6dsox_emb_func_int2_t int2_ctrl; 02505 02506 (void)memset((void *)status, 0x0, sizeof(LSM6DSOX_Event_Status_t)); 02507 02508 if (lsm6dsox_read_reg(&_reg_ctx, LSM6DSOX_WAKE_UP_SRC, (uint8_t *)&wake_up_src, 1) != 0) 02509 { 02510 return 1; 02511 } 02512 02513 if (lsm6dsox_read_reg(&_reg_ctx, LSM6DSOX_TAP_SRC, (uint8_t *)&tap_src, 1) != 0) 02514 { 02515 return 1; 02516 } 02517 02518 if (lsm6dsox_read_reg(&_reg_ctx, LSM6DSOX_D6D_SRC, (uint8_t *)&d6d_src, 1) != 0) 02519 { 02520 return 1; 02521 } 02522 02523 if (lsm6dsox_mem_bank_set(&_reg_ctx, LSM6DSOX_EMBEDDED_FUNC_BANK) != 0) 02524 { 02525 return 1; 02526 } 02527 02528 if (lsm6dsox_read_reg(&_reg_ctx, LSM6DSOX_EMB_FUNC_SRC, (uint8_t *)&func_src, 1) != 0) 02529 { 02530 return 1; 02531 } 02532 02533 if (lsm6dsox_read_reg(&_reg_ctx, LSM6DSOX_EMB_FUNC_INT1, (uint8_t *)&int1_ctrl, 1) != 0) 02534 { 02535 return 1; 02536 } 02537 02538 if (lsm6dsox_read_reg(&_reg_ctx, LSM6DSOX_EMB_FUNC_INT2, (uint8_t *)&int2_ctrl, 1) != 0) 02539 { 02540 return 1; 02541 } 02542 02543 if (lsm6dsox_mem_bank_set(&_reg_ctx, LSM6DSOX_USER_BANK) != 0) 02544 { 02545 return 1; 02546 } 02547 02548 if (lsm6dsox_read_reg(&_reg_ctx, LSM6DSOX_MD1_CFG, (uint8_t *)&md1_cfg, 1) != 0) 02549 { 02550 return 1; 02551 } 02552 02553 if (lsm6dsox_read_reg(&_reg_ctx, LSM6DSOX_MD2_CFG, (uint8_t *)&md2_cfg, 1) != 0) 02554 { 02555 return 1; 02556 } 02557 02558 if (lsm6dsox_tilt_flag_data_ready_get(&_reg_ctx, &tilt_ia) != 0) 02559 { 02560 return 1; 02561 } 02562 02563 if ((md1_cfg.int1_ff == 1U) || (md2_cfg.int2_ff == 1U)) 02564 { 02565 if (wake_up_src.ff_ia == 1U) 02566 { 02567 status->FreeFallStatus = 1; 02568 } 02569 } 02570 02571 if ((md1_cfg.int1_wu == 1U) || (md2_cfg.int2_wu == 1U)) 02572 { 02573 if (wake_up_src.wu_ia == 1U) 02574 { 02575 status->WakeUpStatus = 1; 02576 } 02577 } 02578 02579 if ((md1_cfg.int1_single_tap == 1U) || (md2_cfg.int2_single_tap == 1U)) 02580 { 02581 if (tap_src.single_tap == 1U) 02582 { 02583 status->TapStatus = 1; 02584 } 02585 } 02586 02587 if ((md1_cfg.int1_double_tap == 1U) || (md2_cfg.int2_double_tap == 1U)) 02588 { 02589 if (tap_src.double_tap == 1U) 02590 { 02591 status->DoubleTapStatus = 1; 02592 } 02593 } 02594 02595 if ((md1_cfg.int1_6d == 1U) || (md2_cfg.int2_6d == 1U)) 02596 { 02597 if (d6d_src.d6d_ia == 1U) 02598 { 02599 status->D6DOrientationStatus = 1; 02600 } 02601 } 02602 02603 if (int1_ctrl.int1_step_detector == 1U) 02604 { 02605 if (func_src.step_detected == 1U) 02606 { 02607 status->StepStatus = 1; 02608 } 02609 } 02610 02611 if ((int1_ctrl.int1_tilt == 1U) || (int2_ctrl.int2_tilt == 1U)) 02612 { 02613 if (tilt_ia == 1U) 02614 { 02615 status->TiltStatus = 1; 02616 } 02617 } 02618 02619 return 0; 02620 } 02621 02622 /** 02623 * @brief Set self test 02624 * @param val the value of st_xl in reg CTRL5_C 02625 * @retval 0 in case of success, an error code otherwise 02626 */ 02627 int LSM6DSOXSensor::set_x_self_test(uint8_t val) 02628 { 02629 lsm6dsox_st_xl_t reg; 02630 02631 reg = (val == 0U) ? LSM6DSOX_XL_ST_DISABLE 02632 : (val == 1U) ? LSM6DSOX_XL_ST_POSITIVE 02633 : (val == 2U) ? LSM6DSOX_XL_ST_NEGATIVE 02634 : LSM6DSOX_XL_ST_DISABLE; 02635 02636 if (lsm6dsox_xl_self_test_set(&_reg_ctx, reg) != 0) 02637 { 02638 return 1; 02639 } 02640 02641 return 0; 02642 } 02643 02644 /** 02645 * @brief Get the LSM6DSOX GYRO data ready bit value 02646 * @param status the status of data ready bit 02647 * @retval 0 in case of success, an error code otherwise 02648 */ 02649 int LSM6DSOXSensor::get_g_drdy_status(uint8_t *status) 02650 { 02651 if (lsm6dsox_gy_flag_data_ready_get(&_reg_ctx, status) != 0) 02652 { 02653 return 1; 02654 } 02655 02656 return 0; 02657 } 02658 02659 /** 02660 * @brief Set self test 02661 * @param val the value of st_xl in reg CTRL5_C 02662 * @retval 0 in case of success, an error code otherwise 02663 */ 02664 int LSM6DSOXSensor::set_g_self_test(uint8_t val) 02665 { 02666 lsm6dsox_st_g_t reg; 02667 02668 reg = (val == 0U) ? LSM6DSOX_GY_ST_DISABLE 02669 : (val == 1U) ? LSM6DSOX_GY_ST_POSITIVE 02670 : (val == 2U) ? LSM6DSOX_GY_ST_NEGATIVE 02671 : LSM6DSOX_GY_ST_DISABLE; 02672 02673 02674 if (lsm6dsox_gy_self_test_set(&_reg_ctx, reg) != 0) 02675 { 02676 return 1; 02677 } 02678 02679 return 0; 02680 } 02681 02682 /** 02683 * @brief Get the LSM6DSOX FIFO number of samples 02684 * @param num_samples number of samples 02685 * @retval 0 in case of success, an error code otherwise 02686 */ 02687 int LSM6DSOXSensor::get_fifo_num_samples(uint16_t *num_samples) 02688 { 02689 if (lsm6dsox_fifo_data_level_get(&_reg_ctx, num_samples) != 0) 02690 { 02691 return 1; 02692 } 02693 02694 return 0; 02695 } 02696 02697 /** 02698 * @brief Get the LSM6DSOX FIFO full status 02699 * @param status FIFO full status 02700 * @retval 0 in case of success, an error code otherwise 02701 */ 02702 int LSM6DSOXSensor::get_fifo_full_status(uint8_t *status) 02703 { 02704 lsm6dsox_reg_t reg; 02705 02706 if (lsm6dsox_read_reg(&_reg_ctx, LSM6DSOX_FIFO_STATUS2, ®.byte, 1) != 0) 02707 { 02708 return 1; 02709 } 02710 02711 *status = reg.fifo_status2.fifo_full_ia; 02712 02713 return 0; 02714 } 02715 02716 /** 02717 * @brief Set the LSM6DSOX FIFO full interrupt on INT1 pin 02718 * @param status FIFO full interrupt on INT1 pin status 02719 * @retval 0 in case of success, an error code otherwise 02720 */ 02721 int LSM6DSOXSensor::set_fifo_int1_fifo_full(uint8_t status) 02722 { 02723 lsm6dsox_reg_t reg; 02724 02725 if (lsm6dsox_read_reg(&_reg_ctx, LSM6DSOX_INT1_CTRL, ®.byte, 1) != 0) 02726 { 02727 return 1; 02728 } 02729 02730 reg.int1_ctrl.int1_fifo_full = status; 02731 02732 if (lsm6dsox_write_reg(&_reg_ctx, LSM6DSOX_INT1_CTRL, ®.byte, 1) != 0) 02733 { 02734 return 1; 02735 } 02736 02737 return 0; 02738 } 02739 02740 /** 02741 * @brief Set the LSM6DSOX FIFO watermark level 02742 * @param watermark FIFO watermark level 02743 * @retval 0 in case of success, an error code otherwise 02744 */ 02745 int LSM6DSOXSensor::set_fifo_watermark_level(uint16_t watermark) 02746 { 02747 if (lsm6dsox_fifo_watermark_set(&_reg_ctx, watermark) != 0) 02748 { 02749 return 1; 02750 } 02751 02752 return 0; 02753 } 02754 02755 /** 02756 * @brief Set the LSM6DSOX FIFO stop on watermark 02757 * @param status FIFO stop on watermark status 02758 * @retval 0 in case of success, an error code otherwise 02759 */ 02760 int LSM6DSOXSensor::set_fifo_stop_on_fth(uint8_t status) 02761 { 02762 if (lsm6dsox_fifo_stop_on_wtm_set(&_reg_ctx, status) != 0) 02763 { 02764 return 1; 02765 } 02766 02767 return 0; 02768 } 02769 02770 /** 02771 * @brief Set the LSM6DSOX FIFO mode 02772 * @param mode FIFO mode 02773 * @retval 0 in case of success, an error code otherwise 02774 */ 02775 int LSM6DSOXSensor::set_fifo_mode(uint8_t mode) 02776 { 02777 int ret = 0; 02778 02779 /* Verify that the passed parameter contains one of the valid values. */ 02780 switch ((lsm6dsox_fifo_mode_t)mode) 02781 { 02782 case LSM6DSOX_BYPASS_MODE: 02783 case LSM6DSOX_FIFO_MODE: 02784 case LSM6DSOX_STREAM_TO_FIFO_MODE: 02785 case LSM6DSOX_BYPASS_TO_STREAM_MODE: 02786 case LSM6DSOX_STREAM_MODE: 02787 break; 02788 02789 default: 02790 ret = 1; 02791 break; 02792 } 02793 02794 if (ret == 1) 02795 { 02796 return ret; 02797 } 02798 02799 if (lsm6dsox_fifo_mode_set(&_reg_ctx, (lsm6dsox_fifo_mode_t)mode) != 0) 02800 { 02801 return 1; 02802 } 02803 02804 return ret; 02805 } 02806 02807 /** 02808 * @brief Get the LSM6DSOX FIFO tag 02809 * @param tag FIFO tag 02810 * @retval 0 in case of success, an error code otherwise 02811 */ 02812 int LSM6DSOXSensor::get_fifo_tag(uint8_t *tag) 02813 { 02814 lsm6dsox_fifo_tag_t tag_local; 02815 02816 if (lsm6dsox_fifo_sensor_tag_get(&_reg_ctx, &tag_local) != 0) 02817 { 02818 return 1; 02819 } 02820 02821 *tag = (uint8_t)tag_local; 02822 02823 return 0; 02824 } 02825 02826 /** 02827 * @brief Get the LSM6DSOX FIFO raw data 02828 * @param data FIFO raw data array [6] 02829 * @retval 0 in case of success, an error code otherwise 02830 */ 02831 int LSM6DSOXSensor::get_fifo_data(uint8_t *data) 02832 { 02833 if (lsm6dsox_read_reg(&_reg_ctx, LSM6DSOX_FIFO_DATA_OUT_X_L, data, 6) != 0) 02834 { 02835 return 1; 02836 } 02837 02838 return 0; 02839 } 02840 02841 /** 02842 * @brief Get the LSM6DSOX FIFO accelero single sample (16-bit data per 3 axes) and calculate acceleration [mg] 02843 * @param acceleration FIFO accelero axes [mg] 02844 * @retval 0 in case of success, an error code otherwise 02845 */ 02846 int LSM6DSOXSensor::get_fifo_x_axes(int32_t *acceleration) 02847 { 02848 uint8_t data[6]; 02849 int16_t data_raw[3]; 02850 float sensitivity = 0.0f; 02851 float acceleration_float[3]; 02852 02853 if (get_fifo_data(data) != 0) 02854 { 02855 return 1; 02856 } 02857 02858 data_raw[0] = ((int16_t)data[1] << 8) | data[0]; 02859 data_raw[1] = ((int16_t)data[3] << 8) | data[2]; 02860 data_raw[2] = ((int16_t)data[5] << 8) | data[4]; 02861 02862 if (get_x_sensitivity(&sensitivity) != 0) 02863 { 02864 return 1; 02865 } 02866 02867 acceleration_float[0] = (float)data_raw[0] * sensitivity; 02868 acceleration_float[1] = (float)data_raw[1] * sensitivity; 02869 acceleration_float[2] = (float)data_raw[2] * sensitivity; 02870 02871 acceleration[0] = (int32_t)acceleration_float[0]; 02872 acceleration[1] = (int32_t)acceleration_float[1]; 02873 acceleration[2] = (int32_t)acceleration_float[2]; 02874 02875 return 0; 02876 } 02877 02878 /** 02879 * @brief Set the LSM6DSOX FIFO accelero BDR value 02880 * @param bdr FIFO accelero BDR value 02881 * @retval 0 in case of success, an error code otherwise 02882 */ 02883 int LSM6DSOXSensor::set_fifo_x_bdr(float bdr) 02884 { 02885 lsm6dsox_bdr_xl_t new_bdr; 02886 02887 new_bdr = (bdr <= 0.0f) ? LSM6DSOX_XL_NOT_BATCHED 02888 : (bdr <= 12.5f) ? LSM6DSOX_XL_BATCHED_AT_12Hz5 02889 : (bdr <= 26.0f) ? LSM6DSOX_XL_BATCHED_AT_26Hz 02890 : (bdr <= 52.0f) ? LSM6DSOX_XL_BATCHED_AT_52Hz 02891 : (bdr <= 104.0f) ? LSM6DSOX_XL_BATCHED_AT_104Hz 02892 : (bdr <= 208.0f) ? LSM6DSOX_XL_BATCHED_AT_208Hz 02893 : (bdr <= 416.0f) ? LSM6DSOX_XL_BATCHED_AT_417Hz 02894 : (bdr <= 833.0f) ? LSM6DSOX_XL_BATCHED_AT_833Hz 02895 : (bdr <= 1660.0f) ? LSM6DSOX_XL_BATCHED_AT_1667Hz 02896 : (bdr <= 3330.0f) ? LSM6DSOX_XL_BATCHED_AT_3333Hz 02897 : LSM6DSOX_XL_BATCHED_AT_6667Hz; 02898 02899 if (lsm6dsox_fifo_xl_batch_set(&_reg_ctx, new_bdr) != 0) 02900 { 02901 return 1; 02902 } 02903 02904 return 0; 02905 } 02906 02907 /** 02908 * @brief Get the LSM6DSOX FIFO gyro single sample (16-bit data per 3 axes) and calculate angular velocity [mDPS] 02909 * @param angular_velocity FIFO gyro axes [mDPS] 02910 * @retval 0 in case of success, an error code otherwise 02911 */ 02912 int LSM6DSOXSensor::get_fifo_g_axes(int32_t *angular_velocity) 02913 { 02914 uint8_t data[6]; 02915 int16_t data_raw[3]; 02916 float sensitivity = 0.0f; 02917 float angular_velocity_float[3]; 02918 02919 if (get_fifo_data(data) != 0) 02920 { 02921 return 1; 02922 } 02923 02924 data_raw[0] = ((int16_t)data[1] << 8) | data[0]; 02925 data_raw[1] = ((int16_t)data[3] << 8) | data[2]; 02926 data_raw[2] = ((int16_t)data[5] << 8) | data[4]; 02927 02928 if (get_g_sensitivity(&sensitivity) != 0) 02929 { 02930 return 1; 02931 } 02932 02933 angular_velocity_float[0] = (float)data_raw[0] * sensitivity; 02934 angular_velocity_float[1] = (float)data_raw[1] * sensitivity; 02935 angular_velocity_float[2] = (float)data_raw[2] * sensitivity; 02936 02937 angular_velocity[0] = (int32_t)angular_velocity_float[0]; 02938 angular_velocity[1] = (int32_t)angular_velocity_float[1]; 02939 angular_velocity[2] = (int32_t)angular_velocity_float[2]; 02940 02941 return 0; 02942 } 02943 02944 /** 02945 * @brief Set the LSM6DSOX FIFO gyro BDR value 02946 * @param bdr FIFO gyro BDR value 02947 * @retval 0 in case of success, an error code otherwise 02948 */ 02949 int LSM6DSOXSensor::set_fifo_g_bdr(float bdr) 02950 { 02951 lsm6dsox_bdr_gy_t new_bdr; 02952 02953 new_bdr = (bdr <= 0.0f) ? LSM6DSOX_GY_NOT_BATCHED 02954 : (bdr <= 12.5f) ? LSM6DSOX_GY_BATCHED_AT_12Hz5 02955 : (bdr <= 26.0f) ? LSM6DSOX_GY_BATCHED_AT_26Hz 02956 : (bdr <= 52.0f) ? LSM6DSOX_GY_BATCHED_AT_52Hz 02957 : (bdr <= 104.0f) ? LSM6DSOX_GY_BATCHED_AT_104Hz 02958 : (bdr <= 208.0f) ? LSM6DSOX_GY_BATCHED_AT_208Hz 02959 : (bdr <= 416.0f) ? LSM6DSOX_GY_BATCHED_AT_417Hz 02960 : (bdr <= 833.0f) ? LSM6DSOX_GY_BATCHED_AT_833Hz 02961 : (bdr <= 1660.0f) ? LSM6DSOX_GY_BATCHED_AT_1667Hz 02962 : (bdr <= 3330.0f) ? LSM6DSOX_GY_BATCHED_AT_3333Hz 02963 : LSM6DSOX_GY_BATCHED_AT_6667Hz; 02964 02965 if (lsm6dsox_fifo_gy_batch_set(&_reg_ctx, new_bdr) != 0) 02966 { 02967 return 1; 02968 } 02969 02970 return 0; 02971 } 02972 02973 /** 02974 * @brief Get the LSM6DSOX MLC status 02975 * @param status the status to be set 02976 * @retval 0 in case of success, an error code otherwise 02977 */ 02978 int LSM6DSOXSensor::get_mlc_status(LSM6DSOX_MLC_Status_t *status) 02979 { 02980 if (lsm6dsox_mlc_status_get(&_reg_ctx, (lsm6dsox_mlc_status_mainpage_t *)status) != 0) 02981 { 02982 return 1; 02983 } 02984 02985 return 0; 02986 } 02987 02988 /** 02989 * @brief Get the LSM6DSOX MLC output 02990 * @param output the result of the MLC computation 02991 * @retval 0 in case of success, an error code otherwise 02992 */ 02993 int LSM6DSOXSensor::get_mlc_output(uint8_t *output) 02994 { 02995 if (lsm6dsox_mlc_out_get(&_reg_ctx, output) != 0) 02996 { 02997 return 1; 02998 } 02999 03000 return 0; 03001 } 03002 03003 03004 03005 int32_t LSM6DSOX_io_write(void *handle, uint8_t WriteAddr, uint8_t *pBuffer, uint16_t nBytesToWrite) 03006 { 03007 return ((LSM6DSOXSensor *)handle)->io_write(pBuffer, WriteAddr, nBytesToWrite); 03008 } 03009 03010 int32_t LSM6DSOX_io_read(void *handle, uint8_t ReadAddr, uint8_t *pBuffer, uint16_t nBytesToRead) 03011 { 03012 return ((LSM6DSOXSensor *)handle)->io_read(pBuffer, ReadAddr, nBytesToRead); 03013 }
Generated on Tue Jul 12 2022 20:15:23 by
1.7.2