LSM6DSOSensor.cpp

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