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