From 71857ef5e89a6c19b15cbe1d1036604c84691072 Mon Sep 17 00:00:00 2001 From: Carter Meyers Date: Mon, 31 Jul 2023 19:28:55 -0700 Subject: [PATCH 01/60] Add LSM library Started implementing it on a bno080 copy --- lib/LSM6DSV16X/LSM6DSV16XSensor.cpp | 3213 ++++++++++ lib/LSM6DSV16X/LSM6DSV16XSensor.h | 344 ++ lib/LSM6DSV16X/lsm6dsv16x_reg.c | 8902 +++++++++++++++++++++++++++ lib/LSM6DSV16X/lsm6dsv16x_reg.h | 4663 ++++++++++++++ src/sensors/lsmdsv16x.cpp | 211 + src/sensors/lsmdsv16x.h | 57 + 6 files changed, 17390 insertions(+) create mode 100644 lib/LSM6DSV16X/LSM6DSV16XSensor.cpp create mode 100644 lib/LSM6DSV16X/LSM6DSV16XSensor.h create mode 100644 lib/LSM6DSV16X/lsm6dsv16x_reg.c create mode 100644 lib/LSM6DSV16X/lsm6dsv16x_reg.h create mode 100644 src/sensors/lsmdsv16x.cpp create mode 100644 src/sensors/lsmdsv16x.h diff --git a/lib/LSM6DSV16X/LSM6DSV16XSensor.cpp b/lib/LSM6DSV16X/LSM6DSV16XSensor.cpp new file mode 100644 index 000000000..59202fe9d --- /dev/null +++ b/lib/LSM6DSV16X/LSM6DSV16XSensor.cpp @@ -0,0 +1,3213 @@ +/** + ****************************************************************************** + * @file LSM6DSV16XSensor.cpp + * @author SRA + * @version V1.5.1 + * @date July 2022 + * @brief Implementation of a LSM6DSV16X inertial measurement sensor. + ****************************************************************************** + * @attention + * + *

© COPYRIGHT(c) 2022 STMicroelectronics

+ * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * 3. Neither the name of STMicroelectronics nor the names of its contributors + * may be used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************** + */ + + +/* Includes ------------------------------------------------------------------*/ + +#include "LSM6DSV16XSensor.h" + + +/* Class Implementation ------------------------------------------------------*/ + +/** Constructor + * @param i2c object of an helper class which handles the I2C peripheral + * @param address the address of the component's instance + */ +LSM6DSV16XSensor::LSM6DSV16XSensor(TwoWire *i2c, uint8_t address) : dev_i2c(i2c), address(address) +{ + reg_ctx.write_reg = LSM6DSV16X_io_write; + reg_ctx.read_reg = LSM6DSV16X_io_read; + reg_ctx.handle = (void *)this; + dev_spi = NULL; + acc_is_enabled = 0L; + gyro_is_enabled = 0L; +} + +/** Constructor + * @param spi object of an helper class which handles the SPI peripheral + * @param cs_pin the chip select pin + * @param spi_speed the SPI speed + */ +LSM6DSV16XSensor::LSM6DSV16XSensor(SPIClass *spi, int cs_pin, uint32_t spi_speed) : dev_spi(spi), cs_pin(cs_pin), spi_speed(spi_speed) +{ + reg_ctx.write_reg = LSM6DSV16X_io_write; + reg_ctx.read_reg = LSM6DSV16X_io_read; + reg_ctx.handle = (void *)this; + dev_i2c = NULL; + acc_is_enabled = 0L; + gyro_is_enabled = 0L; +} + +/** + * @brief Initialize the LSM6DSV16X sensor + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::begin() +{ + if (dev_spi) { + // Configure CS pin + pinMode(cs_pin, OUTPUT); + digitalWrite(cs_pin, HIGH); + } + + /* Enable register address automatically incremented during a multiple byte + access with a serial interface. */ + if (lsm6dsv16x_auto_increment_set(®_ctx, PROPERTY_ENABLE) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + /* Enable BDU */ + if (lsm6dsv16x_block_data_update_set(®_ctx, PROPERTY_ENABLE) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + /* FIFO mode selection */ + if (lsm6dsv16x_fifo_mode_set(®_ctx, LSM6DSV16X_BYPASS_MODE) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + /* Select default output data rate. */ + acc_odr = LSM6DSV16X_XL_ODR_AT_120Hz; + + /* Output data rate selection - power down. */ + if (lsm6dsv16x_xl_data_rate_set(®_ctx, LSM6DSV16X_XL_ODR_OFF) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + /* Full scale selection. */ + if (lsm6dsv16x_xl_full_scale_set(®_ctx, LSM6DSV16X_2g) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + /* Select default output data rate. */ + gyro_odr = LSM6DSV16X_GY_ODR_AT_120Hz; + + /* Output data rate selection - power down. */ + if (lsm6dsv16x_gy_data_rate_set(®_ctx, LSM6DSV16X_GY_ODR_OFF) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + /* Full scale selection. */ + if (lsm6dsv16x_gy_full_scale_set(®_ctx, LSM6DSV16X_2000dps) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + initialized = 1U; + + return LSM6DSV16X_OK; +} + +/** + * @brief Deinitialize the LSM6DSV16X sensor + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::end() +{ + /* Disable the component */ + if (Disable_X() != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + if (Disable_G() != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + /* Reset output data rate. */ + acc_odr = LSM6DSV16X_XL_ODR_OFF; + gyro_odr = LSM6DSV16X_GY_ODR_OFF; + + initialized = 0U; + + return LSM6DSV16X_OK; +} + +/** + * @brief Read component ID + * @param Id the WHO_AM_I value + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::ReadID(uint8_t *Id) +{ + if (lsm6dsv16x_device_id_get(®_ctx, Id) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + return LSM6DSV16X_OK; +} + +/** + * @brief Enable the LSM6DSV16X accelerometer sensor + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Enable_X() +{ + /* Check if the component is already enabled */ + if (acc_is_enabled == 1U) { + return LSM6DSV16X_OK; + } + + /* Output data rate selection. */ + if (lsm6dsv16x_xl_data_rate_set(®_ctx, acc_odr) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + acc_is_enabled = 1U; + + return LSM6DSV16X_OK; +} + +/** + * @brief Disable the LSM6DSV16X accelerometer sensor + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Disable_X() +{ + /* Check if the component is already disabled */ + if (acc_is_enabled == 0U) { + return LSM6DSV16X_OK; + } + + /* Get current output data rate. */ + if (lsm6dsv16x_xl_data_rate_get(®_ctx, &acc_odr) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + /* Output data rate selection - power down. */ + if (lsm6dsv16x_xl_data_rate_set(®_ctx, LSM6DSV16X_XL_ODR_OFF) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + acc_is_enabled = 0U; + + return LSM6DSV16X_OK; +} + +/** + * @brief Get the LSM6DSV16X accelerometer sensor sensitivity + * @param Sensitivity pointer + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Get_X_Sensitivity(float *Sensitivity) +{ + LSM6DSV16XStatusTypeDef ret = LSM6DSV16X_OK; + lsm6dsv16x_xl_full_scale_t full_scale; + + /* Read actual full scale selection from sensor. */ + if (lsm6dsv16x_xl_full_scale_get(®_ctx, &full_scale) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + /* Store the Sensitivity based on actual full scale. */ + switch (full_scale) { + case LSM6DSV16X_2g: + *Sensitivity = LSM6DSV16X_ACC_SENSITIVITY_FS_2G; + break; + + case LSM6DSV16X_4g: + *Sensitivity = LSM6DSV16X_ACC_SENSITIVITY_FS_4G; + break; + + case LSM6DSV16X_8g: + *Sensitivity = LSM6DSV16X_ACC_SENSITIVITY_FS_8G; + break; + + case LSM6DSV16X_16g: + *Sensitivity = LSM6DSV16X_ACC_SENSITIVITY_FS_16G; + break; + + default: + ret = LSM6DSV16X_ERROR; + break; + } + + return ret; +} + +/** + * @brief Get the LSM6DSV16X accelerometer sensor output data rate + * @param Odr pointer where the output data rate is written + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Get_X_ODR(float *Odr) +{ + LSM6DSV16XStatusTypeDef ret = LSM6DSV16X_OK; + lsm6dsv16x_xl_data_rate_t odr_low_level; + + /* Get current output data rate. */ + if (lsm6dsv16x_xl_data_rate_get(®_ctx, &odr_low_level) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + switch (odr_low_level) { + case LSM6DSV16X_XL_ODR_OFF: + *Odr = 0.0f; + break; + + case LSM6DSV16X_XL_ODR_AT_1Hz875: + *Odr = 1.875f; + break; + + case LSM6DSV16X_XL_ODR_AT_7Hz5: + *Odr = 7.5f; + break; + + case LSM6DSV16X_XL_ODR_AT_15Hz: + *Odr = 15.0f; + break; + + case LSM6DSV16X_XL_ODR_AT_30Hz: + *Odr = 30.0f; + break; + + case LSM6DSV16X_XL_ODR_AT_60Hz: + *Odr = 60.0f; + break; + + case LSM6DSV16X_XL_ODR_AT_120Hz: + *Odr = 120.0f; + break; + + case LSM6DSV16X_XL_ODR_AT_240Hz: + *Odr = 240.0f; + break; + + case LSM6DSV16X_XL_ODR_AT_480Hz: + *Odr = 480.0f; + break; + + case LSM6DSV16X_XL_ODR_AT_960Hz: + *Odr = 960.0f; + break; + + case LSM6DSV16X_XL_ODR_AT_1920Hz: + *Odr = 1920.0f; + break; + + case LSM6DSV16X_XL_ODR_AT_3840Hz: + *Odr = 3840.0f; + break; + + case LSM6DSV16X_XL_ODR_AT_7680Hz: + *Odr = 7680.0f; + break; + + default: + ret = LSM6DSV16X_ERROR; + break; + } + + return ret; +} + +/** + * @brief Get the LSM6DSV16X accelerometer sensor axes + * @param Acceleration pointer where the values of the axes are written + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Get_X_Axes(int32_t *Acceleration) +{ + lsm6dsv16x_axis3bit16_t data_raw; + float sensitivity = 0.0f; + + /* Read raw data values. */ + if (lsm6dsv16x_acceleration_raw_get(®_ctx, data_raw.i16bit) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + /* Get LSM6DSV16X actual sensitivity. */ + if (Get_X_Sensitivity(&sensitivity) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + /* Calculate the data. */ + Acceleration[0] = (int32_t)((float)((float)data_raw.i16bit[0] * sensitivity)); + Acceleration[1] = (int32_t)((float)((float)data_raw.i16bit[1] * sensitivity)); + Acceleration[2] = (int32_t)((float)((float)data_raw.i16bit[2] * sensitivity)); + + return LSM6DSV16X_OK; +} + +/** + * @brief Set the LSM6DSV16X accelerometer sensor output data rate + * @param Odr the output data rate value to be set + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Set_X_ODR(float Odr, LSM6DSV16X_ACC_Operating_Mode_t Mode) +{ + switch (Mode) { + case LSM6DSV16X_ACC_HIGH_PERFORMANCE_MODE: { + if (lsm6dsv16x_xl_mode_set(®_ctx, LSM6DSV16X_XL_HIGH_PERFORMANCE_MD) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + /* Valid ODR: 7.5Hz <= Odr <= 7.68kHz */ + Odr = (Odr < 7.5f) ? 7.5f + : (Odr > 7680.0f) ? 7680.0f + : Odr; + break; + } + + case LSM6DSV16X_ACC_HIGH_ACCURACY_MODE: + // TODO: Not implemented. + // NOTE: According to datasheet, section `6.5 High-accuracy ODR mode`: + // "... the other sensor also has to be configured in high-accuracy ODR (HAODR) mode." + return LSM6DSV16X_ERROR; + + case LSM6DSV16X_ACC_NORMAL_MODE: { + if (lsm6dsv16x_xl_mode_set(®_ctx, LSM6DSV16X_XL_NORMAL_MD) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + /* Valid ODR: 7.5Hz <= Odr <= 1.92kHz */ + Odr = (Odr < 7.5f) ? 7.5f + : (Odr > 1920.0f) ? 1920.0f + : Odr; + break; + } + + case LSM6DSV16X_ACC_LOW_POWER_MODE1: { + if (lsm6dsv16x_xl_mode_set(®_ctx, LSM6DSV16X_XL_LOW_POWER_2_AVG_MD) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + /* Valid ODR: 1.875Hz; 15Hz <= Odr <= 240kHz */ + Odr = (Odr == 1.875f) ? Odr + : (Odr < 15.000f) ? 15.0f + : (Odr > 240.000f) ? 240.0f + : Odr; + break; + } + + case LSM6DSV16X_ACC_LOW_POWER_MODE2: { + if (lsm6dsv16x_xl_mode_set(®_ctx, LSM6DSV16X_XL_LOW_POWER_4_AVG_MD) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + /* Valid ODR: 1.875Hz; 15Hz <= Odr <= 240kHz */ + Odr = (Odr == 1.875f) ? Odr + : (Odr < 15.000f) ? 15.0f + : (Odr > 240.000f) ? 240.0f + : Odr; + break; + } + + case LSM6DSV16X_ACC_LOW_POWER_MODE3: { + if (lsm6dsv16x_xl_mode_set(®_ctx, LSM6DSV16X_XL_LOW_POWER_8_AVG_MD) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + /* Valid ODR: 1.875Hz; 15Hz <= Odr <= 240kHz */ + Odr = (Odr == 1.875f) ? Odr + : (Odr < 15.000f) ? 15.0f + : (Odr > 240.000f) ? 240.0f + : Odr; + break; + } + + default: + return LSM6DSV16X_ERROR; + } + + if (acc_is_enabled == 1U) { + return Set_X_ODR_When_Enabled(Odr); + } else { + return Set_X_ODR_When_Disabled(Odr); + } +} + +/** + * @brief Set the LSM6DSV16X accelerometer sensor output data rate when enabled + * @param Odr the functional output data rate to be set + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Set_X_ODR_When_Enabled(float Odr) +{ + lsm6dsv16x_xl_data_rate_t new_odr; + + new_odr = (Odr <= 1.875f) ? LSM6DSV16X_XL_ODR_AT_1Hz875 + : (Odr <= 7.5f) ? LSM6DSV16X_XL_ODR_AT_7Hz5 + : (Odr <= 15.0f) ? LSM6DSV16X_XL_ODR_AT_15Hz + : (Odr <= 30.0f) ? LSM6DSV16X_XL_ODR_AT_30Hz + : (Odr <= 60.0f) ? LSM6DSV16X_XL_ODR_AT_60Hz + : (Odr <= 120.0f) ? LSM6DSV16X_XL_ODR_AT_120Hz + : (Odr <= 240.0f) ? LSM6DSV16X_XL_ODR_AT_240Hz + : (Odr <= 480.0f) ? LSM6DSV16X_XL_ODR_AT_480Hz + : (Odr <= 960.0f) ? LSM6DSV16X_XL_ODR_AT_960Hz + : (Odr <= 1920.0f) ? LSM6DSV16X_XL_ODR_AT_1920Hz + : (Odr <= 3840.0f) ? LSM6DSV16X_XL_ODR_AT_3840Hz + : LSM6DSV16X_XL_ODR_AT_7680Hz; + + /* Output data rate selection. */ + if (lsm6dsv16x_xl_data_rate_set(®_ctx, new_odr) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + return LSM6DSV16X_OK; +} + +/** + * @brief Set the LSM6DSV16X accelerometer sensor output data rate when disabled + * @param Odr the functional output data rate to be set + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Set_X_ODR_When_Disabled(float Odr) +{ + acc_odr = (Odr <= 1.875f) ? LSM6DSV16X_XL_ODR_AT_1Hz875 + : (Odr <= 7.5f) ? LSM6DSV16X_XL_ODR_AT_7Hz5 + : (Odr <= 15.0f) ? LSM6DSV16X_XL_ODR_AT_15Hz + : (Odr <= 30.0f) ? LSM6DSV16X_XL_ODR_AT_30Hz + : (Odr <= 60.0f) ? LSM6DSV16X_XL_ODR_AT_60Hz + : (Odr <= 120.0f) ? LSM6DSV16X_XL_ODR_AT_120Hz + : (Odr <= 240.0f) ? LSM6DSV16X_XL_ODR_AT_240Hz + : (Odr <= 480.0f) ? LSM6DSV16X_XL_ODR_AT_480Hz + : (Odr <= 960.0f) ? LSM6DSV16X_XL_ODR_AT_960Hz + : (Odr <= 1920.0f) ? LSM6DSV16X_XL_ODR_AT_1920Hz + : (Odr <= 3840.0f) ? LSM6DSV16X_XL_ODR_AT_3840Hz + : LSM6DSV16X_XL_ODR_AT_7680Hz; + + return LSM6DSV16X_OK; +} + +/** + * @brief Get the LSM6DSV16X accelerometer sensor full scale + * @param FullScale pointer where the full scale is written + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Get_X_FS(int32_t *FullScale) +{ + LSM6DSV16XStatusTypeDef ret = LSM6DSV16X_OK; + lsm6dsv16x_xl_full_scale_t fs_low_level; + + /* Read actual full scale selection from sensor. */ + if (lsm6dsv16x_xl_full_scale_get(®_ctx, &fs_low_level) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + switch (fs_low_level) { + case LSM6DSV16X_2g: + *FullScale = 2; + break; + + case LSM6DSV16X_4g: + *FullScale = 4; + break; + + case LSM6DSV16X_8g: + *FullScale = 8; + break; + + case LSM6DSV16X_16g: + *FullScale = 16; + break; + + default: + ret = LSM6DSV16X_ERROR; + break; + } + + return ret; +} + +/** + * @brief Set the LSM6DSV16X accelerometer sensor full scale + * @param FullScale the functional full scale to be set + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Set_X_FS(int32_t FullScale) +{ + lsm6dsv16x_xl_full_scale_t new_fs; + + /* Seems like MISRA C-2012 rule 14.3a violation but only from single file statical analysis point of view because + the parameter passed to the function is not known at the moment of analysis */ + new_fs = (FullScale <= 2) ? LSM6DSV16X_2g + : (FullScale <= 4) ? LSM6DSV16X_4g + : (FullScale <= 8) ? LSM6DSV16X_8g + : LSM6DSV16X_16g; + + if (lsm6dsv16x_xl_full_scale_set(®_ctx, new_fs) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + return LSM6DSV16X_OK; +} + +/** + * @brief Get the LSM6DSV16X accelerometer sensor raw axes + * @param Value pointer where the raw values of the axes are written + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Get_X_AxesRaw(int16_t *Value) +{ + lsm6dsv16x_axis3bit16_t data_raw; + + /* Read raw data values. */ + if (lsm6dsv16x_acceleration_raw_get(®_ctx, data_raw.i16bit) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + /* Format the data. */ + Value[0] = data_raw.i16bit[0]; + Value[1] = data_raw.i16bit[1]; + Value[2] = data_raw.i16bit[2]; + + return LSM6DSV16X_OK; +} + +/** + * @brief Get the LSM6DSV16X ACC data ready bit value + * @param Status the status of data ready bit + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Get_X_DRDY_Status(uint8_t *Status) +{ + lsm6dsv16x_all_sources_t val; + + if (lsm6dsv16x_all_sources_get(®_ctx, &val) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + *Status = val.drdy_xl; + return LSM6DSV16X_OK; +} + +/** + * @brief Get the status of all hardware events + * @param Status the status of all hardware events + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Get_X_Event_Status(LSM6DSV16X_Event_Status_t *Status) +{ + lsm6dsv16x_emb_func_status_t emb_func_status; + lsm6dsv16x_wake_up_src_t wake_up_src; + lsm6dsv16x_tap_src_t tap_src; + lsm6dsv16x_d6d_src_t d6d_src; + lsm6dsv16x_emb_func_src_t func_src; + lsm6dsv16x_md1_cfg_t md1_cfg; + lsm6dsv16x_md2_cfg_t md2_cfg; + lsm6dsv16x_emb_func_int1_t int1_ctrl; + lsm6dsv16x_emb_func_int2_t int2_ctrl; + + + (void)memset((void *)Status, 0x0, sizeof(LSM6DSV16X_Event_Status_t)); + + if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_WAKE_UP_SRC, (uint8_t *)&wake_up_src, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_TAP_SRC, (uint8_t *)&tap_src, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_D6D_SRC, (uint8_t *)&d6d_src, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + if (lsm6dsv16x_mem_bank_set(®_ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_EMB_FUNC_SRC, (uint8_t *)&func_src, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_EMB_FUNC_INT1, (uint8_t *)&int1_ctrl, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_EMB_FUNC_INT2, (uint8_t *)&int2_ctrl, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_EMB_FUNC_STATUS, (uint8_t *)&emb_func_status, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + if (lsm6dsv16x_mem_bank_set(®_ctx, LSM6DSV16X_MAIN_MEM_BANK) != 0) { + return LSM6DSV16X_ERROR; + } + + if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_MD1_CFG, (uint8_t *)&md1_cfg, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_MD2_CFG, (uint8_t *)&md2_cfg, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + + if ((md1_cfg.int1_ff == 1U) || (md2_cfg.int2_ff == 1U)) { + if (wake_up_src.ff_ia == 1U) { + Status->FreeFallStatus = 1; + } + } + + if ((md1_cfg.int1_wu == 1U) || (md2_cfg.int2_wu == 1U)) { + if (wake_up_src.wu_ia == 1U) { + Status->WakeUpStatus = 1; + } + } + + if ((md1_cfg.int1_single_tap == 1U) || (md2_cfg.int2_single_tap == 1U)) { + if (tap_src.single_tap == 1U) { + Status->TapStatus = 1; + } + } + + if ((md1_cfg.int1_double_tap == 1U) || (md2_cfg.int2_double_tap == 1U)) { + if (tap_src.double_tap == 1U) { + Status->DoubleTapStatus = 1; + } + } + + if ((md1_cfg.int1_6d == 1U) || (md2_cfg.int2_6d == 1U)) { + if (d6d_src.d6d_ia == 1U) { + Status->D6DOrientationStatus = 1; + } + } + + if (int1_ctrl.int1_step_detector == 1U || int2_ctrl.int2_step_detector == 1U) { + if (func_src.step_detected == 1U) { + Status->StepStatus = 1; + } + } + + if ((int1_ctrl.int1_tilt == 1U) || (int2_ctrl.int2_tilt == 1U)) { + if (emb_func_status.is_tilt == 1U) { + Status->TiltStatus = 1; + } + } + + return LSM6DSV16X_OK; +} + +/** + * @brief Set the LSM6DSV16X accelerometer power mode + * @param PowerMode Value of the powerMode + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Set_X_Power_Mode(uint8_t PowerMode) +{ + if (lsm6dsv16x_xl_mode_set(®_ctx, (lsm6dsv16x_xl_mode_t)PowerMode) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + return LSM6DSV16X_OK; +} + +/** + * @brief Set the LSM6DSV16X accelerometer filter mode + * @param LowHighPassFlag 0/1 for setting low-pass/high-pass filter mode + * @param FilterMode Value of the filter Mode + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Set_X_Filter_Mode(uint8_t LowHighPassFlag, uint8_t FilterMode) +{ + if (LowHighPassFlag == 0) { + /*Set accelerometer low_pass filter-mode*/ + + /*Set to 1 LPF2 bit (CTRL8_XL)*/ + if (lsm6dsv16x_filt_xl_lp2_set(®_ctx, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + if (lsm6dsv16x_filt_xl_lp2_bandwidth_set(®_ctx, (lsm6dsv16x_filt_xl_lp2_bandwidth_t)FilterMode) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + } else { + if (lsm6dsv16x_filt_xl_lp2_set(®_ctx, 0) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + /*Set accelerometer high_pass filter-mode*/ + if (lsm6dsv16x_filt_xl_lp2_bandwidth_set(®_ctx, (lsm6dsv16x_filt_xl_lp2_bandwidth_t)FilterMode) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + } + return LSM6DSV16X_OK; +} + +/** + * @brief Enable 6D orientation detection + * @param IntPin interrupt pin line to be used + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Enable_6D_Orientation(LSM6DSV16X_SensorIntPin_t IntPin) +{ + LSM6DSV16XStatusTypeDef ret = LSM6DSV16X_OK; + lsm6dsv16x_md1_cfg_t val1; + lsm6dsv16x_md2_cfg_t val2; + lsm6dsv16x_functions_enable_t functions_enable; + + /* Output Data Rate selection */ + if (Set_X_ODR(480.0f) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + /* Full scale selection */ + if (Set_X_FS(2) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + /* 6D orientation enabled. */ + if (Set_6D_Orientation_Threshold(2) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + /* Enable 6D orientation event on either INT1 or INT2 pin */ + switch (IntPin) { + case LSM6DSV16X_INT1_PIN: + if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_MD1_CFG, (uint8_t *)&val1, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + val1.int1_6d = PROPERTY_ENABLE; + + if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_MD1_CFG, (uint8_t *)&val1, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_FUNCTIONS_ENABLE, (uint8_t *)&functions_enable, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + functions_enable.interrupts_enable = PROPERTY_ENABLE; + + if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_FUNCTIONS_ENABLE, (uint8_t *)&functions_enable, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + break; + + case LSM6DSV16X_INT2_PIN: + if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_MD2_CFG, (uint8_t *)&val2, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + val2.int2_6d = PROPERTY_ENABLE; + + if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_MD2_CFG, (uint8_t *)&val2, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_FUNCTIONS_ENABLE, (uint8_t *)&functions_enable, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + functions_enable.interrupts_enable = PROPERTY_ENABLE; + + if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_FUNCTIONS_ENABLE, (uint8_t *)&functions_enable, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + break; + + default: + ret = LSM6DSV16X_ERROR; + break; + } + + return ret; +} + +/** + * @brief Disable 6D orientation detection + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Disable_6D_Orientation() +{ + lsm6dsv16x_md1_cfg_t val1; + lsm6dsv16x_md2_cfg_t val2; + + /* Reset threshold */ + if (Set_6D_Orientation_Threshold(0) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + /* Disable 6D orientation event on both INT1 and INT2 pins */ + if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_MD1_CFG, (uint8_t *)&val1, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + val1.int1_6d = PROPERTY_DISABLE; + + if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_MD1_CFG, (uint8_t *)&val1, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_MD2_CFG, (uint8_t *)&val2, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + val2.int2_6d = PROPERTY_DISABLE; + + if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_MD2_CFG, (uint8_t *)&val2, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + return LSM6DSV16X_OK; +} + +/** + * @brief Set 6D orientation threshold + * @param Threshold 6D Orientation detection threshold + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Set_6D_Orientation_Threshold(uint8_t Threshold) +{ + LSM6DSV16XStatusTypeDef ret = LSM6DSV16X_OK; + lsm6dsv16x_6d_threshold_t newThreshold = LSM6DSV16X_DEG_80; + + switch (Threshold) { + case 0: + newThreshold = LSM6DSV16X_DEG_80; + break; + case 1: + newThreshold = LSM6DSV16X_DEG_70; + break; + case 2: + newThreshold = LSM6DSV16X_DEG_60; + break; + case 3: + newThreshold = LSM6DSV16X_DEG_50; + break; + default: + ret = LSM6DSV16X_ERROR; + break; + } + + if (ret == LSM6DSV16X_ERROR) { + return LSM6DSV16X_ERROR; + } + + if (lsm6dsv16x_6d_threshold_set(®_ctx, newThreshold) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + return LSM6DSV16X_OK; + +} + +/** + * @brief Get the status of XLow orientation + * @param XLow the status of XLow orientation + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Get_6D_Orientation_XL(uint8_t *XLow) +{ + lsm6dsv16x_d6d_src_t data; + + if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_D6D_SRC, (uint8_t *)&data, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + *XLow = data.xl; + + return LSM6DSV16X_OK; +} + +/** + * @brief Get the status of XHigh orientation + * @param XHigh the status of XHigh orientation + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Get_6D_Orientation_XH(uint8_t *XHigh) +{ + lsm6dsv16x_d6d_src_t data; + + if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_D6D_SRC, (uint8_t *)&data, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + *XHigh = data.xh; + + return LSM6DSV16X_OK; +} + +/** + * @brief Get the status of YLow orientation + * @param YLow the status of YLow orientation + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Get_6D_Orientation_YL(uint8_t *YLow) +{ + lsm6dsv16x_d6d_src_t data; + + if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_D6D_SRC, (uint8_t *)&data, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + *YLow = data.yl; + + return LSM6DSV16X_OK; +} + +/** + * @brief Get the status of YHigh orientation + * @param YHigh the status of YHigh orientation + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Get_6D_Orientation_YH(uint8_t *YHigh) +{ + lsm6dsv16x_d6d_src_t data; + + if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_D6D_SRC, (uint8_t *)&data, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + *YHigh = data.yh; + + return LSM6DSV16X_OK; +} + +/** + * @brief Get the status of ZLow orientation + * @param ZLow the status of ZLow orientation + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Get_6D_Orientation_ZL(uint8_t *ZLow) +{ + lsm6dsv16x_d6d_src_t data; + + if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_D6D_SRC, (uint8_t *)&data, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + *ZLow = data.zl; + + return LSM6DSV16X_OK; +} + +/** + * @brief Get the status of ZHigh orientation + * @param ZHigh the status of ZHigh orientation + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Get_6D_Orientation_ZH(uint8_t *ZHigh) +{ + lsm6dsv16x_d6d_src_t data; + + if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_D6D_SRC, (uint8_t *)&data, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + *ZHigh = data.zh; + + return LSM6DSV16X_OK; +} + + +/** + * @brief Enable free fall detection + * @param IntPin interrupt pin line to be used + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Enable_Free_Fall_Detection(LSM6DSV16X_SensorIntPin_t IntPin) +{ + LSM6DSV16XStatusTypeDef ret = LSM6DSV16X_OK; + lsm6dsv16x_md1_cfg_t val1; + lsm6dsv16x_md2_cfg_t val2; + lsm6dsv16x_functions_enable_t functions_enable; + + /* Output Data Rate selection */ + if (Set_X_ODR(480) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + /* Full scale selection */ + if (Set_X_FS(2) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + /* Set free fall duration.*/ + if (Set_Free_Fall_Duration(3) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + /* Set free fall threshold. */ + if (Set_Free_Fall_Threshold(3) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + /* Enable free fall event on either INT1 or INT2 pin */ + switch (IntPin) { + case LSM6DSV16X_INT1_PIN: + if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_MD1_CFG, (uint8_t *)&val1, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + val1.int1_ff = PROPERTY_ENABLE; + + if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_MD1_CFG, (uint8_t *)&val1, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_FUNCTIONS_ENABLE, (uint8_t *)&functions_enable, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + functions_enable.interrupts_enable = PROPERTY_ENABLE; + + if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_FUNCTIONS_ENABLE, (uint8_t *)&functions_enable, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + break; + + case LSM6DSV16X_INT2_PIN: + if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_MD2_CFG, (uint8_t *)&val2, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + val2.int2_ff = PROPERTY_ENABLE; + + if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_MD2_CFG, (uint8_t *)&val2, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_FUNCTIONS_ENABLE, (uint8_t *)&functions_enable, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + functions_enable.interrupts_enable = PROPERTY_ENABLE; + + if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_FUNCTIONS_ENABLE, (uint8_t *)&functions_enable, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + break; + + default: + ret = LSM6DSV16X_ERROR; + break; + } + + return ret; +} + +/** + * @brief Disable free fall detection + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Disable_Free_Fall_Detection() +{ + lsm6dsv16x_md1_cfg_t val1; + lsm6dsv16x_md2_cfg_t val2; + + /* Disable free fall event on both INT1 and INT2 pins */ + if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_MD1_CFG, (uint8_t *)&val1, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + val1.int1_ff = PROPERTY_DISABLE; + + if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_MD1_CFG, (uint8_t *)&val1, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_MD2_CFG, (uint8_t *)&val2, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + val2.int2_ff = PROPERTY_DISABLE; + + if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_MD2_CFG, (uint8_t *)&val2, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + /* Reset free fall threshold. */ + if (Set_Free_Fall_Threshold(0) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + /* Reset free fall duration */ + if (Set_Free_Fall_Duration(0) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + return LSM6DSV16X_OK; +} + +/** + * @brief Set free fall threshold + * @param Threshold free fall detection threshold + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Set_Free_Fall_Threshold(uint8_t Threshold) +{ + lsm6dsv16x_ff_thresholds_t val; + switch (Threshold) { + case LSM6DSV16X_156_mg: + val = LSM6DSV16X_156_mg; + break; + + case LSM6DSV16X_219_mg: + val = LSM6DSV16X_219_mg; + break; + + case LSM6DSV16X_250_mg: + val = LSM6DSV16X_250_mg; + break; + + case LSM6DSV16X_312_mg: + val = LSM6DSV16X_312_mg; + break; + + case LSM6DSV16X_344_mg: + val = LSM6DSV16X_344_mg; + break; + + case LSM6DSV16X_406_mg: + val = LSM6DSV16X_406_mg; + break; + + case LSM6DSV16X_469_mg: + val = LSM6DSV16X_469_mg; + break; + + case LSM6DSV16X_500_mg: + val = LSM6DSV16X_500_mg; + break; + + default: + val = LSM6DSV16X_156_mg; + break; + } + + /* Set free fall threshold. */ + if (lsm6dsv16x_ff_thresholds_set(®_ctx, val) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + return LSM6DSV16X_OK; +} + + +/** + * @brief Set free fall duration + * @param Duration free fall detection duration + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Set_Free_Fall_Duration(uint8_t Duration) +{ + if (lsm6dsv16x_ff_time_windows_set(®_ctx, Duration) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + return LSM6DSV16X_OK; +} + +/** + * @brief Enable wake up detection + * @param IntPin interrupt pin line to be used + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Enable_Wake_Up_Detection(LSM6DSV16X_SensorIntPin_t IntPin) +{ + LSM6DSV16XStatusTypeDef ret = LSM6DSV16X_OK; + lsm6dsv16x_md1_cfg_t val1; + lsm6dsv16x_md2_cfg_t val2; + lsm6dsv16x_functions_enable_t functions_enable; + + /* Output Data Rate selection */ + if (Set_X_ODR(480) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + /* Full scale selection */ + if (Set_X_FS(2) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + /* Set wake-up threshold */ + if (Set_Wake_Up_Threshold(63) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + /* Set wake-up durantion */ + if (Set_Wake_Up_Duration(0) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + /* Enable wake up event on either INT1 or INT2 pin */ + switch (IntPin) { + case LSM6DSV16X_INT1_PIN: + if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_MD1_CFG, (uint8_t *)&val1, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + val1.int1_wu = PROPERTY_ENABLE; + + if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_MD1_CFG, (uint8_t *)&val1, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_FUNCTIONS_ENABLE, (uint8_t *)&functions_enable, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + functions_enable.interrupts_enable = PROPERTY_ENABLE; + + if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_FUNCTIONS_ENABLE, (uint8_t *)&functions_enable, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + break; + + case LSM6DSV16X_INT2_PIN: + if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_MD2_CFG, (uint8_t *)&val2, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + val2.int2_wu = PROPERTY_ENABLE; + + if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_MD2_CFG, (uint8_t *)&val2, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_FUNCTIONS_ENABLE, (uint8_t *)&functions_enable, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + functions_enable.interrupts_enable = PROPERTY_ENABLE; + + if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_FUNCTIONS_ENABLE, (uint8_t *)&functions_enable, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + break; + + default: + ret = LSM6DSV16X_ERROR; + break; + } + + return ret; +} + + +/** + * @brief Disable wake up detection + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Disable_Wake_Up_Detection() +{ + lsm6dsv16x_md1_cfg_t val1; + lsm6dsv16x_md2_cfg_t val2; + + /* Disable wake up event on both INT1 and INT2 pins */ + if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_MD1_CFG, (uint8_t *)&val1, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + val1.int1_wu = PROPERTY_DISABLE; + + if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_MD1_CFG, (uint8_t *)&val1, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_MD2_CFG, (uint8_t *)&val2, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + val2.int2_wu = PROPERTY_DISABLE; + + if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_MD2_CFG, (uint8_t *)&val2, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + /* Reset wake-up threshold */ + if (Set_Wake_Up_Threshold(0) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + /* Reset wake-up durantion */ + if (Set_Wake_Up_Duration(0) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + return LSM6DSV16X_OK; +} + +/** + * @brief Set wake up threshold + * @param Threshold wake up detection threshold + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Set_Wake_Up_Threshold(uint32_t Threshold) +{ + lsm6dsv16x_act_thresholds_t wake_up_ths; + + if (lsm6dsv16x_act_thresholds_get(®_ctx, &wake_up_ths) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + wake_up_ths.wk_ths_mg = Threshold; + + if (lsm6dsv16x_act_thresholds_set(®_ctx, wake_up_ths) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + return LSM6DSV16X_OK; +} + +/** + * @brief Set wake up duration + * @param Duration wake up detection duration + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Set_Wake_Up_Duration(uint8_t Duration) +{ + lsm6dsv16x_act_wkup_time_windows_t dur_t; + + if (lsm6dsv16x_act_wkup_time_windows_get(®_ctx, &dur_t) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + dur_t.shock = Duration; + + if (lsm6dsv16x_act_wkup_time_windows_set(®_ctx, dur_t) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + return LSM6DSV16X_OK; +} + +/** + * @brief Enable single tap detection + * @param IntPin interrupt pin line to be used + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Enable_Single_Tap_Detection(LSM6DSV16X_SensorIntPin_t IntPin) +{ + LSM6DSV16XStatusTypeDef ret = LSM6DSV16X_OK; + lsm6dsv16x_md1_cfg_t val1; + lsm6dsv16x_md2_cfg_t val2; + lsm6dsv16x_functions_enable_t functions_enable; + + lsm6dsv16x_int_dur2_t tap_dur; + lsm6dsv16x_tap_cfg0_t tap_cfg0; + lsm6dsv16x_tap_ths_6d_t tap_ths_6d; + + /* Output Data Rate selection */ + if (Set_X_ODR(480) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + /* Full scale selection */ + if (Set_X_FS(8) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + /* Enable tap detection on Z-axis. */ + if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_TAP_CFG0, (uint8_t *)&tap_cfg0, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + tap_cfg0.tap_z_en = 0x01U; + + if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_TAP_CFG0, (uint8_t *)&tap_cfg0, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + /* Set Z-axis threshold. */ + if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_TAP_THS_6D, (uint8_t *)&tap_ths_6d, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + tap_ths_6d.tap_ths_z = 0x2U; + + if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_TAP_THS_6D, (uint8_t *)&tap_ths_6d, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + /* Set quiet and shock time windows. */ + if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_INT_DUR2, (uint8_t *)&tap_dur, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + tap_dur.quiet = (uint8_t)0x02U; + tap_dur.shock = (uint8_t)0x01U; + + if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_INT_DUR2, (uint8_t *)&tap_dur, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + /* Set tap mode. */ + if (lsm6dsv16x_tap_mode_set(®_ctx, LSM6DSV16X_ONLY_SINGLE) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + /* Enable single tap event on either INT1 or INT2 pin */ + switch (IntPin) { + case LSM6DSV16X_INT1_PIN: + if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_MD1_CFG, (uint8_t *)&val1, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + val1.int1_single_tap = PROPERTY_ENABLE; + + if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_MD1_CFG, (uint8_t *)&val1, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_FUNCTIONS_ENABLE, (uint8_t *)&functions_enable, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + functions_enable.interrupts_enable = PROPERTY_ENABLE; + + if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_FUNCTIONS_ENABLE, (uint8_t *)&functions_enable, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + break; + + case LSM6DSV16X_INT2_PIN: + if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_MD2_CFG, (uint8_t *)&val2, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + val2.int2_single_tap = PROPERTY_ENABLE; + + if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_MD2_CFG, (uint8_t *)&val2, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_FUNCTIONS_ENABLE, (uint8_t *)&functions_enable, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + functions_enable.interrupts_enable = PROPERTY_ENABLE; + + if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_FUNCTIONS_ENABLE, (uint8_t *)&functions_enable, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + break; + + default: + ret = LSM6DSV16X_ERROR; + break; + } + + return ret; +} + +/** + * @brief Disable single tap detection + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Disable_Single_Tap_Detection() +{ + lsm6dsv16x_md1_cfg_t val1; + lsm6dsv16x_md2_cfg_t val2; + lsm6dsv16x_int_dur2_t tap_dur; + lsm6dsv16x_tap_cfg0_t tap_cfg0; + lsm6dsv16x_tap_ths_6d_t tap_ths_6d; + + + /* Disable single tap event on both INT1 and INT2 pins */ + if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_MD1_CFG, (uint8_t *)&val1, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + val1.int1_single_tap = PROPERTY_DISABLE; + + if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_MD1_CFG, (uint8_t *)&val1, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_MD2_CFG, (uint8_t *)&val2, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + val2.int2_single_tap = PROPERTY_DISABLE; + + if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_MD2_CFG, (uint8_t *)&val2, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + /* Disable tap detection on Z-axis. */ + if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_TAP_CFG0, (uint8_t *)&tap_cfg0, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + tap_cfg0.tap_z_en = 0x0U; + + if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_TAP_CFG0, (uint8_t *)&tap_cfg0, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + /* Reset Z-axis threshold. */ + if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_TAP_THS_6D, (uint8_t *)&tap_ths_6d, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + tap_ths_6d.tap_ths_z = 0x0U; + + if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_TAP_THS_6D, (uint8_t *)&tap_ths_6d, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + /* Reset quiet and shock time windows. */ + if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_INT_DUR2, (uint8_t *)&tap_dur, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + tap_dur.quiet = (uint8_t)0x0U; + tap_dur.shock = (uint8_t)0x0U; + + if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_INT_DUR2, (uint8_t *)&tap_dur, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + return LSM6DSV16X_OK; +} + +/** + * @brief Enable double tap detection + * @param IntPin interrupt pin line to be used + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Enable_Double_Tap_Detection(LSM6DSV16X_SensorIntPin_t IntPin) +{ + LSM6DSV16XStatusTypeDef ret = LSM6DSV16X_OK; + lsm6dsv16x_md1_cfg_t val1; + lsm6dsv16x_md2_cfg_t val2; + lsm6dsv16x_functions_enable_t functions_enable; + + lsm6dsv16x_int_dur2_t tap_dur; + lsm6dsv16x_tap_cfg0_t tap_cfg0; + lsm6dsv16x_tap_ths_6d_t tap_ths_6d; + + + /* Enable tap detection on Z-axis. */ + if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_TAP_CFG0, (uint8_t *)&tap_cfg0, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + tap_cfg0.tap_z_en = 0x01U; + + if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_TAP_CFG0, (uint8_t *)&tap_cfg0, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + /* Set Z-axis threshold. */ + if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_TAP_THS_6D, (uint8_t *)&tap_ths_6d, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + tap_ths_6d.tap_ths_z = 0x03U; + + if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_TAP_THS_6D, (uint8_t *)&tap_ths_6d, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + /* Set quiet shock and duration. */ + if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_INT_DUR2, (uint8_t *)&tap_dur, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + tap_dur.dur = (uint8_t)0x03U; + tap_dur.quiet = (uint8_t)0x02U; + tap_dur.shock = (uint8_t)0x02U; + + if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_INT_DUR2, (uint8_t *)&tap_dur, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + /* Set tap mode. */ + if (lsm6dsv16x_tap_mode_set(®_ctx, LSM6DSV16X_BOTH_SINGLE_DOUBLE) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + /* Output Data Rate selection */ + if (Set_X_ODR(480) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + /* Full scale selection */ + if (Set_X_FS(8) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + /* Enable double tap event on either INT1 or INT2 pin */ + switch (IntPin) { + case LSM6DSV16X_INT1_PIN: + if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_MD1_CFG, (uint8_t *)&val1, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + val1.int1_double_tap = PROPERTY_ENABLE; + + if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_MD1_CFG, (uint8_t *)&val1, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_FUNCTIONS_ENABLE, (uint8_t *)&functions_enable, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + functions_enable.interrupts_enable = PROPERTY_ENABLE; + + if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_FUNCTIONS_ENABLE, (uint8_t *)&functions_enable, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + break; + + case LSM6DSV16X_INT2_PIN: + if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_MD2_CFG, (uint8_t *)&val2, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + val2.int2_double_tap = PROPERTY_ENABLE; + + if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_MD2_CFG, (uint8_t *)&val2, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_FUNCTIONS_ENABLE, (uint8_t *)&functions_enable, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + functions_enable.interrupts_enable = PROPERTY_ENABLE; + + if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_FUNCTIONS_ENABLE, (uint8_t *)&functions_enable, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + break; + + default: + ret = LSM6DSV16X_ERROR; + break; + } + + return ret; +} + +/** + * @brief Disable double tap detection + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Disable_Double_Tap_Detection() +{ + lsm6dsv16x_md1_cfg_t val1; + lsm6dsv16x_md2_cfg_t val2; + + lsm6dsv16x_int_dur2_t tap_dur; + lsm6dsv16x_tap_cfg0_t tap_cfg0; + lsm6dsv16x_tap_ths_6d_t tap_ths_6d; + + /* Disable double tap event on both INT1 and INT2 pins */ + if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_MD1_CFG, (uint8_t *)&val1, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + val1.int1_ff = PROPERTY_DISABLE; + + if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_MD1_CFG, (uint8_t *)&val1, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_MD2_CFG, (uint8_t *)&val2, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + val2.int2_ff = PROPERTY_DISABLE; + + if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_MD2_CFG, (uint8_t *)&val2, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + /* Disable tap detection on Z-axis. */ + if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_TAP_CFG0, (uint8_t *)&tap_cfg0, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + tap_cfg0.tap_z_en = 0x0U; + + if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_TAP_CFG0, (uint8_t *)&tap_cfg0, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + /* Reset Z-axis threshold. */ + if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_TAP_THS_6D, (uint8_t *)&tap_ths_6d, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + tap_ths_6d.tap_ths_z = 0x0U; + + if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_TAP_THS_6D, (uint8_t *)&tap_ths_6d, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + /* Reset quiet shock and duratio. */ + if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_INT_DUR2, (uint8_t *)&tap_dur, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + tap_dur.dur = (uint8_t)0x0U; + tap_dur.quiet = (uint8_t)0x0U; + tap_dur.shock = (uint8_t)0x0U; + + if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_INT_DUR2, (uint8_t *)&tap_dur, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + /* Set tap mode. */ + if (lsm6dsv16x_tap_mode_set(®_ctx, LSM6DSV16X_ONLY_SINGLE) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + + return LSM6DSV16X_OK; +} + +/** + * @brief Set tap threshold + * @param Threshold tap threshold + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Set_Tap_Threshold(uint8_t Threshold) +{ + lsm6dsv16x_tap_ths_6d_t tap_ths_6d; + + /* Set Z-axis threshold */ + if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_TAP_THS_6D, (uint8_t *)&tap_ths_6d, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + tap_ths_6d.tap_ths_z = Threshold; + + if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_TAP_THS_6D, (uint8_t *)&tap_ths_6d, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + return LSM6DSV16X_OK; +} + +/** + * @brief Set tap shock time + * @param Time tap shock time + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Set_Tap_Shock_Time(uint8_t Time) +{ + lsm6dsv16x_int_dur2_t tap_dur; + + /* Set shock time */ + if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_INT_DUR2, (uint8_t *)&tap_dur, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + tap_dur.shock = Time; + + if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_INT_DUR2, (uint8_t *)&tap_dur, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } +} + +/** + * @brief Set tap quiet time + * @param Time tap quiet time + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Set_Tap_Quiet_Time(uint8_t Time) +{ + lsm6dsv16x_int_dur2_t tap_dur; + + /* Set quiet time */ + if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_INT_DUR2, (uint8_t *)&tap_dur, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + tap_dur.quiet = Time; + + if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_INT_DUR2, (uint8_t *)&tap_dur, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + return LSM6DSV16X_OK; +} + +/** + * @brief Set tap duration time + * @param Time tap duration time + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Set_Tap_Duration_Time(uint8_t Time) +{ + lsm6dsv16x_int_dur2_t tap_dur; + + /* Set duration time */ + if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_INT_DUR2, (uint8_t *)&tap_dur, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + tap_dur.dur = Time; + + if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_INT_DUR2, (uint8_t *)&tap_dur, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + return LSM6DSV16X_OK; +} + +/** + * @brief Enable pedometer + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Enable_Pedometer(LSM6DSV16X_SensorIntPin_t IntPin) +{ + lsm6dsv16x_stpcnt_mode_t mode; + lsm6dsv16x_md1_cfg_t val1; + lsm6dsv16x_md2_cfg_t val2; + lsm6dsv16x_emb_func_int1_t emb_func_int1; + lsm6dsv16x_emb_func_int2_t emb_func_int2; + + /* Output Data Rate selection */ + if (Set_X_ODR(30) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + /* Full scale selection */ + if (Set_X_FS(2) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + if (lsm6dsv16x_stpcnt_mode_get(®_ctx, &mode) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + /* Enable pedometer algorithm. */ + mode.step_counter_enable = PROPERTY_ENABLE; + mode.false_step_rej = PROPERTY_DISABLE; + + /* Turn on embedded features */ + if (lsm6dsv16x_stpcnt_mode_set(®_ctx, mode) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + /* Enable free fall event on either INT1 or INT2 pin */ + switch (IntPin) { + case LSM6DSV16X_INT1_PIN: + /* Enable access to embedded functions registers. */ + if (lsm6dsv16x_mem_bank_set(®_ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + /* Step detector interrupt driven to INT1 pin */ + if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_EMB_FUNC_INT1, (uint8_t *)&emb_func_int1, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + emb_func_int1.int1_step_detector = PROPERTY_ENABLE; + + if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_EMB_FUNC_INT1, (uint8_t *)&emb_func_int1, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + /* Disable access to embedded functions registers */ + if (lsm6dsv16x_mem_bank_set(®_ctx, LSM6DSV16X_MAIN_MEM_BANK) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_MD1_CFG, (uint8_t *)&val1, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + val1.int1_emb_func = PROPERTY_ENABLE; + + if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_MD1_CFG, (uint8_t *)&val1, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + break; + + case LSM6DSV16X_INT2_PIN: + /* Enable access to embedded functions registers. */ + if (lsm6dsv16x_mem_bank_set(®_ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + /* Step detector interrupt driven to INT1 pin */ + if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_EMB_FUNC_INT2, (uint8_t *)&emb_func_int2, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + emb_func_int2.int2_step_detector = PROPERTY_ENABLE; + + if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_EMB_FUNC_INT2, (uint8_t *)&emb_func_int2, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + /* Disable access to embedded functions registers */ + if (lsm6dsv16x_mem_bank_set(®_ctx, LSM6DSV16X_MAIN_MEM_BANK) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_MD2_CFG, (uint8_t *)&val2, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + val2.int2_emb_func = PROPERTY_ENABLE; + + if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_MD2_CFG, (uint8_t *)&val2, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + break; + + default: + return LSM6DSV16X_ERROR; + break; + } + + return LSM6DSV16X_OK; +} + + +/** + * @brief Disable pedometer + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Disable_Pedometer() +{ + lsm6dsv16x_md1_cfg_t val1; + lsm6dsv16x_md2_cfg_t val2; + + lsm6dsv16x_emb_func_int1_t emb_func_int1; + lsm6dsv16x_emb_func_int2_t emb_func_int2; + + lsm6dsv16x_stpcnt_mode_t mode; + + + if (lsm6dsv16x_stpcnt_mode_get(®_ctx, &mode) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + /* Enable pedometer algorithm. */ + mode.step_counter_enable = PROPERTY_DISABLE; + mode.false_step_rej = PROPERTY_DISABLE; + + /* Turn off embedded features */ + if (lsm6dsv16x_stpcnt_mode_set(®_ctx, mode) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + /* Disable emb func event on either INT1 or INT2 pin */ + if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_MD1_CFG, (uint8_t *)&val1, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + val1.int1_emb_func = PROPERTY_DISABLE; + + if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_MD1_CFG, (uint8_t *)&val1, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_MD2_CFG, (uint8_t *)&val2, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + val2.int2_emb_func = PROPERTY_DISABLE; + + if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_MD2_CFG, (uint8_t *)&val2, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + /* Enable access to embedded functions registers. */ + if (lsm6dsv16x_mem_bank_set(®_ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + /* Reset interrupt driven to INT1 pin. */ + if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_EMB_FUNC_INT1, (uint8_t *)&emb_func_int1, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + emb_func_int1.int1_step_detector = PROPERTY_DISABLE; + + if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_EMB_FUNC_INT1, (uint8_t *)&emb_func_int1, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + /* Reset interrupt driven to INT2 pin. */ + if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_EMB_FUNC_INT2, (uint8_t *)&emb_func_int2, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + emb_func_int2.int2_step_detector = PROPERTY_DISABLE; + + if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_EMB_FUNC_INT2, (uint8_t *)&emb_func_int2, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + /* Disable access to embedded functions registers. */ + if (lsm6dsv16x_mem_bank_set(®_ctx, LSM6DSV16X_MAIN_MEM_BANK) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + + return LSM6DSV16X_OK; +} + +/** + * @brief Get step count + * @param StepCount step counter + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Get_Step_Count(uint16_t *StepCount) +{ + if (lsm6dsv16x_stpcnt_steps_get(®_ctx, StepCount) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + return LSM6DSV16X_OK; +} + +/** + * @brief Enable step counter reset + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Step_Counter_Reset() +{ + if (lsm6dsv16x_stpcnt_rst_step_set(®_ctx, PROPERTY_ENABLE) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + return LSM6DSV16X_OK; +} + +/** + * @brief Enable tilt detection + * @param IntPin interrupt pin line to be used + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Enable_Tilt_Detection(LSM6DSV16X_SensorIntPin_t IntPin) +{ + LSM6DSV16XStatusTypeDef ret = LSM6DSV16X_OK; + lsm6dsv16x_md1_cfg_t val1; + lsm6dsv16x_md2_cfg_t val2; + lsm6dsv16x_emb_func_en_a_t emb_func_en_a; + lsm6dsv16x_emb_func_int1_t emb_func_int1; + lsm6dsv16x_emb_func_int2_t emb_func_int2; + + /* Output Data Rate selection */ + if (Set_X_ODR(30) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + /* Full scale selection */ + if (Set_X_FS(2) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + switch (IntPin) { + case LSM6DSV16X_INT1_PIN: + /* Enable access to embedded functions registers */ + if (lsm6dsv16x_mem_bank_set(®_ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + /* Enable tilt detection */ + if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_EMB_FUNC_EN_A, (uint8_t *)&emb_func_en_a, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + emb_func_en_a.tilt_en = PROPERTY_ENABLE; + + if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_EMB_FUNC_EN_A, (uint8_t *)&emb_func_en_a, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + /* Tilt interrupt driven to INT1 pin */ + if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_EMB_FUNC_INT1, (uint8_t *)&emb_func_int1, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + emb_func_int1.int1_tilt = PROPERTY_ENABLE; + + if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_EMB_FUNC_INT1, (uint8_t *)&emb_func_int1, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + /* Disable access to embedded functions registers */ + if (lsm6dsv16x_mem_bank_set(®_ctx, LSM6DSV16X_MAIN_MEM_BANK) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + /* Enable routing the embedded functions interrupt */ + if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_MD1_CFG, (uint8_t *)&val1, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + val1.int1_emb_func = PROPERTY_ENABLE; + + if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_MD1_CFG, (uint8_t *)&val1, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + break; + + + case LSM6DSV16X_INT2_PIN: + /* Enable access to embedded functions registers */ + if (lsm6dsv16x_mem_bank_set(®_ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + /* Enable tilt detection */ + if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_EMB_FUNC_EN_A, (uint8_t *)&emb_func_en_a, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + emb_func_en_a.tilt_en = PROPERTY_ENABLE; + + if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_EMB_FUNC_EN_A, (uint8_t *)&emb_func_en_a, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + /* Tilt interrupt driven to INT2 pin */ + if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_EMB_FUNC_INT2, (uint8_t *)&emb_func_int2, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + emb_func_int2.int2_tilt = PROPERTY_ENABLE; + + if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_EMB_FUNC_INT2, (uint8_t *)&emb_func_int2, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + /* Disable access to embedded functions registers */ + if (lsm6dsv16x_mem_bank_set(®_ctx, LSM6DSV16X_MAIN_MEM_BANK) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + /* Enable routing the embedded functions interrupt */ + if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_MD2_CFG, (uint8_t *)&val2, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + val2.int2_emb_func = PROPERTY_ENABLE; + + if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_MD2_CFG, (uint8_t *)&val2, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + break; + + default: + ret = LSM6DSV16X_ERROR; + break; + } + + return ret; +} + +/** + * @brief Disable tilt detection + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Disable_Tilt_Detection() +{ + LSM6DSV16XStatusTypeDef ret = LSM6DSV16X_OK; + lsm6dsv16x_md1_cfg_t val1; + lsm6dsv16x_md2_cfg_t val2; + + lsm6dsv16x_emb_func_en_a_t emb_func_en_a; + lsm6dsv16x_emb_func_int1_t emb_func_int1; + lsm6dsv16x_emb_func_int2_t emb_func_int2; + + /* Disable emb func event on either INT1 or INT2 pin */ + if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_MD1_CFG, (uint8_t *)&val1, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + val1.int1_emb_func = PROPERTY_DISABLE; + + if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_MD1_CFG, (uint8_t *)&val1, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_MD2_CFG, (uint8_t *)&val2, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + val2.int2_emb_func = PROPERTY_DISABLE; + + if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_MD2_CFG, (uint8_t *)&val2, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + /* Enable access to embedded functions registers. */ + if (lsm6dsv16x_mem_bank_set(®_ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + /* Disable tilt detection. */ + if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_EMB_FUNC_EN_A, (uint8_t *)&emb_func_en_a, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + emb_func_en_a.tilt_en = PROPERTY_DISABLE; + + if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_EMB_FUNC_EN_A, (uint8_t *)&emb_func_en_a, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + /* Reset interrupt driven to INT1 pin. */ + if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_EMB_FUNC_INT1, (uint8_t *)&emb_func_int1, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + emb_func_int1.int1_tilt = PROPERTY_DISABLE; + + if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_EMB_FUNC_INT1, (uint8_t *)&emb_func_int1, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + /* Reset interrupt driven to INT2 pin. */ + if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_EMB_FUNC_INT2, (uint8_t *)&emb_func_int2, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + emb_func_int2.int2_tilt = PROPERTY_DISABLE; + + if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_EMB_FUNC_INT2, (uint8_t *)&emb_func_int2, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + /* Disable access to embedded functions registers. */ + if (lsm6dsv16x_mem_bank_set(®_ctx, LSM6DSV16X_MAIN_MEM_BANK) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + return ret; +} + +/** + * @brief Get the LSM6DSV16X FIFO number of samples + + * @param NumSamples number of samples + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::FIFO_Get_Num_Samples(uint16_t *NumSamples) +{ + return (LSM6DSV16XStatusTypeDef) lsm6dsv16x_fifo_data_level_get(®_ctx, NumSamples); +} + +/** + * @brief Get the LSM6DSV16X FIFO full status + + * @param Status FIFO full status + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::FIFO_Get_Full_Status(uint8_t *Status) +{ + lsm6dsv16x_fifo_status2_t val; + + if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_FIFO_STATUS2, (uint8_t *)&val, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + *Status = val.fifo_full_ia; + + return LSM6DSV16X_OK; +} + +/** + * @brief Set the LSM6DSV16X FIFO full interrupt on INT1 pin + + * @param Status FIFO full interrupt on INT1 pin status + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::FIFO_Set_INT1_FIFO_Full(uint8_t Status) +{ + lsm6dsv16x_int1_ctrl_t reg; + + if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_INT1_CTRL, (uint8_t *)®, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + reg.int1_fifo_full = Status; + + if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_INT1_CTRL, (uint8_t *)®, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + return LSM6DSV16X_OK; +} + +/** + * @brief Set the LSM6DSV16X FIFO full interrupt on INT2 pin + + * @param Status FIFO full interrupt on INT1 pin status + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::FIFO_Set_INT2_FIFO_Full(uint8_t Status) +{ + lsm6dsv16x_int2_ctrl_t reg; + + if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_INT2_CTRL, (uint8_t *)®, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + reg.int2_fifo_full = Status; + + if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_INT2_CTRL, (uint8_t *)®, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + return LSM6DSV16X_OK; +} + +/** + * @brief Set the LSM6DSV16X FIFO watermark level + + * @param Watermark FIFO watermark level + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::FIFO_Set_Watermark_Level(uint8_t Watermark) +{ + return (LSM6DSV16XStatusTypeDef) lsm6dsv16x_fifo_watermark_set(®_ctx, Watermark); +} + +/** + * @brief Set the LSM6DSV16X FIFO stop on watermark + + * @param Status FIFO stop on watermark status + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::FIFO_Set_Stop_On_Fth(uint8_t Status) +{ + return (LSM6DSV16XStatusTypeDef) lsm6dsv16x_fifo_stop_on_wtm_set(®_ctx, Status); +} + +/** + * @brief Set the LSM6DSV16X FIFO mode + + * @param Mode FIFO mode + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::FIFO_Set_Mode(uint8_t Mode) +{ + LSM6DSV16XStatusTypeDef ret = LSM6DSV16X_OK; + lsm6dsv16x_fifo_mode_t newMode = LSM6DSV16X_BYPASS_MODE; + + switch (Mode) { + case 0: + newMode = LSM6DSV16X_BYPASS_MODE; + break; + case 1: + newMode = LSM6DSV16X_FIFO_MODE; + break; + case 3: + newMode = LSM6DSV16X_STREAM_TO_FIFO_MODE; + break; + case 4: + newMode = LSM6DSV16X_BYPASS_TO_STREAM_MODE; + break; + case 6: + newMode = LSM6DSV16X_STREAM_MODE; + break; + case 7: + newMode = LSM6DSV16X_BYPASS_TO_FIFO_MODE; + break; + default: + ret = LSM6DSV16X_ERROR; + break; + } + + if (ret == LSM6DSV16X_ERROR) { + return LSM6DSV16X_ERROR; + } + + if (lsm6dsv16x_fifo_mode_set(®_ctx, newMode) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + return ret; +} + +/** + * @brief Get the LSM6DSV16X FIFO tag + + * @param Tag FIFO tag + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::FIFO_Get_Tag(uint8_t *Tag) +{ + lsm6dsv16x_fifo_data_out_tag_t tag_local; + + if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_FIFO_DATA_OUT_TAG, (uint8_t *)&tag_local, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + *Tag = (uint8_t)tag_local.tag_sensor; + + return LSM6DSV16X_OK; +} + +/** + * @brief Get the LSM6DSV16X FIFO raw data + + * @param Data FIFO raw data array [6] + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::FIFO_Get_Data(uint8_t *Data) +{ + return (LSM6DSV16XStatusTypeDef) lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_FIFO_DATA_OUT_X_L, Data, 6); +} + +/** + * @brief Get the LSM6DSV16X FIFO accelero single sample (16-bit data per 3 axes) and calculate acceleration [mg] + * @param Acceleration FIFO accelero axes [mg] + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::FIFO_Get_X_Axes(int32_t *Acceleration) +{ + lsm6dsv16x_axis3bit16_t data_raw; + float sensitivity = 0.0f; + float acceleration_float[3]; + + if (FIFO_Get_Data(data_raw.u8bit) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + if (Get_X_Sensitivity(&sensitivity) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + acceleration_float[0] = (float)data_raw.i16bit[0] * sensitivity; + acceleration_float[1] = (float)data_raw.i16bit[1] * sensitivity; + acceleration_float[2] = (float)data_raw.i16bit[2] * sensitivity; + + Acceleration[0] = (int32_t)acceleration_float[0]; + Acceleration[1] = (int32_t)acceleration_float[1]; + Acceleration[2] = (int32_t)acceleration_float[2]; + + return LSM6DSV16X_OK; + +} + +/** + * @brief Set the LSM6DSV16X FIFO accelero BDR value + + * @param Bdr FIFO accelero BDR value + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::FIFO_Set_X_BDR(float Bdr) +{ + lsm6dsv16x_fifo_xl_batch_t new_bdr; + + new_bdr = (Bdr <= 0.0f) ? LSM6DSV16X_XL_NOT_BATCHED + : (Bdr <= 1.8f) ? LSM6DSV16X_XL_BATCHED_AT_1Hz875 + : (Bdr <= 7.5f) ? LSM6DSV16X_XL_BATCHED_AT_7Hz5 + : (Bdr <= 15.0f) ? LSM6DSV16X_XL_BATCHED_AT_15Hz + : (Bdr <= 30.0f) ? LSM6DSV16X_XL_BATCHED_AT_30Hz + : (Bdr <= 60.0f) ? LSM6DSV16X_XL_BATCHED_AT_60Hz + : (Bdr <= 120.0f) ? LSM6DSV16X_XL_BATCHED_AT_120Hz + : (Bdr <= 240.0f) ? LSM6DSV16X_XL_BATCHED_AT_240Hz + : (Bdr <= 480.0f) ? LSM6DSV16X_XL_BATCHED_AT_480Hz + : (Bdr <= 960.0f) ? LSM6DSV16X_XL_BATCHED_AT_960Hz + : (Bdr <= 1920.0f) ? LSM6DSV16X_XL_BATCHED_AT_1920Hz + : (Bdr <= 3840.0f) ? LSM6DSV16X_XL_BATCHED_AT_3840Hz + : LSM6DSV16X_XL_BATCHED_AT_7680Hz; + + return (LSM6DSV16XStatusTypeDef) lsm6dsv16x_fifo_xl_batch_set(®_ctx, new_bdr); +} + +/** + * @brief Get the LSM6DSV16X FIFO gyro single sample (16-bit data per 3 axes) and calculate angular velocity [mDPS] + + * @param AngularVelocity FIFO gyro axes [mDPS] + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::FIFO_Get_G_Axes(int32_t *AngularVelocity) +{ + lsm6dsv16x_axis3bit16_t data_raw; + float sensitivity = 0.0f; + float angular_velocity_float[3]; + + if (FIFO_Get_Data(data_raw.u8bit) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + if (Get_G_Sensitivity(&sensitivity) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + angular_velocity_float[0] = (float)data_raw.i16bit[0] * sensitivity; + angular_velocity_float[1] = (float)data_raw.i16bit[1] * sensitivity; + angular_velocity_float[2] = (float)data_raw.i16bit[2] * sensitivity; + + AngularVelocity[0] = (int32_t)angular_velocity_float[0]; + AngularVelocity[1] = (int32_t)angular_velocity_float[1]; + AngularVelocity[2] = (int32_t)angular_velocity_float[2]; + + return LSM6DSV16X_OK; +} + +/** + * @brief Set the LSM6DSV16X FIFO gyro BDR value + + * @param Bdr FIFO gyro BDR value + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::FIFO_Set_G_BDR(float Bdr) +{ + lsm6dsv16x_fifo_gy_batch_t new_bdr; + + new_bdr = (Bdr <= 0.0f) ? LSM6DSV16X_GY_NOT_BATCHED + : (Bdr <= 1.8f) ? LSM6DSV16X_GY_BATCHED_AT_1Hz875 + : (Bdr <= 7.5f) ? LSM6DSV16X_GY_BATCHED_AT_7Hz5 + : (Bdr <= 15.0f) ? LSM6DSV16X_GY_BATCHED_AT_15Hz + : (Bdr <= 30.0f) ? LSM6DSV16X_GY_BATCHED_AT_30Hz + : (Bdr <= 60.0f) ? LSM6DSV16X_GY_BATCHED_AT_60Hz + : (Bdr <= 120.0f) ? LSM6DSV16X_GY_BATCHED_AT_120Hz + : (Bdr <= 240.0f) ? LSM6DSV16X_GY_BATCHED_AT_240Hz + : (Bdr <= 480.0f) ? LSM6DSV16X_GY_BATCHED_AT_480Hz + : (Bdr <= 960.0f) ? LSM6DSV16X_GY_BATCHED_AT_960Hz + : (Bdr <= 1920.0f) ? LSM6DSV16X_GY_BATCHED_AT_1920Hz + : (Bdr <= 3840.0f) ? LSM6DSV16X_GY_BATCHED_AT_3840Hz + : LSM6DSV16X_GY_BATCHED_AT_7680Hz; + + return (LSM6DSV16XStatusTypeDef) lsm6dsv16x_fifo_gy_batch_set(®_ctx, new_bdr); +} + +/** + * @brief Enable the LSM6DSV16X gyroscope sensor + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Enable_G() +{ + /* Check if the component is already enabled */ + if (gyro_is_enabled == 1U) { + return LSM6DSV16X_OK; + } + + /* Output data rate selection. */ + if (lsm6dsv16x_gy_data_rate_set(®_ctx, gyro_odr) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + gyro_is_enabled = 1U; + + return LSM6DSV16X_OK; +} + +/** + * @brief Disable the LSM6DSV16X gyroscope sensor + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Disable_G() +{ + /* Check if the component is already disabled */ + if (gyro_is_enabled == 0U) { + return LSM6DSV16X_OK; + } + + /* Get current output data rate. */ + if (lsm6dsv16x_gy_data_rate_get(®_ctx, &gyro_odr) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + /* Output data rate selection - power down. */ + if (lsm6dsv16x_gy_data_rate_set(®_ctx, LSM6DSV16X_GY_ODR_OFF) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + gyro_is_enabled = 0U; + + return LSM6DSV16X_OK; +} + +/** + * @brief Get the LSM6DSV16X gyroscope sensor sensitivity + * @param Sensitivity pointer + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Get_G_Sensitivity(float *Sensitivity) +{ + LSM6DSV16XStatusTypeDef ret = LSM6DSV16X_OK; + lsm6dsv16x_gy_full_scale_t full_scale; + + /* Read actual full scale selection from sensor. */ + if (lsm6dsv16x_gy_full_scale_get(®_ctx, &full_scale) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + /* Store the sensitivity based on actual full scale. */ + switch (full_scale) { + case LSM6DSV16X_125dps: + *Sensitivity = LSM6DSV16X_GYRO_SENSITIVITY_FS_125DPS; + break; + + case LSM6DSV16X_250dps: + *Sensitivity = LSM6DSV16X_GYRO_SENSITIVITY_FS_250DPS; + break; + + case LSM6DSV16X_500dps: + *Sensitivity = LSM6DSV16X_GYRO_SENSITIVITY_FS_500DPS; + break; + + case LSM6DSV16X_1000dps: + *Sensitivity = LSM6DSV16X_GYRO_SENSITIVITY_FS_1000DPS; + break; + + case LSM6DSV16X_2000dps: + *Sensitivity = LSM6DSV16X_GYRO_SENSITIVITY_FS_2000DPS; + break; + + case LSM6DSV16X_4000dps: + *Sensitivity = LSM6DSV16X_GYRO_SENSITIVITY_FS_4000DPS; + break; + + default: + ret = LSM6DSV16X_ERROR; + break; + } + + return ret; +} + +/** + * @brief Get the LSM6DSV16X gyroscope sensor output data rate + * @param Odr pointer where the output data rate is written + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Get_G_ODR(float *Odr) +{ + LSM6DSV16XStatusTypeDef ret = LSM6DSV16X_OK; + lsm6dsv16x_gy_data_rate_t odr_low_level; + + /* Get current output data rate. */ + if (lsm6dsv16x_gy_data_rate_get(®_ctx, &odr_low_level) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + switch (odr_low_level) { + case LSM6DSV16X_GY_ODR_OFF: + *Odr = 0.0f; + break; + + case LSM6DSV16X_GY_ODR_AT_7Hz5: + *Odr = 7.5f; + break; + + case LSM6DSV16X_GY_ODR_AT_15Hz: + *Odr = 15.0f; + break; + + case LSM6DSV16X_GY_ODR_AT_30Hz: + *Odr = 30.0f; + break; + + case LSM6DSV16X_GY_ODR_AT_60Hz: + *Odr = 60.0f; + break; + + case LSM6DSV16X_GY_ODR_AT_120Hz: + *Odr = 120.0f; + break; + + case LSM6DSV16X_GY_ODR_AT_240Hz: + *Odr = 240.0f; + break; + + case LSM6DSV16X_GY_ODR_AT_480Hz: + *Odr = 480.0f; + break; + + case LSM6DSV16X_GY_ODR_AT_960Hz: + *Odr = 960.0f; + break; + + case LSM6DSV16X_GY_ODR_AT_1920Hz: + *Odr = 1920.0f; + break; + + case LSM6DSV16X_GY_ODR_AT_3840Hz: + *Odr = 3840.0f; + break; + + case LSM6DSV16X_GY_ODR_AT_7680Hz: + *Odr = 7680.0f; + break; + + default: + ret = LSM6DSV16X_ERROR; + break; + } + + return ret; +} + +/** + * @brief Set the LSM6DSV16X gyroscope sensor output data rate with operating mode + * @param Odr the output data rate value to be set + * @param Mode the gyroscope operating mode + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Set_G_ODR(float Odr, LSM6DSV16X_GYRO_Operating_Mode_t Mode) +{ + switch (Mode) { + case LSM6DSV16X_GYRO_HIGH_PERFORMANCE_MODE: { + if (lsm6dsv16x_gy_mode_set(®_ctx, LSM6DSV16X_GY_HIGH_PERFORMANCE_MD) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + /* Valid ODR: 7.5Hz <= Odr <= 7.68kHz */ + Odr = (Odr < 7.5f) ? 7.5f + : (Odr > 7680.0f) ? 7680.0f + : Odr; + break; + } + + case LSM6DSV16X_GYRO_HIGH_ACCURACY_MODE: + // TODO: Not implemented. + // NOTE: According to datasheet, section `6.5 High-accuracy ODR mode`: + // "... the other sensor also has to be configured in high-accuracy ODR (HAODR) mode." + return LSM6DSV16X_ERROR; + + case LSM6DSV16X_GYRO_SLEEP_MODE: + // TODO: Not implemented. + // NOTE: Unknown ODR validity for this mode + return LSM6DSV16X_ERROR; + + case LSM6DSV16X_GYRO_LOW_POWER_MODE: { + if (lsm6dsv16x_gy_mode_set(®_ctx, LSM6DSV16X_GY_LOW_POWER_MD) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + /* Valid ODR: 7.5Hz <= Odr <= 240kHz */ + Odr = (Odr < 7.5f) ? 7.5f + : (Odr > 240.0f) ? 240.0f + : Odr; + break; + } + + default: + return LSM6DSV16X_ERROR; + } + + if (gyro_is_enabled == 1U) { + return Set_G_ODR_When_Enabled(Odr); + } else { + return Set_G_ODR_When_Disabled(Odr); + } +} + +/** + * @brief Set the LSM6DSV16X gyroscope sensor output data rate when enabled + * @param Odr the functional output data rate to be set + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Set_G_ODR_When_Enabled(float Odr) +{ + lsm6dsv16x_gy_data_rate_t new_odr; + + new_odr = (Odr <= 7.5f) ? LSM6DSV16X_GY_ODR_AT_7Hz5 + : (Odr <= 15.0f) ? LSM6DSV16X_GY_ODR_AT_15Hz + : (Odr <= 30.0f) ? LSM6DSV16X_GY_ODR_AT_30Hz + : (Odr <= 60.0f) ? LSM6DSV16X_GY_ODR_AT_60Hz + : (Odr <= 120.0f) ? LSM6DSV16X_GY_ODR_AT_120Hz + : (Odr <= 240.0f) ? LSM6DSV16X_GY_ODR_AT_240Hz + : (Odr <= 480.0f) ? LSM6DSV16X_GY_ODR_AT_480Hz + : (Odr <= 960.0f) ? LSM6DSV16X_GY_ODR_AT_960Hz + : (Odr <= 1920.0f) ? LSM6DSV16X_GY_ODR_AT_1920Hz + : (Odr <= 3840.0f) ? LSM6DSV16X_GY_ODR_AT_3840Hz + : LSM6DSV16X_GY_ODR_AT_7680Hz; + + /* Output data rate selection. */ + if (lsm6dsv16x_gy_data_rate_set(®_ctx, new_odr) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + return LSM6DSV16X_OK; +} + +/** + * @brief Set the LSM6DSV16X gyroscope sensor output data rate when disabled + * @param Odr the functional output data rate to be set + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Set_G_ODR_When_Disabled(float Odr) +{ + gyro_odr = (Odr <= 7.5f) ? LSM6DSV16X_GY_ODR_AT_7Hz5 + : (Odr <= 15.0f) ? LSM6DSV16X_GY_ODR_AT_15Hz + : (Odr <= 30.0f) ? LSM6DSV16X_GY_ODR_AT_30Hz + : (Odr <= 60.0f) ? LSM6DSV16X_GY_ODR_AT_60Hz + : (Odr <= 120.0f) ? LSM6DSV16X_GY_ODR_AT_120Hz + : (Odr <= 240.0f) ? LSM6DSV16X_GY_ODR_AT_240Hz + : (Odr <= 480.0f) ? LSM6DSV16X_GY_ODR_AT_480Hz + : (Odr <= 960.0f) ? LSM6DSV16X_GY_ODR_AT_960Hz + : (Odr <= 1920.0f) ? LSM6DSV16X_GY_ODR_AT_1920Hz + : (Odr <= 3840.0f) ? LSM6DSV16X_GY_ODR_AT_3840Hz + : LSM6DSV16X_GY_ODR_AT_7680Hz; + + return LSM6DSV16X_OK; +} + +/** + * @brief Get the LSM6DSV16X gyroscope sensor full scale + * @param FullScale pointer where the full scale is written + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Get_G_FS(int32_t *FullScale) +{ + LSM6DSV16XStatusTypeDef ret = LSM6DSV16X_OK; + lsm6dsv16x_gy_full_scale_t fs_low_level; + + /* Read actual full scale selection from sensor. */ + if (lsm6dsv16x_gy_full_scale_get(®_ctx, &fs_low_level) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + switch (fs_low_level) { + case LSM6DSV16X_125dps: + *FullScale = 125; + break; + + case LSM6DSV16X_250dps: + *FullScale = 250; + break; + + case LSM6DSV16X_500dps: + *FullScale = 500; + break; + + case LSM6DSV16X_1000dps: + *FullScale = 1000; + break; + + case LSM6DSV16X_2000dps: + *FullScale = 2000; + break; + + case LSM6DSV16X_4000dps: + *FullScale = 4000; + break; + + default: + ret = LSM6DSV16X_ERROR; + break; + } + + return ret; +} + +/** + * @brief Set the LSM6DSV16X gyroscope sensor full scale + * @param FullScale the functional full scale to be set + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Set_G_FS(int32_t FullScale) +{ + lsm6dsv16x_gy_full_scale_t new_fs; + + new_fs = (FullScale <= 125) ? LSM6DSV16X_125dps + : (FullScale <= 250) ? LSM6DSV16X_250dps + : (FullScale <= 500) ? LSM6DSV16X_500dps + : (FullScale <= 1000) ? LSM6DSV16X_1000dps + : (FullScale <= 2000) ? LSM6DSV16X_2000dps + : LSM6DSV16X_4000dps; + + if (lsm6dsv16x_gy_full_scale_set(®_ctx, new_fs) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + return LSM6DSV16X_OK; +} + +/** + * @brief Get the LSM6DSV16X gyroscope sensor raw axes + * @param Value pointer where the raw values of the axes are written + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Get_G_AxesRaw(int16_t *Value) +{ + lsm6dsv16x_axis3bit16_t data_raw; + + /* Read raw data values. */ + if (lsm6dsv16x_angular_rate_raw_get(®_ctx, data_raw.i16bit) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + /* Format the data. */ + Value[0] = data_raw.i16bit[0]; + Value[1] = data_raw.i16bit[1]; + Value[2] = data_raw.i16bit[2]; + + return LSM6DSV16X_OK; +} + +/** + * @brief Get the LSM6DSV16X gyroscope sensor axes + * @param AngularRate pointer where the values of the axes are written + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Get_G_Axes(int32_t *AngularRate) +{ + lsm6dsv16x_axis3bit16_t data_raw; + float sensitivity; + + /* Read raw data values. */ + if (lsm6dsv16x_angular_rate_raw_get(®_ctx, data_raw.i16bit) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + /* Get LSM6DSV16X actual sensitivity. */ + if (Get_G_Sensitivity(&sensitivity) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + /* Calculate the data. */ + AngularRate[0] = (int32_t)((float)((float)data_raw.i16bit[0] * sensitivity)); + AngularRate[1] = (int32_t)((float)((float)data_raw.i16bit[1] * sensitivity)); + AngularRate[2] = (int32_t)((float)((float)data_raw.i16bit[2] * sensitivity)); + + return LSM6DSV16X_OK; +} + +/** + * @brief Get the LSM6DSV16X GYRO data ready bit value + * @param Status the status of data ready bit + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Get_G_DRDY_Status(uint8_t *Status) +{ + lsm6dsv16x_all_sources_t val; + + if (lsm6dsv16x_all_sources_get(®_ctx, &val) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + *Status = val.drdy_gy; + return LSM6DSV16X_OK; +} + +/** + * @brief Set the LSM6DSV16X gyroscope power mode + * @param PowerMode Value of the powerMode + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Set_G_Power_Mode(uint8_t PowerMode) +{ + if (lsm6dsv16x_gy_mode_set(®_ctx, (lsm6dsv16x_gy_mode_t)PowerMode) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + return LSM6DSV16X_OK; +} + +/** + * @brief Set the LSM6DSV16X gyroscope filter mode + * @param LowHighPassFlag 0/1 for setting low-pass/high-pass filter mode + * @param FilterMode Value of the filter Mode + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Set_G_Filter_Mode(uint8_t LowHighPassFlag, uint8_t FilterMode) +{ + if (LowHighPassFlag == 0) { + /*Set gyroscope low_pass 1 filter-mode*/ + /* Enable low-pass filter */ + if (lsm6dsv16x_filt_gy_lp1_set(®_ctx, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + if (lsm6dsv16x_filt_gy_lp1_bandwidth_set(®_ctx, (lsm6dsv16x_filt_gy_lp1_bandwidth_t)FilterMode) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + } else { + /*Set gyroscope high_pass filter-mode*/ + /* Enable high-pass filter */ + if (lsm6dsv16x_filt_gy_lp1_set(®_ctx, 0) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + if (lsm6dsv16x_filt_gy_lp1_bandwidth_set(®_ctx, (lsm6dsv16x_filt_gy_lp1_bandwidth_t)FilterMode) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + } + return LSM6DSV16X_OK; +} + + +/** + * @brief Enable the LSM6DSV16X QVAR sensor + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::QVAR_Enable() +{ + lsm6dsv16x_ctrl7_t ctrl7; + + if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_CTRL7, (uint8_t *)&ctrl7, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + ctrl7.ah_qvar_en = 1; + ctrl7.int2_drdy_ah_qvar = 1; + + if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_CTRL7, (uint8_t *)&ctrl7, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + return LSM6DSV16X_OK; +} + + +/** + * @brief Read LSM6DSV16X QVAR output data + * @param Data pointer where the value is written + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::QVAR_GetData(float *Data) +{ + lsm6dsv16x_axis1bit16_t data_raw; + (void)memset(data_raw.u8bit, 0x00, sizeof(int16_t)); + + if (lsm6dsv16x_ah_qvar_raw_get(®_ctx, &data_raw.i16bit) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + *Data = ((float)data_raw.i16bit) / LSM6DSV16X_QVAR_GAIN; + return LSM6DSV16X_OK; +} + + +/** + * @brief Set LSM6DSV16X QVAR equivalent input impedance + * @param val impedance in MOhm (2400MOhm, 730MOhm, 300MOhm, 255MOhm) + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::QVAR_SetImpedance(uint16_t val) +{ + LSM6DSV16XStatusTypeDef ret = LSM6DSV16X_OK; + lsm6dsv16x_ah_qvar_zin_t imp; + switch (val) { + case 2400: + imp = LSM6DSV16X_2400MOhm; + break; + case 730: + imp = LSM6DSV16X_730MOhm; + break; + case 300: + imp = LSM6DSV16X_300MOhm; + break; + case 255: + imp = LSM6DSV16X_255MOhm; + break; + default: + ret = LSM6DSV16X_ERROR; + break; + } + if (ret != LSM6DSV16X_ERROR) { + if (lsm6dsv16x_ah_qvar_zin_set(®_ctx, imp) != LSM6DSV16X_OK) { + ret = LSM6DSV16X_ERROR; + } + } + return ret; +} + +/** + * @brief Read LSM6DSV16X QVAR status + * @param val pointer where the value is written + * @retval 0 in case of success, an error code otherwise + */ + +LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::QVAR_GetStatus(uint8_t *val) +{ + lsm6dsv16x_status_reg_t status; + + if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_STATUS_REG, (uint8_t *)&status, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + *val = status.ah_qvarda; + + return LSM6DSV16X_OK; +} + +/** + * @brief Get MLC status + * @param status pointer where the MLC status is written + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Get_MLC_Status(lsm6dsv16x_mlc_status_mainpage_t *status) +{ + if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_MLC_STATUS_MAINPAGE, (uint8_t *)status, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + return LSM6DSV16X_OK; +} + +/** + * @brief Get MLC output + * @param output pointer where the MLC output is written + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Get_MLC_Output(lsm6dsv16x_mlc_out_t *output) +{ + if (lsm6dsv16x_mlc_out_get(®_ctx, output) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + return LSM6DSV16X_OK; +} + +/** + * @brief Get the LSM6DSV16X register value + * @param Reg address to be read + * @param Data pointer where the value is written + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Read_Reg(uint8_t Reg, uint8_t *Data) +{ + if (lsm6dsv16x_read_reg(®_ctx, Reg, Data, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + return LSM6DSV16X_OK; +} + +/** + * @brief Set the LSM6DSV16X register value + * @param Reg address to be written + * @param Data value to be written + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Write_Reg(uint8_t Reg, uint8_t Data) +{ + if (lsm6dsv16x_write_reg(®_ctx, Reg, &Data, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + return LSM6DSV16X_OK; +} + +int32_t LSM6DSV16X_io_write(void *handle, uint8_t WriteAddr, uint8_t *pBuffer, uint16_t nBytesToWrite) +{ + return ((LSM6DSV16XSensor *)handle)->IO_Write(pBuffer, WriteAddr, nBytesToWrite); +} + +int32_t LSM6DSV16X_io_read(void *handle, uint8_t ReadAddr, uint8_t *pBuffer, uint16_t nBytesToRead) +{ + return ((LSM6DSV16XSensor *)handle)->IO_Read(pBuffer, ReadAddr, nBytesToRead); +} diff --git a/lib/LSM6DSV16X/LSM6DSV16XSensor.h b/lib/LSM6DSV16X/LSM6DSV16XSensor.h new file mode 100644 index 000000000..b48c8cd04 --- /dev/null +++ b/lib/LSM6DSV16X/LSM6DSV16XSensor.h @@ -0,0 +1,344 @@ +/** + ****************************************************************************** + * @file LSM6DSV16XSensor.h + * @author SRA + * @version V1.5.1 + * @date July 2022 + * @brief Abstract Class of a LSM6DSV16X inertial measurement sensor. + ****************************************************************************** + * @attention + * + *

© COPYRIGHT(c) 2022 STMicroelectronics

+ * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * 3. Neither the name of STMicroelectronics nor the names of its contributors + * may be used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************** + */ + + +/* Prevent recursive inclusion -----------------------------------------------*/ + +#ifndef __LSM6DSV16XSensor_H__ +#define __LSM6DSV16XSensor_H__ + + +/* Includes ------------------------------------------------------------------*/ + +#include "Wire.h" +#include "SPI.h" +#include "lsm6dsv16x_reg.h" + + +/* Defines -------------------------------------------------------------------*/ + +#define LSM6DSV16X_ACC_SENSITIVITY_FS_2G 0.061f +#define LSM6DSV16X_ACC_SENSITIVITY_FS_4G 0.122f +#define LSM6DSV16X_ACC_SENSITIVITY_FS_8G 0.244f +#define LSM6DSV16X_ACC_SENSITIVITY_FS_16G 0.488f + +#define LSM6DSV16X_GYRO_SENSITIVITY_FS_125DPS 4.375f +#define LSM6DSV16X_GYRO_SENSITIVITY_FS_250DPS 8.750f +#define LSM6DSV16X_GYRO_SENSITIVITY_FS_500DPS 17.500f +#define LSM6DSV16X_GYRO_SENSITIVITY_FS_1000DPS 35.000f +#define LSM6DSV16X_GYRO_SENSITIVITY_FS_2000DPS 70.000f +#define LSM6DSV16X_GYRO_SENSITIVITY_FS_4000DPS 140.000f + +#define LSM6DSV16X_QVAR_GAIN 78.000f + +/* Typedefs ------------------------------------------------------------------*/ + +typedef enum { + LSM6DSV16X_OK = 0, + LSM6DSV16X_ERROR = -1 +} LSM6DSV16XStatusTypeDef; + +typedef enum { + LSM6DSV16X_INT1_PIN, + LSM6DSV16X_INT2_PIN, +} LSM6DSV16X_SensorIntPin_t; + +typedef struct { + unsigned int FreeFallStatus : 1; + unsigned int TapStatus : 1; + unsigned int DoubleTapStatus : 1; + unsigned int WakeUpStatus : 1; + unsigned int StepStatus : 1; + unsigned int TiltStatus : 1; + unsigned int D6DOrientationStatus : 1; + unsigned int SleepStatus : 1; +} LSM6DSV16X_Event_Status_t; + +typedef union { + int16_t i16bit[3]; + uint8_t u8bit[6]; +} lsm6dsv16x_axis3bit16_t; + +typedef union { + int16_t i16bit; + uint8_t u8bit[2]; +} lsm6dsv16x_axis1bit16_t; + +typedef enum { + LSM6DSV16X_ACC_HIGH_PERFORMANCE_MODE, + LSM6DSV16X_ACC_HIGH_ACCURACY_MODE, + LSM6DSV16X_ACC_NORMAL_MODE, + LSM6DSV16X_ACC_LOW_POWER_MODE1, + LSM6DSV16X_ACC_LOW_POWER_MODE2, + LSM6DSV16X_ACC_LOW_POWER_MODE3 +} LSM6DSV16X_ACC_Operating_Mode_t; + +typedef enum { + LSM6DSV16X_GYRO_HIGH_PERFORMANCE_MODE, + LSM6DSV16X_GYRO_HIGH_ACCURACY_MODE, + LSM6DSV16X_GYRO_SLEEP_MODE, + LSM6DSV16X_GYRO_LOW_POWER_MODE +} LSM6DSV16X_GYRO_Operating_Mode_t; + + +/* Class Declaration ---------------------------------------------------------*/ + +/** + * Abstract class of a LSM6DSV16X sensor. + */ +class LSM6DSV16XSensor { + public: + LSM6DSV16XSensor(TwoWire *i2c, uint8_t address = LSM6DSV16X_I2C_ADD_H); + LSM6DSV16XSensor(SPIClass *spi, int cs_pin, uint32_t spi_speed = 2000000); + + LSM6DSV16XStatusTypeDef begin(); + LSM6DSV16XStatusTypeDef end(); + LSM6DSV16XStatusTypeDef ReadID(uint8_t *Id); + + LSM6DSV16XStatusTypeDef Enable_X(); + LSM6DSV16XStatusTypeDef Disable_X(); + LSM6DSV16XStatusTypeDef Get_X_Sensitivity(float *Sensitivity); + LSM6DSV16XStatusTypeDef Get_X_ODR(float *Odr); + LSM6DSV16XStatusTypeDef Set_X_ODR(float Odr, LSM6DSV16X_ACC_Operating_Mode_t Mode = LSM6DSV16X_ACC_HIGH_PERFORMANCE_MODE); + LSM6DSV16XStatusTypeDef Get_X_FS(int32_t *FullScale); + LSM6DSV16XStatusTypeDef Set_X_FS(int32_t FullScale); + LSM6DSV16XStatusTypeDef Get_X_AxesRaw(int16_t *Value); + LSM6DSV16XStatusTypeDef Get_X_Axes(int32_t *Acceleration); + LSM6DSV16XStatusTypeDef Get_X_DRDY_Status(uint8_t *Status); + LSM6DSV16XStatusTypeDef Get_X_Event_Status(LSM6DSV16X_Event_Status_t *Status); + LSM6DSV16XStatusTypeDef Set_X_Power_Mode(uint8_t PowerMode); + LSM6DSV16XStatusTypeDef Set_X_Filter_Mode(uint8_t LowHighPassFlag, uint8_t FilterMode); + + LSM6DSV16XStatusTypeDef Enable_G(); + LSM6DSV16XStatusTypeDef Disable_G(); + LSM6DSV16XStatusTypeDef Get_G_Sensitivity(float *Sensitivity); + LSM6DSV16XStatusTypeDef Get_G_ODR(float *Odr); + LSM6DSV16XStatusTypeDef Set_G_ODR(float Odr, LSM6DSV16X_GYRO_Operating_Mode_t Mode = LSM6DSV16X_GYRO_HIGH_PERFORMANCE_MODE); + LSM6DSV16XStatusTypeDef Get_G_FS(int32_t *FullScale); + LSM6DSV16XStatusTypeDef Set_G_FS(int32_t FullScale); + LSM6DSV16XStatusTypeDef Get_G_AxesRaw(int16_t *Value); + LSM6DSV16XStatusTypeDef Get_G_Axes(int32_t *AngularRate); + LSM6DSV16XStatusTypeDef Get_G_DRDY_Status(uint8_t *Status); + LSM6DSV16XStatusTypeDef Set_G_Power_Mode(uint8_t PowerMode); + LSM6DSV16XStatusTypeDef Set_G_Filter_Mode(uint8_t LowHighPassFlag, uint8_t FilterMode); + + LSM6DSV16XStatusTypeDef Enable_6D_Orientation(LSM6DSV16X_SensorIntPin_t IntPin); + LSM6DSV16XStatusTypeDef Disable_6D_Orientation(); + LSM6DSV16XStatusTypeDef Set_6D_Orientation_Threshold(uint8_t Threshold); + LSM6DSV16XStatusTypeDef Get_6D_Orientation_XL(uint8_t *XLow); + LSM6DSV16XStatusTypeDef Get_6D_Orientation_XH(uint8_t *XHigh); + LSM6DSV16XStatusTypeDef Get_6D_Orientation_YL(uint8_t *YLow); + LSM6DSV16XStatusTypeDef Get_6D_Orientation_YH(uint8_t *YHigh); + LSM6DSV16XStatusTypeDef Get_6D_Orientation_ZL(uint8_t *ZLow); + LSM6DSV16XStatusTypeDef Get_6D_Orientation_ZH(uint8_t *ZHigh); + + LSM6DSV16XStatusTypeDef Enable_Free_Fall_Detection(LSM6DSV16X_SensorIntPin_t IntPin); + LSM6DSV16XStatusTypeDef Disable_Free_Fall_Detection(); + LSM6DSV16XStatusTypeDef Set_Free_Fall_Threshold(uint8_t Threshold); + LSM6DSV16XStatusTypeDef Set_Free_Fall_Duration(uint8_t Duration); + + LSM6DSV16XStatusTypeDef Enable_Wake_Up_Detection(LSM6DSV16X_SensorIntPin_t IntPin); + LSM6DSV16XStatusTypeDef Disable_Wake_Up_Detection(); + LSM6DSV16XStatusTypeDef Set_Wake_Up_Threshold(uint32_t Threshold); + LSM6DSV16XStatusTypeDef Set_Wake_Up_Duration(uint8_t Duration); + + LSM6DSV16XStatusTypeDef Enable_Single_Tap_Detection(LSM6DSV16X_SensorIntPin_t IntPin); + LSM6DSV16XStatusTypeDef Disable_Single_Tap_Detection(); + LSM6DSV16XStatusTypeDef Enable_Double_Tap_Detection(LSM6DSV16X_SensorIntPin_t IntPin); + LSM6DSV16XStatusTypeDef Disable_Double_Tap_Detection(); + LSM6DSV16XStatusTypeDef Set_Tap_Threshold(uint8_t Threshold); + LSM6DSV16XStatusTypeDef Set_Tap_Shock_Time(uint8_t Time); + LSM6DSV16XStatusTypeDef Set_Tap_Quiet_Time(uint8_t Time); + LSM6DSV16XStatusTypeDef Set_Tap_Duration_Time(uint8_t Time); + + LSM6DSV16XStatusTypeDef Enable_Pedometer(LSM6DSV16X_SensorIntPin_t IntPin); + LSM6DSV16XStatusTypeDef Disable_Pedometer(); + LSM6DSV16XStatusTypeDef Get_Step_Count(uint16_t *StepCount); + LSM6DSV16XStatusTypeDef Step_Counter_Reset(); + + LSM6DSV16XStatusTypeDef Enable_Tilt_Detection(LSM6DSV16X_SensorIntPin_t IntPin); + LSM6DSV16XStatusTypeDef Disable_Tilt_Detection(); + + LSM6DSV16XStatusTypeDef FIFO_Get_Num_Samples(uint16_t *NumSamples); + LSM6DSV16XStatusTypeDef FIFO_Get_Full_Status(uint8_t *Status); + LSM6DSV16XStatusTypeDef FIFO_Set_INT1_FIFO_Full(uint8_t Status); + LSM6DSV16XStatusTypeDef FIFO_Set_INT2_FIFO_Full(uint8_t Status); + LSM6DSV16XStatusTypeDef FIFO_Set_Watermark_Level(uint8_t Watermark); + LSM6DSV16XStatusTypeDef FIFO_Set_Stop_On_Fth(uint8_t Status); + LSM6DSV16XStatusTypeDef FIFO_Set_Mode(uint8_t Mode); + LSM6DSV16XStatusTypeDef FIFO_Get_Tag(uint8_t *Tag); + LSM6DSV16XStatusTypeDef FIFO_Get_Data(uint8_t *Data); + LSM6DSV16XStatusTypeDef FIFO_Get_X_Axes(int32_t *Acceleration); + LSM6DSV16XStatusTypeDef FIFO_Set_X_BDR(float Bdr); + LSM6DSV16XStatusTypeDef FIFO_Get_G_Axes(int32_t *AngularVelocity); + LSM6DSV16XStatusTypeDef FIFO_Set_G_BDR(float Bdr); + + LSM6DSV16XStatusTypeDef QVAR_Enable(); + LSM6DSV16XStatusTypeDef QVAR_GetStatus(uint8_t *val); + LSM6DSV16XStatusTypeDef QVAR_SetImpedance(uint16_t val); + LSM6DSV16XStatusTypeDef QVAR_GetData(float *Data); + + LSM6DSV16XStatusTypeDef Get_MLC_Status(lsm6dsv16x_mlc_status_mainpage_t *status); + LSM6DSV16XStatusTypeDef Get_MLC_Output(lsm6dsv16x_mlc_out_t *output); + + LSM6DSV16XStatusTypeDef Read_Reg(uint8_t Reg, uint8_t *Data); + LSM6DSV16XStatusTypeDef Write_Reg(uint8_t Reg, uint8_t Data); + + /** + * @brief Utility function to read data. + * @param pBuffer: pointer to data to be read. + * @param RegisterAddr: specifies internal address register to be read. + * @param NumByteToRead: number of bytes to be read. + * @retval 0 if ok, an error code otherwise. + */ + uint8_t IO_Read(uint8_t *pBuffer, uint8_t RegisterAddr, uint16_t NumByteToRead) + { + if (dev_spi) { + dev_spi->beginTransaction(SPISettings(spi_speed, MSBFIRST, SPI_MODE3)); + + digitalWrite(cs_pin, LOW); + + /* Write Reg Address */ + dev_spi->transfer(RegisterAddr | 0x80); + /* Read the data */ + for (uint16_t i = 0; i < NumByteToRead; i++) { + *(pBuffer + i) = dev_spi->transfer(0x00); + } + + digitalWrite(cs_pin, HIGH); + + dev_spi->endTransaction(); + + return 0; + } + + if (dev_i2c) { + dev_i2c->beginTransmission(((uint8_t)(((address) >> 1) & 0x7F))); + dev_i2c->write(RegisterAddr); + dev_i2c->endTransmission(false); + + dev_i2c->requestFrom(((uint8_t)(((address) >> 1) & 0x7F)), (uint8_t) NumByteToRead); + + int i = 0; + while (dev_i2c->available()) { + pBuffer[i] = dev_i2c->read(); + i++; + } + + return 0; + } + + return 1; + } + + /** + * @brief Utility function to write data. + * @param pBuffer: pointer to data to be written. + * @param RegisterAddr: specifies internal address register to be written. + * @param NumByteToWrite: number of bytes to write. + * @retval 0 if ok, an error code otherwise. + */ + uint8_t IO_Write(uint8_t *pBuffer, uint8_t RegisterAddr, uint16_t NumByteToWrite) + { + if (dev_spi) { + dev_spi->beginTransaction(SPISettings(spi_speed, MSBFIRST, SPI_MODE3)); + + digitalWrite(cs_pin, LOW); + + /* Write Reg Address */ + dev_spi->transfer(RegisterAddr); + /* Write the data */ + for (uint16_t i = 0; i < NumByteToWrite; i++) { + dev_spi->transfer(pBuffer[i]); + } + + digitalWrite(cs_pin, HIGH); + + dev_spi->endTransaction(); + + return 0; + } + + if (dev_i2c) { + dev_i2c->beginTransmission(((uint8_t)(((address) >> 1) & 0x7F))); + + dev_i2c->write(RegisterAddr); + for (uint16_t i = 0 ; i < NumByteToWrite ; i++) { + dev_i2c->write(pBuffer[i]); + } + + dev_i2c->endTransmission(true); + + return 0; + } + + return 1; + } + + private: + LSM6DSV16XStatusTypeDef Set_X_ODR_When_Enabled(float Odr); + LSM6DSV16XStatusTypeDef Set_X_ODR_When_Disabled(float Odr); + LSM6DSV16XStatusTypeDef Set_G_ODR_When_Enabled(float Odr); + LSM6DSV16XStatusTypeDef Set_G_ODR_When_Disabled(float Odr); + + /* Helper classes. */ + TwoWire *dev_i2c; + SPIClass *dev_spi; + + /* Configuration */ + uint8_t address; + int cs_pin; + uint32_t spi_speed; + + lsm6dsv16x_xl_data_rate_t acc_odr; + lsm6dsv16x_gy_data_rate_t gyro_odr; + uint8_t acc_is_enabled; + uint8_t gyro_is_enabled; + uint8_t initialized; + lsm6dsv16x_ctx_t reg_ctx; +}; + +#ifdef __cplusplus +extern "C" { +#endif +int32_t LSM6DSV16X_io_write(void *handle, uint8_t WriteAddr, uint8_t *pBuffer, uint16_t nBytesToWrite); +int32_t LSM6DSV16X_io_read(void *handle, uint8_t ReadAddr, uint8_t *pBuffer, uint16_t nBytesToRead); +#ifdef __cplusplus +} +#endif + +#endif /* __LSM6DSV16XSensor_H__ */ diff --git a/lib/LSM6DSV16X/lsm6dsv16x_reg.c b/lib/LSM6DSV16X/lsm6dsv16x_reg.c new file mode 100644 index 000000000..d77236c48 --- /dev/null +++ b/lib/LSM6DSV16X/lsm6dsv16x_reg.c @@ -0,0 +1,8902 @@ +/** + ****************************************************************************** + * @file lsm6dsv16x_reg.c + * @author Sensors Software Solution Team + * @brief LSM6DSV16X driver file + ****************************************************************************** + * @attention + * + *

© Copyright (c) 2021 STMicroelectronics. + * All rights reserved.

+ * + * This software component is licensed by ST under BSD 3-Clause license, + * the "License"; You may not use this file except in compliance with the + * License. You may obtain a copy of the License at: + * opensource.org/licenses/BSD-3-Clause + * + ****************************************************************************** + */ + +#include "lsm6dsv16x_reg.h" + +/** + * @defgroup LSM6DSV16X + * @brief This file provides a set of functions needed to drive the + * lsm6dsv16x enhanced inertial module. + * @{ + * + */ + +/** + * @defgroup Interfaces functions + * @brief This section provide a set of functions used to read and + * write a generic register of the device. + * MANDATORY: return 0 -> no Error. + * @{ + * + */ + +/** + * @brief Read generic device register + * + * @param ctx communication interface handler.(ptr) + * @param reg first register address to read. + * @param data buffer for data read.(ptr) + * @param len number of consecutive register to read. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_read_reg(lsm6dsv16x_ctx_t *ctx, uint8_t reg, + uint8_t *data, + uint16_t len) +{ + int32_t ret; + + ret = ctx->read_reg(ctx->handle, reg, data, len); + + return ret; +} + +/** + * @brief Write generic device register + * + * @param ctx communication interface handler.(ptr) + * @param reg first register address to write. + * @param data the buffer contains data to be written.(ptr) + * @param len number of consecutive register to write. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_write_reg(lsm6dsv16x_ctx_t *ctx, uint8_t reg, + uint8_t *data, + uint16_t len) +{ + int32_t ret; + + ret = ctx->write_reg(ctx->handle, reg, data, len); + + return ret; +} + +/** + * @} + * + */ + +/** + * @defgroup Private functions + * @brief Section collect all the utility functions needed by APIs. + * @{ + * + */ + +static void bytecpy(uint8_t *target, uint8_t *source) +{ + if ((target != NULL) && (source != NULL)) { + *target = *source; + } +} + +/** + * @} + * + */ + +/** + * @defgroup Sensitivity + * @brief These functions convert raw-data into engineering units. + * @{ + * + */ +float lsm6dsv16x_from_fs2_to_mg(int16_t lsb) +{ + return ((float)lsb) * 0.061f; +} + +float lsm6dsv16x_from_fs4_to_mg(int16_t lsb) +{ + return ((float)lsb) * 0.122f; +} + +float lsm6dsv16x_from_fs8_to_mg(int16_t lsb) +{ + return ((float)lsb) * 0.244f; +} + +float lsm6dsv16x_from_fs16_to_mg(int16_t lsb) +{ + return ((float)lsb) * 0.488f; +} + +float lsm6dsv16x_from_fs125_to_mdps(int16_t lsb) +{ + return ((float)lsb) * 4.375f; +} + +float lsm6dsv16x_from_fs250_to_mdps(int16_t lsb) +{ + return ((float)lsb) * 8.750f; +} + +float lsm6dsv16x_from_fs500_to_mdps(int16_t lsb) +{ + return ((float)lsb) * 17.50f; +} + +float lsm6dsv16x_from_fs1000_to_mdps(int16_t lsb) +{ + return ((float)lsb) * 35.0f; +} + +float lsm6dsv16x_from_fs2000_to_mdps(int16_t lsb) +{ + return ((float)lsb) * 70.0f; +} + +float lsm6dsv16x_from_fs4000_to_mdps(int16_t lsb) +{ + return ((float)lsb) * 140.0f; +} + +float lsm6dsv16x_from_lsb_to_celsius(int16_t lsb) +{ + return (((float)lsb / 256.0f) + 25.0f); +} + +float lsm6dsv16x_from_lsb_to_nsec(uint32_t lsb) +{ + return ((float)lsb * 21750.0f); +} + +/** + * @} + * + */ + +/** + * @defgroup Accelerometer user offset correction + * @brief This section groups all the functions concerning the + * usage of Accelerometer user offset correction + * @{ + * + */ + +/** + * @brief Enables accelerometer user offset correction block; it is valid for the low-pass path.[set] + * + * @param ctx read / write interface definitions + * @param val Enables accelerometer user offset correction block; it is valid for the low-pass path. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_xl_offset_on_out_set(lsm6dsv16x_ctx_t *ctx, uint8_t val) +{ + lsm6dsv16x_ctrl9_t ctrl9; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL9, (uint8_t *)&ctrl9, 1); + if (ret == 0) { + ctrl9.usr_off_on_out = val; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_CTRL9, (uint8_t *)&ctrl9, 1); + } + + return ret; +} + +/** + * @brief Enables accelerometer user offset correction block; it is valid for the low-pass path.[get] + * + * @param ctx read / write interface definitions + * @param val Enables accelerometer user offset correction block; it is valid for the low-pass path. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_xl_offset_on_out_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val) +{ + lsm6dsv16x_ctrl9_t ctrl9; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL9, (uint8_t *)&ctrl9, 1); + *val = ctrl9.usr_off_on_out; + + return ret; +} + +/** + * @brief Accelerometer user offset correction values in mg.[set] + * + * @param ctx read / write interface definitions + * @param val Accelerometer user offset correction values in mg. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_xl_offset_mg_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_xl_offset_mg_t val) +{ + lsm6dsv16x_z_ofs_usr_t z_ofs_usr; + lsm6dsv16x_y_ofs_usr_t y_ofs_usr; + lsm6dsv16x_x_ofs_usr_t x_ofs_usr; + lsm6dsv16x_ctrl9_t ctrl9; + int32_t ret; + float tmp; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_Z_OFS_USR, (uint8_t *)&z_ofs_usr, 1); + if (ret == 0) { + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_Y_OFS_USR, (uint8_t *)&y_ofs_usr, 1); + } + if (ret == 0) { + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_X_OFS_USR, (uint8_t *)&x_ofs_usr, 1); + } + + + if ((val.x_mg < (0.0078125f * 127.0f)) && (val.x_mg > (0.0078125f * -127.0f)) && + (val.y_mg < (0.0078125f * 127.0f)) && (val.y_mg > (0.0078125f * -127.0f)) && + (val.z_mg < (0.0078125f * 127.0f)) && (val.z_mg > (0.0078125f * -127.0f))) { + ctrl9.usr_off_w = 0; + + tmp = val.z_mg / 0.0078125f; + z_ofs_usr.z_ofs_usr = (uint8_t)tmp; + + tmp = val.y_mg / 0.0078125f; + y_ofs_usr.y_ofs_usr = (uint8_t)tmp; + + tmp = val.x_mg / 0.0078125f; + x_ofs_usr.x_ofs_usr = (uint8_t)tmp; + } else if ((val.x_mg < (0.125f * 127.0f)) && (val.x_mg > (0.125f * -127.0f)) && + (val.y_mg < (0.125f * 127.0f)) && (val.y_mg > (0.125f * -127.0f)) && + (val.z_mg < (0.125f * 127.0f)) && (val.z_mg > (0.125f * -127.0f))) { + ctrl9.usr_off_w = 1; + + tmp = val.z_mg / 0.125f; + z_ofs_usr.z_ofs_usr = (uint8_t)tmp; + + tmp = val.y_mg / 0.125f; + y_ofs_usr.y_ofs_usr = (uint8_t)tmp; + + tmp = val.x_mg / 0.125f; + x_ofs_usr.x_ofs_usr = (uint8_t)tmp; + } else { // out of limit + ctrl9.usr_off_w = 1; + z_ofs_usr.z_ofs_usr = 0xFFU; + y_ofs_usr.y_ofs_usr = 0xFFU; + x_ofs_usr.x_ofs_usr = 0xFFU; + } + + if (ret == 0) { + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_Z_OFS_USR, (uint8_t *)&z_ofs_usr, 1); + } + if (ret == 0) { + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_Y_OFS_USR, (uint8_t *)&y_ofs_usr, 1); + } + if (ret == 0) { + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_X_OFS_USR, (uint8_t *)&x_ofs_usr, 1); + } + if (ret == 0) { + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_CTRL9, (uint8_t *)&ctrl9, 1); + } + return ret; +} + +/** + * @brief Accelerometer user offset correction values in mg.[get] + * + * @param ctx read / write interface definitions + * @param val Accelerometer user offset correction values in mg. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_xl_offset_mg_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_xl_offset_mg_t *val) +{ + lsm6dsv16x_z_ofs_usr_t z_ofs_usr; + lsm6dsv16x_y_ofs_usr_t y_ofs_usr; + lsm6dsv16x_x_ofs_usr_t x_ofs_usr; + lsm6dsv16x_ctrl9_t ctrl9; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL9, (uint8_t *)&ctrl9, 1); + if (ret == 0) { + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_Z_OFS_USR, (uint8_t *)&z_ofs_usr, 1); + } + if (ret == 0) { + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_Y_OFS_USR, (uint8_t *)&y_ofs_usr, 1); + } + if (ret == 0) { + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_X_OFS_USR, (uint8_t *)&x_ofs_usr, 1); + } + + if (ctrl9.usr_off_w == PROPERTY_DISABLE) { + val->z_mg = ((float)z_ofs_usr.z_ofs_usr * 0.0078125f); + val->y_mg = ((float)y_ofs_usr.y_ofs_usr * 0.0078125f); + val->x_mg = ((float)x_ofs_usr.x_ofs_usr * 0.0078125f); + } else { + val->z_mg = ((float)z_ofs_usr.z_ofs_usr * 0.125f); + val->y_mg = ((float)y_ofs_usr.y_ofs_usr * 0.125f); + val->x_mg = ((float)x_ofs_usr.x_ofs_usr * 0.125f); + } + + return ret; +} + +/** + * @} + * + */ + +/** + * @brief Reset of the device.[set] + * + * @param ctx read / write interface definitions + * @param val Reset of the device. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_reset_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_reset_t val) +{ + lsm6dsv16x_func_cfg_access_t func_cfg_access; + lsm6dsv16x_ctrl3_t ctrl3; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL3, (uint8_t *)&ctrl3, 1); + if (ret == 0) { + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FUNC_CFG_ACCESS, (uint8_t *)&func_cfg_access, 1); + } + + ctrl3.boot = ((uint8_t)val & 0x04U) >> 2; + ctrl3.sw_reset = ((uint8_t)val & 0x02U) >> 1; + func_cfg_access.sw_por = (uint8_t)val & 0x01U; + + if (ret == 0) { + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_CTRL3, (uint8_t *)&ctrl3, 1); + } + if (ret == 0) { + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_FUNC_CFG_ACCESS, (uint8_t *)&func_cfg_access, 1); + } + + return ret; +} + +/** + * @brief Global reset of the device.[get] + * + * @param ctx read / write interface definitions + * @param val Global reset of the device. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_reset_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_reset_t *val) +{ + lsm6dsv16x_func_cfg_access_t func_cfg_access; + lsm6dsv16x_ctrl3_t ctrl3; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL3, (uint8_t *)&ctrl3, 1); + if (ret == 0) { + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FUNC_CFG_ACCESS, (uint8_t *)&func_cfg_access, 1); + } + + switch ((ctrl3.sw_reset << 2) + (ctrl3.boot << 1) + func_cfg_access.sw_por) { + case LSM6DSV16X_READY: + *val = LSM6DSV16X_READY; + break; + + case LSM6DSV16X_GLOBAL_RST: + *val = LSM6DSV16X_GLOBAL_RST; + break; + + case LSM6DSV16X_RESTORE_CAL_PARAM: + *val = LSM6DSV16X_RESTORE_CAL_PARAM; + break; + + case LSM6DSV16X_RESTORE_CTRL_REGS: + *val = LSM6DSV16X_RESTORE_CTRL_REGS; + break; + + default: + *val = LSM6DSV16X_GLOBAL_RST; + break; + } + return ret; +} + +/** + * @brief Change memory bank.[set] + * + * @param ctx read / write interface definitions + * @param val MAIN_MEM_BANK, EMBED_FUNC_MEM_BANK, + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_mem_bank_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_mem_bank_t val) +{ + lsm6dsv16x_func_cfg_access_t func_cfg_access; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FUNC_CFG_ACCESS, (uint8_t *)&func_cfg_access, 1); + + if (ret == 0) { + func_cfg_access.shub_reg_access = ((uint8_t)val & 0x02U) >> 1; + func_cfg_access.emb_func_reg_access = (uint8_t)val & 0x01U; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_FUNC_CFG_ACCESS, (uint8_t *)&func_cfg_access, 1); + } + + return ret; +} + +/** + * @brief Change memory bank.[get] + * + * @param ctx read / write interface definitions + * @param val MAIN_MEM_BANK, SENSOR_HUB_MEM_BANK, EMBED_FUNC_MEM_BANK, + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_mem_bank_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_mem_bank_t *val) +{ + lsm6dsv16x_func_cfg_access_t func_cfg_access; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FUNC_CFG_ACCESS, (uint8_t *)&func_cfg_access, 1); + + switch ((func_cfg_access.shub_reg_access << 1) + func_cfg_access.emb_func_reg_access) { + case LSM6DSV16X_MAIN_MEM_BANK: + *val = LSM6DSV16X_MAIN_MEM_BANK; + break; + + case LSM6DSV16X_EMBED_FUNC_MEM_BANK: + *val = LSM6DSV16X_EMBED_FUNC_MEM_BANK; + break; + + case LSM6DSV16X_SENSOR_HUB_MEM_BANK: + *val = LSM6DSV16X_SENSOR_HUB_MEM_BANK; + break; + + default: + *val = LSM6DSV16X_MAIN_MEM_BANK; + break; + } + return ret; +} + +/** + * @brief Device ID.[get] THis function works also for OIS + * (WHO_AM_I and SPI2_WHO_AM_I have same address) + * + * @param ctx read / write interface definitions + * @param val Device ID. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_device_id_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val) +{ + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_WHO_AM_I, val, 1); + + return ret; +} + +/** + * @brief Accelerometer output data rate (ODR) selection.[set] + * + * @param ctx read / write interface definitions + * @param val XL_ODR_OFF, XL_ODR_AT_1Hz875, XL_ODR_AT_7Hz5, XL_ODR_AT_15Hz, XL_ODR_AT_30Hz, XL_ODR_AT_60Hz, XL_ODR_AT_120Hz, XL_ODR_AT_240Hz, XL_ODR_AT_480Hz, XL_ODR_AT_960Hz, XL_ODR_AT_1920Hz, XL_ODR_AT_3840Hz, XL_ODR_AT_7680Hz, + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_xl_data_rate_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_xl_data_rate_t val) +{ + lsm6dsv16x_ctrl1_t ctrl1; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL1, (uint8_t *)&ctrl1, 1); + if (ret == 0) { + ctrl1.odr_xl = (uint8_t)val & 0x0Fu; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_CTRL1, (uint8_t *)&ctrl1, 1); + } + + return ret; +} + +/** + * @brief Accelerometer output data rate (ODR) selection.[get] + * + * @param ctx read / write interface definitions + * @param val XL_ODR_OFF, XL_ODR_AT_1Hz875, XL_ODR_AT_7Hz5, XL_ODR_AT_15Hz, XL_ODR_AT_30Hz, XL_ODR_AT_60Hz, XL_ODR_AT_120Hz, XL_ODR_AT_240Hz, XL_ODR_AT_480Hz, XL_ODR_AT_960Hz, XL_ODR_AT_1920Hz, XL_ODR_AT_3840Hz, XL_ODR_AT_7680Hz, + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_xl_data_rate_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_xl_data_rate_t *val) +{ + lsm6dsv16x_ctrl1_t ctrl1; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL1, (uint8_t *)&ctrl1, 1); + + switch (ctrl1.odr_xl) { + case LSM6DSV16X_XL_ODR_OFF: + *val = LSM6DSV16X_XL_ODR_OFF; + break; + + case LSM6DSV16X_XL_ODR_AT_1Hz875: + *val = LSM6DSV16X_XL_ODR_AT_1Hz875; + break; + + case LSM6DSV16X_XL_ODR_AT_7Hz5: + *val = LSM6DSV16X_XL_ODR_AT_7Hz5; + break; + + case LSM6DSV16X_XL_ODR_AT_15Hz: + *val = LSM6DSV16X_XL_ODR_AT_15Hz; + break; + + case LSM6DSV16X_XL_ODR_AT_30Hz: + *val = LSM6DSV16X_XL_ODR_AT_30Hz; + break; + + case LSM6DSV16X_XL_ODR_AT_60Hz: + *val = LSM6DSV16X_XL_ODR_AT_60Hz; + break; + + case LSM6DSV16X_XL_ODR_AT_120Hz: + *val = LSM6DSV16X_XL_ODR_AT_120Hz; + break; + + case LSM6DSV16X_XL_ODR_AT_240Hz: + *val = LSM6DSV16X_XL_ODR_AT_240Hz; + break; + + case LSM6DSV16X_XL_ODR_AT_480Hz: + *val = LSM6DSV16X_XL_ODR_AT_480Hz; + break; + + case LSM6DSV16X_XL_ODR_AT_960Hz: + *val = LSM6DSV16X_XL_ODR_AT_960Hz; + break; + + case LSM6DSV16X_XL_ODR_AT_1920Hz: + *val = LSM6DSV16X_XL_ODR_AT_1920Hz; + break; + + case LSM6DSV16X_XL_ODR_AT_3840Hz: + *val = LSM6DSV16X_XL_ODR_AT_3840Hz; + break; + + case LSM6DSV16X_XL_ODR_AT_7680Hz: + *val = LSM6DSV16X_XL_ODR_AT_7680Hz; + break; + + default: + *val = LSM6DSV16X_XL_ODR_OFF; + break; + } + return ret; +} + +/** + * @brief Accelerometer operating mode selection.[set] + * + * @param ctx read / write interface definitions + * @param val XL_HIGH_PERFORMANCE_MD, XL_HIGH_ACCURANCY_ODR_MD, XL_LOW_POWER_2_AVG_MD, XL_LOW_POWER_4_AVG_MD, XL_LOW_POWER_8_AVG_MD, XL_NORMAL_MD, + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_xl_mode_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_xl_mode_t val) +{ + lsm6dsv16x_ctrl1_t ctrl1; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL1, (uint8_t *)&ctrl1, 1); + + if (ret == 0) { + ctrl1.op_mode_xl = (uint8_t)val & 0x07U; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_CTRL1, (uint8_t *)&ctrl1, 1); + } + + return ret; +} + +/** + * @brief Accelerometer operating mode selection.[get] + * + * @param ctx read / write interface definitions + * @param val XL_HIGH_PERFORMANCE_MD, XL_HIGH_ACCURANCY_ODR_MD, XL_LOW_POWER_2_AVG_MD, XL_LOW_POWER_4_AVG_MD, XL_LOW_POWER_8_AVG_MD, XL_NORMAL_MD, + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_xl_mode_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_xl_mode_t *val) +{ + lsm6dsv16x_ctrl1_t ctrl1; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL1, (uint8_t *)&ctrl1, 1); + + switch (ctrl1.op_mode_xl) { + case LSM6DSV16X_XL_HIGH_PERFORMANCE_MD: + *val = LSM6DSV16X_XL_HIGH_PERFORMANCE_MD; + break; + + case LSM6DSV16X_XL_HIGH_ACCURANCY_ODR_MD: + *val = LSM6DSV16X_XL_HIGH_ACCURANCY_ODR_MD; + break; + + case LSM6DSV16X_XL_LOW_POWER_2_AVG_MD: + *val = LSM6DSV16X_XL_LOW_POWER_2_AVG_MD; + break; + + case LSM6DSV16X_XL_LOW_POWER_4_AVG_MD: + *val = LSM6DSV16X_XL_LOW_POWER_4_AVG_MD; + break; + + case LSM6DSV16X_XL_LOW_POWER_8_AVG_MD: + *val = LSM6DSV16X_XL_LOW_POWER_8_AVG_MD; + break; + + case LSM6DSV16X_XL_NORMAL_MD: + *val = LSM6DSV16X_XL_NORMAL_MD; + break; + + default: + *val = LSM6DSV16X_XL_HIGH_PERFORMANCE_MD; + break; + } + return ret; +} + +/** + * @brief Gyroscope output data rate (ODR) selection.[set] + * + * @param ctx read / write interface definitions + * @param val GY_ODR_OFF, GY_ODR_AT_7Hz5, GY_ODR_AT_15Hz, GY_ODR_AT_30Hz, GY_ODR_AT_60Hz, GY_ODR_AT_120Hz, GY_ODR_AT_240Hz, GY_ODR_AT_480Hz, GY_ODR_AT_960Hz, GY_ODR_AT_1920Hz, GY_ODR_AT_3840Hz, GY_ODR_AT_7680Hz, + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_gy_data_rate_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_gy_data_rate_t val) +{ + lsm6dsv16x_ctrl2_t ctrl2; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL2, (uint8_t *)&ctrl2, 1); + + if (ret == 0) { + ctrl2.odr_g = (uint8_t)val & 0x0Fu; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_CTRL2, (uint8_t *)&ctrl2, 1); + } + + return ret; +} + +/** + * @brief Gyroscope output data rate (ODR) selection.[get] + * + * @param ctx read / write interface definitions + * @param val GY_ODR_OFF, GY_ODR_AT_7Hz5, GY_ODR_AT_15Hz, GY_ODR_AT_30Hz, GY_ODR_AT_60Hz, GY_ODR_AT_120Hz, GY_ODR_AT_240Hz, GY_ODR_AT_480Hz, GY_ODR_AT_960Hz, GY_ODR_AT_1920Hz, GY_ODR_AT_3840Hz, GY_ODR_AT_7680Hz, + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_gy_data_rate_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_gy_data_rate_t *val) +{ + lsm6dsv16x_ctrl2_t ctrl2; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL2, (uint8_t *)&ctrl2, 1); + + switch (ctrl2.odr_g) { + case LSM6DSV16X_GY_ODR_OFF: + *val = LSM6DSV16X_GY_ODR_OFF; + break; + + case LSM6DSV16X_GY_ODR_AT_7Hz5: + *val = LSM6DSV16X_GY_ODR_AT_7Hz5; + break; + + case LSM6DSV16X_GY_ODR_AT_15Hz: + *val = LSM6DSV16X_GY_ODR_AT_15Hz; + break; + + case LSM6DSV16X_GY_ODR_AT_30Hz: + *val = LSM6DSV16X_GY_ODR_AT_30Hz; + break; + + case LSM6DSV16X_GY_ODR_AT_60Hz: + *val = LSM6DSV16X_GY_ODR_AT_60Hz; + break; + + case LSM6DSV16X_GY_ODR_AT_120Hz: + *val = LSM6DSV16X_GY_ODR_AT_120Hz; + break; + + case LSM6DSV16X_GY_ODR_AT_240Hz: + *val = LSM6DSV16X_GY_ODR_AT_240Hz; + break; + + case LSM6DSV16X_GY_ODR_AT_480Hz: + *val = LSM6DSV16X_GY_ODR_AT_480Hz; + break; + + case LSM6DSV16X_GY_ODR_AT_960Hz: + *val = LSM6DSV16X_GY_ODR_AT_960Hz; + break; + + case LSM6DSV16X_GY_ODR_AT_1920Hz: + *val = LSM6DSV16X_GY_ODR_AT_1920Hz; + break; + + case LSM6DSV16X_GY_ODR_AT_3840Hz: + *val = LSM6DSV16X_GY_ODR_AT_3840Hz; + break; + + case LSM6DSV16X_GY_ODR_AT_7680Hz: + *val = LSM6DSV16X_GY_ODR_AT_7680Hz; + break; + + default: + *val = LSM6DSV16X_GY_ODR_OFF; + break; + } + return ret; +} + +/** + * @brief Gyroscope operating mode selection.[set] + * + * @param ctx read / write interface definitions + * @param val GY_HIGH_PERFORMANCE_MD, GY_HIGH_ACCURANCY_ODR_MD, GY_SLEEP_MD, GY_LOW_POWER_MD, + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_gy_mode_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_gy_mode_t val) +{ + lsm6dsv16x_ctrl2_t ctrl2; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL2, (uint8_t *)&ctrl2, 1); + if (ret == 0) { + ctrl2.op_mode_g = (uint8_t)val & 0x07U; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_CTRL2, (uint8_t *)&ctrl2, 1); + } + + return ret; +} + +/** + * @brief Gyroscope operating mode selection.[get] + * + * @param ctx read / write interface definitions + * @param val GY_HIGH_PERFORMANCE_MD, GY_HIGH_ACCURANCY_ODR_MD, GY_SLEEP_MD, GY_LOW_POWER_MD, + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_gy_mode_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_gy_mode_t *val) +{ + lsm6dsv16x_ctrl2_t ctrl2; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL2, (uint8_t *)&ctrl2, 1); + switch (ctrl2.op_mode_g) { + case LSM6DSV16X_GY_HIGH_PERFORMANCE_MD: + *val = LSM6DSV16X_GY_HIGH_PERFORMANCE_MD; + break; + + case LSM6DSV16X_GY_HIGH_ACCURANCY_ODR_MD: + *val = LSM6DSV16X_GY_HIGH_ACCURANCY_ODR_MD; + break; + + case LSM6DSV16X_GY_SLEEP_MD: + *val = LSM6DSV16X_GY_SLEEP_MD; + break; + + case LSM6DSV16X_GY_LOW_POWER_MD: + *val = LSM6DSV16X_GY_LOW_POWER_MD; + break; + + default: + *val = LSM6DSV16X_GY_HIGH_PERFORMANCE_MD; + break; + } + return ret; +} + +/** + * @brief Register address automatically incremented during a multiple byte access with a serial interface (enable by default).[set] + * + * @param ctx read / write interface definitions + * @param val Register address automatically incremented during a multiple byte access with a serial interface (enable by default). + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_auto_increment_set(lsm6dsv16x_ctx_t *ctx, uint8_t val) +{ + lsm6dsv16x_ctrl3_t ctrl3; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL3, (uint8_t *)&ctrl3, 1); + if (ret == 0) { + ctrl3.if_inc = val; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_CTRL3, (uint8_t *)&ctrl3, 1); + } + + return ret; +} + +/** + * @brief Register address automatically incremented during a multiple byte access with a serial interface (enable by default).[get] + * + * @param ctx read / write interface definitions + * @param val Register address automatically incremented during a multiple byte access with a serial interface (enable by default). + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_auto_increment_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val) +{ + lsm6dsv16x_ctrl3_t ctrl3; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL3, (uint8_t *)&ctrl3, 1); + *val = ctrl3.if_inc; + + + return ret; +} + +/** + * @brief Block Data Update (BDU): output registers are not updated until LSB and MSB have been read). [set] + * + * @param ctx read / write interface definitions + * @param val Block Data Update (BDU): output registers are not updated until LSB and MSB have been read). + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_block_data_update_set(lsm6dsv16x_ctx_t *ctx, uint8_t val) +{ + lsm6dsv16x_ctrl3_t ctrl3; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL3, (uint8_t *)&ctrl3, 1); + + if (ret == 0) { + ctrl3.bdu = val; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_CTRL3, (uint8_t *)&ctrl3, 1); + } + + return ret; +} + +/** + * @brief Block Data Update (BDU): output registers are not updated until LSB and MSB have been read). [get] + * + * @param ctx read / write interface definitions + * @param val Block Data Update (BDU): output registers are not updated until LSB and MSB have been read). + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_block_data_update_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val) +{ + lsm6dsv16x_ctrl3_t ctrl3; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL3, (uint8_t *)&ctrl3, 1); + *val = ctrl3.bdu; + + return ret; +} + +/** + * @brief Enables pulsed data-ready mode (~75 us).[set] + * + * @param ctx read / write interface definitions + * @param val DRDY_LATCHED, DRDY_PULSED, + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_data_ready_mode_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_data_ready_mode_t val) +{ + lsm6dsv16x_ctrl4_t ctrl4; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL4, (uint8_t *)&ctrl4, 1); + + if (ret == 0) { + ctrl4.drdy_pulsed = (uint8_t)val & 0x1U; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_CTRL4, (uint8_t *)&ctrl4, 1); + } + + return ret; +} + +/** + * @brief Enables pulsed data-ready mode (~75 us).[get] + * + * @param ctx read / write interface definitions + * @param val DRDY_LATCHED, DRDY_PULSED, + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_data_ready_mode_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_data_ready_mode_t *val) +{ + lsm6dsv16x_ctrl4_t ctrl4; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL4, (uint8_t *)&ctrl4, 1); + + switch (ctrl4.drdy_pulsed) { + case LSM6DSV16X_DRDY_LATCHED: + *val = LSM6DSV16X_DRDY_LATCHED; + break; + + case LSM6DSV16X_DRDY_PULSED: + *val = LSM6DSV16X_DRDY_PULSED; + break; + + default: + *val = LSM6DSV16X_DRDY_LATCHED; + break; + } + return ret; +} + +/** + * @brief Gyroscope full-scale selection[set] + * + * @param ctx read / write interface definitions + * @param val 125dps, 250dps, 500dps, 1000dps, 2000dps, 4000dps, + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_gy_full_scale_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_gy_full_scale_t val) +{ + lsm6dsv16x_ctrl6_t ctrl6; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL6, (uint8_t *)&ctrl6, 1); + + if (ret == 0) { + ctrl6.fs_g = (uint8_t)val & 0xfu; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_CTRL6, (uint8_t *)&ctrl6, 1); + } + + return ret; +} + +/** + * @brief Gyroscope full-scale selection[get] + * + * @param ctx read / write interface definitions + * @param val 125dps, 250dps, 500dps, 1000dps, 2000dps, 4000dps, + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_gy_full_scale_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_gy_full_scale_t *val) +{ + lsm6dsv16x_ctrl6_t ctrl6; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL6, (uint8_t *)&ctrl6, 1); + + switch (ctrl6.fs_g) { + case LSM6DSV16X_125dps: + *val = LSM6DSV16X_125dps; + break; + + case LSM6DSV16X_250dps: + *val = LSM6DSV16X_250dps; + break; + + case LSM6DSV16X_500dps: + *val = LSM6DSV16X_500dps; + break; + + case LSM6DSV16X_1000dps: + *val = LSM6DSV16X_1000dps; + break; + + case LSM6DSV16X_2000dps: + *val = LSM6DSV16X_2000dps; + break; + + case LSM6DSV16X_4000dps: + *val = LSM6DSV16X_4000dps; + break; + + default: + *val = LSM6DSV16X_125dps; + break; + } + return ret; +} + +/** + * @brief Accelerometer full-scale selection.[set] + * + * @param ctx read / write interface definitions + * @param val 2g, 4g, 8g, 16g, + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_xl_full_scale_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_xl_full_scale_t val) +{ + lsm6dsv16x_ctrl8_t ctrl8; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL8, (uint8_t *)&ctrl8, 1); + + if (ret == 0) { + ctrl8.fs_xl = (uint8_t)val & 0x3U; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_CTRL8, (uint8_t *)&ctrl8, 1); + } + + return ret; +} + +/** + * @brief Accelerometer full-scale selection.[get] + * + * @param ctx read / write interface definitions + * @param val 2g, 4g, 8g, 16g, + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_xl_full_scale_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_xl_full_scale_t *val) +{ + lsm6dsv16x_ctrl8_t ctrl8; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL8, (uint8_t *)&ctrl8, 1); + + switch (ctrl8.fs_xl) { + case LSM6DSV16X_2g: + *val = LSM6DSV16X_2g; + break; + + case LSM6DSV16X_4g: + *val = LSM6DSV16X_4g; + break; + + case LSM6DSV16X_8g: + *val = LSM6DSV16X_8g; + break; + + case LSM6DSV16X_16g: + *val = LSM6DSV16X_16g; + break; + + default: + *val = LSM6DSV16X_2g; + break; + } + return ret; +} + +/** + * @brief It enables the accelerometer Dual channel mode: data with selected full scale and data with maximum full scale are sent simultaneously to two different set of output registers.[set] + * + * @param ctx read / write interface definitions + * @param val It enables the accelerometer Dual channel mode: data with selected full scale and data with maximum full scale are sent simultaneously to two different set of output registers. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_xl_dual_channel_set(lsm6dsv16x_ctx_t *ctx, uint8_t val) +{ + lsm6dsv16x_ctrl8_t ctrl8; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL8, (uint8_t *)&ctrl8, 1); + + if (ret == 0) { + ctrl8.xl_dualc_en = val; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_CTRL8, (uint8_t *)&ctrl8, 1); + } + + return ret; +} + +/** + * @brief It enables the accelerometer Dual channel mode: data with selected full scale and data with maximum full scale are sent simultaneously to two different set of output registers.[get] + * + * @param ctx read / write interface definitions + * @param val It enables the accelerometer Dual channel mode: data with selected full scale and data with maximum full scale are sent simultaneously to two different set of output registers. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_xl_dual_channel_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val) +{ + lsm6dsv16x_ctrl8_t ctrl8; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL8, (uint8_t *)&ctrl8, 1); + *val = ctrl8.xl_dualc_en; + + return ret; +} + +/** + * @brief Accelerometer self-test selection.[set] + * + * @param ctx read / write interface definitions + * @param val XL_ST_DISABLE, XL_ST_POSITIVE, XL_ST_NEGATIVE, + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_xl_self_test_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_xl_self_test_t val) +{ + lsm6dsv16x_ctrl10_t ctrl10; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL10, (uint8_t *)&ctrl10, 1); + + if (ret == 0) { + ctrl10.st_xl = (uint8_t)val & 0x3U; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_CTRL10, (uint8_t *)&ctrl10, 1); + } + + return ret; +} + +/** + * @brief Accelerometer self-test selection.[get] + * + * @param ctx read / write interface definitions + * @param val XL_ST_DISABLE, XL_ST_POSITIVE, XL_ST_NEGATIVE, + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_xl_self_test_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_xl_self_test_t *val) +{ + lsm6dsv16x_ctrl10_t ctrl10; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL10, (uint8_t *)&ctrl10, 1); + + switch (ctrl10.st_xl) { + case LSM6DSV16X_XL_ST_DISABLE: + *val = LSM6DSV16X_XL_ST_DISABLE; + break; + + case LSM6DSV16X_XL_ST_POSITIVE: + *val = LSM6DSV16X_XL_ST_POSITIVE; + break; + + case LSM6DSV16X_XL_ST_NEGATIVE: + *val = LSM6DSV16X_XL_ST_NEGATIVE; + break; + + default: + *val = LSM6DSV16X_XL_ST_DISABLE; + break; + } + return ret; +} + +/** + * @brief Gyroscope self-test selection.[set] + * + * @param ctx read / write interface definitions + * @param val XL_ST_DISABLE, XL_ST_POSITIVE, XL_ST_NEGATIVE, + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_gy_self_test_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_gy_self_test_t val) +{ + lsm6dsv16x_ctrl10_t ctrl10; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL10, (uint8_t *)&ctrl10, 1); + + if (ret == 0) { + ctrl10.st_g = (uint8_t)val & 0x3U; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_CTRL10, (uint8_t *)&ctrl10, 1); + } + + return ret; +} + +/** + * @brief Gyroscope self-test selection.[get] + * + * @param ctx read / write interface definitions + * @param val XL_ST_DISABLE, XL_ST_POSITIVE, XL_ST_NEGATIVE, + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_gy_self_test_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_gy_self_test_t *val) +{ + lsm6dsv16x_ctrl10_t ctrl10; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL10, (uint8_t *)&ctrl10, 1); + + switch (ctrl10.st_g) { + case LSM6DSV16X_GY_ST_DISABLE: + *val = LSM6DSV16X_GY_ST_DISABLE; + break; + + case LSM6DSV16X_GY_ST_POSITIVE: + *val = LSM6DSV16X_GY_ST_POSITIVE; + break; + + case LSM6DSV16X_GY_ST_NEGATIVE: + *val = LSM6DSV16X_GY_ST_NEGATIVE; + break; + + default: + *val = LSM6DSV16X_GY_ST_DISABLE; + break; + } + return ret; +} + +/** + * @brief SPI2 Accelerometer self-test selection.[set] + * + * @param ctx read / write interface definitions + * @param val XL_ST_DISABLE, XL_ST_POSITIVE, XL_ST_NEGATIVE, + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_ois_xl_self_test_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ois_xl_self_test_t val) +{ + lsm6dsv16x_spi2_int_ois_t spi2_int_ois; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_SPI2_INT_OIS, (uint8_t *)&spi2_int_ois, 1); + + if (ret == 0) { + spi2_int_ois.st_xl_ois = ((uint8_t)val & 0x3U); + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_SPI2_INT_OIS, (uint8_t *)&spi2_int_ois, 1); + } + + return ret; +} + +/** + * @brief SPI2 Accelerometer self-test selection.[get] + * + * @param ctx read / write interface definitions + * @param val XL_ST_DISABLE, XL_ST_POSITIVE, XL_ST_NEGATIVE, + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_ois_xl_self_test_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ois_xl_self_test_t *val) +{ + lsm6dsv16x_spi2_int_ois_t spi2_int_ois; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_SPI2_INT_OIS, (uint8_t *)&spi2_int_ois, 1); + + switch (spi2_int_ois.st_xl_ois) { + case LSM6DSV16X_OIS_XL_ST_DISABLE: + *val = LSM6DSV16X_OIS_XL_ST_DISABLE; + break; + + case LSM6DSV16X_OIS_XL_ST_POSITIVE: + *val = LSM6DSV16X_OIS_XL_ST_POSITIVE; + break; + + case LSM6DSV16X_OIS_XL_ST_NEGATIVE: + *val = LSM6DSV16X_OIS_XL_ST_NEGATIVE; + break; + + default: + *val = LSM6DSV16X_OIS_XL_ST_DISABLE; + break; + } + return ret; +} + +/** + * @brief SPI2 Accelerometer self-test selection.[set] + * + * @param ctx read / write interface definitions + * @param val GY_ST_DISABLE, GY_ST_POSITIVE, GY_ST_NEGATIVE, LSM6DSV16X_OIS_GY_ST_CLAMP_POS, LSM6DSV16X_OIS_GY_ST_CLAMP_NEG + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_ois_gy_self_test_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ois_gy_self_test_t val) +{ + lsm6dsv16x_spi2_int_ois_t spi2_int_ois; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_SPI2_INT_OIS, (uint8_t *)&spi2_int_ois, 1); + + if (ret == 0) { + spi2_int_ois.st_g_ois = ((uint8_t)val & 0x3U); + spi2_int_ois.st_ois_clampdis = ((uint8_t)val & 0x04U) >> 2; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_SPI2_INT_OIS, (uint8_t *)&spi2_int_ois, 1); + } + + return ret; +} + +/** + * @brief SPI2 Accelerometer self-test selection.[get] + * + * @param ctx read / write interface definitions + * @param val GY_ST_DISABLE, GY_ST_POSITIVE, GY_ST_NEGATIVE, LSM6DSV16X_OIS_GY_ST_CLAMP_POS, LSM6DSV16X_OIS_GY_ST_CLAMP_NEG + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_ois_gy_self_test_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ois_gy_self_test_t *val) +{ + lsm6dsv16x_spi2_int_ois_t spi2_int_ois; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_SPI2_INT_OIS, (uint8_t *)&spi2_int_ois, 1); + + switch (spi2_int_ois.st_g_ois) { + case LSM6DSV16X_OIS_GY_ST_DISABLE: + *val = LSM6DSV16X_OIS_GY_ST_DISABLE; + break; + + case LSM6DSV16X_OIS_GY_ST_POSITIVE: + *val = (spi2_int_ois.st_ois_clampdis == 1U) ? LSM6DSV16X_OIS_GY_ST_CLAMP_POS : LSM6DSV16X_OIS_GY_ST_POSITIVE; + break; + + case LSM6DSV16X_OIS_GY_ST_NEGATIVE: + *val = (spi2_int_ois.st_ois_clampdis == 1U) ? LSM6DSV16X_OIS_GY_ST_CLAMP_NEG : LSM6DSV16X_OIS_GY_ST_NEGATIVE; + break; + + default: + *val = LSM6DSV16X_OIS_GY_ST_DISABLE; + break; + } + return ret; +} + +/** + * @brief Get the status of all the interrupt sources.[get] + * + * @param ctx read / write interface definitions + * @param val Get the status of all the interrupt sources. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_all_sources_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_all_sources_t *val) +{ + lsm6dsv16x_emb_func_status_mainpage_t emb_func_status_mainpage; + lsm6dsv16x_emb_func_exec_status_t emb_func_exec_status; + lsm6dsv16x_fsm_status_mainpage_t fsm_status_mainpage; + lsm6dsv16x_mlc_status_mainpage_t mlc_status_mainpage; + lsm6dsv16x_functions_enable_t functions_enable; + lsm6dsv16x_emb_func_src_t emb_func_src; + lsm6dsv16x_fifo_status2_t fifo_status2; + lsm6dsv16x_all_int_src_t all_int_src; + lsm6dsv16x_wake_up_src_t wake_up_src; + lsm6dsv16x_status_reg_t status_reg; + lsm6dsv16x_d6d_src_t d6d_src; + lsm6dsv16x_tap_src_t tap_src; + lsm6dsv16x_ui_status_reg_ois_t status_reg_ois; + lsm6dsv16x_status_master_t status_shub; + uint8_t buff[8]; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FUNCTIONS_ENABLE, (uint8_t *)&functions_enable, 1); + if (ret == 0) { + functions_enable.dis_rst_lir_all_int = PROPERTY_ENABLE; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_FUNCTIONS_ENABLE, (uint8_t *)&functions_enable, 1); + } + + if (ret == 0) { + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FIFO_STATUS1, (uint8_t *)&buff, 4); + } + bytecpy((uint8_t *)&fifo_status2, &buff[1]); + bytecpy((uint8_t *)&all_int_src, &buff[2]); + bytecpy((uint8_t *)&status_reg, &buff[3]); + + val->fifo_ovr = fifo_status2.fifo_ovr_ia; + val->fifo_bdr = fifo_status2.counter_bdr_ia; + val->fifo_full = fifo_status2.fifo_full_ia; + val->fifo_th = fifo_status2.fifo_wtm_ia; + + val->free_fall = all_int_src.ff_ia; + val->wake_up = all_int_src.wu_ia; + val->six_d = all_int_src.d6d_ia; + + val->drdy_xl = status_reg.xlda; + val->drdy_gy = status_reg.gda; + val->drdy_temp = status_reg.tda; + val->drdy_ah_qvar = status_reg.ah_qvarda; + val->drdy_eis = status_reg.gda_eis; + val->drdy_ois = status_reg.ois_drdy; + val->timestamp = status_reg.timestamp_endcount; + + if (ret == 0) { + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FUNCTIONS_ENABLE, (uint8_t *)&functions_enable, 1); + } + if (ret == 0) { + functions_enable.dis_rst_lir_all_int = PROPERTY_DISABLE; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_FUNCTIONS_ENABLE, (uint8_t *)&functions_enable, 1); + } + + if (ret == 0) { + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_UI_STATUS_REG_OIS, (uint8_t *)&buff, 8); + } + + bytecpy((uint8_t *)&status_reg_ois, &buff[0]); + bytecpy((uint8_t *)&wake_up_src, &buff[1]); + bytecpy((uint8_t *)&tap_src, &buff[2]); + bytecpy((uint8_t *)&d6d_src, &buff[3]); + bytecpy((uint8_t *)&emb_func_status_mainpage, &buff[5]); + bytecpy((uint8_t *)&fsm_status_mainpage, &buff[6]); + bytecpy((uint8_t *)&mlc_status_mainpage, &buff[7]); + + val->gy_settling = status_reg_ois.gyro_settling; + val->sleep_change = wake_up_src.sleep_change_ia; + val->wake_up_x = wake_up_src.x_wu; + val->wake_up_y = wake_up_src.y_wu; + val->wake_up_z = wake_up_src.z_wu; + val->sleep_state = wake_up_src.sleep_state; + + val->tap_x = tap_src.x_tap; + val->tap_y = tap_src.y_tap; + val->tap_z = tap_src.z_tap; + val->tap_sign = tap_src.tap_sign; + val->double_tap = tap_src.double_tap; + val->single_tap = tap_src.single_tap; + + val->six_d_zl = d6d_src.zl; + val->six_d_zh = d6d_src.zh; + val->six_d_yl = d6d_src.yl; + val->six_d_yh = d6d_src.yh; + val->six_d_xl = d6d_src.xl; + val->six_d_xh = d6d_src.xh; + + val->step_detector = emb_func_status_mainpage.is_step_det; + val->tilt = emb_func_status_mainpage.is_tilt; + val->sig_mot = emb_func_status_mainpage.is_sigmot; + val->fsm_lc = emb_func_status_mainpage.is_fsm_lc; + + val->fsm1 = fsm_status_mainpage.is_fsm1; + val->fsm2 = fsm_status_mainpage.is_fsm2; + val->fsm3 = fsm_status_mainpage.is_fsm3; + val->fsm4 = fsm_status_mainpage.is_fsm4; + val->fsm5 = fsm_status_mainpage.is_fsm5; + val->fsm6 = fsm_status_mainpage.is_fsm6; + val->fsm7 = fsm_status_mainpage.is_fsm7; + val->fsm8 = fsm_status_mainpage.is_fsm8; + + val->mlc1 = mlc_status_mainpage.is_mlc1; + val->mlc2 = mlc_status_mainpage.is_mlc2; + val->mlc3 = mlc_status_mainpage.is_mlc3; + val->mlc4 = mlc_status_mainpage.is_mlc4; + + + if (ret == 0) { + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK); + } + if (ret == 0) { + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_EMB_FUNC_EXEC_STATUS, (uint8_t *)&emb_func_exec_status, 1); + } + if (ret == 0) { + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_EMB_FUNC_SRC, (uint8_t *)&emb_func_src, 1); + } + if (ret == 0) { + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + } + + val->emb_func_stand_by = emb_func_exec_status.emb_func_endop; + val->emb_func_time_exceed = emb_func_exec_status.emb_func_exec_ovr; + val->step_count_inc = emb_func_src.stepcounter_bit_set; + val->step_count_overflow = emb_func_src.step_overflow; + val->step_on_delta_time = emb_func_src.step_count_delta_ia; + + val->step_detector = emb_func_src.step_detected; + + /* sensor hub */ + if (ret == 0) { + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_SENSOR_HUB_MEM_BANK); + } + if (ret == 0) { + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_STATUS_MASTER, (uint8_t *)&status_shub, 1); + } + if (ret == 0) { + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + } + + val->sh_endop = status_shub.sens_hub_endop; + val->sh_wr_once = status_shub.wr_once_done; + val->sh_slave3_nack = status_shub.slave3_nack; + val->sh_slave2_nack = status_shub.slave2_nack; + val->sh_slave1_nack = status_shub.slave1_nack; + val->sh_slave0_nack = status_shub.slave0_nack; + + return ret; +} + +/** + * @brief Mask status bit reset[set] + * + * @param ctx read / write interface definitions + * @param val Mask to prevent status bit being reset + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_int_ack_mask_set(lsm6dsv16x_ctx_t *ctx, uint8_t val) +{ + int32_t ret; + + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_INT_ACK_MASK, &val, 1); + + return ret; +} + +/** + * @brief Mask status bit reset[get] + * + * @param ctx read / write interface definitions + * @param val Mask to prevent status bit being reset + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_int_ack_mask_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val) +{ + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_INT_ACK_MASK, val, 1); + + return ret; +} + +/** + * @brief Temperature data output register[get] + * + * @param ctx read / write interface definitions + * @param val Temperature data output register + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_temperature_raw_get(lsm6dsv16x_ctx_t *ctx, int16_t *val) +{ + uint8_t buff[2]; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_OUT_TEMP_L, &buff[0], 2); + *val = (int16_t)buff[1]; + *val = (*val * 256) + (int16_t)buff[0]; + + return ret; +} + +/** + * @brief Angular rate sensor.[get] + * + * @param ctx read / write interface definitions + * @param val Angular rate sensor. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_angular_rate_raw_get(lsm6dsv16x_ctx_t *ctx, int16_t *val) +{ + uint8_t buff[6]; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_OUTX_L_G, &buff[0], 6); + val[0] = (int16_t)buff[1]; + val[0] = (val[0] * 256) + (int16_t)buff[0]; + val[1] = (int16_t)buff[3]; + val[1] = (val[1] * 256) + (int16_t)buff[2]; + val[2] = (int16_t)buff[5]; + val[2] = (val[2] * 256) + (int16_t)buff[4]; + + return ret; +} + +/** + * @brief Angular rate sensor.[get] + * + * @param ctx read / write interface definitions + * @param val OIS Angular rate sensor (thru SPI2). + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_ois_angular_rate_raw_get(lsm6dsv16x_ctx_t *ctx, int16_t *val) +{ + uint8_t buff[6]; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_SPI2_OUTX_L_G_OIS, &buff[0], 6); + val[0] = (int16_t)buff[1]; + val[0] = (*val * 256) + (int16_t)buff[0]; + val[1] = (int16_t)buff[3]; + val[1] = (*val * 256) + (int16_t)buff[2]; + val[2] = (int16_t)buff[5]; + val[2] = (*val * 256) + (int16_t)buff[4]; + + return ret; +} + +/** + * @brief Angular rate sensor for OIS gyro or the EIS gyro channel.[get] + * + * @param ctx read / write interface definitions + * @param val Angular rate sensor for OIS gyro or the EIS gyro channel. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_ois_eis_angular_rate_raw_get(lsm6dsv16x_ctx_t *ctx, int16_t *val) +{ + uint8_t buff[6]; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_UI_OUTX_L_G_OIS_EIS, &buff[0], 6); + val[0] = (int16_t)buff[1]; + val[0] = (*val * 256) + (int16_t)buff[0]; + val[1] = (int16_t)buff[3]; + val[1] = (*val * 256) + (int16_t)buff[2]; + val[2] = (int16_t)buff[5]; + val[2] = (*val * 256) + (int16_t)buff[4]; + + return ret; +} + +/** + * @brief Linear acceleration sensor.[get] + * + * @param ctx read / write interface definitions + * @param val Linear acceleration sensor. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_acceleration_raw_get(lsm6dsv16x_ctx_t *ctx, int16_t *val) +{ + uint8_t buff[6]; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_OUTX_L_A, &buff[0], 6); + val[0] = (int16_t)buff[1]; + val[0] = (val[0] * 256) + (int16_t)buff[0]; + val[1] = (int16_t)buff[3]; + val[1] = (val[1] * 256) + (int16_t)buff[2]; + val[2] = (int16_t)buff[5]; + val[2] = (val[2] * 256) + (int16_t)buff[4]; + + return ret; +} + +/** + * @brief Linear acceleration sensor for Dual channel mode.[get] + * + * @param ctx read / write interface definitions + * @param val Linear acceleration sensor or Dual channel mode. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_dual_acceleration_raw_get(lsm6dsv16x_ctx_t *ctx, int16_t *val) +{ + uint8_t buff[6]; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_UI_OUTX_L_A_OIS_DUALC, &buff[0], 6); + val[0] = (int16_t)buff[1]; + val[0] = (val[0] * 256) + (int16_t)buff[0]; + val[1] = (int16_t)buff[3]; + val[1] = (val[1] * 256) + (int16_t)buff[2]; + val[2] = (int16_t)buff[5]; + val[2] = (val[2] * 256) + (int16_t)buff[4]; + + return ret; +} + +/** + * @brief ah_qvar data output register.[get] + * + * @param ctx read / write interface definitions + * @param val ah_qvar data output register. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_ah_qvar_raw_get(lsm6dsv16x_ctx_t *ctx, int16_t *val) +{ + uint8_t buff[2]; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_AH_QVAR_OUT_L, &buff[0], 2); + *val = (int16_t)buff[1]; + *val = (*val * 256) + (int16_t)buff[0]; + + return ret; +} + +/** + * @brief Difference in percentage of the effective ODR (and timestamp rate) with respect to the typical. Step: 0.13%. 8-bit format, 2's complement.[get] + * + * @param ctx read / write interface definitions + * @param val Difference in percentage of the effective ODR (and timestamp rate) with respect to the typical. Step: 0.13%. 8-bit format, 2's complement. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_odr_cal_reg_get(lsm6dsv16x_ctx_t *ctx, int8_t *val) +{ + lsm6dsv16x_internal_freq_t internal_freq; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_INTERNAL_FREQ, (uint8_t *)&internal_freq, 1); + *val = (int8_t)internal_freq.freq_fine; + + return ret; +} + +/** + * @brief Write buffer in a page.[set] + * + * @param ctx read / write interface definitions + * @param val Write buffer in a page. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_ln_pg_write(lsm6dsv16x_ctx_t *ctx, uint16_t address, uint8_t *buf, uint8_t len) +{ + lsm6dsv16x_page_address_t page_address; + lsm6dsv16x_page_sel_t page_sel; + lsm6dsv16x_page_rw_t page_rw; + uint8_t msb; + uint8_t lsb; + int32_t ret; + uint8_t i ; + + msb = ((uint8_t)(address >> 8) & 0x0FU); + lsb = (uint8_t)address & 0xFFU; + + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK); + + if (ret == 0) { + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_PAGE_RW, (uint8_t *)&page_rw, 1); + } + + if (ret == 0) { + page_rw.page_read = PROPERTY_DISABLE; + page_rw.page_write = PROPERTY_ENABLE; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_PAGE_RW, (uint8_t *)&page_rw, 1); + } + if (ret == 0) { + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_PAGE_SEL, (uint8_t *)&page_sel, 1); + } + if (ret == 0) { + page_sel.page_sel = msb; + page_sel.not_used0 = 1; // Default value + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_PAGE_SEL, (uint8_t *)&page_sel, 1); + } + if (ret == 0) { + page_address.page_addr = lsb; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_PAGE_ADDRESS, (uint8_t *)&page_address, 1); + } + + if (ret == 0) { + for (i = 0; ((i < len) && (ret == 0)); i++) { + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_PAGE_RW, &buf[i], 1); + lsb++; + + /* Check if page wrap */ + if (((lsb & 0xFFU) == 0x00U) && (ret == 0)) { + msb++; + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_PAGE_SEL, (uint8_t *)&page_sel, 1); + + if (ret == 0) { + page_sel.page_sel = msb; + page_sel.not_used0 = 1; // Default value + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_PAGE_SEL, (uint8_t *)&page_sel, 1); + } + } + } + } + + if (ret == 0) { + page_sel.page_sel = 0; + page_sel.not_used0 = 1;// Default value + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_PAGE_SEL, (uint8_t *)&page_sel, 1); + } + + if (ret == 0) { + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_PAGE_RW, (uint8_t *)&page_rw, 1); + } + if (ret == 0) { + page_rw.page_read = PROPERTY_DISABLE; + page_rw.page_write = PROPERTY_DISABLE; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_PAGE_RW, (uint8_t *)&page_rw, 1); + } + + if (ret == 0) { + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + } + + return ret; +} + +/** + * @defgroup Common + * @brief This section groups common useful functions. + * @{/ + * + */ + +/** + * @brief Write buffer in a page.[set] + * + * @param ctx read / write interface definitions + * @param val Write buffer in a page. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_ln_pg_read(lsm6dsv16x_ctx_t *ctx, uint16_t address, uint8_t *buf, uint8_t len) +{ + lsm6dsv16x_page_address_t page_address; + lsm6dsv16x_page_sel_t page_sel; + lsm6dsv16x_page_rw_t page_rw; + uint8_t msb; + uint8_t lsb; + int32_t ret; + uint8_t i ; + + msb = ((uint8_t)(address >> 8) & 0x0FU); + lsb = (uint8_t)address & 0xFFU; + + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK); + + if (ret == 0) { + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_PAGE_RW, (uint8_t *)&page_rw, 1); + } + + if (ret == 0) { + page_rw.page_read = PROPERTY_ENABLE; + page_rw.page_write = PROPERTY_DISABLE; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_PAGE_RW, (uint8_t *)&page_rw, 1); + } + if (ret == 0) { + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_PAGE_SEL, (uint8_t *)&page_sel, 1); + } + if (ret == 0) { + page_sel.page_sel = msb; + page_sel.not_used0 = 1; // Default value + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_PAGE_SEL, (uint8_t *)&page_sel, 1); + } + if (ret == 0) { + page_address.page_addr = lsb; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_PAGE_ADDRESS, (uint8_t *)&page_address, 1); + } + + if (ret == 0) { + for (i = 0; ((i < len) && (ret == 0)); i++) { + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_PAGE_RW, &buf[i], 1); + lsb++; + + /* Check if page wrap */ + if (((lsb & 0xFFU) == 0x00U) && (ret == 0)) { + msb++; + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_PAGE_SEL, (uint8_t *)&page_sel, 1); + + if (ret == 0) { + page_sel.page_sel = msb; + page_sel.not_used0 = 1; // Default value + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_PAGE_SEL, (uint8_t *)&page_sel, 1); + } + } + } + } + + if (ret == 0) { + page_sel.page_sel = 0; + page_sel.not_used0 = 1;// Default value + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_PAGE_SEL, (uint8_t *)&page_sel, 1); + } + + if (ret == 0) { + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_PAGE_RW, (uint8_t *)&page_rw, 1); + } + if (ret == 0) { + page_rw.page_read = PROPERTY_DISABLE; + page_rw.page_write = PROPERTY_DISABLE; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_PAGE_RW, (uint8_t *)&page_rw, 1); + } + + if (ret == 0) { + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + } + + return ret; +} + +/** + * @} + * + */ + +/** + * @defgroup Data ENable (DEN) + * @brief This section groups all the functions concerning + * DEN functionality. + * @{ + * + */ + +/** + * @brief It changes the polarity of INT2 pin input trigger for data enable (DEN) or embedded functions.[set] + * + * @param ctx read / write interface definitions + * @param val DEN_ACT_LOW, DEN_ACT_HIGH, + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_den_polarity_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_den_polarity_t val) +{ + lsm6dsv16x_ctrl4_t ctrl4; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL4, (uint8_t *)&ctrl4, 1); + + if (ret == 0) { + ctrl4.int2_in_lh = (uint8_t)val & 0x1U; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_CTRL4, (uint8_t *)&ctrl4, 1); + } + + return ret; +} + +/** + * @brief It changes the polarity of INT2 pin input trigger for data enable (DEN) or embedded functions.[get] + * + * @param ctx read / write interface definitions + * @param val DEN_ACT_LOW, DEN_ACT_HIGH, + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_den_polarity_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_den_polarity_t *val) +{ + lsm6dsv16x_ctrl4_t ctrl4; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL4, (uint8_t *)&ctrl4, 1); + switch (ctrl4.int2_in_lh) { + case LSM6DSV16X_DEN_ACT_LOW: + *val = LSM6DSV16X_DEN_ACT_LOW; + break; + + case LSM6DSV16X_DEN_ACT_HIGH: + *val = LSM6DSV16X_DEN_ACT_HIGH; + break; + + default: + *val = LSM6DSV16X_DEN_ACT_LOW; + break; + } + return ret; +} + +/** + * @brief Data ENable (DEN) configuration.[set] + * + * @param ctx read / write interface definitions + * @param val Data ENable (DEN) configuration. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_den_conf_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_den_conf_t val) +{ + lsm6dsv16x_den_t den; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_DEN, (uint8_t *)&den, 1); + if (ret == 0) { + den.den_z = val.den_z; + den.den_y = val.den_y; + den.den_x = val.den_x; + + den.lvl2_en = (uint8_t)val.mode & 0x1U; + den.lvl1_en = ((uint8_t)val.mode & 0x2U) >> 1; + + if (val.stamp_in_gy_data == PROPERTY_ENABLE && val.stamp_in_xl_data == PROPERTY_ENABLE) { + den.den_xl_g = PROPERTY_DISABLE; + den.den_xl_en = PROPERTY_ENABLE; + } else if (val.stamp_in_gy_data == PROPERTY_ENABLE && val.stamp_in_xl_data == PROPERTY_DISABLE) { + den.den_xl_g = PROPERTY_DISABLE; + den.den_xl_en = PROPERTY_DISABLE; + } else if (val.stamp_in_gy_data == PROPERTY_DISABLE && val.stamp_in_xl_data == PROPERTY_ENABLE) { + den.den_xl_g = PROPERTY_ENABLE; + den.den_xl_en = PROPERTY_DISABLE; + } else { + den.den_xl_g = PROPERTY_DISABLE; + den.den_xl_en = PROPERTY_DISABLE; + den.den_z = PROPERTY_DISABLE; + den.den_y = PROPERTY_DISABLE; + den.den_x = PROPERTY_DISABLE; + den.lvl2_en = PROPERTY_DISABLE; + den.lvl1_en = PROPERTY_DISABLE; + } + + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_DEN, (uint8_t *)&den, 1); + } + + return ret; +} + + +/** + * @brief Data ENable (DEN) configuration.[get] + * + * @param ctx read / write interface definitions + * @param val Data ENable (DEN) configuration. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_den_conf_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_den_conf_t *val) +{ + lsm6dsv16x_den_t den; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_DEN, (uint8_t *)&den, 1); + + val->den_z = den.den_z; + val->den_y = den.den_y; + val->den_x = den.den_x; + + if ((den.den_z | den.den_z | den.den_z) == PROPERTY_ENABLE) { + if (den.den_xl_g == PROPERTY_DISABLE && den.den_xl_en == PROPERTY_ENABLE) { + val->stamp_in_gy_data = PROPERTY_ENABLE; + val->stamp_in_xl_data = PROPERTY_ENABLE; + } else if (den.den_xl_g == PROPERTY_DISABLE && den.den_xl_en == PROPERTY_DISABLE) { + val->stamp_in_gy_data = PROPERTY_ENABLE; + val->stamp_in_xl_data = PROPERTY_DISABLE; + } else { // ( (den.den_xl_g & !den.den_xl_en) == PROPERTY_ENABLE ) + val->stamp_in_gy_data = PROPERTY_DISABLE; + val->stamp_in_xl_data = PROPERTY_ENABLE; + } + } else { + val->stamp_in_gy_data = PROPERTY_DISABLE; + val->stamp_in_xl_data = PROPERTY_DISABLE; + } + + switch ((den.lvl1_en << 1) + den.lvl2_en) { + case LEVEL_TRIGGER: + val->mode = LEVEL_TRIGGER; + break; + + case LEVEL_LETCHED: + val->mode = LEVEL_LETCHED; + break; + + default: + val->mode = DEN_NOT_DEFINED; + break; + } + + return ret; +} + +/** + * @} + * + */ + +/** + * @defgroup Electronic Image Stabilization (EIS) + * @brief Electronic Image Stabilization (EIS) + * @{/ + * + */ + +/** + * @brief Gyroscope full-scale selection for EIS channel. WARNING: 4000dps will be available only if also User Interface chain is set to 4000dps[set] + * + * @param ctx read / write interface definitions + * @param val 125dps, 250dps, 500dps, 1000dps, 2000dps, 4000dps, + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_eis_gy_full_scale_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_eis_gy_full_scale_t val) +{ + lsm6dsv16x_ctrl_eis_t ctrl_eis; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL_EIS, (uint8_t *)&ctrl_eis, 1); + + if (ret == 0) { + ctrl_eis.fs_g_eis = (uint8_t)val & 0x7U; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_CTRL_EIS, (uint8_t *)&ctrl_eis, 1); + } + + return ret; +} + +/** + * @brief Gyroscope full-scale selection for EIS channel. WARNING: 4000dps will be available only if also User Interface chain is set to 4000dps[get] + * + * @param ctx read / write interface definitions + * @param val 125dps, 250dps, 500dps, 1000dps, 2000dps + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_eis_gy_full_scale_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_eis_gy_full_scale_t *val) +{ + lsm6dsv16x_ctrl_eis_t ctrl_eis; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL_EIS, (uint8_t *)&ctrl_eis, 1); + + switch (ctrl_eis.fs_g_eis) { + case LSM6DSV16X_EIS_125dps: + *val = LSM6DSV16X_EIS_125dps; + break; + + case LSM6DSV16X_EIS_250dps: + *val = LSM6DSV16X_EIS_250dps; + break; + + case LSM6DSV16X_EIS_500dps: + *val = LSM6DSV16X_EIS_500dps; + break; + + case LSM6DSV16X_EIS_1000dps: + *val = LSM6DSV16X_EIS_1000dps; + break; + + case LSM6DSV16X_EIS_2000dps: + *val = LSM6DSV16X_EIS_2000dps; + break; + + default: + *val = LSM6DSV16X_EIS_125dps; + break; + } + return ret; +} + +/** + * @brief Enables routing of gyroscope EIS outputs on SPI2 (OIS interface). The gyroscope data on SPI2 (OIS interface) cannot be read from User Interface (UI).[set] + * + * @param ctx read / write interface definitions + * @param val Enables routing of gyroscope EIS outputs on SPI2 (OIS interface). The gyroscope data on SPI2 (OIS interface) cannot be read from User Interface (UI). + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_eis_gy_on_spi2_set(lsm6dsv16x_ctx_t *ctx, uint8_t val) +{ + lsm6dsv16x_ctrl_eis_t ctrl_eis; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL_EIS, (uint8_t *)&ctrl_eis, 1); + + if (ret == 0) { + ctrl_eis.g_eis_on_g_ois_out_reg = val; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_CTRL_EIS, (uint8_t *)&ctrl_eis, 1); + } + + return ret; +} + +/** + * @brief Enables routing of gyroscope EIS outputs on SPI2 (OIS interface). The gyroscope data on SPI2 (OIS interface) cannot be read from User Interface (UI).[get] + * + * @param ctx read / write interface definitions + * @param val Enables routing of gyroscope EIS outputs on SPI2 (OIS interface). The gyroscope data on SPI2 (OIS interface) cannot be read from User Interface (UI). + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_eis_gy_on_spi2_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val) +{ + lsm6dsv16x_ctrl_eis_t ctrl_eis; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL_EIS, (uint8_t *)&ctrl_eis, 1); + *val = ctrl_eis.g_eis_on_g_ois_out_reg; + + return ret; +} + +/** + * @brief Enables and selects the ODR of the gyroscope EIS channel.[set] + * + * @param ctx read / write interface definitions + * @param val EIS_1920Hz, EIS_960Hz, + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_gy_eis_data_rate_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_gy_eis_data_rate_t val) +{ + lsm6dsv16x_ctrl_eis_t ctrl_eis; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL_EIS, (uint8_t *)&ctrl_eis, 1); + + if (ret == 0) { + ctrl_eis.odr_g_eis = (uint8_t)val & 0x03U; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_CTRL_EIS, (uint8_t *)&ctrl_eis, 1); + } + + return ret; +} + +/** + * @brief Enables and selects the ODR of the gyroscope EIS channel.[get] + * + * @param ctx read / write interface definitions + * @param val EIS_1920Hz, EIS_960Hz, + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_gy_eis_data_rate_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_gy_eis_data_rate_t *val) +{ + lsm6dsv16x_ctrl_eis_t ctrl_eis; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL_EIS, (uint8_t *)&ctrl_eis, 1); + + switch (ctrl_eis.odr_g_eis) { + case LSM6DSV16X_EIS_ODR_OFF: + *val = LSM6DSV16X_EIS_ODR_OFF; + break; + + case LSM6DSV16X_EIS_1920Hz: + *val = LSM6DSV16X_EIS_1920Hz; + break; + + case LSM6DSV16X_EIS_960Hz: + *val = LSM6DSV16X_EIS_960Hz; + break; + + default: + *val = LSM6DSV16X_EIS_1920Hz; + break; + } + return ret; +} + +/** + * @} + * + */ + +/** + * @defgroup FIFO + * @brief This section group all the functions concerning the FIFO usage + * @{ + * + */ + +/** + * @brief FIFO watermark threshold (1 LSb = TAG (1 Byte) + 1 sensor (6 Bytes) written in FIFO).[set] + * + * @param ctx read / write interface definitions + * @param val FIFO watermark threshold (1 LSb = TAG (1 Byte) + 1 sensor (6 Bytes) written in FIFO). + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_fifo_watermark_set(lsm6dsv16x_ctx_t *ctx, uint8_t val) +{ + lsm6dsv16x_fifo_ctrl1_t fifo_ctrl1; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FIFO_CTRL1, (uint8_t *)&fifo_ctrl1, 1); + + if (ret == 0) { + fifo_ctrl1.wtm = val; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_FIFO_CTRL1, (uint8_t *)&fifo_ctrl1, 1); + } + + return ret; +} + +/** + * @brief FIFO watermark threshold (1 LSb = TAG (1 Byte) + 1 sensor (6 Bytes) written in FIFO).[get] + * + * @param ctx read / write interface definitions + * @param val FIFO watermark threshold (1 LSb = TAG (1 Byte) + 1 sensor (6 Bytes) written in FIFO). + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_fifo_watermark_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val) +{ + lsm6dsv16x_fifo_ctrl1_t fifo_ctrl1; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FIFO_CTRL1, (uint8_t *)&fifo_ctrl1, 1); + *val = fifo_ctrl1.wtm; + + return ret; +} + +/** + * @brief When dual channel mode is enabled, this function enables FSM-triggered batching in FIFO of accelerometer channel 2.[set] + * + * @param ctx read / write interface definitions + * @param val When dual channel mode is enabled, this function enables FSM-triggered batching in FIFO of accelerometer channel 2. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_fifo_xl_dual_fsm_batch_set(lsm6dsv16x_ctx_t *ctx, uint8_t val) +{ + lsm6dsv16x_fifo_ctrl2_t fifo_ctrl2; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FIFO_CTRL2, (uint8_t *)&fifo_ctrl2, 1); + if (ret == 0) { + fifo_ctrl2.xl_dualc_batch_from_fsm = val; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_FIFO_CTRL2, (uint8_t *)&fifo_ctrl2, 1); + } + + return ret; +} + +/** + * @brief When dual channel mode is enabled, this function enables FSM-triggered batching in FIFO of accelerometer channel 2.[get] + * + * @param ctx read / write interface definitions + * @param val When dual channel mode is enabled, this function enables FSM-triggered batching in FIFO of accelerometer channel 2. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_fifo_xl_dual_fsm_batch_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val) +{ + lsm6dsv16x_fifo_ctrl2_t fifo_ctrl2; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FIFO_CTRL2, (uint8_t *)&fifo_ctrl2, 1); + *val = fifo_ctrl2.xl_dualc_batch_from_fsm; + + + return ret; +} + +/** + * @brief It configures the compression algorithm to write non-compressed data at each rate.[set] + * + * @param ctx read / write interface definitions + * @param val CMP_DISABLE, CMP_ALWAYS, CMP_8_TO_1, CMP_16_TO_1, CMP_32_TO_1, + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_fifo_compress_algo_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fifo_compress_algo_t val) +{ + lsm6dsv16x_fifo_ctrl2_t fifo_ctrl2; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FIFO_CTRL2, (uint8_t *)&fifo_ctrl2, 1); + if (ret == 0) { + fifo_ctrl2.uncompr_rate = (uint8_t)val & 0x03U; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_FIFO_CTRL2, (uint8_t *)&fifo_ctrl2, 1); + } + + return ret; +} + +/** + * @brief It configures the compression algorithm to write non-compressed data at each rate.[get] + * + * @param ctx read / write interface definitions + * @param val CMP_DISABLE, CMP_ALWAYS, CMP_8_TO_1, CMP_16_TO_1, CMP_32_TO_1, + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_fifo_compress_algo_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fifo_compress_algo_t *val) +{ + lsm6dsv16x_fifo_ctrl2_t fifo_ctrl2; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FIFO_CTRL2, (uint8_t *)&fifo_ctrl2, 1); + switch (fifo_ctrl2.uncompr_rate) { + case LSM6DSV16X_CMP_DISABLE: + *val = LSM6DSV16X_CMP_DISABLE; + break; + + case LSM6DSV16X_CMP_8_TO_1: + *val = LSM6DSV16X_CMP_8_TO_1; + break; + + case LSM6DSV16X_CMP_16_TO_1: + *val = LSM6DSV16X_CMP_16_TO_1; + break; + + case LSM6DSV16X_CMP_32_TO_1: + *val = LSM6DSV16X_CMP_32_TO_1; + break; + + default: + *val = LSM6DSV16X_CMP_DISABLE; + break; + } + return ret; +} + +/** + * @brief Enables ODR CHANGE virtual sensor to be batched in FIFO.[set] + * + * @param ctx read / write interface definitions + * @param val Enables ODR CHANGE virtual sensor to be batched in FIFO. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_fifo_virtual_sens_odr_chg_set(lsm6dsv16x_ctx_t *ctx, uint8_t val) +{ + lsm6dsv16x_fifo_ctrl2_t fifo_ctrl2; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FIFO_CTRL2, (uint8_t *)&fifo_ctrl2, 1); + if (ret == 0) { + fifo_ctrl2.odr_chg_en = val; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_FIFO_CTRL2, (uint8_t *)&fifo_ctrl2, 1); + } + + return ret; +} + +/** + * @brief Enables ODR CHANGE virtual sensor to be batched in FIFO.[get] + * + * @param ctx read / write interface definitions + * @param val Enables ODR CHANGE virtual sensor to be batched in FIFO. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_fifo_virtual_sens_odr_chg_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val) +{ + lsm6dsv16x_fifo_ctrl2_t fifo_ctrl2; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FIFO_CTRL2, (uint8_t *)&fifo_ctrl2, 1); + *val = fifo_ctrl2.odr_chg_en; + + return ret; +} + +/** + * @brief Enables/Disables compression algorithm runtime.[set] + * + * @param ctx read / write interface definitions + * @param val Enables/Disables compression algorithm runtime. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_fifo_compress_algo_real_time_set(lsm6dsv16x_ctx_t *ctx, uint8_t val) +{ + lsm6dsv16x_emb_func_en_b_t emb_func_en_b; + lsm6dsv16x_fifo_ctrl2_t fifo_ctrl2; + + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FIFO_CTRL2, (uint8_t *)&fifo_ctrl2, 1); + if (ret == 0) { + fifo_ctrl2.fifo_compr_rt_en = val; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_FIFO_CTRL2, (uint8_t *)&fifo_ctrl2, 1); + } + + if (ret == 0) { + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK); + } + if (ret == 0) { + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_EMB_FUNC_EN_B, (uint8_t *)&emb_func_en_b, 1); + } + if (ret == 0) { + emb_func_en_b.fifo_compr_en = val; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_EMB_FUNC_EN_B, (uint8_t *)&emb_func_en_b, 1); + } + if (ret == 0) { + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + } + + return ret; +} + +/** + * @brief Enables/Disables compression algorithm runtime.[get] + * + * @param ctx read / write interface definitions + * @param val Enables/Disables compression algorithm runtime. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_fifo_compress_algo_real_time_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val) +{ + lsm6dsv16x_fifo_ctrl2_t fifo_ctrl2; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FIFO_CTRL2, (uint8_t *)&fifo_ctrl2, 1); + + *val = fifo_ctrl2.fifo_compr_rt_en; + + return ret; +} + +/** + * @brief Sensing chain FIFO stop values memorization at threshold level.[set] + * + * @param ctx read / write interface definitions + * @param val Sensing chain FIFO stop values memorization at threshold level. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_fifo_stop_on_wtm_set(lsm6dsv16x_ctx_t *ctx, uint8_t val) +{ + lsm6dsv16x_fifo_ctrl2_t fifo_ctrl2; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FIFO_CTRL2, (uint8_t *)&fifo_ctrl2, 1); + if (ret == 0) { + fifo_ctrl2.stop_on_wtm = val; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_FIFO_CTRL2, (uint8_t *)&fifo_ctrl2, 1); + } + + return ret; +} + +/** + * @brief Sensing chain FIFO stop values memorization at threshold level.[get] + * + * @param ctx read / write interface definitions + * @param val Sensing chain FIFO stop values memorization at threshold level. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_fifo_stop_on_wtm_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val) +{ + lsm6dsv16x_fifo_ctrl2_t fifo_ctrl2; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FIFO_CTRL2, (uint8_t *)&fifo_ctrl2, 1); + *val = fifo_ctrl2.stop_on_wtm; + + return ret; +} + +/** + * @brief Selects Batch Data Rate (write frequency in FIFO) for accelerometer data.[set] + * + * @param ctx read / write interface definitions + * @param val XL_NOT_BATCHED, XL_BATCHED_AT_1Hz875, XL_BATCHED_AT_7Hz5, XL_BATCHED_AT_15Hz, XL_BATCHED_AT_30Hz, XL_BATCHED_AT_60Hz, XL_BATCHED_AT_120Hz, XL_BATCHED_AT_240Hz, XL_BATCHED_AT_480Hz, XL_BATCHED_AT_960Hz, XL_BATCHED_AT_1920Hz, XL_BATCHED_AT_3840Hz, XL_BATCHED_AT_7680Hz, + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_fifo_xl_batch_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fifo_xl_batch_t val) +{ + lsm6dsv16x_fifo_ctrl3_t fifo_ctrl3; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FIFO_CTRL3, (uint8_t *)&fifo_ctrl3, 1); + if (ret == 0) { + fifo_ctrl3.bdr_xl = (uint8_t)val & 0xFu; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_FIFO_CTRL3, (uint8_t *)&fifo_ctrl3, 1); + } + + return ret; +} + +/** + * @brief Selects Batch Data Rate (write frequency in FIFO) for accelerometer data.[get] + * + * @param ctx read / write interface definitions + * @param val XL_NOT_BATCHED, XL_BATCHED_AT_1Hz875, XL_BATCHED_AT_7Hz5, XL_BATCHED_AT_15Hz, XL_BATCHED_AT_30Hz, XL_BATCHED_AT_60Hz, XL_BATCHED_AT_120Hz, XL_BATCHED_AT_240Hz, XL_BATCHED_AT_480Hz, XL_BATCHED_AT_960Hz, XL_BATCHED_AT_1920Hz, XL_BATCHED_AT_3840Hz, XL_BATCHED_AT_7680Hz, + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_fifo_xl_batch_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fifo_xl_batch_t *val) +{ + lsm6dsv16x_fifo_ctrl3_t fifo_ctrl3; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FIFO_CTRL3, (uint8_t *)&fifo_ctrl3, 1); + switch (fifo_ctrl3.bdr_xl) { + case LSM6DSV16X_XL_NOT_BATCHED: + *val = LSM6DSV16X_XL_NOT_BATCHED; + break; + + case LSM6DSV16X_XL_BATCHED_AT_1Hz875: + *val = LSM6DSV16X_XL_BATCHED_AT_1Hz875; + break; + + case LSM6DSV16X_XL_BATCHED_AT_7Hz5: + *val = LSM6DSV16X_XL_BATCHED_AT_7Hz5; + break; + + case LSM6DSV16X_XL_BATCHED_AT_15Hz: + *val = LSM6DSV16X_XL_BATCHED_AT_15Hz; + break; + + case LSM6DSV16X_XL_BATCHED_AT_30Hz: + *val = LSM6DSV16X_XL_BATCHED_AT_30Hz; + break; + + case LSM6DSV16X_XL_BATCHED_AT_60Hz: + *val = LSM6DSV16X_XL_BATCHED_AT_60Hz; + break; + + case LSM6DSV16X_XL_BATCHED_AT_120Hz: + *val = LSM6DSV16X_XL_BATCHED_AT_120Hz; + break; + + case LSM6DSV16X_XL_BATCHED_AT_240Hz: + *val = LSM6DSV16X_XL_BATCHED_AT_240Hz; + break; + + case LSM6DSV16X_XL_BATCHED_AT_480Hz: + *val = LSM6DSV16X_XL_BATCHED_AT_480Hz; + break; + + case LSM6DSV16X_XL_BATCHED_AT_960Hz: + *val = LSM6DSV16X_XL_BATCHED_AT_960Hz; + break; + + case LSM6DSV16X_XL_BATCHED_AT_1920Hz: + *val = LSM6DSV16X_XL_BATCHED_AT_1920Hz; + break; + + case LSM6DSV16X_XL_BATCHED_AT_3840Hz: + *val = LSM6DSV16X_XL_BATCHED_AT_3840Hz; + break; + + case LSM6DSV16X_XL_BATCHED_AT_7680Hz: + *val = LSM6DSV16X_XL_BATCHED_AT_7680Hz; + break; + + default: + *val = LSM6DSV16X_XL_NOT_BATCHED; + break; + } + return ret; +} + +/** + * @brief Selects Batch Data Rate (write frequency in FIFO) for gyroscope data.[set] + * + * @param ctx read / write interface definitions + * @param val GY_NOT_BATCHED, GY_BATCHED_AT_1Hz875, GY_BATCHED_AT_7Hz5, GY_BATCHED_AT_15Hz, GY_BATCHED_AT_30Hz, GY_BATCHED_AT_60Hz, GY_BATCHED_AT_120Hz, GY_BATCHED_AT_240Hz, GY_BATCHED_AT_480Hz, GY_BATCHED_AT_960Hz, GY_BATCHED_AT_1920Hz, GY_BATCHED_AT_3840Hz, GY_BATCHED_AT_7680Hz, + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_fifo_gy_batch_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fifo_gy_batch_t val) +{ + lsm6dsv16x_fifo_ctrl3_t fifo_ctrl3; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FIFO_CTRL3, (uint8_t *)&fifo_ctrl3, 1); + if (ret == 0) { + fifo_ctrl3.bdr_gy = (uint8_t)val & 0x0Fu; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_FIFO_CTRL3, (uint8_t *)&fifo_ctrl3, 1); + } + + return ret; +} + +/** + * @brief Selects Batch Data Rate (write frequency in FIFO) for gyroscope data.[get] + * + * @param ctx read / write interface definitions + * @param val GY_NOT_BATCHED, GY_BATCHED_AT_1Hz875, GY_BATCHED_AT_7Hz5, GY_BATCHED_AT_15Hz, GY_BATCHED_AT_30Hz, GY_BATCHED_AT_60Hz, GY_BATCHED_AT_120Hz, GY_BATCHED_AT_240Hz, GY_BATCHED_AT_480Hz, GY_BATCHED_AT_960Hz, GY_BATCHED_AT_1920Hz, GY_BATCHED_AT_3840Hz, GY_BATCHED_AT_7680Hz, + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_fifo_gy_batch_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fifo_gy_batch_t *val) +{ + lsm6dsv16x_fifo_ctrl3_t fifo_ctrl3; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FIFO_CTRL3, (uint8_t *)&fifo_ctrl3, 1); + switch (fifo_ctrl3.bdr_gy) { + case LSM6DSV16X_GY_NOT_BATCHED: + *val = LSM6DSV16X_GY_NOT_BATCHED; + break; + + case LSM6DSV16X_GY_BATCHED_AT_1Hz875: + *val = LSM6DSV16X_GY_BATCHED_AT_1Hz875; + break; + + case LSM6DSV16X_GY_BATCHED_AT_7Hz5: + *val = LSM6DSV16X_GY_BATCHED_AT_7Hz5; + break; + + case LSM6DSV16X_GY_BATCHED_AT_15Hz: + *val = LSM6DSV16X_GY_BATCHED_AT_15Hz; + break; + + case LSM6DSV16X_GY_BATCHED_AT_30Hz: + *val = LSM6DSV16X_GY_BATCHED_AT_30Hz; + break; + + case LSM6DSV16X_GY_BATCHED_AT_60Hz: + *val = LSM6DSV16X_GY_BATCHED_AT_60Hz; + break; + + case LSM6DSV16X_GY_BATCHED_AT_120Hz: + *val = LSM6DSV16X_GY_BATCHED_AT_120Hz; + break; + + case LSM6DSV16X_GY_BATCHED_AT_240Hz: + *val = LSM6DSV16X_GY_BATCHED_AT_240Hz; + break; + + case LSM6DSV16X_GY_BATCHED_AT_480Hz: + *val = LSM6DSV16X_GY_BATCHED_AT_480Hz; + break; + + case LSM6DSV16X_GY_BATCHED_AT_960Hz: + *val = LSM6DSV16X_GY_BATCHED_AT_960Hz; + break; + + case LSM6DSV16X_GY_BATCHED_AT_1920Hz: + *val = LSM6DSV16X_GY_BATCHED_AT_1920Hz; + break; + + case LSM6DSV16X_GY_BATCHED_AT_3840Hz: + *val = LSM6DSV16X_GY_BATCHED_AT_3840Hz; + break; + + case LSM6DSV16X_GY_BATCHED_AT_7680Hz: + *val = LSM6DSV16X_GY_BATCHED_AT_7680Hz; + break; + + default: + *val = LSM6DSV16X_GY_NOT_BATCHED; + break; + } + return ret; +} + + +/** + * @brief FIFO mode selection.[set] + * + * @param ctx read / write interface definitions + * @param val BYPASS_MODE, FIFO_MODE, STREAM_TO_FIFO_MODE, BYPASS_TO_STREAM_MODE, STREAM_MODE, BYPASS_TO_FIFO_MODE, + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_fifo_mode_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fifo_mode_t val) +{ + lsm6dsv16x_fifo_ctrl4_t fifo_ctrl4; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FIFO_CTRL4, (uint8_t *)&fifo_ctrl4, 1); + if (ret == 0) { + fifo_ctrl4.fifo_mode = (uint8_t)val & 0x07U; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_FIFO_CTRL4, (uint8_t *)&fifo_ctrl4, 1); + } + + return ret; +} + +/** + * @brief FIFO mode selection.[get] + * + * @param ctx read / write interface definitions + * @param val BYPASS_MODE, FIFO_MODE, STREAM_TO_FIFO_MODE, BYPASS_TO_STREAM_MODE, STREAM_MODE, BYPASS_TO_FIFO_MODE, + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_fifo_mode_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fifo_mode_t *val) +{ + lsm6dsv16x_fifo_ctrl4_t fifo_ctrl4; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FIFO_CTRL4, (uint8_t *)&fifo_ctrl4, 1); + switch (fifo_ctrl4.fifo_mode) { + case LSM6DSV16X_BYPASS_MODE: + *val = LSM6DSV16X_BYPASS_MODE; + break; + + case LSM6DSV16X_FIFO_MODE: + *val = LSM6DSV16X_FIFO_MODE; + break; + + case LSM6DSV16X_STREAM_WTM_TO_FULL_MODE: + *val = LSM6DSV16X_STREAM_WTM_TO_FULL_MODE; + break; + + case LSM6DSV16X_STREAM_TO_FIFO_MODE: + *val = LSM6DSV16X_STREAM_TO_FIFO_MODE; + break; + + case LSM6DSV16X_BYPASS_TO_STREAM_MODE: + *val = LSM6DSV16X_BYPASS_TO_STREAM_MODE; + break; + + case LSM6DSV16X_STREAM_MODE: + *val = LSM6DSV16X_STREAM_MODE; + break; + + case LSM6DSV16X_BYPASS_TO_FIFO_MODE: + *val = LSM6DSV16X_BYPASS_TO_FIFO_MODE; + break; + + default: + *val = LSM6DSV16X_BYPASS_MODE; + break; + } + return ret; +} + +/** + * @brief Enables FIFO batching of EIS gyroscope output values.[set] + * + * @param ctx read / write interface definitions + * @param val Enables FIFO batching of EIS gyroscope output values. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_fifo_gy_eis_batch_set(lsm6dsv16x_ctx_t *ctx, uint8_t val) +{ + lsm6dsv16x_fifo_ctrl4_t fifo_ctrl4; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FIFO_CTRL4, (uint8_t *)&fifo_ctrl4, 1); + if (ret == 0) { + fifo_ctrl4.g_eis_fifo_en = val; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_FIFO_CTRL4, (uint8_t *)&fifo_ctrl4, 1); + } + + return ret; +} + +/** + * @brief Enables FIFO batching of EIS gyroscope output values.[get] + * + * @param ctx read / write interface definitions + * @param val Enables FIFO batching of EIS gyroscope output values. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_fifo_gy_eis_batch_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val) +{ + lsm6dsv16x_fifo_ctrl4_t fifo_ctrl4; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FIFO_CTRL4, (uint8_t *)&fifo_ctrl4, 1); + *val = fifo_ctrl4.g_eis_fifo_en; + + return ret; +} + +/** + * @brief Selects batch data rate (write frequency in FIFO) for temperature data.[set] + * + * @param ctx read / write interface definitions + * @param val TEMP_NOT_BATCHED, TEMP_BATCHED_AT_1Hz875, TEMP_BATCHED_AT_15Hz, TEMP_BATCHED_AT_60Hz, + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_fifo_temp_batch_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fifo_temp_batch_t val) +{ + lsm6dsv16x_fifo_ctrl4_t fifo_ctrl4; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FIFO_CTRL4, (uint8_t *)&fifo_ctrl4, 1); + if (ret == 0) { + fifo_ctrl4.odr_t_batch = (uint8_t)val & 0x03U; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_FIFO_CTRL4, (uint8_t *)&fifo_ctrl4, 1); + } + + return ret; +} + +/** + * @brief Selects batch data rate (write frequency in FIFO) for temperature data.[get] + * + * @param ctx read / write interface definitions + * @param val TEMP_NOT_BATCHED, TEMP_BATCHED_AT_1Hz875, TEMP_BATCHED_AT_15Hz, TEMP_BATCHED_AT_60Hz, + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_fifo_temp_batch_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fifo_temp_batch_t *val) +{ + lsm6dsv16x_fifo_ctrl4_t fifo_ctrl4; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FIFO_CTRL4, (uint8_t *)&fifo_ctrl4, 1); + switch (fifo_ctrl4.odr_t_batch) { + case LSM6DSV16X_TEMP_NOT_BATCHED: + *val = LSM6DSV16X_TEMP_NOT_BATCHED; + break; + + case LSM6DSV16X_TEMP_BATCHED_AT_1Hz875: + *val = LSM6DSV16X_TEMP_BATCHED_AT_1Hz875; + break; + + case LSM6DSV16X_TEMP_BATCHED_AT_15Hz: + *val = LSM6DSV16X_TEMP_BATCHED_AT_15Hz; + break; + + case LSM6DSV16X_TEMP_BATCHED_AT_60Hz: + *val = LSM6DSV16X_TEMP_BATCHED_AT_60Hz; + break; + + default: + *val = LSM6DSV16X_TEMP_NOT_BATCHED; + break; + } + return ret; +} + +/** + * @brief Selects decimation for timestamp batching in FIFO. Write rate will be the maximum rate between XL and GYRO BDR divided by decimation decoder.[set] + * + * @param ctx read / write interface definitions + * @param val TMSTMP_NOT_BATCHED, TMSTMP_DEC_1, TMSTMP_DEC_8, TMSTMP_DEC_32, + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_fifo_timestamp_batch_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fifo_timestamp_batch_t val) +{ + lsm6dsv16x_fifo_ctrl4_t fifo_ctrl4; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FIFO_CTRL4, (uint8_t *)&fifo_ctrl4, 1); + if (ret == 0) { + fifo_ctrl4.dec_ts_batch = (uint8_t)val & 0x03U; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_FIFO_CTRL4, (uint8_t *)&fifo_ctrl4, 1); + } + + return ret; +} + +/** + * @brief Selects decimation for timestamp batching in FIFO. Write rate will be the maximum rate between XL and GYRO BDR divided by decimation decoder.[get] + * + * @param ctx read / write interface definitions + * @param val TMSTMP_NOT_BATCHED, TMSTMP_DEC_1, TMSTMP_DEC_8, TMSTMP_DEC_32, + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_fifo_timestamp_batch_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fifo_timestamp_batch_t *val) +{ + lsm6dsv16x_fifo_ctrl4_t fifo_ctrl4; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FIFO_CTRL4, (uint8_t *)&fifo_ctrl4, 1); + switch (fifo_ctrl4.dec_ts_batch) { + case LSM6DSV16X_TMSTMP_NOT_BATCHED: + *val = LSM6DSV16X_TMSTMP_NOT_BATCHED; + break; + + case LSM6DSV16X_TMSTMP_DEC_1: + *val = LSM6DSV16X_TMSTMP_DEC_1; + break; + + case LSM6DSV16X_TMSTMP_DEC_8: + *val = LSM6DSV16X_TMSTMP_DEC_8; + break; + + case LSM6DSV16X_TMSTMP_DEC_32: + *val = LSM6DSV16X_TMSTMP_DEC_32; + break; + + default: + *val = LSM6DSV16X_TMSTMP_NOT_BATCHED; + break; + } + return ret; +} + +/** + * @brief The threshold for the internal counter of batch events. When this counter reaches the threshold, the counter is reset and the interrupt flag is set to 1.[set] + * + * @param ctx read / write interface definitions + * @param val The threshold for the internal counter of batch events. When this counter reaches the threshold, the counter is reset and the interrupt flag is set to 1. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_fifo_batch_counter_threshold_set(lsm6dsv16x_ctx_t *ctx, uint16_t val) +{ + uint8_t buff[2]; + int32_t ret; + + buff[1] = (uint8_t)(val / 256U); + buff[0] = (uint8_t)(val - (buff[1] * 256U)); + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_COUNTER_BDR_REG1, (uint8_t *)&buff[0], 2); + + return ret; +} + +/** + * @brief The threshold for the internal counter of batch events. When this counter reaches the threshold, the counter is reset and the interrupt flag is set to 1.[get] + * + * @param ctx read / write interface definitions + * @param val The threshold for the internal counter of batch events. When this counter reaches the threshold, the counter is reset and the interrupt flag is set to 1. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_fifo_batch_counter_threshold_get(lsm6dsv16x_ctx_t *ctx, uint16_t *val) +{ + uint8_t buff[2]; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_COUNTER_BDR_REG1, &buff[0], 2); + *val = buff[1]; + *val = (*val * 256U) + buff[0]; + + return ret; +} + +/** + * @brief Selects the trigger for the internal counter of batch events between the accelerometer, gyroscope and EIS gyroscope.[set] + * + * @param ctx read / write interface definitions + * @param val XL_BATCH_EVENT, GY_BATCH_EVENT, GY_EIS_BATCH_EVENT, + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_fifo_batch_cnt_event_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fifo_batch_cnt_event_t val) +{ + lsm6dsv16x_counter_bdr_reg1_t counter_bdr_reg1; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_COUNTER_BDR_REG1, (uint8_t *)&counter_bdr_reg1, 1); + if (ret == 0) { + counter_bdr_reg1.trig_counter_bdr = (uint8_t)val & 0x03U; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_COUNTER_BDR_REG1, (uint8_t *)&counter_bdr_reg1, 1); + } + + return ret; +} + +/** + * @brief Selects the trigger for the internal counter of batch events between the accelerometer, gyroscope and EIS gyroscope.[get] + * + * @param ctx read / write interface definitions + * @param val XL_BATCH_EVENT, GY_BATCH_EVENT, GY_EIS_BATCH_EVENT, + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_fifo_batch_cnt_event_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fifo_batch_cnt_event_t *val) +{ + lsm6dsv16x_counter_bdr_reg1_t counter_bdr_reg1; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_COUNTER_BDR_REG1, (uint8_t *)&counter_bdr_reg1, 1); + switch (counter_bdr_reg1.trig_counter_bdr) { + case LSM6DSV16X_XL_BATCH_EVENT: + *val = LSM6DSV16X_XL_BATCH_EVENT; + break; + + case LSM6DSV16X_GY_BATCH_EVENT: + *val = LSM6DSV16X_GY_BATCH_EVENT; + break; + + case LSM6DSV16X_GY_EIS_BATCH_EVENT: + *val = LSM6DSV16X_GY_EIS_BATCH_EVENT; + break; + + default: + *val = LSM6DSV16X_XL_BATCH_EVENT; + break; + } + return ret; +} + +/** + * @brief Number of unread sensor data (TAG + 6 bytes) stored in FIFO.[get] + * + * @param ctx read / write interface definitions + * @param val Number of unread sensor data (TAG + 6 bytes) stored in FIFO. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_fifo_data_level_get(lsm6dsv16x_ctx_t *ctx, uint16_t *val) +{ + uint8_t buff[2]; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FIFO_STATUS1, &buff[0], 2); + *val = (uint16_t)buff[1] & 0x01U; + *val = (*val * 256U) + buff[0]; + + return ret; +} + +/** + * @brief FIFO data output[get] + * + * @param ctx read / write interface definitions + * @param val FIFO_EMPTY, GY_NC_TAG, XL_NC_TAG, TIMESTAMP_TAG, TEMPERATURE_TAG, CFG_CHANGE_TAG, XL_NC_T_2_TAG, XL_NC_T_1_TAG, XL_2XC_TAG, XL_3XC_TAG, GY_NC_T_2_TAG, GY_NC_T_1_TAG, GY_2XC_TAG, GY_3XC_TAG, SENSORHUB_SLAVE0_TAG, SENSORHUB_SLAVE1_TAG, SENSORHUB_SLAVE2_TAG, SENSORHUB_SLAVE3_TAG, STEP_CPUNTER_TAG, SENSORHUB_NACK_TAG, MLC_RESULT_TAG, MLC_FILTER, MLC_FEATURE, XL_DUAL_CORE, AH_QVAR, + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_fifo_out_raw_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fifo_out_raw_t *val) +{ + lsm6dsv16x_fifo_data_out_tag_t fifo_data_out_tag; + uint8_t buff[7]; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FIFO_DATA_OUT_TAG, buff, 7); + bytecpy((uint8_t *)&fifo_data_out_tag, &buff[0]); + + switch (fifo_data_out_tag.tag_sensor) { + case LSM6DSV16X_FIFO_EMPTY: + val->tag = LSM6DSV16X_FIFO_EMPTY; + break; + + case LSM6DSV16X_GY_NC_TAG: + val->tag = LSM6DSV16X_GY_NC_TAG; + break; + + case LSM6DSV16X_XL_NC_TAG: + val->tag = LSM6DSV16X_XL_NC_TAG; + break; + + case LSM6DSV16X_TIMESTAMP_TAG: + val->tag = LSM6DSV16X_TIMESTAMP_TAG; + break; + + case LSM6DSV16X_TEMPERATURE_TAG: + val->tag = LSM6DSV16X_TEMPERATURE_TAG; + break; + + case LSM6DSV16X_CFG_CHANGE_TAG: + val->tag = LSM6DSV16X_CFG_CHANGE_TAG; + break; + + case LSM6DSV16X_XL_NC_T_2_TAG: + val->tag = LSM6DSV16X_XL_NC_T_2_TAG; + break; + + case LSM6DSV16X_XL_NC_T_1_TAG: + val->tag = LSM6DSV16X_XL_NC_T_1_TAG; + break; + + case LSM6DSV16X_XL_2XC_TAG: + val->tag = LSM6DSV16X_XL_2XC_TAG; + break; + + case LSM6DSV16X_XL_3XC_TAG: + val->tag = LSM6DSV16X_XL_3XC_TAG; + break; + + case LSM6DSV16X_GY_NC_T_2_TAG: + val->tag = LSM6DSV16X_GY_NC_T_2_TAG; + break; + + case LSM6DSV16X_GY_NC_T_1_TAG: + val->tag = LSM6DSV16X_GY_NC_T_1_TAG; + break; + + case LSM6DSV16X_GY_2XC_TAG: + val->tag = LSM6DSV16X_GY_2XC_TAG; + break; + + case LSM6DSV16X_GY_3XC_TAG: + val->tag = LSM6DSV16X_GY_3XC_TAG; + break; + + case LSM6DSV16X_SENSORHUB_SLAVE0_TAG: + val->tag = LSM6DSV16X_SENSORHUB_SLAVE0_TAG; + break; + + case LSM6DSV16X_SENSORHUB_SLAVE1_TAG: + val->tag = LSM6DSV16X_SENSORHUB_SLAVE1_TAG; + break; + + case LSM6DSV16X_SENSORHUB_SLAVE2_TAG: + val->tag = LSM6DSV16X_SENSORHUB_SLAVE2_TAG; + break; + + case LSM6DSV16X_SENSORHUB_SLAVE3_TAG: + val->tag = LSM6DSV16X_SENSORHUB_SLAVE3_TAG; + break; + + case LSM6DSV16X_STEP_CPUNTER_TAG: + val->tag = LSM6DSV16X_STEP_CPUNTER_TAG; + break; + + case LSM6DSV16X_SENSORHUB_NACK_TAG: + val->tag = LSM6DSV16X_SENSORHUB_NACK_TAG; + break; + + case LSM6DSV16X_MLC_RESULT_TAG: + val->tag = LSM6DSV16X_MLC_RESULT_TAG; + break; + + case LSM6DSV16X_MLC_FILTER: + val->tag = LSM6DSV16X_MLC_FILTER; + break; + + case LSM6DSV16X_MLC_FEATURE: + val->tag = LSM6DSV16X_MLC_FEATURE; + break; + + case LSM6DSV16X_XL_DUAL_CORE: + val->tag = LSM6DSV16X_XL_DUAL_CORE; + break; + + case LSM6DSV16X_AH_QVAR: + val->tag = LSM6DSV16X_AH_QVAR; + break; + + default: + val->tag = LSM6DSV16X_FIFO_EMPTY; + break; + } + + val->cnt = fifo_data_out_tag.tag_cnt; + + val->data[0] = buff[1]; + val->data[1] = buff[2]; + val->data[2] = buff[3]; + val->data[3] = buff[4]; + val->data[4] = buff[5]; + val->data[5] = buff[6]; + + return ret; +} + +/** + * @brief Batching in FIFO buffer of step counter value.[set] + * + * @param ctx read / write interface definitions + * @param val Batching in FIFO buffer of step counter value. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_fifo_stpcnt_batch_set(lsm6dsv16x_ctx_t *ctx, uint8_t val) +{ + lsm6dsv16x_emb_func_fifo_en_a_t emb_func_fifo_en_a; + int32_t ret; + + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK); + if (ret == 0) { + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_EMB_FUNC_FIFO_EN_A, (uint8_t *)&emb_func_fifo_en_a, 1); + } + + if (ret == 0) { + emb_func_fifo_en_a.step_counter_fifo_en = val; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_EMB_FUNC_FIFO_EN_A, (uint8_t *)&emb_func_fifo_en_a, 1); + } + if (ret == 0) { + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + } + + return ret; +} + +/** + * @brief Batching in FIFO buffer of step counter value.[get] + * + * @param ctx read / write interface definitions + * @param val Batching in FIFO buffer of step counter value. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_fifo_stpcnt_batch_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val) +{ + lsm6dsv16x_emb_func_fifo_en_a_t emb_func_fifo_en_a; + int32_t ret; + + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK); + if (ret == 0) { + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_EMB_FUNC_FIFO_EN_A, (uint8_t *)&emb_func_fifo_en_a, 1); + } + + *val = emb_func_fifo_en_a.step_counter_fifo_en; + + if (ret == 0) { + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + } + + return ret; +} + +/** + * @brief Batching in FIFO buffer of machine learning core results.[set] + * + * @param ctx read / write interface definitions + * @param val Batching in FIFO buffer of machine learning core results. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_fifo_mlc_batch_set(lsm6dsv16x_ctx_t *ctx, uint8_t val) +{ + lsm6dsv16x_emb_func_fifo_en_a_t emb_func_fifo_en_a; + int32_t ret; + + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK); + if (ret == 0) { + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_EMB_FUNC_FIFO_EN_A, (uint8_t *)&emb_func_fifo_en_a, 1); + } + + if (ret == 0) { + emb_func_fifo_en_a.mlc_fifo_en = val; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_EMB_FUNC_FIFO_EN_A, (uint8_t *)&emb_func_fifo_en_a, 1); + } + if (ret == 0) { + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + } + + return ret; +} + +/** + * @brief Batching in FIFO buffer of machine learning core results.[get] + * + * @param ctx read / write interface definitions + * @param val Batching in FIFO buffer of machine learning core results. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_fifo_mlc_batch_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val) +{ + lsm6dsv16x_emb_func_fifo_en_a_t emb_func_fifo_en_a; + int32_t ret; + + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK); + if (ret == 0) { + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_EMB_FUNC_FIFO_EN_A, (uint8_t *)&emb_func_fifo_en_a, 1); + } + + *val = emb_func_fifo_en_a.mlc_fifo_en; + + if (ret == 0) { + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + } + + return ret; +} + +/** + * @brief Enables batching in FIFO buffer of machine learning core filters and features.[set] + * + * @param ctx read / write interface definitions + * @param val Enables batching in FIFO buffer of machine learning core filters and features. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_fifo_mlc_filt_batch_set(lsm6dsv16x_ctx_t *ctx, uint8_t val) +{ + lsm6dsv16x_emb_func_fifo_en_b_t emb_func_fifo_en_b; + int32_t ret; + + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK); + if (ret == 0) { + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_EMB_FUNC_FIFO_EN_B, (uint8_t *)&emb_func_fifo_en_b, 1); + } + + if (ret == 0) { + emb_func_fifo_en_b.mlc_filter_feature_fifo_en = val; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_EMB_FUNC_FIFO_EN_B, (uint8_t *)&emb_func_fifo_en_b, 1); + } + if (ret == 0) { + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + } + + return ret; +} + +/** + * @brief Enables batching in FIFO buffer of machine learning core filters and features.[get] + * + * @param ctx read / write interface definitions + * @param val Enables batching in FIFO buffer of machine learning core filters and features. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_fifo_mlc_filt_batch_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val) +{ + lsm6dsv16x_emb_func_fifo_en_b_t emb_func_fifo_en_b; + int32_t ret; + + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK); + if (ret == 0) { + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_EMB_FUNC_FIFO_EN_B, (uint8_t *)&emb_func_fifo_en_b, 1); + } + + *val = emb_func_fifo_en_b.mlc_filter_feature_fifo_en; + + if (ret == 0) { + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + } + + return ret; +} + +/** + * @brief Enable FIFO data batching of first slave.[set] + * + * @param ctx read / write interface definitions + * @param val Enable FIFO data batching of first slave. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_fifo_batch_sh_slave_0_set(lsm6dsv16x_ctx_t *ctx, uint8_t val) +{ + lsm6dsv16x_slv0_config_t slv0_config; + int32_t ret; + + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_SENSOR_HUB_MEM_BANK); + if (ret == 0) { + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_SLV0_CONFIG, (uint8_t *)&slv0_config, 1); + } + + if (ret == 0) { + slv0_config.batch_ext_sens_0_en = val; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_SLV0_CONFIG, (uint8_t *)&slv0_config, 1); + } + if (ret == 0) { + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + } + + return ret; +} + +/** + * @brief Enable FIFO data batching of first slave.[get] + * + * @param ctx read / write interface definitions + * @param val Enable FIFO data batching of first slave. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_fifo_batch_sh_slave_0_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val) +{ + lsm6dsv16x_slv0_config_t slv0_config; + int32_t ret; + + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_SENSOR_HUB_MEM_BANK); + if (ret == 0) { + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_SLV0_CONFIG, (uint8_t *)&slv0_config, 1); + } + + *val = slv0_config.batch_ext_sens_0_en; + + if (ret == 0) { + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + } + + return ret; +} + +/** + * @brief Enable FIFO data batching of second slave.[set] + * + * @param ctx read / write interface definitions + * @param val Enable FIFO data batching of second slave. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_fifo_batch_sh_slave_1_set(lsm6dsv16x_ctx_t *ctx, uint8_t val) +{ + lsm6dsv16x_slv1_config_t slv1_config; + int32_t ret; + + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_SENSOR_HUB_MEM_BANK); + if (ret == 0) { + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_SLV1_CONFIG, (uint8_t *)&slv1_config, 1); + } + + if (ret == 0) { + slv1_config.batch_ext_sens_1_en = val; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_SLV1_CONFIG, (uint8_t *)&slv1_config, 1); + } + if (ret == 0) { + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + } + + return ret; +} + +/** + * @brief Enable FIFO data batching of second slave.[get] + * + * @param ctx read / write interface definitions + * @param val Enable FIFO data batching of second slave. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_fifo_batch_sh_slave_1_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val) +{ + lsm6dsv16x_slv1_config_t slv1_config; + int32_t ret; + + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_SENSOR_HUB_MEM_BANK); + if (ret == 0) { + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_SLV1_CONFIG, (uint8_t *)&slv1_config, 1); + } + + *val = slv1_config.batch_ext_sens_1_en; + + if (ret == 0) { + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + } + + return ret; +} + +/** + * @brief Enable FIFO data batching of third slave.[set] + * + * @param ctx read / write interface definitions + * @param val Enable FIFO data batching of third slave. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_fifo_batch_sh_slave_2_set(lsm6dsv16x_ctx_t *ctx, uint8_t val) +{ + lsm6dsv16x_slv2_config_t slv2_config; + int32_t ret; + + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_SENSOR_HUB_MEM_BANK); + if (ret == 0) { + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_SLV2_CONFIG, (uint8_t *)&slv2_config, 1); + } + + if (ret == 0) { + slv2_config.batch_ext_sens_2_en = val; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_SLV2_CONFIG, (uint8_t *)&slv2_config, 1); + } + if (ret == 0) { + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + } + + return ret; +} + +/** + * @brief Enable FIFO data batching of third slave.[get] + * + * @param ctx read / write interface definitions + * @param val Enable FIFO data batching of third slave. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_fifo_batch_sh_slave_2_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val) +{ + lsm6dsv16x_slv2_config_t slv2_config; + int32_t ret; + + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_SENSOR_HUB_MEM_BANK); + if (ret == 0) { + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_SLV2_CONFIG, (uint8_t *)&slv2_config, 1); + } + + *val = slv2_config.batch_ext_sens_2_en; + + if (ret == 0) { + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + } + + return ret; +} + +/** + * @brief Enable FIFO data batching of fourth slave.[set] + * + * @param ctx read / write interface definitions + * @param val Enable FIFO data batching of fourth slave. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_fifo_batch_sh_slave_3_set(lsm6dsv16x_ctx_t *ctx, uint8_t val) +{ + lsm6dsv16x_slv3_config_t slv3_config; + int32_t ret; + + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_SENSOR_HUB_MEM_BANK); + if (ret == 0) { + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_SLV3_CONFIG, (uint8_t *)&slv3_config, 1); + } + + if (ret == 0) { + slv3_config.batch_ext_sens_3_en = val; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_SLV3_CONFIG, (uint8_t *)&slv3_config, 1); + } + if (ret == 0) { + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + } + + return ret; +} + +/** + * @brief Enable FIFO data batching of fourth slave.[get] + * + * @param ctx read / write interface definitions + * @param val Enable FIFO data batching of fourth slave. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_fifo_batch_sh_slave_3_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val) +{ + lsm6dsv16x_slv3_config_t slv3_config; + int32_t ret; + + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_SENSOR_HUB_MEM_BANK); + if (ret == 0) { + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_SLV3_CONFIG, (uint8_t *)&slv3_config, 1); + } + + *val = slv3_config.batch_ext_sens_3_en; + + if (ret == 0) { + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + } + + return ret; +} + +/** + * @} + * + */ + +/** + * @defgroup Filters + * @brief This section group all the functions concerning the + * filters configuration + * @{ + * + */ + +/** + * @brief Protocol anti-spike filters.[set] + * + * @param ctx read / write interface definitions + * @param val AUTO, ALWAYS_ACTIVE, + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_filt_anti_spike_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_filt_anti_spike_t val) +{ + lsm6dsv16x_if_cfg_t if_cfg; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_IF_CFG, (uint8_t *)&if_cfg, 1); + + if (ret == 0) { + if_cfg.asf_ctrl = (uint8_t)val & 0x01U; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_IF_CFG, (uint8_t *)&if_cfg, 1); + } + + return ret; +} + +/** + * @brief Protocol anti-spike filters.[get] + * + * @param ctx read / write interface definitions + * @param val AUTO, ALWAYS_ACTIVE, + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_filt_anti_spike_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_filt_anti_spike_t *val) +{ + lsm6dsv16x_if_cfg_t if_cfg; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_IF_CFG, (uint8_t *)&if_cfg, 1); + switch (if_cfg.asf_ctrl) { + case LSM6DSV16X_AUTO: + *val = LSM6DSV16X_AUTO; + break; + + case LSM6DSV16X_ALWAYS_ACTIVE: + *val = LSM6DSV16X_ALWAYS_ACTIVE; + break; + + default: + *val = LSM6DSV16X_AUTO; + break; + } + return ret; +} + +/** + * @brief It masks DRDY and Interrupts RQ until filter settling ends.[set] + * + * @param ctx read / write interface definitions + * @param val It masks DRDY and Interrupts RQ until filter settling ends. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_filt_settling_mask_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_filt_settling_mask_t val) +{ + lsm6dsv16x_emb_func_cfg_t emb_func_cfg; + lsm6dsv16x_ui_int_ois_t ui_int_ois; + lsm6dsv16x_ctrl4_t ctrl4; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL4, (uint8_t *)&ctrl4, 1); + + if (ret == 0) { + ctrl4.drdy_mask = val.drdy; + + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_CTRL4, (uint8_t *)&ctrl4, 1); + } + + if (ret == 0) { + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_EMB_FUNC_CFG, (uint8_t *)&emb_func_cfg, 1); + } + + if (ret == 0) { + emb_func_cfg.emb_func_irq_mask_xl_settl = val.irq_xl; + emb_func_cfg.emb_func_irq_mask_g_settl = val.irq_g; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_EMB_FUNC_CFG, (uint8_t *)&emb_func_cfg, 1); + } + + if (ret == 0) { + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_UI_INT_OIS, (uint8_t *)&ui_int_ois, 1); + } + + if (ret == 0) { + ui_int_ois.drdy_mask_ois = val.ois_drdy; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_UI_INT_OIS, (uint8_t *)&ui_int_ois, 1); + } + + return ret; +} + +/** + * @brief It masks DRDY and Interrupts RQ until filter settling ends.[get] + * + * @param ctx read / write interface definitions + * @param val It masks DRDY and Interrupts RQ until filter settling ends. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_filt_settling_mask_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_filt_settling_mask_t *val) +{ + lsm6dsv16x_emb_func_cfg_t emb_func_cfg; + lsm6dsv16x_ui_int_ois_t ui_int_ois; + lsm6dsv16x_ctrl4_t ctrl4; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL4, (uint8_t *)&ctrl4, 1); + if (ret == 0) { + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_EMB_FUNC_CFG, (uint8_t *)&emb_func_cfg, 1); + } + if (ret == 0) { + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_UI_INT_OIS, (uint8_t *)&ui_int_ois, 1); + } + + val->irq_xl = emb_func_cfg.emb_func_irq_mask_xl_settl; + val->irq_g = emb_func_cfg.emb_func_irq_mask_g_settl; + val->drdy = ctrl4.drdy_mask; + + return ret; +} + +/** + * @brief It masks DRDY and Interrupts RQ until filter settling ends.[set] + * + * @param ctx read / write interface definitions + * @param val It masks DRDY and Interrupts RQ until filter settling ends from OIS interface. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_filt_ois_settling_mask_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_filt_ois_settling_mask_t val) +{ + lsm6dsv16x_spi2_int_ois_t spi2_int_ois; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_SPI2_INT_OIS, (uint8_t *)&spi2_int_ois, 1); + + if (ret == 0) { + spi2_int_ois.drdy_mask_ois = val.ois_drdy; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_SPI2_INT_OIS, (uint8_t *)&spi2_int_ois, 1); + } + + return ret; +} + +/** + * @brief It masks DRDY and Interrupts RQ until filter settling ends.[get] + * + * @param ctx read / write interface definitions + * @param val It masks DRDY and Interrupts RQ until filter settling ends. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_filt_ois_settling_mask_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_filt_ois_settling_mask_t *val) +{ + + lsm6dsv16x_spi2_int_ois_t spi2_int_ois; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_SPI2_INT_OIS, (uint8_t *)&spi2_int_ois, 1); + val->ois_drdy = spi2_int_ois.drdy_mask_ois; + + return ret; +} + +/** + * @brief Gyroscope low-pass filter (LPF1) bandwidth selection.[set] + * + * @param ctx read / write interface definitions + * @param val GY_ULTRA_LIGHT, GY_VERY_LIGHT, GY_LIGHT, GY_MEDIUM, GY_STRONG, GY_VERY_STRONG, GY_AGGRESSIVE, GY_XTREME, + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_filt_gy_lp1_bandwidth_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_filt_gy_lp1_bandwidth_t val) +{ + lsm6dsv16x_ctrl6_t ctrl6; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL6, (uint8_t *)&ctrl6, 1); + if (ret == 0) { + ctrl6.lpf1_g_bw = (uint8_t)val & 0x0Fu; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_CTRL6, (uint8_t *)&ctrl6, 1); + } + + return ret; +} + +/** + * @brief Gyroscope low-pass filter (LPF1) bandwidth selection.[get] + * + * @param ctx read / write interface definitions + * @param val GY_ULTRA_LIGHT, GY_VERY_LIGHT, GY_LIGHT, GY_MEDIUM, GY_STRONG, GY_VERY_STRONG, GY_AGGRESSIVE, GY_XTREME, + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_filt_gy_lp1_bandwidth_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_filt_gy_lp1_bandwidth_t *val) +{ + lsm6dsv16x_ctrl6_t ctrl6; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL6, (uint8_t *)&ctrl6, 1); + switch (ctrl6.lpf1_g_bw) { + case LSM6DSV16X_GY_ULTRA_LIGHT: + *val = LSM6DSV16X_GY_ULTRA_LIGHT; + break; + + case LSM6DSV16X_GY_VERY_LIGHT: + *val = LSM6DSV16X_GY_VERY_LIGHT; + break; + + case LSM6DSV16X_GY_LIGHT: + *val = LSM6DSV16X_GY_LIGHT; + break; + + case LSM6DSV16X_GY_MEDIUM: + *val = LSM6DSV16X_GY_MEDIUM; + break; + + case LSM6DSV16X_GY_STRONG: + *val = LSM6DSV16X_GY_STRONG; + break; + + case LSM6DSV16X_GY_VERY_STRONG: + *val = LSM6DSV16X_GY_VERY_STRONG; + break; + + case LSM6DSV16X_GY_AGGRESSIVE: + *val = LSM6DSV16X_GY_AGGRESSIVE; + break; + + case LSM6DSV16X_GY_XTREME: + *val = LSM6DSV16X_GY_XTREME; + break; + + default: + *val = LSM6DSV16X_GY_ULTRA_LIGHT; + break; + } + return ret; +} + +/** + * @brief It enables gyroscope digital LPF1 filter. If the OIS chain is disabled, the bandwidth can be selected through LPF1_G_BW.[set] + * + * @param ctx read / write interface definitions + * @param val It enables gyroscope digital LPF1 filter. If the OIS chain is disabled, the bandwidth can be selected through LPF1_G_BW. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_filt_gy_lp1_set(lsm6dsv16x_ctx_t *ctx, uint8_t val) +{ + lsm6dsv16x_ctrl7_t ctrl7; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL7, (uint8_t *)&ctrl7, 1); + if (ret == 0) { + ctrl7.lpf1_g_en = val; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_CTRL7, (uint8_t *)&ctrl7, 1); + } + + return ret; +} + + +/** + * @brief It enables gyroscope digital LPF1 filter. If the OIS chain is disabled, the bandwidth can be selected through LPF1_G_BW.[get] + * + * @param ctx read / write interface definitions + * @param val It enables gyroscope digital LPF1 filter. If the OIS chain is disabled, the bandwidth can be selected through LPF1_G_BW. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_filt_gy_lp1_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val) +{ + lsm6dsv16x_ctrl7_t ctrl7; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL7, (uint8_t *)&ctrl7, 1); + *val = ctrl7.lpf1_g_en; + + return ret; +} + +/** + * @brief Accelerometer LPF2 and high pass filter configuration and cutoff setting.[set] + * + * @param ctx read / write interface definitions + * @param val XL_ULTRA_LIGHT, XL_VERY_LIGHT, XL_LIGHT, XL_MEDIUM, XL_STRONG, XL_VERY_STRONG, XL_AGGRESSIVE, XL_XTREME, + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_filt_xl_lp2_bandwidth_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_filt_xl_lp2_bandwidth_t val) +{ + lsm6dsv16x_ctrl8_t ctrl8; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL8, (uint8_t *)&ctrl8, 1); + if (ret == 0) { + ctrl8.hp_lpf2_xl_bw = (uint8_t)val & 0x07U; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_CTRL8, (uint8_t *)&ctrl8, 1); + } + + return ret; +} + +/** + * @brief Accelerometer LPF2 and high pass filter configuration and cutoff setting.[get] + * + * @param ctx read / write interface definitions + * @param val XL_ULTRA_LIGHT, XL_VERY_LIGHT, XL_LIGHT, XL_MEDIUM, XL_STRONG, XL_VERY_STRONG, XL_AGGRESSIVE, XL_XTREME, + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_filt_xl_lp2_bandwidth_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_filt_xl_lp2_bandwidth_t *val) +{ + lsm6dsv16x_ctrl8_t ctrl8; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL8, (uint8_t *)&ctrl8, 1); + switch (ctrl8.hp_lpf2_xl_bw) { + case LSM6DSV16X_XL_ULTRA_LIGHT: + *val = LSM6DSV16X_XL_ULTRA_LIGHT; + break; + + case LSM6DSV16X_XL_VERY_LIGHT: + *val = LSM6DSV16X_XL_VERY_LIGHT; + break; + + case LSM6DSV16X_XL_LIGHT: + *val = LSM6DSV16X_XL_LIGHT; + break; + + case LSM6DSV16X_XL_MEDIUM: + *val = LSM6DSV16X_XL_MEDIUM; + break; + + case LSM6DSV16X_XL_STRONG: + *val = LSM6DSV16X_XL_STRONG; + break; + + case LSM6DSV16X_XL_VERY_STRONG: + *val = LSM6DSV16X_XL_VERY_STRONG; + break; + + case LSM6DSV16X_XL_AGGRESSIVE: + *val = LSM6DSV16X_XL_AGGRESSIVE; + break; + + case LSM6DSV16X_XL_XTREME: + *val = LSM6DSV16X_XL_XTREME; + break; + + default: + *val = LSM6DSV16X_XL_ULTRA_LIGHT; + break; + } + return ret; +} + +/** + * @brief Enable accelerometer LPS2 (Low Pass Filter 2) filtering stage.[set] + * + * @param ctx read / write interface definitions + * @param val Enable accelerometer LPS2 (Low Pass Filter 2) filtering stage. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_filt_xl_lp2_set(lsm6dsv16x_ctx_t *ctx, uint8_t val) +{ + lsm6dsv16x_ctrl9_t ctrl9; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL9, (uint8_t *)&ctrl9, 1); + if (ret == 0) { + ctrl9.lpf2_xl_en = val; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_CTRL9, (uint8_t *)&ctrl9, 1); + } + + return ret; +} + +/** + * @brief Enable accelerometer LPS2 (Low Pass Filter 2) filtering stage.[get] + * + * @param ctx read / write interface definitions + * @param val Enable accelerometer LPS2 (Low Pass Filter 2) filtering stage. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_filt_xl_lp2_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val) +{ + lsm6dsv16x_ctrl9_t ctrl9; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL9, (uint8_t *)&ctrl9, 1); + *val = ctrl9.lpf2_xl_en; + + return ret; +} + +/** + * @brief Accelerometer slope filter / high-pass filter selection.[set] + * + * @param ctx read / write interface definitions + * @param val Accelerometer slope filter / high-pass filter selection. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_filt_xl_hp_set(lsm6dsv16x_ctx_t *ctx, uint8_t val) +{ + lsm6dsv16x_ctrl9_t ctrl9; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL9, (uint8_t *)&ctrl9, 1); + if (ret == 0) { + ctrl9.hp_slope_xl_en = val; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_CTRL9, (uint8_t *)&ctrl9, 1); + } + + return ret; +} + +/** + * @brief Accelerometer slope filter / high-pass filter selection.[get] + * + * @param ctx read / write interface definitions + * @param val Accelerometer slope filter / high-pass filter selection. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_filt_xl_hp_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val) +{ + lsm6dsv16x_ctrl9_t ctrl9; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL9, (uint8_t *)&ctrl9, 1); + *val = ctrl9.hp_slope_xl_en; + + return ret; +} + +/** + * @brief Enables accelerometer LPF2 and HPF fast-settling mode. The filter sets the first sample.[set] + * + * @param ctx read / write interface definitions + * @param val Enables accelerometer LPF2 and HPF fast-settling mode. The filter sets the first sample. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_filt_xl_fast_settling_set(lsm6dsv16x_ctx_t *ctx, uint8_t val) +{ + lsm6dsv16x_ctrl9_t ctrl9; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL9, (uint8_t *)&ctrl9, 1); + if (ret == 0) { + ctrl9.xl_fastsettl_mode = val; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_CTRL9, (uint8_t *)&ctrl9, 1); + } + + return ret; +} + +/** + * @brief Enables accelerometer LPF2 and HPF fast-settling mode. The filter sets the first sample.[get] + * + * @param ctx read / write interface definitions + * @param val Enables accelerometer LPF2 and HPF fast-settling mode. The filter sets the first sample. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_filt_xl_fast_settling_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val) +{ + lsm6dsv16x_ctrl9_t ctrl9; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL9, (uint8_t *)&ctrl9, 1); + *val = ctrl9.xl_fastsettl_mode; + + return ret; +} + +/** + * @brief Accelerometer high-pass filter mode.[set] + * + * @param ctx read / write interface definitions + * @param val HP_MD_NORMAL, HP_MD_REFERENCE, + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_filt_xl_hp_mode_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_filt_xl_hp_mode_t val) +{ + lsm6dsv16x_ctrl9_t ctrl9; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL9, (uint8_t *)&ctrl9, 1); + if (ret == 0) { + ctrl9.hp_ref_mode_xl = (uint8_t)val & 0x01U; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_CTRL9, (uint8_t *)&ctrl9, 1); + } + + return ret; +} + +/** + * @brief Accelerometer high-pass filter mode.[get] + * + * @param ctx read / write interface definitions + * @param val HP_MD_NORMAL, HP_MD_REFERENCE, + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_filt_xl_hp_mode_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_filt_xl_hp_mode_t *val) +{ + lsm6dsv16x_ctrl9_t ctrl9; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL9, (uint8_t *)&ctrl9, 1); + switch (ctrl9.hp_ref_mode_xl) { + case LSM6DSV16X_HP_MD_NORMAL: + *val = LSM6DSV16X_HP_MD_NORMAL; + break; + + case LSM6DSV16X_HP_MD_REFERENCE: + *val = LSM6DSV16X_HP_MD_REFERENCE; + break; + + default: + *val = LSM6DSV16X_HP_MD_NORMAL; + break; + } + return ret; +} + +/** + * @brief HPF or SLOPE filter selection on wake-up and Activity/Inactivity functions.[set] + * + * @param ctx read / write interface definitions + * @param val WK_FEED_SLOPE, WK_FEED_HIGH_PASS, WK_FEED_LP_WITH_OFFSET, + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_filt_wkup_act_feed_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_filt_wkup_act_feed_t val) +{ + lsm6dsv16x_wake_up_ths_t wake_up_ths; + lsm6dsv16x_tap_cfg0_t tap_cfg0; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_WAKE_UP_THS, (uint8_t *)&wake_up_ths, 1); + if (ret == 0) { + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_TAP_CFG0, (uint8_t *)&tap_cfg0, 1); + } + + if (ret == 0) { + tap_cfg0.slope_fds = (uint8_t)val & 0x01U; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_TAP_CFG0, (uint8_t *)&tap_cfg0, 1); + } + if (ret == 0) { + wake_up_ths.usr_off_on_wu = ((uint8_t)val & 0x02U) >> 1; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_WAKE_UP_THS, (uint8_t *)&wake_up_ths, 1); + } + + return ret; +} + +/** + * @brief HPF or SLOPE filter selection on wake-up and Activity/Inactivity functions.[get] + * + * @param ctx read / write interface definitions + * @param val WK_FEED_SLOPE, WK_FEED_HIGH_PASS, WK_FEED_LP_WITH_OFFSET, + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_filt_wkup_act_feed_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_filt_wkup_act_feed_t *val) +{ + lsm6dsv16x_wake_up_ths_t wake_up_ths; + lsm6dsv16x_tap_cfg0_t tap_cfg0; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_WAKE_UP_THS, (uint8_t *)&wake_up_ths, 1); + if (ret == 0) { + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_TAP_CFG0, (uint8_t *)&tap_cfg0, 1); + } + + switch ((wake_up_ths.usr_off_on_wu << 1) + tap_cfg0.slope_fds) { + case LSM6DSV16X_WK_FEED_SLOPE: + *val = LSM6DSV16X_WK_FEED_SLOPE; + break; + + case LSM6DSV16X_WK_FEED_HIGH_PASS: + *val = LSM6DSV16X_WK_FEED_HIGH_PASS; + break; + + case LSM6DSV16X_WK_FEED_LP_WITH_OFFSET: + *val = LSM6DSV16X_WK_FEED_LP_WITH_OFFSET; + break; + + default: + *val = LSM6DSV16X_WK_FEED_SLOPE; + break; + } + return ret; +} + +/** + * @brief LPF2 filter on 6D (sixd) function selection.[set] + * + * @param ctx read / write interface definitions + * @param val SIXD_FEED_ODR_DIV_2, SIXD_FEED_LOW_PASS, + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_filt_sixd_feed_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_filt_sixd_feed_t val) +{ + lsm6dsv16x_tap_cfg0_t tap_cfg0; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_TAP_CFG0, (uint8_t *)&tap_cfg0, 1); + + if (ret == 0) { + tap_cfg0.low_pass_on_6d = (uint8_t)val & 0x01U; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_TAP_CFG0, (uint8_t *)&tap_cfg0, 1); + } + + return ret; +} + +/** + * @brief LPF2 filter on 6D (sixd) function selection.[get] + * + * @param ctx read / write interface definitions + * @param val SIXD_FEED_ODR_DIV_2, SIXD_FEED_LOW_PASS, + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_filt_sixd_feed_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_filt_sixd_feed_t *val) +{ + lsm6dsv16x_tap_cfg0_t tap_cfg0; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_TAP_CFG0, (uint8_t *)&tap_cfg0, 1); + + switch (tap_cfg0.low_pass_on_6d) { + case LSM6DSV16X_SIXD_FEED_ODR_DIV_2: + *val = LSM6DSV16X_SIXD_FEED_ODR_DIV_2; + break; + + case LSM6DSV16X_SIXD_FEED_LOW_PASS: + *val = LSM6DSV16X_SIXD_FEED_LOW_PASS; + break; + + default: + *val = LSM6DSV16X_SIXD_FEED_ODR_DIV_2; + break; + } + return ret; +} + +/** + * @brief Gyroscope digital LPF_EIS filter bandwidth selection.[set] + * + * @param ctx read / write interface definitions + * @param val EIS_LP_NORMAL, EIS_LP_LIGHT, + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_filt_gy_eis_lp_bandwidth_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_filt_gy_eis_lp_bandwidth_t val) +{ + lsm6dsv16x_ctrl_eis_t ctrl_eis; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL_EIS, (uint8_t *)&ctrl_eis, 1); + + if (ret == 0) { + ctrl_eis.lpf_g_eis_bw = (uint8_t)val & 0x01U; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_CTRL_EIS, (uint8_t *)&ctrl_eis, 1); + } + + return ret; +} + +/** + * @brief Gyroscope digital LPF_EIS filter bandwidth selection.[get] + * + * @param ctx read / write interface definitions + * @param val EIS_LP_NORMAL, EIS_LP_LIGHT, + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_filt_gy_eis_lp_bandwidth_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_filt_gy_eis_lp_bandwidth_t *val) +{ + lsm6dsv16x_ctrl_eis_t ctrl_eis; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL_EIS, (uint8_t *)&ctrl_eis, 1); + + switch (ctrl_eis.lpf_g_eis_bw) { + case LSM6DSV16X_EIS_LP_NORMAL: + *val = LSM6DSV16X_EIS_LP_NORMAL; + break; + + case LSM6DSV16X_EIS_LP_LIGHT: + *val = LSM6DSV16X_EIS_LP_LIGHT; + break; + + default: + *val = LSM6DSV16X_EIS_LP_NORMAL; + break; + } + return ret; +} + +/** + * @brief Gyroscope OIS digital LPF1 filter bandwidth selection. This function works also on OIS interface (SPI2_CTRL2_OIS = UI_CTRL2_OIS).[set] + * + * @param ctx read / write interface definitions + * @param val OIS_GY_LP_NORMAL, OIS_GY_LP_STRONG, OIS_GY_LP_AGGRESSIVE, OIS_GY_LP_LIGHT, + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_filt_gy_ois_lp_bandwidth_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_filt_gy_ois_lp_bandwidth_t val) +{ + lsm6dsv16x_ui_ctrl2_ois_t ui_ctrl2_ois; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_UI_CTRL2_OIS, (uint8_t *)&ui_ctrl2_ois, 1); + + if (ret == 0) { + ui_ctrl2_ois.lpf1_g_ois_bw = (uint8_t)val & 0x03U; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_UI_CTRL2_OIS, (uint8_t *)&ui_ctrl2_ois, 1); + } + + return ret; +} + +/** + * @brief Gyroscope OIS digital LPF1 filter bandwidth selection. This function works also on OIS interface (SPI2_CTRL2_OIS = UI_CTRL2_OIS).[get] + * + * @param ctx read / write interface definitions + * @param val OIS_GY_LP_NORMAL, OIS_GY_LP_STRONG, OIS_GY_LP_AGGRESSIVE, OIS_GY_LP_LIGHT, + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_filt_gy_ois_lp_bandwidth_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_filt_gy_ois_lp_bandwidth_t *val) +{ + + lsm6dsv16x_ui_ctrl2_ois_t ui_ctrl2_ois; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_UI_CTRL2_OIS, (uint8_t *)&ui_ctrl2_ois, 1); + + switch (ui_ctrl2_ois.lpf1_g_ois_bw) { + case LSM6DSV16X_OIS_GY_LP_NORMAL: + *val = LSM6DSV16X_OIS_GY_LP_NORMAL; + break; + + case LSM6DSV16X_OIS_GY_LP_STRONG: + *val = LSM6DSV16X_OIS_GY_LP_STRONG; + break; + + case LSM6DSV16X_OIS_GY_LP_AGGRESSIVE: + *val = LSM6DSV16X_OIS_GY_LP_AGGRESSIVE; + break; + + case LSM6DSV16X_OIS_GY_LP_LIGHT: + *val = LSM6DSV16X_OIS_GY_LP_LIGHT; + break; + + default: + *val = LSM6DSV16X_OIS_GY_LP_NORMAL; + break; + } + return ret; +} + +/** + * @brief Selects accelerometer OIS channel bandwidth. This function works also on OIS interface (SPI2_CTRL3_OIS = UI_CTRL3_OIS).[set] + * + * @param ctx read / write interface definitions + * @param val OIS_XL_LP_ULTRA_LIGHT, OIS_XL_LP_VERY_LIGHT, OIS_XL_LP_LIGHT, OIS_XL_LP_NORMAL, OIS_XL_LP_STRONG, OIS_XL_LP_VERY_STRONG, OIS_XL_LP_AGGRESSIVE, OIS_XL_LP_XTREME, + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_filt_xl_ois_lp_bandwidth_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_filt_xl_ois_lp_bandwidth_t val) +{ + lsm6dsv16x_ui_ctrl3_ois_t ui_ctrl3_ois; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_UI_CTRL3_OIS, (uint8_t *)&ui_ctrl3_ois, 1); + + if (ret == 0) { + ui_ctrl3_ois.lpf_xl_ois_bw = (uint8_t)val & 0x07U; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_UI_CTRL3_OIS, (uint8_t *)&ui_ctrl3_ois, 1); + } + + return ret; +} + +/** + * @brief Selects accelerometer OIS channel bandwidth. This function works also on OIS interface (SPI2_CTRL3_OIS = UI_CTRL3_OIS).[get] + * + * @param ctx read / write interface definitions + * @param val OIS_XL_LP_ULTRA_LIGHT, OIS_XL_LP_VERY_LIGHT, OIS_XL_LP_LIGHT, OIS_XL_LP_NORMAL, OIS_XL_LP_STRONG, OIS_XL_LP_VERY_STRONG, OIS_XL_LP_AGGRESSIVE, OIS_XL_LP_XTREME, + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_filt_xl_ois_lp_bandwidth_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_filt_xl_ois_lp_bandwidth_t *val) +{ + lsm6dsv16x_ui_ctrl3_ois_t ui_ctrl3_ois; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_UI_CTRL3_OIS, (uint8_t *)&ui_ctrl3_ois, 1); + + switch (ui_ctrl3_ois.lpf_xl_ois_bw) { + case LSM6DSV16X_OIS_XL_LP_ULTRA_LIGHT: + *val = LSM6DSV16X_OIS_XL_LP_ULTRA_LIGHT; + break; + + case LSM6DSV16X_OIS_XL_LP_VERY_LIGHT: + *val = LSM6DSV16X_OIS_XL_LP_VERY_LIGHT; + break; + + case LSM6DSV16X_OIS_XL_LP_LIGHT: + *val = LSM6DSV16X_OIS_XL_LP_LIGHT; + break; + + case LSM6DSV16X_OIS_XL_LP_NORMAL: + *val = LSM6DSV16X_OIS_XL_LP_NORMAL; + break; + + case LSM6DSV16X_OIS_XL_LP_STRONG: + *val = LSM6DSV16X_OIS_XL_LP_STRONG; + break; + + case LSM6DSV16X_OIS_XL_LP_VERY_STRONG: + *val = LSM6DSV16X_OIS_XL_LP_VERY_STRONG; + break; + + case LSM6DSV16X_OIS_XL_LP_AGGRESSIVE: + *val = LSM6DSV16X_OIS_XL_LP_AGGRESSIVE; + break; + + case LSM6DSV16X_OIS_XL_LP_XTREME: + *val = LSM6DSV16X_OIS_XL_LP_XTREME; + break; + + default: + *val = LSM6DSV16X_OIS_XL_LP_ULTRA_LIGHT; + break; + } + return ret; +} + +/** + * @} + * + */ + +/** + * @defgroup Finite State Machine (FSM) + * @brief This section groups all the functions that manage the + * state_machine. + * @{ + * + */ + +/** + * @brief Enables the control of the CTRL registers to FSM (FSM can change some configurations of the device autonomously).[set] + * + * @param ctx read / write interface definitions + * @param val PROTECT_CTRL_REGS, WRITE_CTRL_REG, + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_fsm_permission_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fsm_permission_t val) +{ + lsm6dsv16x_func_cfg_access_t func_cfg_access; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FUNC_CFG_ACCESS, (uint8_t *)&func_cfg_access, 1); + + if (ret == 0) { + func_cfg_access.fsm_wr_ctrl_en = (uint8_t)val & 0x01U; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_FUNC_CFG_ACCESS, (uint8_t *)&func_cfg_access, 1); + } + + return ret; +} + +/** + * @brief Enables the control of the CTRL registers to FSM (FSM can change some configurations of the device autonomously).[get] + * + * @param ctx read / write interface definitions + * @param val PROTECT_CTRL_REGS, WRITE_CTRL_REG, + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_fsm_permission_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fsm_permission_t *val) +{ + lsm6dsv16x_func_cfg_access_t func_cfg_access; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FUNC_CFG_ACCESS, (uint8_t *)&func_cfg_access, 1); + + switch (func_cfg_access.fsm_wr_ctrl_en) { + case LSM6DSV16X_PROTECT_CTRL_REGS: + *val = LSM6DSV16X_PROTECT_CTRL_REGS; + break; + + case LSM6DSV16X_WRITE_CTRL_REG: + *val = LSM6DSV16X_WRITE_CTRL_REG; + break; + + default: + *val = LSM6DSV16X_PROTECT_CTRL_REGS; + break; + } + return ret; +} + +/** + * @brief Get the FSM permission status + * + * @param ctx read / write interface definitions + * @param val 0: All reg writable from std if - 1: some regs are under FSM control. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_fsm_permission_status(lsm6dsv16x_ctx_t *ctx, uint8_t *val) +{ + lsm6dsv16x_ctrl_status_t ctrl_status; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL_STATUS, (uint8_t *)&ctrl_status, 1); + + *val = ctrl_status.fsm_wr_ctrl_status; + + return ret; +} + +/** + * @brief Enable Finite State Machine (FSM) feature.[set] + * + * @param ctx read / write interface definitions + * @param val Enable Finite State Machine (FSM) feature. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_fsm_mode_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fsm_mode_t val) +{ + lsm6dsv16x_emb_func_en_b_t emb_func_en_b; + lsm6dsv16x_fsm_enable_t fsm_enable; + int32_t ret; + + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK); + if (ret == 0) { + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_EMB_FUNC_EN_B, (uint8_t *)&emb_func_en_b, 1); + } + if (ret == 0) { + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FSM_ENABLE, (uint8_t *)&fsm_enable, 1); + } + if ((val.fsm1_en | val.fsm2_en | val.fsm1_en | val.fsm1_en + | val.fsm1_en | val.fsm2_en | val.fsm1_en | val.fsm1_en) == PROPERTY_ENABLE) { + emb_func_en_b.fsm_en = PROPERTY_ENABLE; + } else { + emb_func_en_b.fsm_en = PROPERTY_DISABLE; + } + if (ret == 0) { + fsm_enable.fsm1_en = val.fsm1_en; + fsm_enable.fsm2_en = val.fsm2_en; + fsm_enable.fsm3_en = val.fsm3_en; + fsm_enable.fsm4_en = val.fsm4_en; + fsm_enable.fsm5_en = val.fsm5_en; + fsm_enable.fsm6_en = val.fsm6_en; + fsm_enable.fsm7_en = val.fsm7_en; + fsm_enable.fsm8_en = val.fsm8_en; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_FSM_ENABLE, (uint8_t *)&fsm_enable, 1); + } + if (ret == 0) { + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_EMB_FUNC_EN_B, (uint8_t *)&emb_func_en_b, 1); + } + if (ret == 0) { + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + } + + return ret; +} + +/** + * @brief Enable Finite State Machine (FSM) feature.[get] + * + * @param ctx read / write interface definitions + * @param val Enable Finite State Machine (FSM) feature. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_fsm_mode_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fsm_mode_t *val) +{ + lsm6dsv16x_fsm_enable_t fsm_enable; + int32_t ret; + + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK); + if (ret == 0) { + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FSM_ENABLE, (uint8_t *)&fsm_enable, 1); + } + if (ret == 0) { + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + } + + val->fsm1_en = fsm_enable.fsm1_en; + val->fsm2_en = fsm_enable.fsm2_en; + val->fsm3_en = fsm_enable.fsm3_en; + val->fsm4_en = fsm_enable.fsm4_en; + val->fsm5_en = fsm_enable.fsm5_en; + val->fsm6_en = fsm_enable.fsm6_en; + val->fsm7_en = fsm_enable.fsm7_en; + val->fsm8_en = fsm_enable.fsm8_en; + + return ret; +} + +/** + * @brief FSM long counter status register. Long counter value is an unsigned integer value (16-bit format).[set] + * + * @param ctx read / write interface definitions + * @param val FSM long counter status register. Long counter value is an unsigned integer value (16-bit format). + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_fsm_long_cnt_set(lsm6dsv16x_ctx_t *ctx, uint16_t val) +{ + uint8_t buff[2]; + int32_t ret; + + buff[1] = (uint8_t)(val / 256U); + buff[0] = (uint8_t)(val - (buff[1] * 256U)); + + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_FSM_LONG_COUNTER_L, (uint8_t *)&buff[0], 2); + if (ret == 0) { + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + } + + return ret; +} + +/** + * @brief FSM long counter status register. Long counter value is an unsigned integer value (16-bit format).[get] + * + * @param ctx read / write interface definitions + * @param val FSM long counter status register. Long counter value is an unsigned integer value (16-bit format). + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_fsm_long_cnt_get(lsm6dsv16x_ctx_t *ctx, uint16_t *val) +{ + uint8_t buff[2]; + int32_t ret; + + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK); + if (ret == 0) { + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FSM_LONG_COUNTER_L, &buff[0], 2); + } + if (ret == 0) { + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + } + + *val = buff[1]; + *val = (*val * 256U) + buff[0]; + + return ret; +} + +/** + * @brief FSM output registers[get] + * + * @param ctx read / write interface definitions + * @param val FSM output registers + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_fsm_out_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fsm_out_t *val) +{ + int32_t ret; + + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK); + if (ret == 0) { + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FSM_OUTS1, (uint8_t *)val, 8); + } + if (ret == 0) { + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + } + + return ret; +} + +/** + * @brief Finite State Machine Output Data Rate (ODR) configuration.[set] + * + * @param ctx read / write interface definitions + * @param val FSM_15Hz, FSM_30Hz, FSM_60Hz, FSM_120Hz, FSM_240Hz, FSM_480Hz, FSM_960Hz, + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_fsm_data_rate_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fsm_data_rate_t val) +{ + lsm6dsv16x_fsm_odr_t fsm_odr; + int32_t ret; + + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK); + if (ret == 0) { + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FSM_ODR, (uint8_t *)&fsm_odr, 1); + } + + if (ret == 0) { + fsm_odr.fsm_odr = (uint8_t)val & 0x07U; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_FSM_ODR, (uint8_t *)&fsm_odr, 1); + } + if (ret == 0) { + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + } + + return ret; +} + +/** + * @brief Finite State Machine Output Data Rate (ODR) configuration.[get] + * + * @param ctx read / write interface definitions + * @param val FSM_15Hz, FSM_30Hz, FSM_60Hz, FSM_120Hz, FSM_240Hz, FSM_480Hz, FSM_960Hz, + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_fsm_data_rate_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fsm_data_rate_t *val) +{ + lsm6dsv16x_fsm_odr_t fsm_odr; + int32_t ret; + + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK); + if (ret == 0) { + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FSM_ODR, (uint8_t *)&fsm_odr, 1); + } + if (ret == 0) { + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + } + + + switch (fsm_odr.fsm_odr) { + case LSM6DSV16X_FSM_15Hz: + *val = LSM6DSV16X_FSM_15Hz; + break; + + case LSM6DSV16X_FSM_30Hz: + *val = LSM6DSV16X_FSM_30Hz; + break; + + case LSM6DSV16X_FSM_60Hz: + *val = LSM6DSV16X_FSM_60Hz; + break; + + case LSM6DSV16X_FSM_120Hz: + *val = LSM6DSV16X_FSM_120Hz; + break; + + case LSM6DSV16X_FSM_240Hz: + *val = LSM6DSV16X_FSM_240Hz; + break; + + case LSM6DSV16X_FSM_480Hz: + *val = LSM6DSV16X_FSM_480Hz; + break; + + case LSM6DSV16X_FSM_960Hz: + *val = LSM6DSV16X_FSM_960Hz; + break; + + default: + *val = LSM6DSV16X_FSM_15Hz; + break; + } + return ret; +} + +/** + * @brief External sensor sensitivity value register for the Finite State Machine (r/w). This register corresponds to the conversion value of the external sensor. The register value is expressed as half-precision floating-point format: SEEEEEFFFFFFFFFF (S: 1 sign bit; E: 5 exponent bits; F: 10 fraction bits). Default value is 0x1624 (when using an external magnetometer this value corresponds to 0.0015 gauss/LSB).[set] + * + * @param ctx read / write interface definitions + * @param val External sensor sensitivity value register for the Finite State Machine (r/w). This register corresponds to the conversion value of the external sensor. The register value is expressed as half-precision floating-point format: SEEEEEFFFFFFFFFF (S: 1 sign bit; E: 5 exponent bits; F: 10 fraction bits). Default value is 0x1624 (when using an external magnetometer this value corresponds to 0.0015 gauss/LSB). + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_fsm_ext_sens_sensitivity_set(lsm6dsv16x_ctx_t *ctx, uint16_t val) +{ + uint8_t buff[2]; + int32_t ret; + + buff[1] = (uint8_t)(val / 256U); + buff[0] = (uint8_t)(val - (buff[1] * 256U)); + ret = lsm6dsv16x_ln_pg_write(ctx, LSM6DSV16X_FSM_EXT_SENSITIVITY_L, (uint8_t *)&buff[0], 2); + + return ret; +} + +/** + * @brief External sensor sensitivity value register for the Finite State Machine (r/w). This register corresponds to the conversion value of the external sensor. The register value is expressed as half-precision floating-point format: SEEEEEFFFFFFFFFF (S: 1 sign bit; E: 5 exponent bits; F: 10 fraction bits). Default value is 0x1624 (when using an external magnetometer this value corresponds to 0.0015 gauss/LSB).[get] + * + * @param ctx read / write interface definitions + * @param val External sensor sensitivity value register for the Finite State Machine (r/w). This register corresponds to the conversion value of the external sensor. The register value is expressed as half-precision floating-point format: SEEEEEFFFFFFFFFF (S: 1 sign bit; E: 5 exponent bits; F: 10 fraction bits). Default value is 0x1624 (when using an external magnetometer this value corresponds to 0.0015 gauss/LSB). + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_fsm_ext_sens_sensitivity_get(lsm6dsv16x_ctx_t *ctx, uint16_t *val) +{ + uint8_t buff[2]; + int32_t ret; + + ret = lsm6dsv16x_ln_pg_read(ctx, LSM6DSV16X_FSM_EXT_SENSITIVITY_L, &buff[0], 2); + *val = buff[1]; + *val = (*val * 256U) + buff[0]; + + return ret; +} + +/** + * @brief External sensor offsets (X,Y,Z). The values are expressed as half-precision floating-point format: SEEEEEFFFFFFFFFF (S: 1 sign bit; E: 5 exponent bits; F: 10 fraction bits).[set] + * + * @param ctx read / write interface definitions + * @param val External sensor offsets (X,Y,Z). The values are expressed as half-precision floating-point format: SEEEEEFFFFFFFFFF (S: 1 sign bit; E: 5 exponent bits; F: 10 fraction bits). + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_fsm_ext_sens_offset_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_xl_fsm_ext_sens_offset_t val) +{ + uint8_t buff[6]; + int32_t ret; + + buff[1] = (uint8_t)(val.x / 256U); + buff[0] = (uint8_t)(val.x - (buff[1] * 256U)); + buff[3] = (uint8_t)(val.y / 256U); + buff[2] = (uint8_t)(val.y - (buff[3] * 256U)); + buff[5] = (uint8_t)(val.z / 256U); + buff[4] = (uint8_t)(val.z - (buff[5] * 256U)); + ret = lsm6dsv16x_ln_pg_write(ctx, LSM6DSV16X_FSM_EXT_OFFX_L, (uint8_t *)&buff[0], 6); + + return ret; +} + +/** + * @brief External sensor offsets (X,Y,Z). The values are expressed as half-precision floating-point format: SEEEEEFFFFFFFFFF (S: 1 sign bit; E: 5 exponent bits; F: 10 fraction bits).[get] + * + * @param ctx read / write interface definitions + * @param val External sensor offsets (X,Y,Z). The values are expressed as half-precision floating-point format: SEEEEEFFFFFFFFFF (S: 1 sign bit; E: 5 exponent bits; F: 10 fraction bits). + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_fsm_ext_sens_offset_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_xl_fsm_ext_sens_offset_t *val) +{ + uint8_t buff[6]; + int32_t ret; + + ret = lsm6dsv16x_ln_pg_read(ctx, LSM6DSV16X_FSM_EXT_OFFX_L, &buff[0], 6); + + val->x = buff[1]; + val->x = (val->x * 256U) + buff[0]; + val->y = buff[3]; + val->y = (val->y * 256U) + buff[2]; + val->z = buff[5]; + val->z = (val->z * 256U) + buff[4]; + + return ret; +} + +/** + * @brief External sensor transformation matrix. The value is expressed as half-precision floating-point format: SEEEEEFFFFFFFFFF (S: 1 sign bit; E: 5 exponent bits; F: 10 fraction bits).[set] + * + * @param ctx read / write interface definitions + * @param val External sensor transformation matrix. The value is expressed as half-precision floating-point format: SEEEEEFFFFFFFFFF (S: 1 sign bit; E: 5 exponent bits; F: 10 fraction bits). + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_fsm_ext_sens_matrix_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_xl_fsm_ext_sens_matrix_t val) +{ + uint8_t buff[12]; + int32_t ret; + + buff[1] = (uint8_t)(val.xx / 256U); + buff[0] = (uint8_t)(val.xx - (buff[1] * 256U)); + buff[3] = (uint8_t)(val.xy / 256U); + buff[2] = (uint8_t)(val.xy - (buff[3] * 256U)); + buff[5] = (uint8_t)(val.xz / 256U); + buff[4] = (uint8_t)(val.xz - (buff[5] * 256U)); + buff[7] = (uint8_t)(val.yy / 256U); + buff[6] = (uint8_t)(val.yy - (buff[7] * 256U)); + buff[9] = (uint8_t)(val.yz / 256U); + buff[8] = (uint8_t)(val.yz - (buff[9] * 256U)); + buff[11] = (uint8_t)(val.zz / 256U); + buff[10] = (uint8_t)(val.zz - (buff[11] * 256U)); + ret = lsm6dsv16x_ln_pg_write(ctx, LSM6DSV16X_FSM_EXT_MATRIX_XX_L, (uint8_t *)&buff[0], 12); + + return ret; +} + +/** + * @brief External sensor transformation matrix. The value is expressed as half-precision floating-point format: SEEEEEFFFFFFFFFF (S: 1 sign bit; E: 5 exponent bits; F: 10 fraction bits).[get] + * + * @param ctx read / write interface definitions + * @param val External sensor transformation matrix. The value is expressed as half-precision floating-point format: SEEEEEFFFFFFFFFF (S: 1 sign bit; E: 5 exponent bits; F: 10 fraction bits). + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_fsm_ext_sens_matrix_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_xl_fsm_ext_sens_matrix_t *val) +{ + uint8_t buff[12]; + int32_t ret; + + ret = lsm6dsv16x_ln_pg_read(ctx, LSM6DSV16X_FSM_EXT_MATRIX_XX_L, &buff[0], 12); + + val->xx = buff[1]; + val->xx = (val->xx * 256U) + buff[0]; + val->xy = buff[3]; + val->xy = (val->xy * 256U) + buff[2]; + val->xz = buff[5]; + val->xz = (val->xz * 256U) + buff[4]; + val->yy = buff[7]; + val->yy = (val->yy * 256U) + buff[6]; + val->yz = buff[9]; + val->yz = (val->yz * 256U) + buff[8]; + val->zz = buff[11]; + val->zz = (val->zz * 256U) + buff[10]; + + return ret; +} + +/** + * @brief External sensor z-axis coordinates rotation.[set] + * + * @param ctx read / write interface definitions + * @param val Z_EQ_Y, Z_EQ_MIN_Y, Z_EQ_X, Z_EQ_MIN_X, Z_EQ_MIN_Z, Z_EQ_Z, + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_fsm_ext_sens_z_orient_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fsm_ext_sens_z_orient_t val) +{ + lsm6dsv16x_ext_cfg_a_t ext_cfg_a; + int32_t ret; + + ret = lsm6dsv16x_ln_pg_read(ctx, LSM6DSV16X_EXT_CFG_A, (uint8_t *)&ext_cfg_a, 1); + if (ret == 0) { + ext_cfg_a.ext_z_axis = (uint8_t)val & 0x07U; + ret = lsm6dsv16x_ln_pg_write(ctx, LSM6DSV16X_EXT_CFG_A, (uint8_t *)&ext_cfg_a, 1); + } + + return ret; +} + +/** + * @brief External sensor z-axis coordinates rotation.[get] + * + * @param ctx read / write interface definitions + * @param val Z_EQ_Y, Z_EQ_MIN_Y, Z_EQ_X, Z_EQ_MIN_X, Z_EQ_MIN_Z, Z_EQ_Z, + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_fsm_ext_sens_z_orient_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fsm_ext_sens_z_orient_t *val) +{ + lsm6dsv16x_ext_cfg_a_t ext_cfg_a; + int32_t ret; + + ret = lsm6dsv16x_ln_pg_read(ctx, LSM6DSV16X_EXT_CFG_A, (uint8_t *)&ext_cfg_a, 1); + switch (ext_cfg_a.ext_z_axis) { + case LSM6DSV16X_Z_EQ_Y: + *val = LSM6DSV16X_Z_EQ_Y; + break; + + case LSM6DSV16X_Z_EQ_MIN_Y: + *val = LSM6DSV16X_Z_EQ_MIN_Y; + break; + + case LSM6DSV16X_Z_EQ_X: + *val = LSM6DSV16X_Z_EQ_X; + break; + + case LSM6DSV16X_Z_EQ_MIN_X: + *val = LSM6DSV16X_Z_EQ_MIN_X; + break; + + case LSM6DSV16X_Z_EQ_MIN_Z: + *val = LSM6DSV16X_Z_EQ_MIN_Z; + break; + + case LSM6DSV16X_Z_EQ_Z: + *val = LSM6DSV16X_Z_EQ_Z; + break; + + default: + *val = LSM6DSV16X_Z_EQ_Y; + break; + } + return ret; +} + +/** + * @brief External sensor Y-axis coordinates rotation.[set] + * + * @param ctx read / write interface definitions + * @param val Y_EQ_Y, Y_EQ_MIN_Y, Y_EQ_X, Y_EQ_MIN_X, Y_EQ_MIN_Z, Y_EQ_Z, + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_fsm_ext_sens_y_orient_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fsm_ext_sens_y_orient_t val) +{ + lsm6dsv16x_ext_cfg_a_t ext_cfg_a; + int32_t ret; + + ret = lsm6dsv16x_ln_pg_read(ctx, LSM6DSV16X_EXT_CFG_A, (uint8_t *)&ext_cfg_a, 1); + if (ret == 0) { + ext_cfg_a.ext_y_axis = (uint8_t)val & 0x7U; + ret = lsm6dsv16x_ln_pg_write(ctx, LSM6DSV16X_EXT_CFG_A, (uint8_t *)&ext_cfg_a, 1); + } + + return ret; +} + +/** + * @brief External sensor Y-axis coordinates rotation.[get] + * + * @param ctx read / write interface definitions + * @param val Y_EQ_Y, Y_EQ_MIN_Y, Y_EQ_X, Y_EQ_MIN_X, Y_EQ_MIN_Z, Y_EQ_Z, + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_fsm_ext_sens_y_orient_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fsm_ext_sens_y_orient_t *val) +{ + lsm6dsv16x_ext_cfg_a_t ext_cfg_a; + int32_t ret; + + ret = lsm6dsv16x_ln_pg_read(ctx, LSM6DSV16X_EXT_CFG_A, (uint8_t *)&ext_cfg_a, 1); + switch (ext_cfg_a.ext_y_axis) { + case LSM6DSV16X_Y_EQ_Y: + *val = LSM6DSV16X_Y_EQ_Y; + break; + + case LSM6DSV16X_Y_EQ_MIN_Y: + *val = LSM6DSV16X_Y_EQ_MIN_Y; + break; + + case LSM6DSV16X_Y_EQ_X: + *val = LSM6DSV16X_Y_EQ_X; + break; + + case LSM6DSV16X_Y_EQ_MIN_X: + *val = LSM6DSV16X_Y_EQ_MIN_X; + break; + + case LSM6DSV16X_Y_EQ_MIN_Z: + *val = LSM6DSV16X_Y_EQ_MIN_Z; + break; + + case LSM6DSV16X_Y_EQ_Z: + *val = LSM6DSV16X_Y_EQ_Z; + break; + + default: + *val = LSM6DSV16X_Y_EQ_Y; + break; + } + return ret; +} + +/** + * @brief External sensor X-axis coordinates rotation.[set] + * + * @param ctx read / write interface definitions + * @param val X_EQ_Y, X_EQ_MIN_Y, X_EQ_X, X_EQ_MIN_X, X_EQ_MIN_Z, X_EQ_Z, + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_fsm_ext_sens_x_orient_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fsm_ext_sens_x_orient_t val) +{ + lsm6dsv16x_ext_cfg_b_t ext_cfg_b; + int32_t ret; + + ret = lsm6dsv16x_ln_pg_read(ctx, LSM6DSV16X_EXT_CFG_B, (uint8_t *)&ext_cfg_b, 1); + if (ret == 0) { + ext_cfg_b.ext_x_axis = (uint8_t)val & 0x7U; + ret = lsm6dsv16x_ln_pg_write(ctx, LSM6DSV16X_EXT_CFG_B, (uint8_t *)&ext_cfg_b, 1); + } + + return ret; +} + +/** + * @brief External sensor X-axis coordinates rotation.[get] + * + * @param ctx read / write interface definitions + * @param val X_EQ_Y, X_EQ_MIN_Y, X_EQ_X, X_EQ_MIN_X, X_EQ_MIN_Z, X_EQ_Z, + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_fsm_ext_sens_x_orient_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fsm_ext_sens_x_orient_t *val) +{ + lsm6dsv16x_ext_cfg_b_t ext_cfg_b; + int32_t ret; + + ret = lsm6dsv16x_ln_pg_read(ctx, LSM6DSV16X_EXT_CFG_B, (uint8_t *)&ext_cfg_b, 1); + switch (ext_cfg_b.ext_x_axis) { + case LSM6DSV16X_X_EQ_Y: + *val = LSM6DSV16X_X_EQ_Y; + break; + + case LSM6DSV16X_X_EQ_MIN_Y: + *val = LSM6DSV16X_X_EQ_MIN_Y; + break; + + case LSM6DSV16X_X_EQ_X: + *val = LSM6DSV16X_X_EQ_X; + break; + + case LSM6DSV16X_X_EQ_MIN_X: + *val = LSM6DSV16X_X_EQ_MIN_X; + break; + + case LSM6DSV16X_X_EQ_MIN_Z: + *val = LSM6DSV16X_X_EQ_MIN_Z; + break; + + case LSM6DSV16X_X_EQ_Z: + *val = LSM6DSV16X_X_EQ_Z; + break; + + default: + *val = LSM6DSV16X_X_EQ_Y; + break; + } + return ret; +} + +/** + * @brief FSM long counter timeout. The long counter timeout value is an unsigned integer value (16-bit format). When the long counter value reached this value, the FSM generates an interrupt.[set] + * + * @param ctx read / write interface definitions + * @param val FSM long counter timeout. The long counter timeout value is an unsigned integer value (16-bit format). When the long counter value reached this value, the FSM generates an interrupt. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_fsm_long_cnt_timeout_set(lsm6dsv16x_ctx_t *ctx, uint16_t val) +{ + uint8_t buff[2]; + int32_t ret; + + buff[1] = (uint8_t)(val / 256U); + buff[0] = (uint8_t)(val - (buff[1] * 256U)); + ret = lsm6dsv16x_ln_pg_write(ctx, LSM6DSV16X_EMB_ADV_PG_1 + LSM6DSV16X_FSM_LC_TIMEOUT_L, (uint8_t *)&buff[0], 2); + + return ret; +} + +/** + * @brief FSM long counter timeout. The long counter timeout value is an unsigned integer value (16-bit format). When the long counter value reached this value, the FSM generates an interrupt.[get] + * + * @param ctx read / write interface definitions + * @param val FSM long counter timeout. The long counter timeout value is an unsigned integer value (16-bit format). When the long counter value reached this value, the FSM generates an interrupt. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_fsm_long_cnt_timeout_get(lsm6dsv16x_ctx_t *ctx, uint16_t *val) +{ + uint8_t buff[2]; + int32_t ret; + + ret = lsm6dsv16x_ln_pg_read(ctx, LSM6DSV16X_EMB_ADV_PG_1 + LSM6DSV16X_FSM_LC_TIMEOUT_L, &buff[0], 2); + *val = buff[1]; + *val = (*val * 256U) + buff[0]; + + return ret; +} + +/** + * @brief FSM number of programs.[set] + * + * @param ctx read / write interface definitions + * @param val FSM number of programs. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_fsm_number_of_programs_set(lsm6dsv16x_ctx_t *ctx, uint8_t val) +{ + lsm6dsv16x_fsm_programs_t fsm_programs; + int32_t ret; + + ret = lsm6dsv16x_ln_pg_read(ctx, LSM6DSV16X_EMB_ADV_PG_1 + LSM6DSV16X_FSM_PROGRAMS, (uint8_t *)&fsm_programs, 1); + if (ret == 0) { + fsm_programs.fsm_n_prog = val; + ret = lsm6dsv16x_ln_pg_write(ctx, LSM6DSV16X_EMB_ADV_PG_1 + LSM6DSV16X_FSM_PROGRAMS, (uint8_t *)&fsm_programs, 1); + } + + return ret; +} + +/** + * @brief FSM number of programs.[get] + * + * @param ctx read / write interface definitions + * @param val FSM number of programs. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_fsm_number_of_programs_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val) +{ + lsm6dsv16x_fsm_programs_t fsm_programs; + int32_t ret; + + ret = lsm6dsv16x_ln_pg_read(ctx, LSM6DSV16X_EMB_ADV_PG_1 + LSM6DSV16X_FSM_PROGRAMS, (uint8_t *)&fsm_programs, 1); + *val = fsm_programs.fsm_n_prog; + + return ret; +} + +/** + * @brief FSM start address. First available address is 0x35C.[set] + * + * @param ctx read / write interface definitions + * @param val FSM start address. First available address is 0x35C. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_fsm_start_address_set(lsm6dsv16x_ctx_t *ctx, uint16_t val) +{ + uint8_t buff[2]; + int32_t ret; + + buff[1] = (uint8_t)(val / 256U); + buff[0] = (uint8_t)(val - (buff[1] * 256U)); + ret = lsm6dsv16x_ln_pg_write(ctx, LSM6DSV16X_EMB_ADV_PG_1 + LSM6DSV16X_FSM_START_ADD_L, (uint8_t *)&buff[0], 2); + + return ret; +} + +/** + * @brief FSM start address. First available address is 0x35C.[get] + * + * @param ctx read / write interface definitions + * @param val FSM start address. First available address is 0x35C. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_fsm_start_address_get(lsm6dsv16x_ctx_t *ctx, uint16_t *val) +{ + uint8_t buff[2]; + int32_t ret; + + ret = lsm6dsv16x_ln_pg_read(ctx, LSM6DSV16X_EMB_ADV_PG_1 + LSM6DSV16X_FSM_START_ADD_L, &buff[0], 2); + *val = buff[1]; + *val = (*val * 256U) + buff[0]; + + return ret; +} + +/** + * @} + * + */ + +/** + * @defgroup Free fall + * @brief This section group all the functions concerning the free + * fall detection. + * @{ + * + */ + +/** + * @brief Time windows configuration for Free Fall detection 1 LSB = 1/ODR_XL time[set] + * + * @param ctx read / write interface definitions + * @param val Time windows configuration for Free Fall detection 1 LSB = 1/ODR_XL time + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_ff_time_windows_set(lsm6dsv16x_ctx_t *ctx, uint8_t val) +{ + lsm6dsv16x_wake_up_dur_t wake_up_dur; + lsm6dsv16x_free_fall_t free_fall; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_WAKE_UP_DUR, (uint8_t *)&wake_up_dur, 1); + if (ret == 0) { + wake_up_dur.ff_dur = ((uint8_t)val & 0x20U) >> 5; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_WAKE_UP_DUR, (uint8_t *)&wake_up_dur, 1); + } + if (ret == 0) { + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FREE_FALL, (uint8_t *)&free_fall, 1); + } + + if (ret == 0) { + free_fall.ff_dur = (uint8_t)val & 0x1FU; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_FREE_FALL, (uint8_t *)&free_fall, 1); + } + + return ret; +} + +/** + * @brief Time windows configuration for Free Fall detection 1 LSB = 1/ODR_XL time[get] + * + * @param ctx read / write interface definitions + * @param val Time windows configuration for Free Fall detection 1 LSB = 1/ODR_XL time + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_ff_time_windows_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val) +{ + lsm6dsv16x_wake_up_dur_t wake_up_dur; + lsm6dsv16x_free_fall_t free_fall; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_WAKE_UP_DUR, (uint8_t *)&wake_up_dur, 1); + if (ret == 0) { + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FREE_FALL, (uint8_t *)&free_fall, 1); + } + + *val = (wake_up_dur.ff_dur << 5) + free_fall.ff_dur; + + return ret; +} + +/** + * @brief Free fall threshold setting.[set] + * + * @param ctx read / write interface definitions + * @param val 156_mg, 219_mg, 250_mg, 312_mg, 344_mg, 406_mg, 469_mg, 500_mg, + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_ff_thresholds_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ff_thresholds_t val) +{ + lsm6dsv16x_free_fall_t free_fall; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FREE_FALL, (uint8_t *)&free_fall, 1); + if (ret == 0) { + free_fall.ff_ths = (uint8_t)val & 0x7U; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_FREE_FALL, (uint8_t *)&free_fall, 1); + } + + return ret; +} + +/** + * @brief Free fall threshold setting.[get] + * + * @param ctx read / write interface definitions + * @param val 156_mg, 219_mg, 250_mg, 312_mg, 344_mg, 406_mg, 469_mg, 500_mg, + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_ff_thresholds_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ff_thresholds_t *val) +{ + lsm6dsv16x_free_fall_t free_fall; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FREE_FALL, (uint8_t *)&free_fall, 1); + switch (free_fall.ff_ths) { + case LSM6DSV16X_156_mg: + *val = LSM6DSV16X_156_mg; + break; + + case LSM6DSV16X_219_mg: + *val = LSM6DSV16X_219_mg; + break; + + case LSM6DSV16X_250_mg: + *val = LSM6DSV16X_250_mg; + break; + + case LSM6DSV16X_312_mg: + *val = LSM6DSV16X_312_mg; + break; + + case LSM6DSV16X_344_mg: + *val = LSM6DSV16X_344_mg; + break; + + case LSM6DSV16X_406_mg: + *val = LSM6DSV16X_406_mg; + break; + + case LSM6DSV16X_469_mg: + *val = LSM6DSV16X_469_mg; + break; + + case LSM6DSV16X_500_mg: + *val = LSM6DSV16X_500_mg; + break; + + default: + *val = LSM6DSV16X_156_mg; + break; + } + return ret; +} + +/** + * @} + * + */ + +/** + * @defgroup Machine Learning Core (MLC) + * @brief This section group all the functions concerning the + * usage of Machine Learning Core + * @{ + * + */ + +/** + * @brief It enables Machine Learning Core feature (MLC). When the Machine Learning Core is enabled the Finite State Machine (FSM) programs are executed before executing the MLC algorithms.[set] + * + * @param ctx read / write interface definitions + * @param val DISABLE, MLC_BEFORE_FSM, MLC_AFTER_FSM, + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_mlc_mode_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_mlc_mode_t val) +{ + lsm6dsv16x_emb_func_en_b_t emb_func_en_b; + lsm6dsv16x_emb_func_en_a_t emb_func_en_a; + int32_t ret; + + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK); + if (ret == 0) { + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_EMB_FUNC_EN_A, (uint8_t *)&emb_func_en_a, 1); + } + if (ret == 0) { + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_EMB_FUNC_EN_B, (uint8_t *)&emb_func_en_b, 1); + } + emb_func_en_a.mlc_before_fsm_en = (uint8_t)val & 0x01U; + emb_func_en_b.mlc_en = ((uint8_t)val & 0x02U) >> 1; + if (ret == 0) { + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_EMB_FUNC_EN_A, (uint8_t *)&emb_func_en_a, 1); + } + if (ret == 0) { + + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_EMB_FUNC_EN_B, (uint8_t *)&emb_func_en_b, 1); + } + if (ret == 0) { + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + } + + return ret; +} + +/** + * @brief It enables Machine Learning Core feature (MLC). When the Machine Learning Core is enabled the Finite State Machine (FSM) programs are executed before executing the MLC algorithms.[get] + * + * @param ctx read / write interface definitions + * @param val DISABLE, MLC_BEFORE_FSM, MLC_AFTER_FSM, + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_mlc_mode_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_mlc_mode_t *val) +{ + lsm6dsv16x_emb_func_en_a_t emb_func_en_a; + lsm6dsv16x_emb_func_en_b_t emb_func_en_b; + int32_t ret; + + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK); + if (ret == 0) { + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_EMB_FUNC_EN_A, (uint8_t *)&emb_func_en_a, 1); + } + if (ret == 0) { + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_EMB_FUNC_EN_B, (uint8_t *)&emb_func_en_b, 1); + } + if (ret == 0) { + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + } + + switch ((emb_func_en_b.mlc_en << 1) + emb_func_en_a.mlc_before_fsm_en) { + case LSM6DSV16X_DISABLE: + *val = LSM6DSV16X_DISABLE; + break; + + case LSM6DSV16X_MLC_BEFORE_FSM: + *val = LSM6DSV16X_MLC_BEFORE_FSM; + break; + + case LSM6DSV16X_MLC_AFTER_FSM: + *val = LSM6DSV16X_MLC_AFTER_FSM; + break; + + default: + *val = LSM6DSV16X_DISABLE; + break; + } + return ret; +} + +/** + * @brief Machine Learning Core Output Data Rate (ODR) configuration.[set] + * + * @param ctx read / write interface definitions + * @param val MLC_15Hz, MLC_30Hz, MLC_60Hz, MLC_120Hz, MLC_240Hz, MLC_480Hz, MLC_960Hz, + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_mlc_data_rate_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_mlc_data_rate_t val) +{ + lsm6dsv16x_mlc_odr_t mlc_odr; + int32_t ret; + + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK); + if (ret == 0) { + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_MLC_ODR, (uint8_t *)&mlc_odr, 1); + } + + if (ret == 0) { + mlc_odr.mlc_odr = (uint8_t)val & 0x07U; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_MLC_ODR, (uint8_t *)&mlc_odr, 1); + } + if (ret == 0) { + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + } + + return ret; +} + +/** + * @brief Machine Learning Core Output Data Rate (ODR) configuration.[get] + * + * @param ctx read / write interface definitions + * @param val MLC_15Hz, MLC_30Hz, MLC_60Hz, MLC_120Hz, MLC_240Hz, MLC_480Hz, MLC_960Hz, + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_mlc_data_rate_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_mlc_data_rate_t *val) +{ + lsm6dsv16x_mlc_odr_t mlc_odr; + int32_t ret; + + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK); + if (ret == 0) { + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_MLC_ODR, (uint8_t *)&mlc_odr, 1); + } + if (ret == 0) { + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + } + + switch (mlc_odr.mlc_odr) { + case LSM6DSV16X_MLC_15Hz: + *val = LSM6DSV16X_MLC_15Hz; + break; + + case LSM6DSV16X_MLC_30Hz: + *val = LSM6DSV16X_MLC_30Hz; + break; + + case LSM6DSV16X_MLC_60Hz: + *val = LSM6DSV16X_MLC_60Hz; + break; + + case LSM6DSV16X_MLC_120Hz: + *val = LSM6DSV16X_MLC_120Hz; + break; + + case LSM6DSV16X_MLC_240Hz: + *val = LSM6DSV16X_MLC_240Hz; + break; + + case LSM6DSV16X_MLC_480Hz: + *val = LSM6DSV16X_MLC_480Hz; + break; + + case LSM6DSV16X_MLC_960Hz: + *val = LSM6DSV16X_MLC_960Hz; + break; + + default: + *val = LSM6DSV16X_MLC_15Hz; + break; + } + return ret; +} + +/** + * @brief Output value of all MLC decision trees.[get] + * + * @param ctx read / write interface definitions + * @param val Output value of all MLC decision trees. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_mlc_out_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_mlc_out_t *val) +{ + int32_t ret; + + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK); + if (ret == 0) { + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_MLC1_SRC, (uint8_t *)val, 4); + } + if (ret == 0) { + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + } + return ret; +} + +/** + * @brief External sensor sensitivity value register for the Machine Learning Core. This register corresponds to the conversion value of the external sensor. The register value is expressed as half-precision floating-point format: SEEEEEFFFFFFFFFF (S: 1 sign bit; E: 5 exponent bits; F: 10 fraction bits).Default value is 0x3C00 (when using an external magnetometer this value corresponds to 1 gauss/LSB).[set] + * + * @param ctx read / write interface definitions + * @param val External sensor sensitivity value register for the Machine Learning Core. This register corresponds to the conversion value of the external sensor. The register value is expressed as half-precision floating-point format: SEEEEEFFFFFFFFFF (S: 1 sign bit; E: 5 exponent bits; F: 10 fraction bits).Default value is 0x3C00 (when using an external magnetometer this value corresponds to 1 gauss/LSB). + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_mlc_ext_sens_sensitivity_set(lsm6dsv16x_ctx_t *ctx, uint16_t val) +{ + uint8_t buff[2]; + int32_t ret; + + buff[1] = (uint8_t)(val / 256U); + buff[0] = (uint8_t)(val - (buff[1] * 256U)); + + ret = lsm6dsv16x_ln_pg_write(ctx, LSM6DSV16X_EMB_ADV_PG_1 + LSM6DSV16X_MLC_EXT_SENSITIVITY_L, (uint8_t *)&buff[0], 2); + + return ret; +} + +/** + * @brief External sensor sensitivity value register for the Machine Learning Core. This register corresponds to the conversion value of the external sensor. The register value is expressed as half-precision floating-point format: SEEEEEFFFFFFFFFF (S: 1 sign bit; E: 5 exponent bits; F: 10 fraction bits).Default value is 0x3C00 (when using an external magnetometer this value corresponds to 1 gauss/LSB).[get] + * + * @param ctx read / write interface definitions + * @param val External sensor sensitivity value register for the Machine Learning Core. This register corresponds to the conversion value of the external sensor. The register value is expressed as half-precision floating-point format: SEEEEEFFFFFFFFFF (S: 1 sign bit; E: 5 exponent bits; F: 10 fraction bits).Default value is 0x3C00 (when using an external magnetometer this value corresponds to 1 gauss/LSB). + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_mlc_ext_sens_sensitivity_get(lsm6dsv16x_ctx_t *ctx, uint16_t *val) +{ + uint8_t buff[2]; + int32_t ret; + + ret = lsm6dsv16x_ln_pg_read(ctx, LSM6DSV16X_EMB_ADV_PG_1 + LSM6DSV16X_MLC_EXT_SENSITIVITY_L, &buff[0], 2); + *val = buff[1]; + *val = (*val * 256U) + buff[0]; + + return ret; +} + +/** + * @} + * + */ + +/** + * @defgroup Optical Image Stabilization (OIS) + * @brief This section groups all the functions concerning + * Optical Image Stabilization (OIS). + * @{ + * + */ + +/** + * @brief Enable the full control of OIS configurations from the UI (User Interface).[set] + * + * @param ctx read / write interface definitions + * @param val OIS_CTRL_FROM_OIS, OIS_CTRL_FROM_UI, + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_ois_ctrl_mode_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ois_ctrl_mode_t val) +{ + lsm6dsv16x_func_cfg_access_t func_cfg_access; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FUNC_CFG_ACCESS, (uint8_t *)&func_cfg_access, 1); + if (ret == 0) { + func_cfg_access.ois_ctrl_from_ui = (uint8_t)val & 0x1U; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_FUNC_CFG_ACCESS, (uint8_t *)&func_cfg_access, 1); + } + + return ret; +} + +/** + * @brief Enable the full control of OIS configurations from the UI (User Interface).[get] + * + * @param ctx read / write interface definitions + * @param val OIS_CTRL_FROM_OIS, OIS_CTRL_FROM_UI, + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_ois_ctrl_mode_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ois_ctrl_mode_t *val) +{ + lsm6dsv16x_func_cfg_access_t func_cfg_access; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FUNC_CFG_ACCESS, (uint8_t *)&func_cfg_access, 1); + switch (func_cfg_access.ois_ctrl_from_ui) { + case LSM6DSV16X_OIS_CTRL_FROM_OIS: + *val = LSM6DSV16X_OIS_CTRL_FROM_OIS; + break; + + case LSM6DSV16X_OIS_CTRL_FROM_UI: + *val = LSM6DSV16X_OIS_CTRL_FROM_UI; + break; + + default: + *val = LSM6DSV16X_OIS_CTRL_FROM_OIS; + break; + } + return ret; +} + +/** + * @brief Resets the control registers of OIS from the UI (User Interface)[set] + * + * @param ctx read / write interface definitions + * @param val Resets the control registers of OIS from the UI (User Interface) + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_ois_reset_set(lsm6dsv16x_ctx_t *ctx, int8_t val) +{ + lsm6dsv16x_func_cfg_access_t func_cfg_access; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FUNC_CFG_ACCESS, (uint8_t *)&func_cfg_access, 1); + if (ret == 0) { + func_cfg_access.spi2_reset = (uint8_t)val; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_FUNC_CFG_ACCESS, (uint8_t *)&func_cfg_access, 1); + } + + return ret; +} + +/** + * @brief Resets the control registers of OIS from the UI (User Interface)[get] + * + * @param ctx read / write interface definitions + * @param val Resets the control registers of OIS from the UI (User Interface) + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_ois_reset_get(lsm6dsv16x_ctx_t *ctx, int8_t *val) +{ + lsm6dsv16x_func_cfg_access_t func_cfg_access; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FUNC_CFG_ACCESS, (uint8_t *)&func_cfg_access, 1); + *val = (int8_t)func_cfg_access.spi2_reset; + + return ret; +} + +/** + * @brief Enable/disable pull up on OIS interface.[set] + * + * @param ctx read / write interface definitions + * @param val Enable/disable pull up on OIS interface. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_ois_interface_pull_up_set(lsm6dsv16x_ctx_t *ctx, uint8_t val) +{ + lsm6dsv16x_pin_ctrl_t pin_ctrl; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_PIN_CTRL, (uint8_t *)&pin_ctrl, 1); + if (ret == 0) { + pin_ctrl.ois_pu_dis = val; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_PIN_CTRL, (uint8_t *)&pin_ctrl, 1); + } + + return ret; +} + +/** + * @brief Enable/disable pull up on OIS interface.[get] + * + * @param ctx read / write interface definitions + * @param val Enable/disable pull up on OIS interface. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_ois_interface_pull_up_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val) +{ + lsm6dsv16x_pin_ctrl_t pin_ctrl; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_PIN_CTRL, (uint8_t *)&pin_ctrl, 1); + *val = pin_ctrl.ois_pu_dis; + + return ret; +} + +/** + * @brief Handshake for (User Interface) UI / (OIS interface) SPI2 shared registers. ACK: This bit acknowledges the handshake. If the secondary interface is not accessing the shared registers, this bit is set to 1 by the device and the R/W operation on the UI_SPI2_SHARED registers is allowed on the primary interface. REQ: This bit is used by the primary interface master to request access to the UI_SPI2_SHARED registers. When the R/W operation is finished, the master must reset this bit.[set] + * + * @param ctx read / write interface definitions + * @param val Handshake for (User Interface) UI / (OIS interface) SPI2 shared registers. ACK: This bit acknowledges the handshake. If the secondary interface is not accessing the shared registers, this bit is set to 1 by the device and the R/W operation on the UI_SPI2_SHARED registers is allowed on the primary interface. REQ: This bit is used by the primary interface master to request access to the UI_SPI2_SHARED registers. When the R/W operation is finished, the master must reset this bit. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_ois_handshake_from_ui_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ois_handshake_t val) +{ + lsm6dsv16x_ui_handshake_ctrl_t ui_handshake_ctrl; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_UI_HANDSHAKE_CTRL, (uint8_t *)&ui_handshake_ctrl, 1); + if (ret == 0) { + ui_handshake_ctrl.ui_shared_ack = val.ack; + ui_handshake_ctrl.ui_shared_req = val.req; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_UI_HANDSHAKE_CTRL, (uint8_t *)&ui_handshake_ctrl, 1); + } + + return ret; +} + +/** + * @brief Handshake for (User Interface) UI / (OIS interface) SPI2 shared registers. ACK: This bit acknowledges the handshake. If the secondary interface is not accessing the shared registers, this bit is set to 1 by the device and the R/W operation on the UI_SPI2_SHARED registers is allowed on the primary interface. REQ: This bit is used by the primary interface master to request access to the UI_SPI2_SHARED registers. When the R/W operation is finished, the master must reset this bit.[get] + * + * @param ctx read / write interface definitions + * @param val Handshake for (User Interface) UI / (OIS interface) SPI2 shared registers. ACK: This bit acknowledges the handshake. If the secondary interface is not accessing the shared registers, this bit is set to 1 by the device and the R/W operation on the UI_SPI2_SHARED registers is allowed on the primary interface. REQ: This bit is used by the primary interface master to request access to the UI_SPI2_SHARED registers. When the R/W operation is finished, the master must reset this bit. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_ois_handshake_from_ui_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ois_handshake_t *val) +{ + lsm6dsv16x_ui_handshake_ctrl_t ui_handshake_ctrl; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_UI_HANDSHAKE_CTRL, (uint8_t *)&ui_handshake_ctrl, 1); + val->ack = ui_handshake_ctrl.ui_shared_ack; + val->req = ui_handshake_ctrl.ui_shared_req; + + return ret; +} + +/** + * @brief Handshake for (User Interface) UI / (OIS interface) SPI2 shared registers. ACK: This bit acknowledges the handshake. If the secondary interface is not accessing the shared registers, this bit is set to 1 by the device and the R/W operation on the UI_SPI2_SHARED registers is allowed on the primary interface. REQ: This bit is used by the primary interface master to request access to the UI_SPI2_SHARED registers. When the R/W operation is finished, the master must reset this bit.[set] + * + * @param ctx read / write interface definitions + * @param val Handshake for (User Interface) UI / (OIS interface) SPI2 shared registers. ACK: This bit acknowledges the handshake. If the secondary interface is not accessing the shared registers, this bit is set to 1 by the device and the R/W operation on the UI_SPI2_SHARED registers is allowed on the primary interface. REQ: This bit is used by the primary interface master to request access to the UI_SPI2_SHARED registers. When the R/W operation is finished, the master must reset this bit. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_ois_handshake_from_ois_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ois_handshake_t val) +{ + lsm6dsv16x_spi2_handshake_ctrl_t spi2_handshake_ctrl; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_SPI2_HANDSHAKE_CTRL, (uint8_t *)&spi2_handshake_ctrl, 1); + if (ret == 0) { + spi2_handshake_ctrl.spi2_shared_ack = val.ack; + spi2_handshake_ctrl.spi2_shared_req = val.req; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_SPI2_HANDSHAKE_CTRL, (uint8_t *)&spi2_handshake_ctrl, 1); + } + + return ret; +} + +/** + * @brief Handshake for (User Interface) UI / (OIS interface) SPI2 shared registers. ACK: This bit acknowledges the handshake. If the secondary interface is not accessing the shared registers, this bit is set to 1 by the device and the R/W operation on the UI_SPI2_SHARED registers is allowed on the primary interface. REQ: This bit is used by the primary interface master to request access to the UI_SPI2_SHARED registers. When the R/W operation is finished, the master must reset this bit.[get] + * + * @param ctx read / write interface definitions + * @param val Handshake for (User Interface) UI / (OIS interface) SPI2 shared registers. ACK: This bit acknowledges the handshake. If the secondary interface is not accessing the shared registers, this bit is set to 1 by the device and the R/W operation on the UI_SPI2_SHARED registers is allowed on the primary interface. REQ: This bit is used by the primary interface master to request access to the UI_SPI2_SHARED registers. When the R/W operation is finished, the master must reset this bit. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_ois_handshake_from_ois_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ois_handshake_t *val) +{ + lsm6dsv16x_spi2_handshake_ctrl_t spi2_handshake_ctrl; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_SPI2_HANDSHAKE_CTRL, (uint8_t *)&spi2_handshake_ctrl, 1); + val->ack = spi2_handshake_ctrl.spi2_shared_ack; + val->req = spi2_handshake_ctrl.spi2_shared_req; + + return ret; +} + +/** + * @brief User interface (UI) / SPI2 (OIS) shared registers[set] + * + * @param ctx read / write interface definitions + * @param val User interface (UI) / SPI2 (OIS) shared registers + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_ois_shared_set(lsm6dsv16x_ctx_t *ctx, uint8_t val[6]) +{ + int32_t ret; + + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_UI_SPI2_SHARED_0, val, 6); + + return ret; +} + +/** + * @brief User interface (UI) / SPI2 (OIS) shared registers[get] + * + * @param ctx read / write interface definitions + * @param val User interface (UI) / SPI2 (OIS) shared registers + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_ois_shared_get(lsm6dsv16x_ctx_t *ctx, uint8_t val[6]) +{ + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_UI_SPI2_SHARED_0, val, 6); + + return ret; +} + +/** + * @brief In User Interface (UI) full control mode, enables SPI2 (OIS Interface) for reading OIS data. This function works also on OIS (UI_CTRL1_OIS = SPI2_CTRL1_OIS).[set] + * + * @param ctx read / write interface definitions + * @param val In User Interface (UI) full control mode, enables SPI2 (OIS Interface) for reading OIS data. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_ois_on_spi2_set(lsm6dsv16x_ctx_t *ctx, uint8_t val) +{ + lsm6dsv16x_ui_ctrl1_ois_t ui_ctrl1_ois; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_UI_CTRL1_OIS, (uint8_t *)&ui_ctrl1_ois, 1); + if (ret == 0) { + ui_ctrl1_ois.spi2_read_en = val; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_UI_CTRL1_OIS, (uint8_t *)&ui_ctrl1_ois, 1); + } + + return ret; +} + +/** + * @brief In User Interface (UI) full control mode, enables SPI2 (OIS Interface) for reading OIS data. This function works also on OIS (UI_CTRL1_OIS = SPI2_CTRL1_OIS).[get] + * + * @param ctx read / write interface definitions + * @param val In User Interface (UI) full control mode, enables SPI2 (OIS Interface) for reading OIS data. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_ois_on_spi2_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val) +{ + lsm6dsv16x_ui_ctrl1_ois_t ui_ctrl1_ois; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_UI_CTRL1_OIS, (uint8_t *)&ui_ctrl1_ois, 1); + *val = ui_ctrl1_ois.spi2_read_en; + + return ret; +} + +/** + * @brief Enables gyroscope/accelerometer OIS chain. This function works also on OIS (UI_CTRL1_OIS = SPI2_CTRL1_OIS).[set] + * + * @param ctx read / write interface definitions + * @param val Enables gyroscope/accelerometer OIS chain. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_ois_chain_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ois_chain_t val) +{ + lsm6dsv16x_ui_ctrl1_ois_t ui_ctrl1_ois; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_UI_CTRL1_OIS, (uint8_t *)&ui_ctrl1_ois, 1); + if (ret == 0) { + ui_ctrl1_ois.ois_g_en = val.gy; + ui_ctrl1_ois.ois_xl_en = val.xl; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_UI_CTRL1_OIS, (uint8_t *)&ui_ctrl1_ois, 1); + } + + return ret; +} + +/** + * @brief Enables gyroscope/accelerometer OIS chain.[get] + * + * @param ctx read / write interface definitions + * @param val Enables gyroscope/accelerometer OIS chain. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_ois_chain_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ois_chain_t *val) +{ + lsm6dsv16x_ui_ctrl1_ois_t ui_ctrl1_ois; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_UI_CTRL1_OIS, (uint8_t *)&ui_ctrl1_ois, 1); + val->gy = ui_ctrl1_ois.ois_g_en; + val->xl = ui_ctrl1_ois.ois_xl_en; + + return ret; +} + +/** + * @brief Gyroscope OIS full-scale selection[set] + * + * @param ctx read / write interface definitions + * @param val OIS_125dps, OIS_250dps, OIS_500dps, OIS_1000dps, OIS_2000dps, + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_ois_gy_full_scale_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ois_gy_full_scale_t val) +{ + lsm6dsv16x_ui_ctrl2_ois_t ui_ctrl2_ois; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_UI_CTRL2_OIS, (uint8_t *)&ui_ctrl2_ois, 1); + if (ret == 0) { + ui_ctrl2_ois.fs_g_ois = (uint8_t)val & 0x03U; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_UI_CTRL2_OIS, (uint8_t *)&ui_ctrl2_ois, 1); + } + + return ret; +} + +/** + * @brief Gyroscope OIS full-scale selection[get] + * + * @param ctx read / write interface definitions + * @param val OIS_125dps, OIS_250dps, OIS_500dps, OIS_1000dps, OIS_2000dps, + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_ois_gy_full_scale_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ois_gy_full_scale_t *val) +{ + lsm6dsv16x_ui_ctrl2_ois_t ui_ctrl2_ois; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_UI_CTRL2_OIS, (uint8_t *)&ui_ctrl2_ois, 1); + switch (ui_ctrl2_ois.fs_g_ois) { + case LSM6DSV16X_OIS_125dps: + *val = LSM6DSV16X_OIS_125dps; + break; + + case LSM6DSV16X_OIS_250dps: + *val = LSM6DSV16X_OIS_250dps; + break; + + case LSM6DSV16X_OIS_500dps: + *val = LSM6DSV16X_OIS_500dps; + break; + + case LSM6DSV16X_OIS_1000dps: + *val = LSM6DSV16X_OIS_1000dps; + break; + + case LSM6DSV16X_OIS_2000dps: + *val = LSM6DSV16X_OIS_2000dps; + break; + + default: + *val = LSM6DSV16X_OIS_125dps; + break; + } + return ret; +} + +/** + * @brief Selects accelerometer OIS channel full-scale.[set] + * + * @param ctx read / write interface definitions + * @param val OIS_2g, OIS_4g, OIS_8g, OIS_16g, + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_ois_xl_full_scale_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ois_xl_full_scale_t val) +{ + lsm6dsv16x_ui_ctrl3_ois_t ui_ctrl3_ois; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_UI_CTRL3_OIS, (uint8_t *)&ui_ctrl3_ois, 1); + if (ret == 0) { + ui_ctrl3_ois.fs_xl_ois = (uint8_t)val & 0x3U; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_UI_CTRL3_OIS, (uint8_t *)&ui_ctrl3_ois, 1); + } + + return ret; +} + +/** + * @brief Selects accelerometer OIS channel full-scale.[get] + * + * @param ctx read / write interface definitions + * @param val OIS_2g, OIS_4g, OIS_8g, OIS_16g, + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_ois_xl_full_scale_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ois_xl_full_scale_t *val) +{ + lsm6dsv16x_ui_ctrl3_ois_t ui_ctrl3_ois; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_UI_CTRL3_OIS, (uint8_t *)&ui_ctrl3_ois, 1); + switch (ui_ctrl3_ois.fs_xl_ois) { + case LSM6DSV16X_OIS_2g: + *val = LSM6DSV16X_OIS_2g; + break; + + case LSM6DSV16X_OIS_4g: + *val = LSM6DSV16X_OIS_4g; + break; + + case LSM6DSV16X_OIS_8g: + *val = LSM6DSV16X_OIS_8g; + break; + + case LSM6DSV16X_OIS_16g: + *val = LSM6DSV16X_OIS_16g; + break; + + default: + *val = LSM6DSV16X_OIS_2g; + break; + } + return ret; +} + +/** + * @} + * + */ + +/** + * @defgroup Orientation 6D (and 4D) + * @brief This section groups all the functions concerning six position + * detection (6D). + * @{ + * + */ + +/** + * @brief Threshold for 4D/6D function.[set] + * + * @param ctx read / write interface definitions + * @param val DEG_80, DEG_70, DEG_60, DEG_50, + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_6d_threshold_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_6d_threshold_t val) +{ + lsm6dsv16x_tap_ths_6d_t tap_ths_6d; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_TAP_THS_6D, (uint8_t *)&tap_ths_6d, 1); + if (ret == 0) { + tap_ths_6d.sixd_ths = (uint8_t)val & 0x03U; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_TAP_THS_6D, (uint8_t *)&tap_ths_6d, 1); + } + + return ret; +} + +/** + * @brief Threshold for 4D/6D function.[get] + * + * @param ctx read / write interface definitions + * @param val DEG_80, DEG_70, DEG_60, DEG_50, + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_6d_threshold_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_6d_threshold_t *val) +{ + lsm6dsv16x_tap_ths_6d_t tap_ths_6d; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_TAP_THS_6D, (uint8_t *)&tap_ths_6d, 1); + switch (tap_ths_6d.sixd_ths) { + case LSM6DSV16X_DEG_80: + *val = LSM6DSV16X_DEG_80; + break; + + case LSM6DSV16X_DEG_70: + *val = LSM6DSV16X_DEG_70; + break; + + case LSM6DSV16X_DEG_60: + *val = LSM6DSV16X_DEG_60; + break; + + case LSM6DSV16X_DEG_50: + *val = LSM6DSV16X_DEG_50; + break; + + default: + *val = LSM6DSV16X_DEG_80; + break; + } + return ret; +} + +/** + * @brief 4D orientation detection enable. Z-axis position detection is disabled.[set] + * + * @param ctx read / write interface definitions + * @param val 4D orientation detection enable. Z-axis position detection is disabled. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_4d_mode_set(lsm6dsv16x_ctx_t *ctx, uint8_t val) +{ + lsm6dsv16x_tap_ths_6d_t tap_ths_6d; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_TAP_THS_6D, (uint8_t *)&tap_ths_6d, 1); + if (ret == 0) { + tap_ths_6d.d4d_en = val; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_TAP_THS_6D, (uint8_t *)&tap_ths_6d, 1); + } + + return ret; +} + +/** + * @brief 4D orientation detection enable. Z-axis position detection is disabled.[get] + * + * @param ctx read / write interface definitions + * @param val 4D orientation detection enable. Z-axis position detection is disabled. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_4d_mode_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val) +{ + lsm6dsv16x_tap_ths_6d_t tap_ths_6d; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_TAP_THS_6D, (uint8_t *)&tap_ths_6d, 1); + *val = tap_ths_6d.d4d_en; + + return ret; +} + +/** + * @} + * + */ + +/** + * @defgroup AH_QVAR + * @brief This section group all the functions concerning the + * usage of AH_QVAR + * @{ + * + */ + +/** + * @brief Configures the equivalent input impedance of the AH_QVAR buffers.[set] + * + * @param ctx read / write interface definitions + * @param val 2400MOhm, 730MOhm, 300MOhm, 255MOhm, + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_ah_qvar_zin_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ah_qvar_zin_t val) +{ + lsm6dsv16x_ctrl7_t ctrl7; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL7, (uint8_t *)&ctrl7, 1); + if (ret == 0) { + ctrl7.ah_qvar_c_zin = (uint8_t)val & 0x03U; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_CTRL7, (uint8_t *)&ctrl7, 1); + } + + return ret; +} + +/** + * @brief Configures the equivalent input impedance of the AH_QVAR buffers.[get] + * + * @param ctx read / write interface definitions + * @param val 2400MOhm, 730MOhm, 300MOhm, 255MOhm, + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_ah_qvar_zin_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ah_qvar_zin_t *val) +{ + lsm6dsv16x_ctrl7_t ctrl7; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL7, (uint8_t *)&ctrl7, 1); + switch (ctrl7.ah_qvar_c_zin) { + case LSM6DSV16X_2400MOhm: + *val = LSM6DSV16X_2400MOhm; + break; + + case LSM6DSV16X_730MOhm: + *val = LSM6DSV16X_730MOhm; + break; + + case LSM6DSV16X_300MOhm: + *val = LSM6DSV16X_300MOhm; + break; + + case LSM6DSV16X_255MOhm: + *val = LSM6DSV16X_255MOhm; + break; + + default: + *val = LSM6DSV16X_2400MOhm; + break; + } + return ret; +} + +/** + * @brief Enables AH_QVAR chain. When this bit is set to ‘1’, the AH_QVAR buffers are connected to the SDx/AH1/Qvar1 and SCx/AH2/Qvar2 pins. Before setting this bit to 1, the accelerometer and gyroscope sensor have to be configured in power-down mode.[set] + * + * @param ctx read / write interface definitions + * @param val Enables AH_QVAR chain. When this bit is set to ‘1’, the AH_QVAR buffers are connected to the SDx/AH1/Qvar1 and SCx/AH2/Qvar2 pins. Before setting this bit to 1, the accelerometer and gyroscope sensor have to be configured in power-down mode. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_ah_qvar_mode_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ah_qvar_mode_t val) +{ + lsm6dsv16x_ctrl7_t ctrl7; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL7, (uint8_t *)&ctrl7, 1); + if (ret == 0) { + ctrl7.ah_qvar_en = val.ah_qvar_en; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_CTRL7, (uint8_t *)&ctrl7, 1); + } + + return ret; +} + +/** + * @brief Enables AH_QVAR chain. When this bit is set to ‘1’, the AH_QVAR buffers are connected to the SDx/AH1/Qvar1 and SCx/AH2/Qvar2 pins. Before setting this bit to 1, the accelerometer and gyroscope sensor have to be configured in power-down mode.[get] + * + * @param ctx read / write interface definitions + * @param val Enables AH_QVAR chain. When this bit is set to ‘1’, the AH_QVAR buffers are connected to the SDx/AH1/Qvar1 and SCx/AH2/Qvar2 pins. Before setting this bit to 1, the accelerometer and gyroscope sensor have to be configured in power-down mode. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_ah_qvar_mode_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ah_qvar_mode_t *val) +{ + lsm6dsv16x_ctrl7_t ctrl7; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL7, (uint8_t *)&ctrl7, 1); + val->ah_qvar_en = ctrl7.ah_qvar_en; + + return ret; +} + +/** + * @} + * + */ + +/** + * @defgroup SenseWire (I3C) + * @brief This section group all the functions concerning the + * usage of SenseWire (I3C) + * @{ + * + */ + +/** + * @brief Selects the action the device will perform after "Reset whole chip" I3C pattern.[set] + * + * @param ctx read / write interface definitions + * @param val SW_RST_DYN_ADDRESS_RST, GLOBAL_RST, + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_i3c_reset_mode_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_i3c_reset_mode_t val) +{ + lsm6dsv16x_pin_ctrl_t pin_ctrl; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_PIN_CTRL, (uint8_t *)&pin_ctrl, 1); + if (ret == 0) { + pin_ctrl.ibhr_por_en = (uint8_t)val & 0x01U; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_PIN_CTRL, (uint8_t *)&pin_ctrl, 1); + } + + return ret; +} + +/** + * @brief Selects the action the device will perform after "Reset whole chip" I3C pattern.[get] + * + * @param ctx read / write interface definitions + * @param val SW_RST_DYN_ADDRESS_RST, I3C_GLOBAL_RST, + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_i3c_reset_mode_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_i3c_reset_mode_t *val) +{ + lsm6dsv16x_pin_ctrl_t pin_ctrl; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_PIN_CTRL, (uint8_t *)&pin_ctrl, 1); + switch (pin_ctrl.ibhr_por_en) { + case LSM6DSV16X_SW_RST_DYN_ADDRESS_RST: + *val = LSM6DSV16X_SW_RST_DYN_ADDRESS_RST; + break; + + case LSM6DSV16X_I3C_GLOBAL_RST: + *val = LSM6DSV16X_I3C_GLOBAL_RST; + break; + + default: + *val = LSM6DSV16X_SW_RST_DYN_ADDRESS_RST; + break; + } + return ret; +} + +/** + * @brief Select the us activity time for IBI (In-Band Interrupt) with I3C[set] + * + * @param ctx read / write interface definitions + * @param val IBI_2us, IBI_50us, IBI_1ms, IBI_25ms, + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_i3c_ibi_time_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_i3c_ibi_time_t val) +{ + lsm6dsv16x_ctrl5_t ctrl5; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL5, (uint8_t *)&ctrl5, 1); + if (ret == 0) { + ctrl5.bus_act_sel = (uint8_t)val & 0x03U; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_CTRL5, (uint8_t *)&ctrl5, 1); + } + + return ret; +} + +/** + * @brief Select the us activity time for IBI (In-Band Interrupt) with I3C[get] + * + * @param ctx read / write interface definitions + * @param val IBI_2us, IBI_50us, IBI_1ms, IBI_25ms, + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_i3c_ibi_time_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_i3c_ibi_time_t *val) +{ + lsm6dsv16x_ctrl5_t ctrl5; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL5, (uint8_t *)&ctrl5, 1); + switch (ctrl5.bus_act_sel) { + case LSM6DSV16X_IBI_2us: + *val = LSM6DSV16X_IBI_2us; + break; + + case LSM6DSV16X_IBI_50us: + *val = LSM6DSV16X_IBI_50us; + break; + + case LSM6DSV16X_IBI_1ms: + *val = LSM6DSV16X_IBI_1ms; + break; + + case LSM6DSV16X_IBI_25ms: + *val = LSM6DSV16X_IBI_25ms; + break; + + default: + *val = LSM6DSV16X_IBI_2us; + break; + } + return ret; +} + +/** + * @} + * + */ + +/** + * @defgroup Sensor hub + * @brief This section groups all the functions that manage the + * sensor hub. + * @{ + * + */ + +/** + * @brief Sensor Hub master I2C pull-up enable.[set] + * + * @param ctx read / write interface definitions + * @param val Sensor Hub master I2C pull-up enable. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_sh_master_interface_pull_up_set(lsm6dsv16x_ctx_t *ctx, uint8_t val) +{ + lsm6dsv16x_if_cfg_t if_cfg; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_IF_CFG, (uint8_t *)&if_cfg, 1); + if (ret == 0) { + if_cfg.shub_pu_en = val; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_IF_CFG, (uint8_t *)&if_cfg, 1); + } + + return ret; +} + +/** + * @brief Sensor Hub master I2C pull-up enable.[get] + * + * @param ctx read / write interface definitions + * @param val Sensor Hub master I2C pull-up enable. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_sh_master_interface_pull_up_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val) +{ + lsm6dsv16x_if_cfg_t if_cfg; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_IF_CFG, (uint8_t *)&if_cfg, 1); + *val = if_cfg.shub_pu_en; + + return ret; +} + +/** + * @brief Sensor hub output registers.[get] + * + * @param ctx read / write interface definitions + * @param val Sensor hub output registers. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_sh_read_data_raw_get(lsm6dsv16x_ctx_t *ctx, + lsm6dsv16x_emb_sh_read_t *val, + uint8_t len) +{ + int32_t ret; + + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_SENSOR_HUB_MEM_BANK); + if (ret == 0) { + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_SENSOR_HUB_1, (uint8_t *) val, + len); + } + if (ret == 0) { + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + } + + + return ret; +} + +/** + * @brief Number of external sensors to be read by the sensor hub.[set] + * + * @param ctx read / write interface definitions + * @param val SLV_0, SLV_0_1, SLV_0_1_2, SLV_0_1_2_3, + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_sh_slave_connected_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_sh_slave_connected_t val) +{ + lsm6dsv16x_master_config_t master_config; + int32_t ret; + + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_SENSOR_HUB_MEM_BANK); + if (ret == 0) { + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_MASTER_CONFIG, (uint8_t *)&master_config, 1); + } + + if (ret == 0) { + master_config.aux_sens_on = (uint8_t)val & 0x3U; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_MASTER_CONFIG, (uint8_t *)&master_config, 1); + } + if (ret == 0) { + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + } + + return ret; +} + +/** + * @brief Number of external sensors to be read by the sensor hub.[get] + * + * @param ctx read / write interface definitions + * @param val SLV_0, SLV_0_1, SLV_0_1_2, SLV_0_1_2_3, + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_sh_slave_connected_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_sh_slave_connected_t *val) +{ + lsm6dsv16x_master_config_t master_config; + int32_t ret; + + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_SENSOR_HUB_MEM_BANK); + if (ret == 0) { + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_MASTER_CONFIG, (uint8_t *)&master_config, 1); + } + if (ret == 0) { + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + } + + switch (master_config.aux_sens_on) { + case LSM6DSV16X_SLV_0: + *val = LSM6DSV16X_SLV_0; + break; + + case LSM6DSV16X_SLV_0_1: + *val = LSM6DSV16X_SLV_0_1; + break; + + case LSM6DSV16X_SLV_0_1_2: + *val = LSM6DSV16X_SLV_0_1_2; + break; + + case LSM6DSV16X_SLV_0_1_2_3: + *val = LSM6DSV16X_SLV_0_1_2_3; + break; + + default: + *val = LSM6DSV16X_SLV_0; + break; + } + return ret; +} + +/** + * @brief Sensor hub I2C master enable.[set] + * + * @param ctx read / write interface definitions + * @param val Sensor hub I2C master enable. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_sh_master_set(lsm6dsv16x_ctx_t *ctx, uint8_t val) +{ + lsm6dsv16x_master_config_t master_config; + int32_t ret; + + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_SENSOR_HUB_MEM_BANK); + if (ret == 0) { + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_MASTER_CONFIG, (uint8_t *)&master_config, 1); + } + + if (ret == 0) { + master_config.master_on = val; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_MASTER_CONFIG, (uint8_t *)&master_config, 1); + } + if (ret == 0) { + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + } + + return ret; +} + +/** + * @brief Sensor hub I2C master enable.[get] + * + * @param ctx read / write interface definitions + * @param val Sensor hub I2C master enable. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_sh_master_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val) +{ + lsm6dsv16x_master_config_t master_config; + int32_t ret; + + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_SENSOR_HUB_MEM_BANK); + if (ret == 0) { + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_MASTER_CONFIG, (uint8_t *)&master_config, 1); + } + + *val = master_config.master_on; + + if (ret == 0) { + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + } + + return ret; +} + +/** + * @brief I2C interface pass-through.[set] + * + * @param ctx read / write interface definitions + * @param val I2C interface pass-through. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_sh_pass_through_set(lsm6dsv16x_ctx_t *ctx, uint8_t val) +{ + lsm6dsv16x_master_config_t master_config; + int32_t ret; + + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_SENSOR_HUB_MEM_BANK); + if (ret == 0) { + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_MASTER_CONFIG, (uint8_t *)&master_config, 1); + } + + if (ret == 0) { + master_config.pass_through_mode = val; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_MASTER_CONFIG, (uint8_t *)&master_config, 1); + } + if (ret == 0) { + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + } + + return ret; +} + +/** + * @brief I2C interface pass-through.[get] + * + * @param ctx read / write interface definitions + * @param val I2C interface pass-through. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_sh_pass_through_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val) +{ + lsm6dsv16x_master_config_t master_config; + int32_t ret; + + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_SENSOR_HUB_MEM_BANK); + if (ret == 0) { + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_MASTER_CONFIG, (uint8_t *)&master_config, 1); + } + + *val = master_config.pass_through_mode; + + if (ret == 0) { + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + } + + return ret; +} + +/** + * @brief Sensor hub trigger signal selection.[set] + * + * @param ctx read / write interface definitions + * @param val SH_TRG_XL_GY_DRDY, SH_TRIG_INT2, + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_sh_syncro_mode_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_sh_syncro_mode_t val) +{ + lsm6dsv16x_master_config_t master_config; + int32_t ret; + + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_SENSOR_HUB_MEM_BANK); + if (ret == 0) { + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_MASTER_CONFIG, (uint8_t *)&master_config, 1); + } + + if (ret == 0) { + master_config.start_config = (uint8_t)val & 0x01U; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_MASTER_CONFIG, (uint8_t *)&master_config, 1); + } + if (ret == 0) { + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + } + + return ret; +} + +/** + * @brief Sensor hub trigger signal selection.[get] + * + * @param ctx read / write interface definitions + * @param val SH_TRG_XL_GY_DRDY, SH_TRIG_INT2, + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_sh_syncro_mode_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_sh_syncro_mode_t *val) +{ + lsm6dsv16x_master_config_t master_config; + int32_t ret; + + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_SENSOR_HUB_MEM_BANK); + if (ret == 0) { + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_MASTER_CONFIG, (uint8_t *)&master_config, 1); + } + if (ret == 0) { + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + } + + switch (master_config.start_config) { + case LSM6DSV16X_SH_TRG_XL_GY_DRDY: + *val = LSM6DSV16X_SH_TRG_XL_GY_DRDY; + break; + + case LSM6DSV16X_SH_TRIG_INT2: + *val = LSM6DSV16X_SH_TRIG_INT2; + break; + + default: + *val = LSM6DSV16X_SH_TRG_XL_GY_DRDY; + break; + } + return ret; +} + +/** + * @brief Slave 0 write operation is performed only at the first sensor hub cycle.[set] + * + * @param ctx read / write interface definitions + * @param val EACH_SH_CYCLE, ONLY_FIRST_CYCLE, + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_sh_write_mode_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_sh_write_mode_t val) +{ + lsm6dsv16x_master_config_t master_config; + int32_t ret; + + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_SENSOR_HUB_MEM_BANK); + if (ret == 0) { + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_MASTER_CONFIG, (uint8_t *)&master_config, 1); + } + + if (ret == 0) { + master_config.write_once = (uint8_t)val & 0x01U; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_MASTER_CONFIG, (uint8_t *)&master_config, 1); + } + if (ret == 0) { + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + } + + return ret; +} + +/** + * @brief Slave 0 write operation is performed only at the first sensor hub cycle.[get] + * + * @param ctx read / write interface definitions + * @param val EACH_SH_CYCLE, ONLY_FIRST_CYCLE, + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_sh_write_mode_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_sh_write_mode_t *val) +{ + lsm6dsv16x_master_config_t master_config; + int32_t ret; + + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_SENSOR_HUB_MEM_BANK); + if (ret == 0) { + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_MASTER_CONFIG, (uint8_t *)&master_config, 1); + } + if (ret == 0) { + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + } + + + switch (master_config.write_once) { + case LSM6DSV16X_EACH_SH_CYCLE: + *val = LSM6DSV16X_EACH_SH_CYCLE; + break; + + case LSM6DSV16X_ONLY_FIRST_CYCLE: + *val = LSM6DSV16X_ONLY_FIRST_CYCLE; + break; + + default: + *val = LSM6DSV16X_EACH_SH_CYCLE; + break; + } + return ret; +} + +/** + * @brief Reset Master logic and output registers. Must be set to ‘1’ and then set it to ‘0’.[set] + * + * @param ctx read / write interface definitions + * @param val Reset Master logic and output registers. Must be set to ‘1’ and then set it to ‘0’. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_sh_reset_set(lsm6dsv16x_ctx_t *ctx, uint8_t val) +{ + lsm6dsv16x_master_config_t master_config; + int32_t ret; + + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_SENSOR_HUB_MEM_BANK); + if (ret == 0) { + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_MASTER_CONFIG, (uint8_t *)&master_config, 1); + } + + if (ret == 0) { + master_config.rst_master_regs = val; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_MASTER_CONFIG, (uint8_t *)&master_config, 1); + } + if (ret == 0) { + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + } + + return ret; +} + +/** + * @brief Reset Master logic and output registers. Must be set to ‘1’ and then set it to ‘0’.[get] + * + * @param ctx read / write interface definitions + * @param val Reset Master logic and output registers. Must be set to ‘1’ and then set it to ‘0’. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_sh_reset_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val) +{ + lsm6dsv16x_master_config_t master_config; + int32_t ret; + + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_SENSOR_HUB_MEM_BANK); + if (ret == 0) { + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_MASTER_CONFIG, (uint8_t *)&master_config, 1); + } + + *val = master_config.rst_master_regs; + + if (ret == 0) { + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + } + + return ret; +} + +/** + * @brief Configure slave 0 for perform a write.[set] + * + * @param ctx read / write interface definitions + * @param val a structure that contain + * - uint8_t slv1_add; 8 bit i2c device address + * - uint8_t slv1_subadd; 8 bit register device address + * - uint8_t slv1_data; 8 bit data to write + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_sh_cfg_write(lsm6dsv16x_ctx_t *ctx, + lsm6dsv16x_sh_cfg_write_t *val) +{ + lsm6dsv16x_slv0_add_t reg; + int32_t ret; + + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_SENSOR_HUB_MEM_BANK); + if (ret == 0) { + reg.slave0_add = val->slv0_add; + reg.rw_0 = 0; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_SLV0_ADD, (uint8_t *)®, 1); + } + + if (ret == 0) { + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_SLV0_SUBADD, + &(val->slv0_subadd), 1); + } + + if (ret == 0) { + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_DATAWRITE_SLV0, + &(val->slv0_data), 1); + } + + if (ret == 0) { + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + } + + return ret; +} + +/** + * @brief Rate at which the master communicates.[set] + * + * @param ctx read / write interface definitions + * @param val SH_15Hz, SH_30Hz, SH_60Hz, SH_120Hz, SH_240Hz, SH_480Hz, + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_sh_data_rate_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_sh_data_rate_t val) +{ + lsm6dsv16x_slv0_config_t slv0_config; + int32_t ret; + + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_SENSOR_HUB_MEM_BANK); + if (ret == 0) { + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_SLV0_CONFIG, (uint8_t *)&slv0_config, 1); + } + + if (ret == 0) { + slv0_config.shub_odr = (uint8_t)val & 0x07U; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_SLV0_CONFIG, (uint8_t *)&slv0_config, 1); + } + if (ret == 0) { + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + } + + return ret; +} + +/** + * @brief Rate at which the master communicates.[get] + * + * @param ctx read / write interface definitions + * @param val SH_15Hz, SH_30Hz, SH_60Hz, SH_120Hz, SH_240Hz, SH_480Hz, + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_sh_data_rate_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_sh_data_rate_t *val) +{ + lsm6dsv16x_slv0_config_t slv0_config; + int32_t ret; + + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_SENSOR_HUB_MEM_BANK); + if (ret == 0) { + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_SLV0_CONFIG, (uint8_t *)&slv0_config, 1); + } + if (ret == 0) { + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + } + + switch (slv0_config.shub_odr) { + case LSM6DSV16X_SH_15Hz: + *val = LSM6DSV16X_SH_15Hz; + break; + + case LSM6DSV16X_SH_30Hz: + *val = LSM6DSV16X_SH_30Hz; + break; + + case LSM6DSV16X_SH_60Hz: + *val = LSM6DSV16X_SH_60Hz; + break; + + case LSM6DSV16X_SH_120Hz: + *val = LSM6DSV16X_SH_120Hz; + break; + + case LSM6DSV16X_SH_240Hz: + *val = LSM6DSV16X_SH_240Hz; + break; + + case LSM6DSV16X_SH_480Hz: + *val = LSM6DSV16X_SH_480Hz; + break; + + default: + *val = LSM6DSV16X_SH_15Hz; + break; + } + return ret; +} + +/** + * @brief Configure slave 0 for perform a read.[set] + * + * @param ctx read / write interface definitions + * @param val Structure that contain + * - uint8_t slv1_add; 8 bit i2c device address + * - uint8_t slv1_subadd; 8 bit register device address + * - uint8_t slv1_len; num of bit to read + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_sh_slv0_cfg_read(lsm6dsv16x_ctx_t *ctx, + lsm6dsv16x_sh_cfg_read_t *val) +{ + lsm6dsv16x_slv0_add_t slv0_add; + lsm6dsv16x_slv0_config_t slv0_config; + int32_t ret; + + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_SENSOR_HUB_MEM_BANK); + + if (ret == 0) { + slv0_add.slave0_add = val->slv_add; + slv0_add.rw_0 = 1; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_SLV0_ADD, (uint8_t *)&slv0_add, 1); + } + + if (ret == 0) { + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_SLV0_SUBADD, + &(val->slv_subadd), 1); + } + + if (ret == 0) { + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_SLV0_CONFIG, + (uint8_t *)&slv0_config, 1); + } + + if (ret == 0) { + slv0_config.slave0_numop = val->slv_len; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_SLV0_CONFIG, + (uint8_t *)&slv0_config, 1); + } + + if (ret == 0) { + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + } + + return ret; +} + +/** + * @brief Configure slave 0 for perform a write/read.[set] + * + * @param ctx read / write interface definitions + * @param val Structure that contain + * - uint8_t slv1_add; 8 bit i2c device address + * - uint8_t slv1_subadd; 8 bit register device address + * - uint8_t slv1_len; num of bit to read + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_sh_slv1_cfg_read(lsm6dsv16x_ctx_t *ctx, + lsm6dsv16x_sh_cfg_read_t *val) +{ + lsm6dsv16x_slv1_add_t slv1_add; + lsm6dsv16x_slv1_config_t slv1_config; + int32_t ret; + + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_SENSOR_HUB_MEM_BANK); + + if (ret == 0) { + slv1_add.slave1_add = val->slv_add; + slv1_add.r_1 = 1; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_SLV1_ADD, (uint8_t *)&slv1_add, 1); + } + + if (ret == 0) { + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_SLV1_SUBADD, + &(val->slv_subadd), 1); + } + + if (ret == 0) { + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_SLV1_CONFIG, + (uint8_t *)&slv1_config, 1); + } + + if (ret == 0) { + slv1_config.slave1_numop = val->slv_len; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_SLV1_CONFIG, + (uint8_t *)&slv1_config, 1); + } + + if (ret == 0) { + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + } + + return ret; +} + +/** + * @brief Configure slave 0 for perform a write/read.[set] + * + * @param ctx read / write interface definitions + * @param val Structure that contain + * - uint8_t slv2_add; 8 bit i2c device address + * - uint8_t slv2_subadd; 8 bit register device address + * - uint8_t slv2_len; num of bit to read + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_sh_slv2_cfg_read(lsm6dsv16x_ctx_t *ctx, + lsm6dsv16x_sh_cfg_read_t *val) +{ + lsm6dsv16x_slv2_add_t slv2_add; + lsm6dsv16x_slv2_config_t slv2_config; + int32_t ret; + + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_SENSOR_HUB_MEM_BANK); + + if (ret == 0) { + slv2_add.slave2_add = val->slv_add; + slv2_add.r_2 = 1; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_SLV2_ADD, (uint8_t *)&slv2_add, 1); + } + + if (ret == 0) { + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_SLV2_SUBADD, + &(val->slv_subadd), 1); + } + + if (ret == 0) { + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_SLV2_CONFIG, + (uint8_t *)&slv2_config, 1); + } + + if (ret == 0) { + slv2_config.slave2_numop = val->slv_len; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_SLV2_CONFIG, + (uint8_t *)&slv2_config, 1); + } + + if (ret == 0) { + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + } + + return ret; +} + +/** + * @brief Configure slave 0 for perform a write/read.[set] + * + * @param ctx read / write interface definitions + * @param val Structure that contain + * - uint8_t slv3_add; 8 bit i2c device address + * - uint8_t slv3_subadd; 8 bit register device address + * - uint8_t slv3_len; num of bit to read + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_sh_slv3_cfg_read(lsm6dsv16x_ctx_t *ctx, + lsm6dsv16x_sh_cfg_read_t *val) +{ + lsm6dsv16x_slv3_add_t slv3_add; + lsm6dsv16x_slv3_config_t slv3_config; + int32_t ret; + + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_SENSOR_HUB_MEM_BANK); + + if (ret == 0) { + slv3_add.slave3_add = val->slv_add; + slv3_add.r_3 = 1; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_SLV3_ADD, (uint8_t *)&slv3_add, 1); + } + + if (ret == 0) { + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_SLV3_SUBADD, + &(val->slv_subadd), 1); + } + + if (ret == 0) { + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_SLV3_CONFIG, + (uint8_t *)&slv3_config, 1); + } + + if (ret == 0) { + slv3_config.slave3_numop = val->slv_len; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_SLV3_CONFIG, + (uint8_t *)&slv3_config, 1); + } + + if (ret == 0) { + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + } + + return ret; +} + +/** + * @} + * + */ + +/** + * @defgroup Sensors for Smart Mobile Devices (S4S) + * @brief This section groups all the functions that manage the + * Sensors for Smart Mobile Devices. + * @{ + * + */ + +/** + * @brief Sensor synchronization time frame register expressed in number of samples[set] + * + * @param ctx read / write interface definitions + * @param val Sensor synchronization time frame register expressed in number of samples + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_s4s_sync_samples_set(lsm6dsv16x_ctx_t *ctx, uint16_t val) +{ + uint8_t buff[2]; + int32_t ret; + + buff[1] = (uint8_t)(val / 256U); + buff[0] = (uint8_t)(val - (buff[1] * 256U)); + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_S4S_TPH_L, (uint8_t *)&buff[0], 2); + + return ret; +} + +/** + * @brief Sensor synchronization time frame register expressed in number of samples[get] + * + * @param ctx read / write interface definitions + * @param val Sensor synchronization time frame register expressed in number of samples + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_s4s_sync_samples_get(lsm6dsv16x_ctx_t *ctx, uint16_t *val) +{ + uint8_t buff[2]; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_S4S_TPH_L, &buff[0], 2); + *val = buff[1]; + *val = (*val * 256U) + buff[0]; + + return ret; +} + +/** + * @brief Sensor synchronization resolution ratio.[set] + * + * @param ctx read / write interface definitions + * @param val S4S_DT_RES_11, S4S_DT_RES_12, S4S_DT_RES_13, S4S_DT_RES_14, + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_s4s_res_ratio_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_s4s_res_ratio_t val) +{ + lsm6dsv16x_s4s_rr_t s4s_rr; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_S4S_RR, (uint8_t *)&s4s_rr, 1); + if (ret == 0) { + s4s_rr.rr = (uint8_t)val & 0x3U; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_S4S_RR, (uint8_t *)&s4s_rr, 1); + } + + return ret; +} + +/** + * @brief Sensor synchronization resolution ratio.[get] + * + * @param ctx read / write interface definitions + * @param val S4S_DT_RES_11, S4S_DT_RES_12, S4S_DT_RES_13, S4S_DT_RES_14, + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_s4s_res_ratio_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_s4s_res_ratio_t *val) +{ + lsm6dsv16x_s4s_rr_t s4s_rr; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_S4S_RR, (uint8_t *)&s4s_rr, 1); + switch (s4s_rr.rr) { + case LSM6DSV16X_S4S_DT_RES_11: + *val = LSM6DSV16X_S4S_DT_RES_11; + break; + + case LSM6DSV16X_S4S_DT_RES_12: + *val = LSM6DSV16X_S4S_DT_RES_12; + break; + + case LSM6DSV16X_S4S_DT_RES_13: + *val = LSM6DSV16X_S4S_DT_RES_13; + break; + + case LSM6DSV16X_S4S_DT_RES_14: + *val = LSM6DSV16X_S4S_DT_RES_14; + break; + + default: + *val = LSM6DSV16X_S4S_DT_RES_11; + break; + } + return ret; +} + +/** + * @brief Master command code used for S4S.[set] + * + * @param ctx read / write interface definitions + * @param val Master command code used for S4S. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_s4s_command_set(lsm6dsv16x_ctx_t *ctx, uint8_t val) +{ + lsm6dsv16x_s4s_st_cmd_code_t s4s_st_cmd_code; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_S4S_ST_CMD_CODE, (uint8_t *)&s4s_st_cmd_code, 1); + if (ret == 0) { + s4s_st_cmd_code.st_cmd_code = val; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_S4S_ST_CMD_CODE, (uint8_t *)&s4s_st_cmd_code, 1); + } + + return ret; +} + +/** + * @brief Master command code used for S4S.[get] + * + * @param ctx read / write interface definitions + * @param val Master command code used for S4S. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_s4s_command_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val) +{ + lsm6dsv16x_s4s_st_cmd_code_t s4s_st_cmd_code; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_S4S_ST_CMD_CODE, (uint8_t *)&s4s_st_cmd_code, 1); + *val = s4s_st_cmd_code.st_cmd_code; + + return ret; +} + +/** + * @brief DT used for S4S.[set] + * + * @param ctx read / write interface definitions + * @param val DT used for S4S. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_s4s_dt_set(lsm6dsv16x_ctx_t *ctx, uint8_t val) +{ + lsm6dsv16x_s4s_dt_reg_t s4s_dt_reg; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_S4S_DT_REG, (uint8_t *)&s4s_dt_reg, 1); + if (ret == 0) { + s4s_dt_reg.dt = val; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_S4S_DT_REG, (uint8_t *)&s4s_dt_reg, 1); + } + + return ret; +} + +/** + * @brief DT used for S4S.[get] + * + * @param ctx read / write interface definitions + * @param val DT used for S4S. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_s4s_dt_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val) +{ + lsm6dsv16x_s4s_dt_reg_t s4s_dt_reg; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_S4S_DT_REG, (uint8_t *)&s4s_dt_reg, 1); + *val = s4s_dt_reg.dt; + + return ret; +} + +/** + * @} + * + */ + +/** + * @defgroup Serial interfaces + * @brief This section groups all the functions concerning + * serial interfaces management (not auxiliary) + * @{ + * + */ + +/** + * @brief Enables pull-up on SDO pin of UI (User Interface).[set] + * + * @param ctx read / write interface definitions + * @param val Enables pull-up on SDO pin of UI (User Interface). + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_ui_sdo_pull_up_set(lsm6dsv16x_ctx_t *ctx, uint8_t val) +{ + lsm6dsv16x_pin_ctrl_t pin_ctrl; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_PIN_CTRL, (uint8_t *)&pin_ctrl, 1); + if (ret == 0) { + pin_ctrl.sdo_pu_en = val; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_PIN_CTRL, (uint8_t *)&pin_ctrl, 1); + } + + return ret; +} + +/** + * @brief Enables pull-up on SDO pin of UI (User Interface).[get] + * + * @param ctx read / write interface definitions + * @param val Enables pull-up on SDO pin of UI (User Interface). + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_ui_sdo_pull_up_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val) +{ + lsm6dsv16x_pin_ctrl_t pin_ctrl; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_PIN_CTRL, (uint8_t *)&pin_ctrl, 1); + *val = pin_ctrl.sdo_pu_en; + + return ret; +} + +/** + * @brief Disables I2C and I3C on UI (User Interface).[set] + * + * @param ctx read / write interface definitions + * @param val I2C_I3C_ENABLE, I2C_I3C_DISABLE, + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_ui_i2c_i3c_mode_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ui_i2c_i3c_mode_t val) +{ + lsm6dsv16x_if_cfg_t if_cfg; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_IF_CFG, (uint8_t *)&if_cfg, 1); + if (ret == 0) { + if_cfg.i2c_i3c_disable = (uint8_t)val & 0x1U; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_IF_CFG, (uint8_t *)&if_cfg, 1); + } + + return ret; +} + +/** + * @brief Disables I2C and I3C on UI (User Interface).[get] + * + * @param ctx read / write interface definitions + * @param val I2C_I3C_ENABLE, I2C_I3C_DISABLE, + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_ui_i2c_i3c_mode_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ui_i2c_i3c_mode_t *val) +{ + lsm6dsv16x_if_cfg_t if_cfg; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_IF_CFG, (uint8_t *)&if_cfg, 1); + switch (if_cfg.i2c_i3c_disable) { + case LSM6DSV16X_I2C_I3C_ENABLE: + *val = LSM6DSV16X_I2C_I3C_ENABLE; + break; + + case LSM6DSV16X_I2C_I3C_DISABLE: + *val = LSM6DSV16X_I2C_I3C_DISABLE; + break; + + default: + *val = LSM6DSV16X_I2C_I3C_ENABLE; + break; + } + return ret; +} + +/** + * @brief SPI Serial Interface Mode selection.[set] + * + * @param ctx read / write interface definitions + * @param val SPI_4_WIRE, SPI_3_WIRE, + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_spi_mode_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_spi_mode_t val) +{ + lsm6dsv16x_if_cfg_t if_cfg; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_IF_CFG, (uint8_t *)&if_cfg, 1); + if (ret == 0) { + if_cfg.sim = (uint8_t)val & 0x01U; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_IF_CFG, (uint8_t *)&if_cfg, 1); + } + + return ret; +} + +/** + * @brief SPI Serial Interface Mode selection.[get] + * + * @param ctx read / write interface definitions + * @param val SPI_4_WIRE, SPI_3_WIRE, + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_spi_mode_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_spi_mode_t *val) +{ + lsm6dsv16x_if_cfg_t if_cfg; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_IF_CFG, (uint8_t *)&if_cfg, 1); + switch (if_cfg.sim) { + case LSM6DSV16X_SPI_4_WIRE: + *val = LSM6DSV16X_SPI_4_WIRE; + break; + + case LSM6DSV16X_SPI_3_WIRE: + *val = LSM6DSV16X_SPI_3_WIRE; + break; + + default: + *val = LSM6DSV16X_SPI_4_WIRE; + break; + } + return ret; +} + +/** + * @brief Enables pull-up on SDA pin.[set] + * + * @param ctx read / write interface definitions + * @param val Enables pull-up on SDA pin. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_ui_sda_pull_up_set(lsm6dsv16x_ctx_t *ctx, uint8_t val) +{ + lsm6dsv16x_if_cfg_t if_cfg; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_IF_CFG, (uint8_t *)&if_cfg, 1); + if (ret == 0) { + if_cfg.sda_pu_en = val; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_IF_CFG, (uint8_t *)&if_cfg, 1); + } + + return ret; +} + +/** + * @brief Enables pull-up on SDA pin.[get] + * + * @param ctx read / write interface definitions + * @param val Enables pull-up on SDA pin. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_ui_sda_pull_up_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val) +{ + lsm6dsv16x_if_cfg_t if_cfg; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_IF_CFG, (uint8_t *)&if_cfg, 1); + *val = if_cfg.sda_pu_en; + + return ret; +} + +/** + * @brief SPI2 (OIS Interface) Serial Interface Mode selection. This function works also on OIS (UI_CTRL1_OIS = SPI2_CTRL1_OIS).[set] + * + * @param ctx read / write interface definitions + * @param val SPI2_4_WIRE, SPI2_3_WIRE, + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_spi2_mode_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_spi2_mode_t val) +{ + lsm6dsv16x_ui_ctrl1_ois_t ui_ctrl1_ois; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_UI_CTRL1_OIS, (uint8_t *)&ui_ctrl1_ois, 1); + if (ret == 0) { + ui_ctrl1_ois.sim_ois = (uint8_t)val & 0x01U; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_UI_CTRL1_OIS, (uint8_t *)&ui_ctrl1_ois, 1); + } + + return ret; +} + +/** + * @brief SPI2 (OIS Interface) Serial Interface Mode selection. This function works also on OIS (UI_CTRL1_OIS = SPI2_CTRL1_OIS).[get] + * + * @param ctx read / write interface definitions + * @param val SPI2_4_WIRE, SPI2_3_WIRE, + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_spi2_mode_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_spi2_mode_t *val) +{ + lsm6dsv16x_ui_ctrl1_ois_t ui_ctrl1_ois; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_UI_CTRL1_OIS, (uint8_t *)&ui_ctrl1_ois, 1); + switch (ui_ctrl1_ois.sim_ois) { + case LSM6DSV16X_SPI2_4_WIRE: + *val = LSM6DSV16X_SPI2_4_WIRE; + break; + + case LSM6DSV16X_SPI2_3_WIRE: + *val = LSM6DSV16X_SPI2_3_WIRE; + break; + + default: + *val = LSM6DSV16X_SPI2_4_WIRE; + break; + } + return ret; +} + +/** + * @} + * + */ + +/** + * @defgroup Significant motion detection + * @brief This section groups all the functions that manage the + * significant motion detection. + * @{ + * + */ + + +/** + * @brief Enables significant motion detection function.[set] + * + * @param ctx read / write interface definitions + * @param val Enables significant motion detection function. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_sigmot_mode_set(lsm6dsv16x_ctx_t *ctx, uint8_t val) +{ + lsm6dsv16x_emb_func_en_a_t emb_func_en_a; + int32_t ret; + + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK); + if (ret == 0) { + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_EMB_FUNC_EN_A, (uint8_t *)&emb_func_en_a, 1); + } + if (ret == 0) { + emb_func_en_a.sign_motion_en = val; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_EMB_FUNC_EN_A, (uint8_t *)&emb_func_en_a, 1); + } + if (ret == 0) { + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + } + + + return ret; +} + +/** + * @brief Enables significant motion detection function.[get] + * + * @param ctx read / write interface definitions + * @param val Enables significant motion detection function. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_sigmot_mode_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val) +{ + lsm6dsv16x_emb_func_en_a_t emb_func_en_a; + int32_t ret; + + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK); + if (ret == 0) { + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_EMB_FUNC_EN_A, (uint8_t *)&emb_func_en_a, 1); + } + *val = emb_func_en_a.sign_motion_en; + + if (ret == 0) { + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + } + + return ret; +} + +/** + * @} + * + */ + +/** + * @defgroup Step Counter (Pedometer) + * @brief This section groups all the functions that manage pedometer. + * @{ + * + */ + +/** + * @brief Step counter mode[set] + * + * @param ctx read / write interface definitions + * @param val Step counter mode + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_stpcnt_mode_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_stpcnt_mode_t val) +{ + lsm6dsv16x_emb_func_en_a_t emb_func_en_a; + lsm6dsv16x_emb_func_en_b_t emb_func_en_b; + lsm6dsv16x_pedo_cmd_reg_t pedo_cmd_reg; + int32_t ret; + + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK); + if (ret == 0) { + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_EMB_FUNC_EN_A, (uint8_t *)&emb_func_en_a, 1); + } + if (ret == 0) { + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_EMB_FUNC_EN_B, (uint8_t *)&emb_func_en_b, 1); + } + if ((val.false_step_rej == PROPERTY_ENABLE) && ((emb_func_en_a.mlc_before_fsm_en & emb_func_en_b.mlc_en) == PROPERTY_DISABLE)) { + emb_func_en_a.mlc_before_fsm_en = PROPERTY_ENABLE; + } + if (ret == 0) { + emb_func_en_a.pedo_en = val.step_counter_enable; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_EMB_FUNC_EN_A, (uint8_t *)&emb_func_en_a, 1); + } + if (ret == 0) { + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + } + + if (ret == 0) { + ret = lsm6dsv16x_ln_pg_read(ctx, LSM6DSV16X_EMB_ADV_PG_1 + LSM6DSV16X_PEDO_CMD_REG, (uint8_t *)&pedo_cmd_reg, 1); + } + if (ret == 0) { + pedo_cmd_reg.fp_rejection_en = val.false_step_rej; + ret = lsm6dsv16x_ln_pg_write(ctx, LSM6DSV16X_EMB_ADV_PG_1 + LSM6DSV16X_PEDO_CMD_REG, (uint8_t *)&pedo_cmd_reg, 1); + } + + return ret; +} + +/** + * @brief Step counter mode[get] + * + * @param ctx read / write interface definitions + * @param val false_step_rej, step_counter, step_detector, + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_stpcnt_mode_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_stpcnt_mode_t *val) +{ + lsm6dsv16x_emb_func_en_a_t emb_func_en_a; + lsm6dsv16x_pedo_cmd_reg_t pedo_cmd_reg; + int32_t ret; + + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK); + if (ret == 0) { + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_EMB_FUNC_EN_A, (uint8_t *)&emb_func_en_a, 1); + } + if (ret == 0) { + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + } + + if (ret == 0) { + ret = lsm6dsv16x_ln_pg_read(ctx, LSM6DSV16X_EMB_ADV_PG_1 + LSM6DSV16X_PEDO_CMD_REG, (uint8_t *)&pedo_cmd_reg, 1); + } + val->false_step_rej = pedo_cmd_reg.fp_rejection_en; + val->step_counter_enable = emb_func_en_a.pedo_en; + + return ret; +} + +/** + * @brief Step counter output, number of detected steps.[get] + * + * @param ctx read / write interface definitions + * @param val Step counter output, number of detected steps. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_stpcnt_steps_get(lsm6dsv16x_ctx_t *ctx, uint16_t *val) +{ + uint8_t buff[2]; + int32_t ret; + + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK); + if (ret == 0) { + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_STEP_COUNTER_L, &buff[0], 2); + } + if (ret == 0) { + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + } + *val = buff[1]; + *val = (*val * 256U) + buff[0]; + + return ret; +} + +/** + * @brief Reset step counter.[set] + * + * @param ctx read / write interface definitions + * @param val Reset step counter. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_stpcnt_rst_step_set(lsm6dsv16x_ctx_t *ctx, uint8_t val) +{ + lsm6dsv16x_emb_func_src_t emb_func_src; + int32_t ret; + + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK); + if (ret == 0) { + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_EMB_FUNC_SRC, (uint8_t *)&emb_func_src, 1); + } + + if (ret == 0) { + emb_func_src.pedo_rst_step = val; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_EMB_FUNC_SRC, (uint8_t *)&emb_func_src, 1); + } + if (ret == 0) { + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + } + + return ret; +} + +/** + * @brief Reset step counter.[get] + * + * @param ctx read / write interface definitions + * @param val Reset step counter. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_stpcnt_rst_step_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val) +{ + lsm6dsv16x_emb_func_src_t emb_func_src; + int32_t ret; + + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK); + if (ret == 0) { + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_EMB_FUNC_SRC, (uint8_t *)&emb_func_src, 1); + } + + *val = emb_func_src.pedo_rst_step; + + if (ret == 0) { + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + } + + return ret; +} + +/** + * @brief Pedometer debounce configuration.[set] + * + * @param ctx read / write interface definitions + * @param val Pedometer debounce configuration. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_stpcnt_debounce_set(lsm6dsv16x_ctx_t *ctx, uint8_t val) +{ + lsm6dsv16x_pedo_deb_steps_conf_t pedo_deb_steps_conf; + int32_t ret; + + ret = lsm6dsv16x_ln_pg_read(ctx, LSM6DSV16X_EMB_ADV_PG_1 + LSM6DSV16X_PEDO_DEB_STEPS_CONF, (uint8_t *)&pedo_deb_steps_conf, 1); + if (ret == 0) { + pedo_deb_steps_conf.deb_step = val; + ret = lsm6dsv16x_ln_pg_write(ctx, LSM6DSV16X_EMB_ADV_PG_1 + LSM6DSV16X_PEDO_DEB_STEPS_CONF, (uint8_t *)&pedo_deb_steps_conf, 1); + } + + return ret; +} + +/** + * @brief Pedometer debounce configuration.[get] + * + * @param ctx read / write interface definitions + * @param val Pedometer debounce configuration. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_stpcnt_debounce_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val) +{ + lsm6dsv16x_pedo_deb_steps_conf_t pedo_deb_steps_conf; + int32_t ret; + + ret = lsm6dsv16x_ln_pg_read(ctx, LSM6DSV16X_EMB_ADV_PG_1 + LSM6DSV16X_PEDO_DEB_STEPS_CONF, (uint8_t *)&pedo_deb_steps_conf, 1); + *val = pedo_deb_steps_conf.deb_step; + + return ret; +} + +/** + * @brief Time period register for step detection on delta time.[set] + * + * @param ctx read / write interface definitions + * @param val Time period register for step detection on delta time. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_stpcnt_period_set(lsm6dsv16x_ctx_t *ctx, uint16_t val) +{ + uint8_t buff[2]; + int32_t ret; + + buff[1] = (uint8_t)(val / 256U); + buff[0] = (uint8_t)(val - (buff[1] * 256U)); + ret = lsm6dsv16x_ln_pg_write(ctx, LSM6DSV16X_EMB_ADV_PG_1 + LSM6DSV16X_PEDO_SC_DELTAT_L, (uint8_t *)&buff[0], 2); + + return ret; +} + +/** + * @brief Time period register for step detection on delta time.[get] + * + * @param ctx read / write interface definitions + * @param val Time period register for step detection on delta time. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_stpcnt_period_get(lsm6dsv16x_ctx_t *ctx, uint16_t *val) +{ + uint8_t buff[2]; + int32_t ret; + + ret = lsm6dsv16x_ln_pg_read(ctx, LSM6DSV16X_EMB_ADV_PG_1 + LSM6DSV16X_PEDO_SC_DELTAT_L, &buff[0], 2); + *val = buff[1]; + *val = (*val * 256U) + buff[0]; + + return ret; +} + +/** + * @} + * + */ + +/** + * @defgroup Tap - Double Tap + * @brief This section groups all the functions that manage the + * tap and double tap event generation. + * @{ + * + */ + +/** + * @brief Enable axis for Tap - Double Tap detection.[set] + * + * @param ctx read / write interface definitions + * @param val Enable axis for Tap - Double Tap detection. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_tap_detection_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_tap_detection_t val) +{ + lsm6dsv16x_tap_cfg0_t tap_cfg0; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_TAP_CFG0, (uint8_t *)&tap_cfg0, 1); + if (ret == 0) { + tap_cfg0.tap_z_en = val.tap_x_en; + tap_cfg0.tap_y_en = val.tap_y_en; + tap_cfg0.tap_x_en = val.tap_z_en; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_TAP_CFG0, (uint8_t *)&tap_cfg0, 1); + } + + return ret; +} + +/** + * @brief Enable axis for Tap - Double Tap detection.[get] + * + * @param ctx read / write interface definitions + * @param val Enable axis for Tap - Double Tap detection. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_tap_detection_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_tap_detection_t *val) +{ + lsm6dsv16x_tap_cfg0_t tap_cfg0; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_TAP_CFG0, (uint8_t *)&tap_cfg0, 1); + val->tap_x_en = tap_cfg0.tap_z_en; + val->tap_y_en = tap_cfg0.tap_y_en; + val->tap_z_en = tap_cfg0.tap_z_en; + + return ret; +} + +/** + * @brief axis Tap - Double Tap recognition thresholds.[set] + * + * @param ctx read / write interface definitions + * @param val axis Tap - Double Tap recognition thresholds. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_tap_thresholds_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_tap_thresholds_t val) +{ + lsm6dsv16x_tap_ths_6d_t tap_ths_6d; + lsm6dsv16x_tap_cfg2_t tap_cfg2; + lsm6dsv16x_tap_cfg1_t tap_cfg1; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_TAP_CFG1, (uint8_t *)&tap_cfg1, 1); + if (ret == 0) { + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_TAP_CFG2, (uint8_t *)&tap_cfg2, 1); + } + if (ret == 0) { + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_TAP_THS_6D, (uint8_t *)&tap_ths_6d, 1); + } + + tap_cfg1.tap_ths_x = val.x; + tap_cfg2.tap_ths_y = val.y; + tap_ths_6d.tap_ths_z = val.z; + + if (ret == 0) { + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_TAP_THS_6D, (uint8_t *)&tap_ths_6d, 1); + } + if (ret == 0) { + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_TAP_CFG2, (uint8_t *)&tap_cfg2, 1); + } + if (ret == 0) { + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_TAP_CFG1, (uint8_t *)&tap_cfg1, 1); + } + + return ret; +} + +/** + * @brief axis Tap - Double Tap recognition thresholds.[get] + * + * @param ctx read / write interface definitions + * @param val axis Tap - Double Tap recognition thresholds. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_tap_thresholds_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_tap_thresholds_t *val) +{ + lsm6dsv16x_tap_ths_6d_t tap_ths_6d; + lsm6dsv16x_tap_cfg2_t tap_cfg2; + lsm6dsv16x_tap_cfg1_t tap_cfg1; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_TAP_CFG1, (uint8_t *)&tap_cfg1, 1); + if (ret == 0) { + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_TAP_CFG2, (uint8_t *)&tap_cfg2, 1); + } + if (ret == 0) { + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_TAP_THS_6D, (uint8_t *)&tap_ths_6d, 1); + } + + val->x = tap_cfg1.tap_ths_x; + val->y = tap_cfg2.tap_ths_y; + val->z = tap_ths_6d.tap_ths_z; + + return ret; +} + +/** + * @brief Selection of axis priority for TAP detection.[set] + * + * @param ctx read / write interface definitions + * @param val XYZ , YXZ , XZY, ZYX , YZX , ZXY , + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_tap_axis_priority_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_tap_axis_priority_t val) +{ + lsm6dsv16x_tap_cfg1_t tap_cfg1; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_TAP_CFG1, (uint8_t *)&tap_cfg1, 1); + if (ret == 0) { + tap_cfg1.tap_priority = (uint8_t)val & 0x7U; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_TAP_CFG1, (uint8_t *)&tap_cfg1, 1); + } + + return ret; +} + +/** + * @brief Selection of axis priority for TAP detection.[get] + * + * @param ctx read / write interface definitions + * @param val XYZ , YXZ , XZY, ZYX , YZX , ZXY , + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_tap_axis_priority_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_tap_axis_priority_t *val) +{ + lsm6dsv16x_tap_cfg1_t tap_cfg1; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_TAP_CFG1, (uint8_t *)&tap_cfg1, 1); + switch (tap_cfg1.tap_priority) { + case LSM6DSV16X_XYZ : + *val = LSM6DSV16X_XYZ ; + break; + + case LSM6DSV16X_YXZ : + *val = LSM6DSV16X_YXZ ; + break; + + case LSM6DSV16X_XZY: + *val = LSM6DSV16X_XZY; + break; + + case LSM6DSV16X_ZYX : + *val = LSM6DSV16X_ZYX ; + break; + + case LSM6DSV16X_YZX : + *val = LSM6DSV16X_YZX ; + break; + + case LSM6DSV16X_ZXY : + *val = LSM6DSV16X_ZXY ; + break; + + default: + *val = LSM6DSV16X_XYZ ; + break; + } + return ret; +} + +/** + * @brief Time windows configuration for Tap - Double Tap SHOCK, QUIET, DUR : SHOCK Maximum duration is the maximum time of an overthreshold signal detection to be recognized as a tap event. The default value of these bits is 00b which corresponds to 4/ODR_XL time. If the SHOCK bits are set to a different value, 1LSB corresponds to 8/ODR_XL time. QUIET Expected quiet time after a tap detection. Quiet time is the time after the first detected tap in which there must not be any overthreshold event. The default value of these bits is 00b which corresponds to 2/ODR_XL time. If the QUIET bits are set to a different value, 1LSB corresponds to 4/ODR_XL time. DUR Duration of maximum time gap for double tap recognition. When double tap recognition is enabled, this register expresses the maximum time between two consecutive detected taps to determine a double tap event. The default value of these bits is 0000b which corresponds to 16/ODR_XL time. If the DUR_[3:0] bits are set to a different value, 1LSB corresponds to 32/ODR_XL time.[set] + * + * @param ctx read / write interface definitions + * @param val Time windows configuration for Tap - Double Tap SHOCK, QUIET, DUR : SHOCK Maximum duration is the maximum time of an overthreshold signal detection to be recognized as a tap event. The default value of these bits is 00b which corresponds to 4/ODR_XL time. If the SHOCK bits are set to a different value, 1LSB corresponds to 8/ODR_XL time. QUIET Expected quiet time after a tap detection. Quiet time is the time after the first detected tap in which there must not be any overthreshold event. The default value of these bits is 00b which corresponds to 2/ODR_XL time. If the QUIET bits are set to a different value, 1LSB corresponds to 4/ODR_XL time. DUR Duration of maximum time gap for double tap recognition. When double tap recognition is enabled, this register expresses the maximum time between two consecutive detected taps to determine a double tap event. The default value of these bits is 0000b which corresponds to 16/ODR_XL time. If the DUR_[3:0] bits are set to a different value, 1LSB corresponds to 32/ODR_XL time. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_tap_time_windows_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_tap_time_windows_t val) +{ + lsm6dsv16x_int_dur2_t int_dur2; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_INT_DUR2, (uint8_t *)&int_dur2, 1); + if (ret == 0) { + int_dur2.shock = val.shock; + int_dur2.quiet = val.quiet; + int_dur2.dur = val.tap_gap; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_INT_DUR2, (uint8_t *)&int_dur2, 1); + } + + return ret; +} + +/** + * @brief Time windows configuration for Tap - Double Tap SHOCK, QUIET, DUR : SHOCK Maximum duration is the maximum time of an overthreshold signal detection to be recognized as a tap event. The default value of these bits is 00b which corresponds to 4/ODR_XL time. If the SHOCK bits are set to a different value, 1LSB corresponds to 8/ODR_XL time. QUIET Expected quiet time after a tap detection. Quiet time is the time after the first detected tap in which there must not be any overthreshold event. The default value of these bits is 00b which corresponds to 2/ODR_XL time. If the QUIET bits are set to a different value, 1LSB corresponds to 4/ODR_XL time. DUR Duration of maximum time gap for double tap recognition. When double tap recognition is enabled, this register expresses the maximum time between two consecutive detected taps to determine a double tap event. The default value of these bits is 0000b which corresponds to 16/ODR_XL time. If the DUR_[3:0] bits are set to a different value, 1LSB corresponds to 32/ODR_XL time.[get] + * + * @param ctx read / write interface definitions + * @param val Time windows configuration for Tap - Double Tap SHOCK, QUIET, DUR : SHOCK Maximum duration is the maximum time of an overthreshold signal detection to be recognized as a tap event. The default value of these bits is 00b which corresponds to 4/ODR_XL time. If the SHOCK bits are set to a different value, 1LSB corresponds to 8/ODR_XL time. QUIET Expected quiet time after a tap detection. Quiet time is the time after the first detected tap in which there must not be any overthreshold event. The default value of these bits is 00b which corresponds to 2/ODR_XL time. If the QUIET bits are set to a different value, 1LSB corresponds to 4/ODR_XL time. DUR Duration of maximum time gap for double tap recognition. When double tap recognition is enabled, this register expresses the maximum time between two consecutive detected taps to determine a double tap event. The default value of these bits is 0000b which corresponds to 16/ODR_XL time. If the DUR_[3:0] bits are set to a different value, 1LSB corresponds to 32/ODR_XL time. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_tap_time_windows_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_tap_time_windows_t *val) +{ + lsm6dsv16x_int_dur2_t int_dur2; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_INT_DUR2, (uint8_t *)&int_dur2, 1); + val->shock = int_dur2.shock; + val->quiet = int_dur2.quiet; + val->tap_gap = int_dur2.dur; + + return ret; +} + +/** + * @brief Single/double-tap event enable.[set] + * + * @param ctx read / write interface definitions + * @param val ONLY_SINGLE, BOTH_SINGLE_DOUBLE, + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_tap_mode_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_tap_mode_t val) +{ + lsm6dsv16x_wake_up_ths_t wake_up_ths; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_WAKE_UP_THS, (uint8_t *)&wake_up_ths, 1); + if (ret == 0) { + wake_up_ths.single_double_tap = (uint8_t)val & 0x01U; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_WAKE_UP_THS, (uint8_t *)&wake_up_ths, 1); + } + + return ret; +} + +/** + * @brief Single/double-tap event enable.[get] + * + * @param ctx read / write interface definitions + * @param val ONLY_SINGLE, BOTH_SINGLE_DOUBLE, + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_tap_mode_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_tap_mode_t *val) +{ + lsm6dsv16x_wake_up_ths_t wake_up_ths; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_WAKE_UP_THS, (uint8_t *)&wake_up_ths, 1); + switch (wake_up_ths.single_double_tap) { + case LSM6DSV16X_ONLY_SINGLE: + *val = LSM6DSV16X_ONLY_SINGLE; + break; + + case LSM6DSV16X_BOTH_SINGLE_DOUBLE: + *val = LSM6DSV16X_BOTH_SINGLE_DOUBLE; + break; + + default: + *val = LSM6DSV16X_ONLY_SINGLE; + break; + } + return ret; +} + +/** + * @} + * + */ + +/** + * @defgroup Tilt detection + * @brief This section groups all the functions that manage the tilt + * event detection. + * @{ + * + */ + +/** + * @brief Tilt calculation.[set] + * + * @param ctx read / write interface definitions + * @param val Tilt calculation. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_tilt_mode_set(lsm6dsv16x_ctx_t *ctx, uint8_t val) +{ + lsm6dsv16x_emb_func_en_a_t emb_func_en_a; + int32_t ret; + + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK); + if (ret == 0) { + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_EMB_FUNC_EN_A, (uint8_t *)&emb_func_en_a, 1); + } + if (ret == 0) { + emb_func_en_a.tilt_en = val; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_EMB_FUNC_EN_A, (uint8_t *)&emb_func_en_a, 1); + } + if (ret == 0) { + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + } + + return ret; +} + +/** + * @brief Tilt calculation.[get] + * + * @param ctx read / write interface definitions + * @param val Tilt calculation. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_tilt_mode_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val) +{ + lsm6dsv16x_emb_func_en_a_t emb_func_en_a; + int32_t ret; + + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK); + if (ret == 0) { + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_EMB_FUNC_EN_A, (uint8_t *)&emb_func_en_a, 1); + } + *val = emb_func_en_a.tilt_en; + + if (ret == 0) { + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + } + + return ret; +} + +/** + * @} + * + */ + +/** + * @defgroup Timestamp + * @brief This section groups all the functions that manage the + * timestamp generation. + * @{ + * + */ + +/** + * @brief Timestamp data output.[get] + * + * @param ctx read / write interface definitions + * @param val Timestamp data output. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_timestamp_raw_get(lsm6dsv16x_ctx_t *ctx, uint32_t *val) +{ + uint8_t buff[4]; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_TIMESTAMP0, &buff[0], 4); + *val = buff[3]; + *val = (*val * 256U) + buff[2]; + *val = (*val * 256U) + buff[1]; + *val = (*val * 256U) + buff[0]; + + return ret; +} + +/** + * @brief Enables timestamp counter.[set] + * + * @param ctx read / write interface definitions + * @param val Enables timestamp counter. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_timestamp_set(lsm6dsv16x_ctx_t *ctx, uint8_t val) +{ + lsm6dsv16x_functions_enable_t functions_enable; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FUNCTIONS_ENABLE, (uint8_t *)&functions_enable, 1); + if (ret == 0) { + functions_enable.timestamp_en = val; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_FUNCTIONS_ENABLE, (uint8_t *)&functions_enable, 1); + } + + return ret; +} + +/** + * @brief Enables timestamp counter.[get] + * + * @param ctx read / write interface definitions + * @param val Enables timestamp counter. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_timestamp_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val) +{ + lsm6dsv16x_functions_enable_t functions_enable; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FUNCTIONS_ENABLE, (uint8_t *)&functions_enable, 1); + *val = functions_enable.timestamp_en; + + return ret; +} + +/** + * @} + * + */ + +/** + * @defgroup Wake Up - Activity - Inactivity (Sleep) + * @brief This section groups all the functions that manage the Wake Up + * event generation. + * @{ + * + */ + +/** + * @brief Enable activity/inactivity (sleep) function.[set] + * + * @param ctx read / write interface definitions + * @param val XL_AND_GY_NOT_AFFECTED, XL_LOW_POWER_GY_NOT_AFFECTED, XL_LOW_POWER_GY_SLEEP, XL_LOW_POWER_GY_POWER_DOWN, + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_act_mode_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_act_mode_t val) +{ + lsm6dsv16x_functions_enable_t functions_enable; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FUNCTIONS_ENABLE, (uint8_t *)&functions_enable, 1); + if (ret == 0) { + functions_enable.inact_en = (uint8_t)val & 0x3U; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_FUNCTIONS_ENABLE, (uint8_t *)&functions_enable, 1); + } + + return ret; +} + +/** + * @brief Enable activity/inactivity (sleep) function.[get] + * + * @param ctx read / write interface definitions + * @param val XL_AND_GY_NOT_AFFECTED, XL_LOW_POWER_GY_NOT_AFFECTED, XL_LOW_POWER_GY_SLEEP, XL_LOW_POWER_GY_POWER_DOWN, + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_act_mode_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_act_mode_t *val) +{ + lsm6dsv16x_functions_enable_t functions_enable; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FUNCTIONS_ENABLE, (uint8_t *)&functions_enable, 1); + switch (functions_enable.inact_en) { + case LSM6DSV16X_XL_AND_GY_NOT_AFFECTED: + *val = LSM6DSV16X_XL_AND_GY_NOT_AFFECTED; + break; + + case LSM6DSV16X_XL_LOW_POWER_GY_NOT_AFFECTED: + *val = LSM6DSV16X_XL_LOW_POWER_GY_NOT_AFFECTED; + break; + + case LSM6DSV16X_XL_LOW_POWER_GY_SLEEP: + *val = LSM6DSV16X_XL_LOW_POWER_GY_SLEEP; + break; + + case LSM6DSV16X_XL_LOW_POWER_GY_POWER_DOWN: + *val = LSM6DSV16X_XL_LOW_POWER_GY_POWER_DOWN; + break; + + default: + *val = LSM6DSV16X_XL_AND_GY_NOT_AFFECTED; + break; + } + return ret; +} + +/** + * @brief Duration in the transition from Stationary to Motion (from Inactivity to Activity).[set] + * + * @param ctx read / write interface definitions + * @param val SLEEP_TO_ACT_AT_1ST_SAMPLE, SLEEP_TO_ACT_AT_2ND_SAMPLE, SLEEP_TO_ACT_AT_3RD_SAMPLE, SLEEP_TO_ACT_AT_4th_SAMPLE, + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_act_from_sleep_to_act_dur_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_act_from_sleep_to_act_dur_t val) +{ + lsm6dsv16x_inactivity_dur_t inactivity_dur; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_INACTIVITY_DUR, (uint8_t *)&inactivity_dur, 1); + if (ret == 0) { + inactivity_dur.inact_dur = (uint8_t)val & 0x3U; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_INACTIVITY_DUR, (uint8_t *)&inactivity_dur, 1); + } + + return ret; +} + +/** + * @brief Duration in the transition from Stationary to Motion (from Inactivity to Activity).[get] + * + * @param ctx read / write interface definitions + * @param val SLEEP_TO_ACT_AT_1ST_SAMPLE, SLEEP_TO_ACT_AT_2ND_SAMPLE, SLEEP_TO_ACT_AT_3RD_SAMPLE, SLEEP_TO_ACT_AT_4th_SAMPLE, + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_act_from_sleep_to_act_dur_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_act_from_sleep_to_act_dur_t *val) +{ + lsm6dsv16x_inactivity_dur_t inactivity_dur; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_INACTIVITY_DUR, (uint8_t *)&inactivity_dur, 1); + switch (inactivity_dur.inact_dur) { + case LSM6DSV16X_SLEEP_TO_ACT_AT_1ST_SAMPLE: + *val = LSM6DSV16X_SLEEP_TO_ACT_AT_1ST_SAMPLE; + break; + + case LSM6DSV16X_SLEEP_TO_ACT_AT_2ND_SAMPLE: + *val = LSM6DSV16X_SLEEP_TO_ACT_AT_2ND_SAMPLE; + break; + + case LSM6DSV16X_SLEEP_TO_ACT_AT_3RD_SAMPLE: + *val = LSM6DSV16X_SLEEP_TO_ACT_AT_3RD_SAMPLE; + break; + + case LSM6DSV16X_SLEEP_TO_ACT_AT_4th_SAMPLE: + *val = LSM6DSV16X_SLEEP_TO_ACT_AT_4th_SAMPLE; + break; + + default: + *val = LSM6DSV16X_SLEEP_TO_ACT_AT_1ST_SAMPLE; + break; + } + return ret; +} + +/** + * @brief Selects the accelerometer data rate during Inactivity.[set] + * + * @param ctx read / write interface definitions + * @param val 1Hz875, 15Hz, 30Hz, 60Hz, + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_act_sleep_xl_odr_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_act_sleep_xl_odr_t val) +{ + lsm6dsv16x_inactivity_dur_t inactivity_dur; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_INACTIVITY_DUR, (uint8_t *)&inactivity_dur, 1); + if (ret == 0) { + inactivity_dur.xl_inact_odr = (uint8_t)val & 0x03U; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_INACTIVITY_DUR, (uint8_t *)&inactivity_dur, 1); + } + + return ret; +} + +/** + * @brief Selects the accelerometer data rate during Inactivity.[get] + * + * @param ctx read / write interface definitions + * @param val 1Hz875, 15Hz, 30Hz, 60Hz, + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_act_sleep_xl_odr_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_act_sleep_xl_odr_t *val) +{ + lsm6dsv16x_inactivity_dur_t inactivity_dur; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_INACTIVITY_DUR, (uint8_t *)&inactivity_dur, 1); + switch (inactivity_dur.xl_inact_odr) { + case LSM6DSV16X_1Hz875: + *val = LSM6DSV16X_1Hz875; + break; + + case LSM6DSV16X_15Hz: + *val = LSM6DSV16X_15Hz; + break; + + case LSM6DSV16X_30Hz: + *val = LSM6DSV16X_30Hz; + break; + + case LSM6DSV16X_60Hz: + *val = LSM6DSV16X_60Hz; + break; + + default: + *val = LSM6DSV16X_1Hz875; + break; + } + return ret; +} + +/** + * @brief Wakeup and activity/inactivity threshold.[set] + * + * @param ctx read / write interface definitions + * @param val Wakeup and activity/inactivity threshold. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_act_thresholds_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_act_thresholds_t val) +{ + lsm6dsv16x_inactivity_ths_t inactivity_ths; + lsm6dsv16x_inactivity_dur_t inactivity_dur; + lsm6dsv16x_wake_up_ths_t wake_up_ths; + int32_t ret; + float tmp; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_INACTIVITY_DUR, (uint8_t *)&inactivity_dur, 1); + if (ret == 0) { + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_INACTIVITY_THS, (uint8_t *)&inactivity_ths, 1); + } + + if (ret == 0) { + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_WAKE_UP_THS, (uint8_t *)&wake_up_ths, 1); + } + + if ((val.wk_ths_mg < (uint32_t)(7.8125f * 63.0f)) && (val.inact_ths_mg < (uint32_t)(7.8125f * 63.0f))) { + inactivity_dur.wu_inact_ths_w = 0; + + tmp = (float)val.inact_ths_mg / 7.8125f; + inactivity_ths.inact_ths = (uint8_t)tmp; + + tmp = (float)val.wk_ths_mg / 7.8125f; + wake_up_ths.wk_ths = (uint8_t)tmp; + } else if ((val.wk_ths_mg < (uint32_t)(15.625f * 63.0f)) && (val.inact_ths_mg < (uint32_t)(15.625f * 63.0f))) { + inactivity_dur.wu_inact_ths_w = 1; + + tmp = (float)val.inact_ths_mg / 15.625f; + inactivity_ths.inact_ths = (uint8_t)tmp; + + tmp = (float)val.wk_ths_mg / 15.625f; + wake_up_ths.wk_ths = (uint8_t)tmp; + } else if ((val.wk_ths_mg < (uint32_t)(31.25f * 63.0f)) && (val.inact_ths_mg < (uint32_t)(31.25f * 63.0f))) { + inactivity_dur.wu_inact_ths_w = 2; + + tmp = (float)val.inact_ths_mg / 31.25f; + inactivity_ths.inact_ths = (uint8_t)tmp; + + tmp = (float)val.wk_ths_mg / 31.25f; + wake_up_ths.wk_ths = (uint8_t)tmp; + } else if ((val.wk_ths_mg < (uint32_t)(62.5f * 63.0f)) && (val.inact_ths_mg < (uint32_t)(62.5f * 63.0f))) { + inactivity_dur.wu_inact_ths_w = 3; + + tmp = (float)val.inact_ths_mg / 62.5f; + inactivity_ths.inact_ths = (uint8_t)tmp; + + tmp = (float)val.wk_ths_mg / 62.5f; + wake_up_ths.wk_ths = (uint8_t)tmp; + } else if ((val.wk_ths_mg < (uint32_t)(125.0f * 63.0f)) && (val.inact_ths_mg < (uint32_t)(125.0f * 63.0f))) { + inactivity_dur.wu_inact_ths_w = 4; + + tmp = (float)val.inact_ths_mg / 125.0f; + inactivity_ths.inact_ths = (uint8_t)tmp; + + tmp = (float)val.wk_ths_mg / 125.0f; + wake_up_ths.wk_ths = (uint8_t)tmp; + } else if ((val.wk_ths_mg < (uint32_t)(250.0f * 63.0f)) && (val.inact_ths_mg < (uint32_t)(250.0f * 63.0f))) { + inactivity_dur.wu_inact_ths_w = 5; + + tmp = (float)val.inact_ths_mg / 250.0f; + inactivity_ths.inact_ths = (uint8_t)tmp; + + tmp = (float)val.wk_ths_mg / 250.0f; + wake_up_ths.wk_ths = (uint8_t)tmp; + } else { // out of limit + inactivity_dur.wu_inact_ths_w = 5; + inactivity_ths.inact_ths = 0x3FU; + wake_up_ths.wk_ths = 0x3FU; + } + + if (ret == 0) { + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_INACTIVITY_DUR, (uint8_t *)&inactivity_dur, 1); + } + if (ret == 0) { + + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_INACTIVITY_THS, (uint8_t *)&inactivity_ths, 1); + } + if (ret == 0) { + + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_WAKE_UP_THS, (uint8_t *)&wake_up_ths, 1); + } + + return ret; +} + +/** + * @brief Wakeup and activity/inactivity threshold.[get] + * + * @param ctx read / write interface definitions + * @param val Wakeup and activity/inactivity threshold. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_act_thresholds_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_act_thresholds_t *val) +{ + lsm6dsv16x_inactivity_dur_t inactivity_dur; + lsm6dsv16x_inactivity_ths_t inactivity_ths; + lsm6dsv16x_wake_up_ths_t wake_up_ths; + int32_t ret; + float tmp; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_INACTIVITY_DUR, (uint8_t *)&inactivity_dur, 1); + if (ret == 0) { + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_INACTIVITY_THS, (uint8_t *)&inactivity_ths, 1); + } + if (ret == 0) { + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_WAKE_UP_THS, (uint8_t *)&wake_up_ths, 1); + } + + switch (inactivity_dur.wu_inact_ths_w) { + case 0: + tmp = (float)wake_up_ths.wk_ths * 7.8125f; + val->wk_ths_mg = (uint32_t)tmp; + + tmp = (float)inactivity_ths.inact_ths * 7.8125f; + val->inact_ths_mg = (uint32_t)tmp; + break; + + case 1: + tmp = (float)wake_up_ths.wk_ths * 15.625f; + val->wk_ths_mg = (uint32_t)tmp; + + tmp = (float)inactivity_ths.inact_ths * 15.625f; + val->inact_ths_mg = (uint32_t)tmp; + break; + + case 2: + tmp = (float)wake_up_ths.wk_ths * 31.25f; + val->wk_ths_mg = (uint32_t)tmp; + + tmp = (float)inactivity_ths.inact_ths * 31.25f; + val->inact_ths_mg = (uint32_t)tmp; + break; + + case 3: + tmp = (float)wake_up_ths.wk_ths * 62.5f; + val->wk_ths_mg = (uint32_t)tmp; + + tmp = (float)inactivity_ths.inact_ths * 62.5f; + val->inact_ths_mg = (uint32_t)tmp; + break; + + case 4: + tmp = (float)wake_up_ths.wk_ths * 125.0f; + val->wk_ths_mg = (uint32_t)tmp; + + tmp = (float)inactivity_ths.inact_ths * 125.0f; + val->inact_ths_mg = (uint32_t)tmp; + break; + + default: + tmp = (float)wake_up_ths.wk_ths * 250.0f; + val->wk_ths_mg = (uint32_t)tmp; + + tmp = (float)inactivity_ths.inact_ths * 250.0f; + val->inact_ths_mg = (uint32_t)tmp; + break; + } + + return ret; +} + +/** + * @brief Time windows configuration for Wake Up - Activity - Inactivity (SLEEP, WAKE). Duration to go in sleep mode. Default value: 0000 (this corresponds to 16 ODR) 1 LSB = 512/ODR_XL time. Wake up duration event. 1 LSB = 1/ODR_XL time. [set] + * + * @param ctx read / write interface definitions + * @param val Time windows configuration for Wake Up - Activity - Inactivity (SLEEP, WAKE). Duration to go in sleep mode. Default value: 0000 (this corresponds to 16 ODR) 1 LSB = 512/ODR_XL time. Wake up duration event. 1 LSB = 1/ODR_XL time. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_act_wkup_time_windows_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_act_wkup_time_windows_t val) +{ + lsm6dsv16x_wake_up_dur_t wake_up_dur; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_WAKE_UP_DUR, (uint8_t *)&wake_up_dur, 1); + if (ret == 0) { + wake_up_dur.wake_dur = val.shock; + wake_up_dur.sleep_dur = val.quiet; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_WAKE_UP_DUR, (uint8_t *)&wake_up_dur, 1); + } + + return ret; +} + +/** + * @brief Time windows configuration for Wake Up - Activity - Inactivity (SLEEP, WAKE). Duration to go in sleep mode. Default value: 0000 (this corresponds to 16 ODR) 1 LSB = 512/ODR_XL time. Wake up duration event. 1 LSB = 1/ODR_XL time. [get] + * + * @param ctx read / write interface definitions + * @param val Time windows configuration for Wake Up - Activity - Inactivity (SLEEP, WAKE). Duration to go in sleep mode. Default value: 0000 (this corresponds to 16 ODR) 1 LSB = 512/ODR_XL time. Wake up duration event. 1 LSB = 1/ODR_XL time. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_act_wkup_time_windows_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_act_wkup_time_windows_t *val) +{ + lsm6dsv16x_wake_up_dur_t wake_up_dur; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_WAKE_UP_DUR, (uint8_t *)&wake_up_dur, 1); + val->shock = wake_up_dur.wake_dur; + val->quiet = wake_up_dur.sleep_dur; + + return ret; +} + +/** + * @} + * + */ diff --git a/lib/LSM6DSV16X/lsm6dsv16x_reg.h b/lib/LSM6DSV16X/lsm6dsv16x_reg.h new file mode 100644 index 000000000..37ec2ae9d --- /dev/null +++ b/lib/LSM6DSV16X/lsm6dsv16x_reg.h @@ -0,0 +1,4663 @@ +/** + ****************************************************************************** + * @file lsm6dsv16x_reg.h + * @author Sensors Software Solution Team + * @brief This file contains all the functions prototypes for the + * lsm6dsv16x_reg.c driver. + ****************************************************************************** + * @attention + * + *

© Copyright (c) 2021 STMicroelectronics. + * All rights reserved.

+ * + * This software component is licensed by ST under BSD 3-Clause license, + * the "License"; You may not use this file except in compliance with the + * License. You may obtain a copy of the License at: + * opensource.org/licenses/BSD-3-Clause + * + ****************************************************************************** + */ + +/* Define to prevent recursive inclusion -------------------------------------*/ +#ifndef LSM6DSV16X_REGS_H +#define LSM6DSV16X_REGS_H + +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ------------------------------------------------------------------*/ +#include +#include +#include + +/** @addtogroup LSM6DSV16X + * @{ + * + */ + +/** @defgroup Endianness definitions + * @{ + * + */ + +#ifndef DRV_BYTE_ORDER +#ifndef __BYTE_ORDER__ + +#define DRV_LITTLE_ENDIAN 1234 +#define DRV_BIG_ENDIAN 4321 + +/** if _BYTE_ORDER is not defined, choose the endianness of your architecture + * by uncommenting the define which fits your platform endianness + */ +//#define DRV_BYTE_ORDER DRV_BIG_ENDIAN +#define DRV_BYTE_ORDER DRV_LITTLE_ENDIAN + +#else /* defined __BYTE_ORDER__ */ + +#define DRV_LITTLE_ENDIAN __ORDER_LITTLE_ENDIAN__ +#define DRV_BIG_ENDIAN __ORDER_BIG_ENDIAN__ +#define DRV_BYTE_ORDER __BYTE_ORDER__ + +#endif /* __BYTE_ORDER__*/ +#endif /* DRV_BYTE_ORDER */ + +/** + * @} + * + */ + +/** @defgroup STMicroelectronics sensors common types + * @{ + * + */ + +#ifndef MEMS_SHARED_TYPES +#define MEMS_SHARED_TYPES + +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t bit0 : 1; + uint8_t bit1 : 1; + uint8_t bit2 : 1; + uint8_t bit3 : 1; + uint8_t bit4 : 1; + uint8_t bit5 : 1; + uint8_t bit6 : 1; + uint8_t bit7 : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t bit7 : 1; + uint8_t bit6 : 1; + uint8_t bit5 : 1; + uint8_t bit4 : 1; + uint8_t bit3 : 1; + uint8_t bit2 : 1; + uint8_t bit1 : 1; + uint8_t bit0 : 1; +#endif /* DRV_BYTE_ORDER */ +} bitwise_t; + +#define PROPERTY_DISABLE (0U) +#define PROPERTY_ENABLE (1U) + +#endif /* MEMS_SHARED_TYPES */ + +/** + * @} + * + */ + +/** @addtogroup Interfaces_Functions + * @brief This section provide a set of functions used to read and + * write a generic register of the device. + * MANDATORY: return 0 -> no Error. + * @{ + * + */ + +typedef int32_t (*lsm6dsv16x_write_ptr)(void *, uint8_t, uint8_t *, uint16_t); +typedef int32_t (*lsm6dsv16x_read_ptr)(void *, uint8_t, uint8_t *, uint16_t); + +typedef struct { + /** Component mandatory fields **/ + lsm6dsv16x_write_ptr write_reg; + lsm6dsv16x_read_ptr read_reg; + /** Customizable optional pointer **/ + void *handle; +} lsm6dsv16x_ctx_t; + +/** + * @} + * + */ + +#ifndef MEMS_UCF_SHARED_TYPES +#define MEMS_UCF_SHARED_TYPES + +/** @defgroup Generic address-data structure definition + * @brief This structure is useful to load a predefined configuration + * of a sensor. + * You can create a sensor configuration by your own or using + * Unico / Unicleo tools available on STMicroelectronics + * web site. + * + * @{ + * + */ + +typedef struct { + uint8_t address; + uint8_t data; +} ucf_line_t; + +/** + * @} + * + */ + +#endif /* MEMS_UCF_SHARED_TYPES */ + +/** @defgroup LSM6DSV16X_Infos + * @{ + * + */ + +/** I2C Device Address 8 bit format if SA0=0 -> D5 if SA0=1 -> D7 **/ +#define LSM6DSV16X_I2C_ADD_L 0xD5U +#define LSM6DSV16X_I2C_ADD_H 0xD7U + +/** Device Identification (Who am I) **/ +#define LSM6DSV16X_ID 0x70U + +/** + * @} + * + */ + +/** @defgroup bitfields page main + * @{ + * + */ + +#define LSM6DSV16X_FUNC_CFG_ACCESS 0x1U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t ois_ctrl_from_ui : 1; + uint8_t spi2_reset : 1; + uint8_t sw_por : 1; + uint8_t fsm_wr_ctrl_en : 1; + uint8_t not_used0 : 2; + uint8_t shub_reg_access : 1; + uint8_t emb_func_reg_access : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t emb_func_reg_access : 1; + uint8_t shub_reg_access : 1; + uint8_t not_used0 : 2; + uint8_t fsm_wr_ctrl_en : 1; + uint8_t sw_por : 1; + uint8_t spi2_reset : 1; + uint8_t ois_ctrl_from_ui : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_func_cfg_access_t; + +#define LSM6DSV16X_PIN_CTRL 0x2U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t not_used0 : 5; + uint8_t ibhr_por_en : 1; + uint8_t sdo_pu_en : 1; + uint8_t ois_pu_dis : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t ois_pu_dis : 1; + uint8_t sdo_pu_en : 1; + uint8_t ibhr_por_en : 1; + uint8_t not_used0 : 5; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_pin_ctrl_t; + +#define LSM6DSV16X_IF_CFG 0x3U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t i2c_i3c_disable : 1; + uint8_t not_used0 : 1; + uint8_t sim : 1; + uint8_t pp_od : 1; + uint8_t h_lactive : 1; + uint8_t asf_ctrl : 1; + uint8_t shub_pu_en : 1; + uint8_t sda_pu_en : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t sda_pu_en : 1; + uint8_t shub_pu_en : 1; + uint8_t asf_ctrl : 1; + uint8_t h_lactive : 1; + uint8_t pp_od : 1; + uint8_t sim : 1; + uint8_t not_used0 : 1; + uint8_t i2c_i3c_disable : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_if_cfg_t; + +#define LSM6DSV16X_S4S_TPH_L 0x4U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t tph : 7; + uint8_t tph_h_sel : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t tph_h_sel : 1; + uint8_t tph : 7; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_s4s_tph_l_t; + +#define LSM6DSV16X_S4S_TPH_H 0x5U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t tph : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t tph : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_s4s_tph_h_t; + +#define LSM6DSV16X_S4S_RR 0x6U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t rr : 2; + uint8_t not_used0 : 6; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t not_used0 : 6; + uint8_t rr : 2; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_s4s_rr_t; + +#define LSM6DSV16X_FIFO_CTRL1 0x7U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t wtm : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t wtm : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_fifo_ctrl1_t; + +#define LSM6DSV16X_FIFO_CTRL2 0x8U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t xl_dualc_batch_from_fsm : 1; + uint8_t uncompr_rate : 2; + uint8_t not_used0 : 1; + uint8_t odr_chg_en : 1; + uint8_t not_used1 : 1; + uint8_t fifo_compr_rt_en : 1; + uint8_t stop_on_wtm : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t stop_on_wtm : 1; + uint8_t fifo_compr_rt_en : 1; + uint8_t not_used1 : 1; + uint8_t odr_chg_en : 1; + uint8_t not_used0 : 1; + uint8_t uncompr_rate : 2; + uint8_t xl_dualc_batch_from_fsm : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_fifo_ctrl2_t; + +#define LSM6DSV16X_FIFO_CTRL3 0x9U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t bdr_xl : 4; + uint8_t bdr_gy : 4; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t bdr_gy : 4; + uint8_t bdr_xl : 4; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_fifo_ctrl3_t; + +#define LSM6DSV16X_FIFO_CTRL4 0x0AU +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t fifo_mode : 3; + uint8_t g_eis_fifo_en : 1; + uint8_t odr_t_batch : 2; + uint8_t dec_ts_batch : 2; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t dec_ts_batch : 2; + uint8_t odr_t_batch : 2; + uint8_t g_eis_fifo_en : 1; + uint8_t fifo_mode : 3; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_fifo_ctrl4_t; + +#define LSM6DSV16X_COUNTER_BDR_REG1 0x0BU +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t cnt_bdr_th : 2; + uint8_t not_used0 : 3; + uint8_t trig_counter_bdr : 2; + uint8_t not_used1 : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t not_used1 : 1; + uint8_t trig_counter_bdr : 2; + uint8_t not_used0 : 3; + uint8_t cnt_bdr_th : 2; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_counter_bdr_reg1_t; + +#define LSM6DSV16X_COUNTER_BDR_REG2 0x0CU +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t cnt_bdr_th : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t cnt_bdr_th : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_counter_bdr_reg2_t; + +#define LSM6DSV16X_INT1_CTRL 0x0DU +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t int1_drdy_xl : 1; + uint8_t int1_drdy_g : 1; + uint8_t not_used0 : 1; + uint8_t int1_fifo_th : 1; + uint8_t int1_fifo_ovr : 1; + uint8_t int1_fifo_full : 1; + uint8_t int1_cnt_bdr : 1; + uint8_t not_used1 : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t not_used1 : 1; + uint8_t int1_cnt_bdr : 1; + uint8_t int1_fifo_full : 1; + uint8_t int1_fifo_ovr : 1; + uint8_t int1_fifo_th : 1; + uint8_t not_used0 : 1; + uint8_t int1_drdy_g : 1; + uint8_t int1_drdy_xl : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_int1_ctrl_t; + +#define LSM6DSV16X_INT2_CTRL 0x0EU +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t int2_drdy_xl : 1; + uint8_t int2_drdy_g : 1; + uint8_t int2_drdy_g_eis : 1; + uint8_t int2_fifo_th : 1; + uint8_t int2_fifo_ovr : 1; + uint8_t int2_fifo_full : 1; + uint8_t int2_cnt_bdr : 1; + uint8_t int2_emb_func_endop : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t int2_emb_func_endop : 1; + uint8_t int2_cnt_bdr : 1; + uint8_t int2_fifo_full : 1; + uint8_t int2_fifo_ovr : 1; + uint8_t int2_fifo_th : 1; + uint8_t int2_drdy_g_eis : 1; + uint8_t int2_drdy_g : 1; + uint8_t int2_drdy_xl : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_int2_ctrl_t; + +#define LSM6DSV16X_WHO_AM_I 0x0FU +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t id : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t id : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_who_am_i_t; + +#define LSM6DSV16X_CTRL1 0x10U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t odr_xl : 4; + uint8_t op_mode_xl : 3; + uint8_t not_used0 : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t not_used0 : 1; + uint8_t op_mode_xl : 3; + uint8_t odr_xl : 4; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_ctrl1_t; + +#define LSM6DSV16X_CTRL2 0x11U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t odr_g : 4; + uint8_t op_mode_g : 3; + uint8_t not_used0 : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t not_used0 : 1; + uint8_t op_mode_g : 3; + uint8_t odr_g : 4; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_ctrl2_t; + +#define LSM6DSV16X_CTRL3 0x12U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t sw_reset : 1; + uint8_t not_used0 : 1; + uint8_t if_inc : 1; + uint8_t not_used1 : 3; + uint8_t bdu : 1; + uint8_t boot : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t boot : 1; + uint8_t bdu : 1; + uint8_t not_used1 : 3; + uint8_t if_inc : 1; + uint8_t not_used0 : 1; + uint8_t sw_reset : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_ctrl3_t; + +#define LSM6DSV16X_CTRL4 0x13U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t int2_in_lh : 1; + uint8_t drdy_pulsed : 1; + uint8_t int2_drdy_temp : 1; + uint8_t drdy_mask : 1; + uint8_t int2_on_int1 : 1; + uint8_t not_used0 : 3; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t not_used0 : 3; + uint8_t int2_on_int1 : 1; + uint8_t drdy_mask : 1; + uint8_t int2_drdy_temp : 1; + uint8_t drdy_pulsed : 1; + uint8_t int2_in_lh : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_ctrl4_t; + +#define LSM6DSV16X_CTRL5 0x14U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t int_en_i3c : 1; + uint8_t bus_act_sel : 2; + uint8_t not_used0 : 5; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t not_used0 : 5; + uint8_t bus_act_sel : 2; + uint8_t int_en_i3c : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_ctrl5_t; + +#define LSM6DSV16X_CTRL6 0x15U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t fs_g : 4; + uint8_t lpf1_g_bw : 3; + uint8_t not_used0 : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t not_used0 : 1; + uint8_t lpf1_g_bw : 3; + uint8_t fs_g : 4; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_ctrl6_t; + +#define LSM6DSV16X_CTRL7 0x16U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t lpf1_g_en : 1; + uint8_t not_used0 : 3; + uint8_t ah_qvar_c_zin : 2; + uint8_t int2_drdy_ah_qvar : 1; + uint8_t ah_qvar_en : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t ah_qvar_en : 1; + uint8_t int2_drdy_ah_qvar : 1; + uint8_t ah_qvar_c_zin : 2; + uint8_t not_used0 : 3; + uint8_t lpf1_g_en : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_ctrl7_t; + +#define LSM6DSV16X_CTRL8 0x17U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t fs_xl : 2; + uint8_t not_used0 : 1; + uint8_t xl_dualc_en : 1; + uint8_t not_used1 : 1; + uint8_t hp_lpf2_xl_bw : 3; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t hp_lpf2_xl_bw : 3; + uint8_t not_used1 : 1; + uint8_t xl_dualc_en : 1; + uint8_t not_used0 : 1; + uint8_t fs_xl : 2; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_ctrl8_t; + +#define LSM6DSV16X_CTRL9 0x18U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t usr_off_on_out : 1; + uint8_t usr_off_w : 1; + uint8_t not_used0 : 1; + uint8_t lpf2_xl_en : 1; + uint8_t hp_slope_xl_en : 1; + uint8_t xl_fastsettl_mode : 1; + uint8_t hp_ref_mode_xl : 1; + uint8_t not_used1 : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t not_used1 : 1; + uint8_t hp_ref_mode_xl : 1; + uint8_t xl_fastsettl_mode : 1; + uint8_t hp_slope_xl_en : 1; + uint8_t lpf2_xl_en : 1; + uint8_t not_used0 : 1; + uint8_t usr_off_w : 1; + uint8_t usr_off_on_out : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_ctrl9_t; + +#define LSM6DSV16X_CTRL10 0x19U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t st_xl : 2; + uint8_t st_g : 2; + uint8_t not_used0 : 4; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t not_used0 : 4; + uint8_t st_g : 2; + uint8_t st_xl : 2; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_ctrl10_t; + +#define LSM6DSV16X_CTRL_STATUS 0x1AU +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t not_used1 : 5; + uint8_t fsm_wr_ctrl_status : 1; + uint8_t not_used0 : 2; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t not_used0 : 2; + uint8_t fsm_wr_ctrl_status : 1; + uint8_t not_used1 : 5; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_ctrl_status_t; + +#define LSM6DSV16X_FIFO_STATUS1 0x1BU +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t diff_fifo : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t diff_fifo : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_fifo_status1_t; + +#define LSM6DSV16X_FIFO_STATUS2 0x1CU +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t diff_fifo : 1; + uint8_t not_used0 : 2; + uint8_t fifo_ovr_latched : 1; + uint8_t counter_bdr_ia : 1; + uint8_t fifo_full_ia : 1; + uint8_t fifo_ovr_ia : 1; + uint8_t fifo_wtm_ia : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t fifo_wtm_ia : 1; + uint8_t fifo_ovr_ia : 1; + uint8_t fifo_full_ia : 1; + uint8_t counter_bdr_ia : 1; + uint8_t fifo_ovr_latched : 1; + uint8_t not_used0 : 2; + uint8_t diff_fifo : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_fifo_status2_t; + +#define LSM6DSV16X_ALL_INT_SRC 0x1DU +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t ff_ia : 1; + uint8_t wu_ia : 1; + uint8_t tap_ia : 1; + uint8_t not_used0 : 1; + uint8_t d6d_ia : 1; + uint8_t sleep_change_ia : 1; + uint8_t shub_ia : 1; + uint8_t emb_func_ia : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t emb_func_ia : 1; + uint8_t shub_ia : 1; + uint8_t sleep_change_ia : 1; + uint8_t d6d_ia : 1; + uint8_t not_used0 : 1; + uint8_t tap_ia : 1; + uint8_t wu_ia : 1; + uint8_t ff_ia : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_all_int_src_t; + +#define LSM6DSV16X_STATUS_REG 0x1EU +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t xlda : 1; + uint8_t gda : 1; + uint8_t tda : 1; + uint8_t ah_qvarda : 1; + uint8_t gda_eis : 1; + uint8_t ois_drdy : 1; + uint8_t not_used0 : 1; + uint8_t timestamp_endcount : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t timestamp_endcount : 1; + uint8_t not_used0 : 1; + uint8_t ois_drdy : 1; + uint8_t gda_eis : 1; + uint8_t ah_qvarda : 1; + uint8_t tda : 1; + uint8_t gda : 1; + uint8_t xlda : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_status_reg_t; + +#define LSM6DSV16X_OUT_TEMP_L 0x20U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t temp : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t temp : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_out_temp_l_t; + +#define LSM6DSV16X_OUT_TEMP_H 0x21U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t temp : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t temp : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_out_temp_h_t; + +#define LSM6DSV16X_OUTX_L_G 0x22U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t outx_g : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t outx_g : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_outx_l_g_t; + +#define LSM6DSV16X_OUTX_H_G 0x23U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t outx_g : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t outx_g : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_outx_h_g_t; + +#define LSM6DSV16X_OUTY_L_G 0x24U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t outy_g : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t outy_g : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_outy_l_g_t; + +#define LSM6DSV16X_OUTY_H_G 0x25U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t outy_g : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t outy_g : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_outy_h_g_t; + +#define LSM6DSV16X_OUTZ_L_G 0x26U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t outz_g : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t outz_g : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_outz_l_g_t; + +#define LSM6DSV16X_OUTZ_H_G 0x27U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t outz_g : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t outz_g : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_outz_h_g_t; + +#define LSM6DSV16X_OUTX_L_A 0x28U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t outx_a : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t outx_a : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_outx_l_a_t; + +#define LSM6DSV16X_OUTX_H_A 0x29U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t outx_a : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t outx_a : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_outx_h_a_t; + +#define LSM6DSV16X_OUTY_L_A 0x2AU +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t outy_a : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t outy_a : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_outy_l_a_t; + +#define LSM6DSV16X_OUTY_H_A 0x2BU +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t outy_a : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t outy_a : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_outy_h_a_t; + +#define LSM6DSV16X_OUTZ_L_A 0x2CU +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t outz_a : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t outz_a : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_outz_l_a_t; + +#define LSM6DSV16X_OUTZ_H_A 0x2DU +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t outz_a : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t outz_a : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_outz_h_a_t; + +#define LSM6DSV16X_UI_OUTX_L_G_OIS_EIS 0x2EU +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t ui_outx_g_ois_eis : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t ui_outx_g_ois_eis : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_ui_outx_l_g_ois_eis_t; + +#define LSM6DSV16X_UI_OUTX_H_G_OIS_EIS 0x2FU +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t ui_outx_g_ois_eis : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t ui_outx_g_ois_eis : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_ui_outx_h_g_ois_eis_t; + +#define LSM6DSV16X_UI_OUTY_L_G_OIS_EIS 0x30U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t ui_outy_g_ois_eis : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t ui_outy_g_ois_eis : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_ui_outy_l_g_ois_eis_t; + +#define LSM6DSV16X_UI_OUTY_H_G_OIS_EIS 0x31U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t ui_outy_g_ois_eis : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t ui_outy_g_ois_eis : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_ui_outy_h_g_ois_eis_t; + +#define LSM6DSV16X_UI_OUTZ_L_G_OIS_EIS 0x32U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t ui_outz_g_ois_eis : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t ui_outz_g_ois_eis : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_ui_outz_l_g_ois_eis_t; + +#define LSM6DSV16X_UI_OUTZ_H_G_OIS_EIS 0x33U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t ui_outz_g_ois_eis : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t ui_outz_g_ois_eis : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_ui_outz_h_g_ois_eis_t; + +#define LSM6DSV16X_UI_OUTX_L_A_OIS_DUALC 0x34U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t ui_outx_a_ois_dualc : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t ui_outx_a_ois_dualc : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_ui_outx_l_a_ois_dualc_t; + +#define LSM6DSV16X_UI_OUTX_H_A_OIS_DUALC 0x35U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t ui_outx_a_ois_dualc : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t ui_outx_a_ois_dualc : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_ui_outx_h_a_ois_dualc_t; + +#define LSM6DSV16X_UI_OUTY_L_A_OIS_DUALC 0x36U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t ui_outy_a_ois_dualc : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t ui_outy_a_ois_dualc : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_ui_outy_l_a_ois_dualc_t; + +#define LSM6DSV16X_UI_OUTY_H_A_OIS_DUALC 0x37U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t ui_outy_a_ois_dualc : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t ui_outy_a_ois_dualc : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_ui_outy_h_a_ois_dualc_t; + +#define LSM6DSV16X_UI_OUTZ_L_A_OIS_DUALC 0x38U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t ui_outz_a_ois_dualc : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t ui_outz_a_ois_dualc : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_ui_outz_l_a_ois_dualc_t; + +#define LSM6DSV16X_UI_OUTZ_H_A_OIS_DUALC 0x39U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t ui_outz_a_ois_dualc : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t ui_outz_a_ois_dualc : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_ui_outz_h_a_ois_dualc_t; + +#define LSM6DSV16X_AH_QVAR_OUT_L 0x3AU +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t ah_qvar : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t ah_qvar : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_ah_qvar_out_l_t; + +#define LSM6DSV16X_AH_QVAR_OUT_H 0x3BU +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t ah_qvar : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t ah_qvar : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_ah_qvar_out_h_t; + +#define LSM6DSV16X_TIMESTAMP0 0x40U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t timestamp : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t timestamp : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_timestamp0_t; + +#define LSM6DSV16X_TIMESTAMP1 0x41U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t timestamp : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t timestamp : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_timestamp1_t; + +#define LSM6DSV16X_TIMESTAMP2 0x42U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t timestamp : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t timestamp : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_timestamp2_t; + +#define LSM6DSV16X_TIMESTAMP3 0x43U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t timestamp : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t timestamp : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_timestamp3_t; + +#define LSM6DSV16X_UI_STATUS_REG_OIS 0x44U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t xlda_ois : 1; + uint8_t gda_ois : 1; + uint8_t gyro_settling : 1; + uint8_t not_used0 : 5; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t not_used0 : 5; + uint8_t gyro_settling : 1; + uint8_t gda_ois : 1; + uint8_t xlda_ois : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_ui_status_reg_ois_t; + +#define LSM6DSV16X_WAKE_UP_SRC 0x45U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t z_wu : 1; + uint8_t y_wu : 1; + uint8_t x_wu : 1; + uint8_t wu_ia : 1; + uint8_t sleep_state : 1; + uint8_t ff_ia : 1; + uint8_t sleep_change_ia : 1; + uint8_t not_used0 : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t not_used0 : 1; + uint8_t sleep_change_ia : 1; + uint8_t ff_ia : 1; + uint8_t sleep_state : 1; + uint8_t wu_ia : 1; + uint8_t x_wu : 1; + uint8_t y_wu : 1; + uint8_t z_wu : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_wake_up_src_t; + +#define LSM6DSV16X_TAP_SRC 0x46U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t z_tap : 1; + uint8_t y_tap : 1; + uint8_t x_tap : 1; + uint8_t tap_sign : 1; + uint8_t double_tap : 1; + uint8_t single_tap : 1; + uint8_t tap_ia : 1; + uint8_t not_used0 : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t not_used0 : 1; + uint8_t tap_ia : 1; + uint8_t single_tap : 1; + uint8_t double_tap : 1; + uint8_t tap_sign : 1; + uint8_t x_tap : 1; + uint8_t y_tap : 1; + uint8_t z_tap : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_tap_src_t; + +#define LSM6DSV16X_D6D_SRC 0x47U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t xl : 1; + uint8_t xh : 1; + uint8_t yl : 1; + uint8_t yh : 1; + uint8_t zl : 1; + uint8_t zh : 1; + uint8_t d6d_ia : 1; + uint8_t not_used0 : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t not_used0 : 1; + uint8_t d6d_ia : 1; + uint8_t zh : 1; + uint8_t zl : 1; + uint8_t yh : 1; + uint8_t yl : 1; + uint8_t xh : 1; + uint8_t xl : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_d6d_src_t; + +#define LSM6DSV16X_STATUS_MASTER_MAINPAGE 0x48U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t sens_hub_endop : 1; + uint8_t not_used0 : 2; + uint8_t slave0_nack : 1; + uint8_t slave1_nack : 1; + uint8_t slave2_nack : 1; + uint8_t slave3_nack : 1; + uint8_t wr_once_done : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t wr_once_done : 1; + uint8_t slave3_nack : 1; + uint8_t slave2_nack : 1; + uint8_t slave1_nack : 1; + uint8_t slave0_nack : 1; + uint8_t not_used0 : 2; + uint8_t sens_hub_endop : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_status_master_mainpage_t; + +#define LSM6DSV16X_EMB_FUNC_STATUS_MAINPAGE 0x49U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t not_used0 : 3; + uint8_t is_step_det : 1; + uint8_t is_tilt : 1; + uint8_t is_sigmot : 1; + uint8_t not_used1 : 1; + uint8_t is_fsm_lc : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t is_fsm_lc : 1; + uint8_t not_used1 : 1; + uint8_t is_sigmot : 1; + uint8_t is_tilt : 1; + uint8_t is_step_det : 1; + uint8_t not_used0 : 3; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_emb_func_status_mainpage_t; + +#define LSM6DSV16X_FSM_STATUS_MAINPAGE 0x4AU +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t is_fsm1 : 1; + uint8_t is_fsm2 : 1; + uint8_t is_fsm3 : 1; + uint8_t is_fsm4 : 1; + uint8_t is_fsm5 : 1; + uint8_t is_fsm6 : 1; + uint8_t is_fsm7 : 1; + uint8_t is_fsm8 : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t is_fsm8 : 1; + uint8_t is_fsm7 : 1; + uint8_t is_fsm6 : 1; + uint8_t is_fsm5 : 1; + uint8_t is_fsm4 : 1; + uint8_t is_fsm3 : 1; + uint8_t is_fsm2 : 1; + uint8_t is_fsm1 : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_fsm_status_mainpage_t; + +#define LSM6DSV16X_MLC_STATUS_MAINPAGE 0x4BU +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t is_mlc1 : 1; + uint8_t is_mlc2 : 1; + uint8_t is_mlc3 : 1; + uint8_t is_mlc4 : 1; + uint8_t not_used0 : 4; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t not_used0 : 4; + uint8_t is_mlc4 : 1; + uint8_t is_mlc3 : 1; + uint8_t is_mlc2 : 1; + uint8_t is_mlc1 : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_mlc_status_mainpage_t; + +#define LSM6DSV16X_INTERNAL_FREQ 0x4FU +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t freq_fine : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t freq_fine : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_internal_freq_t; + +#define LSM6DSV16X_FUNCTIONS_ENABLE 0x50U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t inact_en : 2; + uint8_t not_used0 : 1; + uint8_t dis_rst_lir_all_int : 1; + uint8_t not_used1 : 2; + uint8_t timestamp_en : 1; + uint8_t interrupts_enable : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t interrupts_enable : 1; + uint8_t timestamp_en : 1; + uint8_t not_used1 : 2; + uint8_t dis_rst_lir_all_int : 1; + uint8_t not_used0 : 1; + uint8_t inact_en : 2; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_functions_enable_t; + +#define LSM6DSV16X_DEN 0x51U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t den_xl_g : 1; + uint8_t den_z : 1; + uint8_t den_y : 1; + uint8_t den_x : 1; + uint8_t den_xl_en : 1; + uint8_t lvl2_en : 1; + uint8_t lvl1_en : 1; + uint8_t not_used0 : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t not_used0 : 1; + uint8_t lvl1_en : 1; + uint8_t lvl2_en : 1; + uint8_t den_xl_en : 1; + uint8_t den_x : 1; + uint8_t den_y : 1; + uint8_t den_z : 1; + uint8_t den_xl_g : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_den_t; + +#define LSM6DSV16X_INACTIVITY_DUR 0x54U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t inact_dur : 2; + uint8_t xl_inact_odr : 2; + uint8_t wu_inact_ths_w : 3; + uint8_t sleep_status_on_int : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t sleep_status_on_int : 1; + uint8_t wu_inact_ths_w : 3; + uint8_t xl_inact_odr : 2; + uint8_t inact_dur : 2; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_inactivity_dur_t; + +#define LSM6DSV16X_INACTIVITY_THS 0x55U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t inact_ths : 6; + uint8_t not_used0 : 2; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t not_used0 : 2; + uint8_t inact_ths : 6; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_inactivity_ths_t; + +#define LSM6DSV16X_TAP_CFG0 0x56U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t lir : 1; + uint8_t tap_z_en : 1; + uint8_t tap_y_en : 1; + uint8_t tap_x_en : 1; + uint8_t slope_fds : 1; + uint8_t not_used0 : 1; + uint8_t low_pass_on_6d : 1; + uint8_t not_used1 : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t not_used1 : 1; + uint8_t low_pass_on_6d : 1; + uint8_t not_used0 : 1; + uint8_t slope_fds : 1; + uint8_t tap_x_en : 1; + uint8_t tap_y_en : 1; + uint8_t tap_z_en : 1; + uint8_t lir : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_tap_cfg0_t; + +#define LSM6DSV16X_TAP_CFG1 0x57U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t tap_ths_x : 5; + uint8_t tap_priority : 3; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t tap_priority : 3; + uint8_t tap_ths_x : 5; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_tap_cfg1_t; + +#define LSM6DSV16X_TAP_CFG2 0x58U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t tap_ths_y : 5; + uint8_t not_used0 : 3; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t not_used0 : 3; + uint8_t tap_ths_y : 5; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_tap_cfg2_t; + +#define LSM6DSV16X_TAP_THS_6D 0x59U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t tap_ths_z : 5; + uint8_t sixd_ths : 2; + uint8_t d4d_en : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t d4d_en : 1; + uint8_t sixd_ths : 2; + uint8_t tap_ths_z : 5; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_tap_ths_6d_t; + +#define LSM6DSV16X_INT_DUR2 0x5AU +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t shock : 2; + uint8_t quiet : 2; + uint8_t dur : 4; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t dur : 4; + uint8_t quiet : 2; + uint8_t shock : 2; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_int_dur2_t; + +#define LSM6DSV16X_WAKE_UP_THS 0x5BU +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t wk_ths : 6; + uint8_t usr_off_on_wu : 1; + uint8_t single_double_tap : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t single_double_tap : 1; + uint8_t usr_off_on_wu : 1; + uint8_t wk_ths : 6; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_wake_up_ths_t; + +#define LSM6DSV16X_WAKE_UP_DUR 0x5CU +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t sleep_dur : 4; + uint8_t not_used0 : 1; + uint8_t wake_dur : 2; + uint8_t ff_dur : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t ff_dur : 1; + uint8_t wake_dur : 2; + uint8_t not_used0 : 1; + uint8_t sleep_dur : 4; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_wake_up_dur_t; + +#define LSM6DSV16X_FREE_FALL 0x5DU +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t ff_ths : 3; + uint8_t ff_dur : 5; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t ff_dur : 5; + uint8_t ff_ths : 3; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_free_fall_t; + +#define LSM6DSV16X_MD1_CFG 0x5EU +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t int1_shub : 1; + uint8_t int1_emb_func : 1; + uint8_t int1_6d : 1; + uint8_t int1_double_tap : 1; + uint8_t int1_ff : 1; + uint8_t int1_wu : 1; + uint8_t int1_single_tap : 1; + uint8_t int1_sleep_change : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t int1_sleep_change : 1; + uint8_t int1_single_tap : 1; + uint8_t int1_wu : 1; + uint8_t int1_ff : 1; + uint8_t int1_double_tap : 1; + uint8_t int1_6d : 1; + uint8_t int1_emb_func : 1; + uint8_t int1_shub : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_md1_cfg_t; + +#define LSM6DSV16X_MD2_CFG 0x5FU +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t int2_timestamp : 1; + uint8_t int2_emb_func : 1; + uint8_t int2_6d : 1; + uint8_t int2_double_tap : 1; + uint8_t int2_ff : 1; + uint8_t int2_wu : 1; + uint8_t int2_single_tap : 1; + uint8_t int2_sleep_change : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t int2_sleep_change : 1; + uint8_t int2_single_tap : 1; + uint8_t int2_wu : 1; + uint8_t int2_ff : 1; + uint8_t int2_double_tap : 1; + uint8_t int2_6d : 1; + uint8_t int2_emb_func : 1; + uint8_t int2_timestamp : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_md2_cfg_t; + +#define LSM6DSV16X_S4S_ST_CMD_CODE 0x60U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t st_cmd_code : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t st_cmd_code : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_s4s_st_cmd_code_t; + +#define LSM6DSV16X_S4S_DT_REG 0x61U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t dt : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t dt : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_s4s_dt_reg_t; + +#define LSM6DSV16X_EMB_FUNC_CFG 0x63U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t not_used0 : 3; + uint8_t emb_func_disable : 1; + uint8_t emb_func_irq_mask_xl_settl : 1; + uint8_t emb_func_irq_mask_g_settl : 1; + uint8_t not_used1 : 2; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t not_used1 : 2; + uint8_t emb_func_irq_mask_g_settl : 1; + uint8_t emb_func_irq_mask_xl_settl : 1; + uint8_t emb_func_disable : 1; + uint8_t not_used0 : 3; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_emb_func_cfg_t; + +#define LSM6DSV16X_UI_HANDSHAKE_CTRL 0x64U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t ui_shared_req : 1; + uint8_t ui_shared_ack : 1; + uint8_t not_used0 : 6; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t not_used0 : 6; + uint8_t ui_shared_ack : 1; + uint8_t ui_shared_req : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_ui_handshake_ctrl_t; + +#define LSM6DSV16X_UI_SPI2_SHARED_0 0x65U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t ui_spi2_shared : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t ui_spi2_shared : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_ui_spi2_shared_0_t; + +#define LSM6DSV16X_UI_SPI2_SHARED_1 0x66U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t ui_spi2_shared : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t ui_spi2_shared : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_ui_spi2_shared_1_t; + +#define LSM6DSV16X_UI_SPI2_SHARED_2 0x67U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t ui_spi2_shared : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t ui_spi2_shared : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_ui_spi2_shared_2_t; + +#define LSM6DSV16X_UI_SPI2_SHARED_3 0x68U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t ui_spi2_shared : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t ui_spi2_shared : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_ui_spi2_shared_3_t; + +#define LSM6DSV16X_UI_SPI2_SHARED_4 0x69U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t ui_spi2_shared : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t ui_spi2_shared : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_ui_spi2_shared_4_t; + +#define LSM6DSV16X_UI_SPI2_SHARED_5 0x6AU +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t ui_spi2_shared : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t ui_spi2_shared : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_ui_spi2_shared_5_t; + +#define LSM6DSV16X_CTRL_EIS 0x6BU +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t fs_g_eis : 3; + uint8_t g_eis_on_g_ois_out_reg : 1; + uint8_t lpf_g_eis_bw : 1; + uint8_t not_used0 : 1; + uint8_t odr_g_eis : 2; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t odr_g_eis : 2; + uint8_t not_used0 : 1; + uint8_t lpf_g_eis_bw : 1; + uint8_t g_eis_on_g_ois_out_reg : 1; + uint8_t fs_g_eis : 3; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_ctrl_eis_t; + +#define LSM6DSV16X_UI_INT_OIS 0x6FU +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t not_used0 : 4; + uint8_t st_ois_clampdis : 1; + uint8_t not_used1 : 1; + uint8_t drdy_mask_ois : 1; + uint8_t int2_drdy_ois : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t int2_drdy_ois : 1; + uint8_t drdy_mask_ois : 1; + uint8_t not_used1 : 1; + uint8_t st_ois_clampdis : 1; + uint8_t not_used0 : 4; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_ui_int_ois_t; + +#define LSM6DSV16X_UI_CTRL1_OIS 0x70U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t spi2_read_en : 1; + uint8_t ois_g_en : 1; + uint8_t ois_xl_en : 1; + uint8_t not_used0 : 2; + uint8_t sim_ois : 1; + uint8_t not_used1 : 2; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t not_used1 : 2; + uint8_t sim_ois : 1; + uint8_t not_used0 : 2; + uint8_t ois_xl_en : 1; + uint8_t ois_g_en : 1; + uint8_t spi2_read_en : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_ui_ctrl1_ois_t; + +#define LSM6DSV16X_UI_CTRL2_OIS 0x71U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t fs_g_ois : 3; + uint8_t lpf1_g_ois_bw : 2; + uint8_t not_used0 : 3; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t not_used0 : 3; + uint8_t lpf1_g_ois_bw : 2; + uint8_t fs_g_ois : 3; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_ui_ctrl2_ois_t; + +#define LSM6DSV16X_UI_CTRL3_OIS 0x72U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t fs_xl_ois : 2; + uint8_t not_used0 : 1; + uint8_t lpf_xl_ois_bw : 3; + uint8_t not_used1 : 2; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t not_used1 : 2; + uint8_t lpf_xl_ois_bw : 3; + uint8_t not_used0 : 1; + uint8_t fs_xl_ois : 2; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_ui_ctrl3_ois_t; + +#define LSM6DSV16X_X_OFS_USR 0x73U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t x_ofs_usr : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t x_ofs_usr : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_x_ofs_usr_t; + +#define LSM6DSV16X_Y_OFS_USR 0x74U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t y_ofs_usr : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t y_ofs_usr : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_y_ofs_usr_t; + +#define LSM6DSV16X_Z_OFS_USR 0x75U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t z_ofs_usr : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t z_ofs_usr : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_z_ofs_usr_t; + +#define LSM6DSV16X_FIFO_DATA_OUT_TAG 0x78U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t not_used0 : 1; + uint8_t tag_cnt : 2; + uint8_t tag_sensor : 5; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t tag_sensor : 5; + uint8_t tag_cnt : 2; + uint8_t not_used0 : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_fifo_data_out_tag_t; + +#define LSM6DSV16X_FIFO_DATA_OUT_X_L 0x79U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t fifo_data_out : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t fifo_data_out : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_fifo_data_out_x_l_t; + +#define LSM6DSV16X_FIFO_DATA_OUT_X_H 0x7AU +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t fifo_data_out : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t fifo_data_out : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_fifo_data_out_x_h_t; + +#define LSM6DSV16X_FIFO_DATA_OUT_Y_L 0x7BU +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t fifo_data_out : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t fifo_data_out : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_fifo_data_out_y_l_t; + +#define LSM6DSV16X_FIFO_DATA_OUT_Y_H 0x7CU +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t fifo_data_out : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t fifo_data_out : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_fifo_data_out_y_h_t; + +#define LSM6DSV16X_FIFO_DATA_OUT_Z_L 0x7DU +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t fifo_data_out : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t fifo_data_out : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_fifo_data_out_z_l_t; + +#define LSM6DSV16X_FIFO_DATA_OUT_Z_H 0x7EU +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t fifo_data_out : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t fifo_data_out : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_fifo_data_out_z_h_t; + +/** + * @} + * + */ + +/** @defgroup bitfields page spi2 + * @{ + * + */ + +#define LSM6DSV16X_SPI2_WHO_AM_I 0x0FU +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t id : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t id : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_spi2_who_am_i_t; + +#define LSM6DSV16X_SPI2_STATUS_REG_OIS 0x1EU +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t xlda : 1; + uint8_t gda : 1; + uint8_t gyro_settling : 1; + uint8_t not_used0 : 5; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t not_used0 : 5; + uint8_t gyro_settling : 1; + uint8_t gda : 1; + uint8_t xlda : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_spi2_status_reg_ois_t; + +#define LSM6DSV16X_SPI2_OUT_TEMP_L 0x20U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t temp : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t temp : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_spi2_out_temp_l_t; + +#define LSM6DSV16X_SPI2_OUT_TEMP_H 0x21U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t temp : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t temp : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_spi2_out_temp_h_t; + +#define LSM6DSV16X_SPI2_OUTX_L_G_OIS 0x22U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t spi2_outx_g_ois : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t spi2_outx_g_ois : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_spi2_outx_l_g_ois_t; + +#define LSM6DSV16X_SPI2_OUTX_H_G_OIS 0x23U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t spi2_outx_g_ois : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t spi2_outx_g_ois : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_spi2_outx_h_g_ois_t; + +#define LSM6DSV16X_SPI2_OUTY_L_G_OIS 0x24U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t spi2_outy_g_ois : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t spi2_outy_g_ois : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_spi2_outy_l_g_ois_t; + +#define LSM6DSV16X_SPI2_OUTY_H_G_OIS 0x25U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t spi2_outy_g_ois : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t spi2_outy_g_ois : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_spi2_outy_h_g_ois_t; + +#define LSM6DSV16X_SPI2_OUTZ_L_G_OIS 0x26U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t spi2_outz_g_ois : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t spi2_outz_g_ois : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_spi2_outz_l_g_ois_t; + +#define LSM6DSV16X_SPI2_OUTZ_H_G_OIS 0x27U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t spi2_outz_g_ois : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t spi2_outz_g_ois : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_spi2_outz_h_g_ois_t; + +#define LSM6DSV16X_SPI2_OUTX_L_A_OIS 0x28U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t spi2_outx_a_ois : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t spi2_outx_a_ois : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_spi2_outx_l_a_ois_t; + +#define LSM6DSV16X_SPI2_OUTX_H_A_OIS 0x29U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t spi2_outx_a_ois : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t spi2_outx_a_ois : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_spi2_outx_h_a_ois_t; + +#define LSM6DSV16X_SPI2_OUTY_L_A_OIS 0x2AU +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t spi2_outy_a_ois : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t spi2_outy_a_ois : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_spi2_outy_l_a_ois_t; + +#define LSM6DSV16X_SPI2_OUTY_H_A_OIS 0x2BU +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t spi2_outy_a_ois : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t spi2_outy_a_ois : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_spi2_outy_h_a_ois_t; + +#define LSM6DSV16X_SPI2_OUTZ_L_A_OIS 0x2CU +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t spi2_outz_a_ois : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t spi2_outz_a_ois : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_spi2_outz_l_a_ois_t; + +#define LSM6DSV16X_SPI2_OUTZ_H_A_OIS 0x2DU +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t spi2_outz_a_ois : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t spi2_outz_a_ois : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_spi2_outz_h_a_ois_t; + +#define LSM6DSV16X_SPI2_HANDSHAKE_CTRL 0x6EU +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t spi2_shared_ack : 1; + uint8_t spi2_shared_req : 1; + uint8_t not_used0 : 6; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t not_used0 : 6; + uint8_t spi2_shared_req : 1; + uint8_t spi2_shared_ack : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_spi2_handshake_ctrl_t; + +#define LSM6DSV16X_SPI2_INT_OIS 0x6FU +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t st_xl_ois : 2; + uint8_t st_g_ois : 2; + uint8_t st_ois_clampdis : 1; + uint8_t not_used0 : 1; + uint8_t drdy_mask_ois : 1; + uint8_t int2_drdy_ois : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t int2_drdy_ois : 1; + uint8_t drdy_mask_ois : 1; + uint8_t not_used0 : 1; + uint8_t st_ois_clampdis : 1; + uint8_t st_g_ois : 2; + uint8_t st_xl_ois : 2; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_spi2_int_ois_t; + +#define LSM6DSV16X_SPI2_CTRL1_OIS 0x70U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t spi2_read_en : 1; + uint8_t ois_g_en : 1; + uint8_t ois_xl_en : 1; + uint8_t not_used0 : 2; + uint8_t sim_ois : 1; + uint8_t not_used1 : 2; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t not_used1 : 2; + uint8_t sim_ois : 1; + uint8_t not_used0 : 2; + uint8_t ois_xl_en : 1; + uint8_t ois_g_en : 1; + uint8_t spi2_read_en : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_spi2_ctrl1_ois_t; + +#define LSM6DSV16X_SPI2_CTRL2_OIS 0x71U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t fs_g_ois : 3; + uint8_t lpf1_g_ois_bw : 2; + uint8_t not_used0 : 3; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t not_used0 : 3; + uint8_t lpf1_g_ois_bw : 2; + uint8_t fs_g_ois : 3; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_spi2_ctrl2_ois_t; + +#define LSM6DSV16X_SPI2_CTRL3_OIS 0x72U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t fs_xl_ois : 2; + uint8_t not_used0 : 1; + uint8_t lpf_xl_ois_bw : 3; + uint8_t not_used1 : 2; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t not_used1 : 2; + uint8_t lpf_xl_ois_bw : 3; + uint8_t not_used0 : 1; + uint8_t fs_xl_ois : 2; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_spi2_ctrl3_ois_t; + +/** + * @} + * + */ + +/** @defgroup bitfields page embedded + * @{ + * + */ + +#define LSM6DSV16X_PAGE_SEL 0x2U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t not_used0 : 4; + uint8_t page_sel : 4; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t page_sel : 4; + uint8_t not_used0 : 4; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_page_sel_t; + +#define LSM6DSV16X_EMB_FUNC_EN_A 0x4U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t not_used0 : 3; + uint8_t pedo_en : 1; + uint8_t tilt_en : 1; + uint8_t sign_motion_en : 1; + uint8_t not_used1 : 1; + uint8_t mlc_before_fsm_en : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t mlc_before_fsm_en : 1; + uint8_t not_used1 : 1; + uint8_t sign_motion_en : 1; + uint8_t tilt_en : 1; + uint8_t pedo_en : 1; + uint8_t not_used0 : 3; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_emb_func_en_a_t; + +#define LSM6DSV16X_EMB_FUNC_EN_B 0x5U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t fsm_en : 1; + uint8_t not_used0 : 2; + uint8_t fifo_compr_en : 1; + uint8_t mlc_en : 1; + uint8_t not_used1 : 3; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t not_used1 : 3; + uint8_t mlc_en : 1; + uint8_t fifo_compr_en : 1; + uint8_t not_used0 : 2; + uint8_t fsm_en : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_emb_func_en_b_t; + +#define LSM6DSV16X_EMB_FUNC_EXEC_STATUS 0x7U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t emb_func_endop : 1; + uint8_t emb_func_exec_ovr : 1; + uint8_t not_used0 : 6; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t not_used0 : 6; + uint8_t emb_func_exec_ovr : 1; + uint8_t emb_func_endop : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_emb_func_exec_status_t; + +#define LSM6DSV16X_PAGE_ADDRESS 0x8U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t page_addr : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t page_addr : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_page_address_t; + +#define LSM6DSV16X_PAGE_VALUE 0x9U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t page_value : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t page_value : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_page_value_t; + +#define LSM6DSV16X_EMB_FUNC_INT1 0x0AU +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t not_used0 : 3; + uint8_t int1_step_detector : 1; + uint8_t int1_tilt : 1; + uint8_t int1_sig_mot : 1; + uint8_t not_used1 : 1; + uint8_t int1_fsm_lc : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t int1_fsm_lc : 1; + uint8_t not_used1 : 1; + uint8_t int1_sig_mot : 1; + uint8_t int1_tilt : 1; + uint8_t int1_step_detector : 1; + uint8_t not_used0 : 3; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_emb_func_int1_t; + +#define LSM6DSV16X_FSM_INT1 0x0BU +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t int1_fsm1 : 1; + uint8_t int1_fsm2 : 1; + uint8_t int1_fsm3 : 1; + uint8_t int1_fsm4 : 1; + uint8_t int1_fsm5 : 1; + uint8_t int1_fsm6 : 1; + uint8_t int1_fsm7 : 1; + uint8_t int1_fsm8 : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t int1_fsm8 : 1; + uint8_t int1_fsm7 : 1; + uint8_t int1_fsm6 : 1; + uint8_t int1_fsm5 : 1; + uint8_t int1_fsm4 : 1; + uint8_t int1_fsm3 : 1; + uint8_t int1_fsm2 : 1; + uint8_t int1_fsm1 : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_fsm_int1_t; + +#define LSM6DSV16X_MLC_INT1 0x0DU +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t int1_mlc1 : 1; + uint8_t int1_mlc2 : 1; + uint8_t int1_mlc3 : 1; + uint8_t int1_mlc4 : 1; + uint8_t not_used0 : 4; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t not_used0 : 4; + uint8_t int1_mlc4 : 1; + uint8_t int1_mlc3 : 1; + uint8_t int1_mlc2 : 1; + uint8_t int1_mlc1 : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_mlc_int1_t; + +#define LSM6DSV16X_EMB_FUNC_INT2 0x0EU +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t not_used0 : 3; + uint8_t int2_step_detector : 1; + uint8_t int2_tilt : 1; + uint8_t int2_sig_mot : 1; + uint8_t not_used1 : 1; + uint8_t int2_fsm_lc : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t int2_fsm_lc : 1; + uint8_t not_used1 : 1; + uint8_t int2_sig_mot : 1; + uint8_t int2_tilt : 1; + uint8_t int2_step_detector : 1; + uint8_t not_used0 : 3; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_emb_func_int2_t; + +#define LSM6DSV16X_FSM_INT2 0x0FU +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t int2_fsm1 : 1; + uint8_t int2_fsm2 : 1; + uint8_t int2_fsm3 : 1; + uint8_t int2_fsm4 : 1; + uint8_t int2_fsm5 : 1; + uint8_t int2_fsm6 : 1; + uint8_t int2_fsm7 : 1; + uint8_t int2_fsm8 : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t int2_fsm8 : 1; + uint8_t int2_fsm7 : 1; + uint8_t int2_fsm6 : 1; + uint8_t int2_fsm5 : 1; + uint8_t int2_fsm4 : 1; + uint8_t int2_fsm3 : 1; + uint8_t int2_fsm2 : 1; + uint8_t int2_fsm1 : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_fsm_int2_t; + +#define LSM6DSV16X_MLC_INT2 0x11U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t int2_mlc1 : 1; + uint8_t int2_mlc2 : 1; + uint8_t int2_mlc3 : 1; + uint8_t int2_mlc4 : 1; + uint8_t not_used0 : 4; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t not_used0 : 4; + uint8_t int2_mlc4 : 1; + uint8_t int2_mlc3 : 1; + uint8_t int2_mlc2 : 1; + uint8_t int2_mlc1 : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_mlc_int2_t; + +#define LSM6DSV16X_EMB_FUNC_STATUS 0x12U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t not_used0 : 3; + uint8_t is_step_det : 1; + uint8_t is_tilt : 1; + uint8_t is_sigmot : 1; + uint8_t not_used1 : 1; + uint8_t is_fsm_lc : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t is_fsm_lc : 1; + uint8_t not_used1 : 1; + uint8_t is_sigmot : 1; + uint8_t is_tilt : 1; + uint8_t is_step_det : 1; + uint8_t not_used0 : 3; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_emb_func_status_t; + +#define LSM6DSV16X_FSM_STATUS 0x13U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t is_fsm1 : 1; + uint8_t is_fsm2 : 1; + uint8_t is_fsm3 : 1; + uint8_t is_fsm4 : 1; + uint8_t is_fsm5 : 1; + uint8_t is_fsm6 : 1; + uint8_t is_fsm7 : 1; + uint8_t is_fsm8 : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t is_fsm8 : 1; + uint8_t is_fsm7 : 1; + uint8_t is_fsm6 : 1; + uint8_t is_fsm5 : 1; + uint8_t is_fsm4 : 1; + uint8_t is_fsm3 : 1; + uint8_t is_fsm2 : 1; + uint8_t is_fsm1 : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_fsm_status_t; + +#define LSM6DSV16X_MLC_STATUS 0x15U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t is_mlc1 : 1; + uint8_t is_mlc2 : 1; + uint8_t is_mlc3 : 1; + uint8_t is_mlc4 : 1; + uint8_t not_used0 : 4; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t not_used0 : 4; + uint8_t is_mlc4 : 1; + uint8_t is_mlc3 : 1; + uint8_t is_mlc2 : 1; + uint8_t is_mlc1 : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_mlc_status_t; + +#define LSM6DSV16X_PAGE_RW 0x17U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t not_used0 : 5; + uint8_t page_read : 1; + uint8_t page_write : 1; + uint8_t emb_func_lir : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t emb_func_lir : 1; + uint8_t page_write : 1; + uint8_t page_read : 1; + uint8_t not_used0 : 5; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_page_rw_t; + +#define LSM6DSV16X_EMB_FUNC_FIFO_EN_A 0x44U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t not_used0 : 6; + uint8_t step_counter_fifo_en : 1; + uint8_t mlc_fifo_en : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t mlc_fifo_en : 1; + uint8_t step_counter_fifo_en : 1; + uint8_t not_used0 : 6; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_emb_func_fifo_en_a_t; + +#define LSM6DSV16X_EMB_FUNC_FIFO_EN_B 0x45U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t not_used0 : 1; + uint8_t mlc_filter_feature_fifo_en : 1; + uint8_t not_used1 : 6; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t not_used1 : 6; + uint8_t mlc_filter_feature_fifo_en : 1; + uint8_t not_used0 : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_emb_func_fifo_en_b_t; + +#define LSM6DSV16X_FSM_ENABLE 0x46U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t fsm1_en : 1; + uint8_t fsm2_en : 1; + uint8_t fsm3_en : 1; + uint8_t fsm4_en : 1; + uint8_t fsm5_en : 1; + uint8_t fsm6_en : 1; + uint8_t fsm7_en : 1; + uint8_t fsm8_en : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t fsm8_en : 1; + uint8_t fsm7_en : 1; + uint8_t fsm6_en : 1; + uint8_t fsm5_en : 1; + uint8_t fsm4_en : 1; + uint8_t fsm3_en : 1; + uint8_t fsm2_en : 1; + uint8_t fsm1_en : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_fsm_enable_t; + +#define LSM6DSV16X_FSM_LONG_COUNTER_L 0x48U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t fsm_lc : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t fsm_lc : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_fsm_long_counter_l_t; + +#define LSM6DSV16X_FSM_LONG_COUNTER_H 0x49U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t fsm_lc : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t fsm_lc : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_fsm_long_counter_h_t; + +#define LSM6DSV16X_INT_ACK_MASK 0x4BU +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t iack_mask : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t iack_mask : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_int_ack_mask_t; + +#define LSM6DSV16X_FSM_OUTS1 0x4CU +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t fsm1_n_v : 1; + uint8_t fsm1_p_v : 1; + uint8_t fsm1_n_z : 1; + uint8_t fsm1_p_z : 1; + uint8_t fsm1_n_y : 1; + uint8_t fsm1_p_y : 1; + uint8_t fsm1_n_x : 1; + uint8_t fsm1_p_x : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t fsm1_p_x : 1; + uint8_t fsm1_n_x : 1; + uint8_t fsm1_p_y : 1; + uint8_t fsm1_n_y : 1; + uint8_t fsm1_p_z : 1; + uint8_t fsm1_n_z : 1; + uint8_t fsm1_p_v : 1; + uint8_t fsm1_n_v : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_fsm_outs1_t; + +#define LSM6DSV16X_FSM_OUTS2 0x4DU +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t fsm2_n_v : 1; + uint8_t fsm2_p_v : 1; + uint8_t fsm2_n_z : 1; + uint8_t fsm2_p_z : 1; + uint8_t fsm2_n_y : 1; + uint8_t fsm2_p_y : 1; + uint8_t fsm2_n_x : 1; + uint8_t fsm2_p_x : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t fsm2_p_x : 1; + uint8_t fsm2_n_x : 1; + uint8_t fsm2_p_y : 1; + uint8_t fsm2_n_y : 1; + uint8_t fsm2_p_z : 1; + uint8_t fsm2_n_z : 1; + uint8_t fsm2_p_v : 1; + uint8_t fsm2_n_v : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_fsm_outs2_t; + +#define LSM6DSV16X_FSM_OUTS3 0x4EU +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t fsm3_n_v : 1; + uint8_t fsm3_p_v : 1; + uint8_t fsm3_n_z : 1; + uint8_t fsm3_p_z : 1; + uint8_t fsm3_n_y : 1; + uint8_t fsm3_p_y : 1; + uint8_t fsm3_n_x : 1; + uint8_t fsm3_p_x : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t fsm3_p_x : 1; + uint8_t fsm3_n_x : 1; + uint8_t fsm3_p_y : 1; + uint8_t fsm3_n_y : 1; + uint8_t fsm3_p_z : 1; + uint8_t fsm3_n_z : 1; + uint8_t fsm3_p_v : 1; + uint8_t fsm3_n_v : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_fsm_outs3_t; + +#define LSM6DSV16X_FSM_OUTS4 0x4FU +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t fsm4_n_v : 1; + uint8_t fsm4_p_v : 1; + uint8_t fsm4_n_z : 1; + uint8_t fsm4_p_z : 1; + uint8_t fsm4_n_y : 1; + uint8_t fsm4_p_y : 1; + uint8_t fsm4_n_x : 1; + uint8_t fsm4_p_x : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t fsm4_p_x : 1; + uint8_t fsm4_n_x : 1; + uint8_t fsm4_p_y : 1; + uint8_t fsm4_n_y : 1; + uint8_t fsm4_p_z : 1; + uint8_t fsm4_n_z : 1; + uint8_t fsm4_p_v : 1; + uint8_t fsm4_n_v : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_fsm_outs4_t; + +#define LSM6DSV16X_FSM_OUTS5 0x50U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t fsm5_n_v : 1; + uint8_t fsm5_p_v : 1; + uint8_t fsm5_n_z : 1; + uint8_t fsm5_p_z : 1; + uint8_t fsm5_n_y : 1; + uint8_t fsm5_p_y : 1; + uint8_t fsm5_n_x : 1; + uint8_t fsm5_p_x : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t fsm5_p_x : 1; + uint8_t fsm5_n_x : 1; + uint8_t fsm5_p_y : 1; + uint8_t fsm5_n_y : 1; + uint8_t fsm5_p_z : 1; + uint8_t fsm5_n_z : 1; + uint8_t fsm5_p_v : 1; + uint8_t fsm5_n_v : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_fsm_outs5_t; + +#define LSM6DSV16X_FSM_OUTS6 0x51U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t fsm6_n_v : 1; + uint8_t fsm6_p_v : 1; + uint8_t fsm6_n_z : 1; + uint8_t fsm6_p_z : 1; + uint8_t fsm6_n_y : 1; + uint8_t fsm6_p_y : 1; + uint8_t fsm6_n_x : 1; + uint8_t fsm6_p_x : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t fsm6_p_x : 1; + uint8_t fsm6_n_x : 1; + uint8_t fsm6_p_y : 1; + uint8_t fsm6_n_y : 1; + uint8_t fsm6_p_z : 1; + uint8_t fsm6_n_z : 1; + uint8_t fsm6_p_v : 1; + uint8_t fsm6_n_v : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_fsm_outs6_t; + +#define LSM6DSV16X_FSM_OUTS7 0x52U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t fsm7_n_v : 1; + uint8_t fsm7_p_v : 1; + uint8_t fsm7_n_z : 1; + uint8_t fsm7_p_z : 1; + uint8_t fsm7_n_y : 1; + uint8_t fsm7_p_y : 1; + uint8_t fsm7_n_x : 1; + uint8_t fsm7_p_x : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t fsm7_p_x : 1; + uint8_t fsm7_n_x : 1; + uint8_t fsm7_p_y : 1; + uint8_t fsm7_n_y : 1; + uint8_t fsm7_p_z : 1; + uint8_t fsm7_n_z : 1; + uint8_t fsm7_p_v : 1; + uint8_t fsm7_n_v : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_fsm_outs7_t; + +#define LSM6DSV16X_FSM_OUTS8 0x53U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t fsm8_n_v : 1; + uint8_t fsm8_p_v : 1; + uint8_t fsm8_n_z : 1; + uint8_t fsm8_p_z : 1; + uint8_t fsm8_n_y : 1; + uint8_t fsm8_p_y : 1; + uint8_t fsm8_n_x : 1; + uint8_t fsm8_p_x : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t fsm8_p_x : 1; + uint8_t fsm8_n_x : 1; + uint8_t fsm8_p_y : 1; + uint8_t fsm8_n_y : 1; + uint8_t fsm8_p_z : 1; + uint8_t fsm8_n_z : 1; + uint8_t fsm8_p_v : 1; + uint8_t fsm8_n_v : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_fsm_outs8_t; + +#define LSM6DSV16X_FSM_ODR 0x5FU +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t not_used0 : 3; + uint8_t fsm_odr : 3; + uint8_t not_used1 : 2; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t not_used1 : 2; + uint8_t fsm_odr : 3; + uint8_t not_used0 : 3; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_fsm_odr_t; + +#define LSM6DSV16X_MLC_ODR 0x60U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t not_used0 : 4; + uint8_t mlc_odr : 3; + uint8_t not_used1 : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t not_used1 : 1; + uint8_t mlc_odr : 3; + uint8_t not_used0 : 4; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_mlc_odr_t; + +#define LSM6DSV16X_STEP_COUNTER_L 0x62U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t step : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t step : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_step_counter_l_t; + +#define LSM6DSV16X_STEP_COUNTER_H 0x63U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t step : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t step : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_step_counter_h_t; + +#define LSM6DSV16X_EMB_FUNC_SRC 0x64U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t not_used0 : 2; + uint8_t stepcounter_bit_set : 1; + uint8_t step_overflow : 1; + uint8_t step_count_delta_ia : 1; + uint8_t step_detected : 1; + uint8_t not_used1 : 1; + uint8_t pedo_rst_step : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t pedo_rst_step : 1; + uint8_t not_used1 : 1; + uint8_t step_detected : 1; + uint8_t step_count_delta_ia : 1; + uint8_t step_overflow : 1; + uint8_t stepcounter_bit_set : 1; + uint8_t not_used0 : 2; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_emb_func_src_t; + +#define LSM6DSV16X_EMB_FUNC_INIT_A 0x66U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t not_used0 : 3; + uint8_t step_det_init : 1; + uint8_t tilt_init : 1; + uint8_t sig_mot_init : 1; + uint8_t not_used1 : 1; + uint8_t mlc_before_fsm_init : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t mlc_before_fsm_init : 1; + uint8_t not_used1 : 1; + uint8_t sig_mot_init : 1; + uint8_t tilt_init : 1; + uint8_t step_det_init : 1; + uint8_t not_used0 : 3; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_emb_func_init_a_t; + +#define LSM6DSV16X_EMB_FUNC_INIT_B 0x67U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t fsm_init : 1; + uint8_t not_used0 : 2; + uint8_t fifo_compr_init : 1; + uint8_t mlc_init : 1; + uint8_t not_used1 : 3; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t not_used1 : 3; + uint8_t mlc_init : 1; + uint8_t fifo_compr_init : 1; + uint8_t not_used0 : 2; + uint8_t fsm_init : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_emb_func_init_b_t; + +#define LSM6DSV16X_MLC1_SRC 0x70U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t mlc1_src : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t mlc1_src : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_mlc1_src_t; + +#define LSM6DSV16X_MLC2_SRC 0x71U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t mlc2_src : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t mlc2_src : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_mlc2_src_t; + +#define LSM6DSV16X_MLC3_SRC 0x72U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t mlc3_src : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t mlc3_src : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_mlc3_src_t; + +#define LSM6DSV16X_MLC4_SRC 0x73U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t mlc4_src : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t mlc4_src : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_mlc4_src_t; + +/** + * @} + * + */ + +/** @defgroup bitfields page pg0_emb_adv + * @{ + * + */ +#define LSM6DSV16X_EMB_ADV_PG_0 0x000 + +#define LSM6DSV16X_FSM_EXT_SENSITIVITY_L 0xBAU +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t fsm_ext_s : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t fsm_ext_s : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_fsm_ext_sensitivity_l_t; + +#define LSM6DSV16X_FSM_EXT_SENSITIVITY_H 0xBBU +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t fsm_ext_s : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t fsm_ext_s : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_fsm_ext_sensitivity_h_t; + +#define LSM6DSV16X_FSM_EXT_OFFX_L 0xC0U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t fsm_ext_offx : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t fsm_ext_offx : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_fsm_ext_offx_l_t; + +#define LSM6DSV16X_FSM_EXT_OFFX_H 0xC1U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t fsm_ext_offx : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t fsm_ext_offx : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_fsm_ext_offx_h_t; + +#define LSM6DSV16X_FSM_EXT_OFFY_L 0xC2U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t fsm_ext_offy : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t fsm_ext_offy : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_fsm_ext_offy_l_t; + +#define LSM6DSV16X_FSM_EXT_OFFY_H 0xC3U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t fsm_ext_offy : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t fsm_ext_offy : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_fsm_ext_offy_h_t; + +#define LSM6DSV16X_FSM_EXT_OFFZ_L 0xC4U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t fsm_ext_offz : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t fsm_ext_offz : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_fsm_ext_offz_l_t; + +#define LSM6DSV16X_FSM_EXT_OFFZ_H 0xC5U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t fsm_ext_offz : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t fsm_ext_offz : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_fsm_ext_offz_h_t; + +#define LSM6DSV16X_FSM_EXT_MATRIX_XX_L 0xC6U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t fsm_ext_mat_xx : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t fsm_ext_mat_xx : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_fsm_ext_matrix_xx_l_t; + +#define LSM6DSV16X_FSM_EXT_MATRIX_XX_H 0xC7U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t fsm_ext_mat_xx : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t fsm_ext_mat_xx : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_fsm_ext_matrix_xx_h_t; + +#define LSM6DSV16X_FSM_EXT_MATRIX_XY_L 0xC8U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t fsm_ext_mat_xy : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t fsm_ext_mat_xy : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_fsm_ext_matrix_xy_l_t; + +#define LSM6DSV16X_FSM_EXT_MATRIX_XY_H 0xC9U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t fsm_ext_mat_xy : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t fsm_ext_mat_xy : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_fsm_ext_matrix_xy_h_t; + +#define LSM6DSV16X_FSM_EXT_MATRIX_XZ_L 0xCAU +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t fsm_ext_mat_xz : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t fsm_ext_mat_xz : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_fsm_ext_matrix_xz_l_t; + +#define LSM6DSV16X_FSM_EXT_MATRIX_XZ_H 0xCBU +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t fsm_ext_mat_xz : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t fsm_ext_mat_xz : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_fsm_ext_matrix_xz_h_t; + +#define LSM6DSV16X_FSM_EXT_MATRIX_YY_L 0xCCU +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t fsm_ext_mat_yy : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t fsm_ext_mat_yy : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_fsm_ext_matrix_yy_l_t; + +#define LSM6DSV16X_FSM_EXT_MATRIX_YY_H 0xCDU +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t fsm_ext_mat_yy : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t fsm_ext_mat_yy : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_fsm_ext_matrix_yy_h_t; + +#define LSM6DSV16X_FSM_EXT_MATRIX_YZ_L 0xCEU +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t fsm_ext_mat_yz : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t fsm_ext_mat_yz : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_fsm_ext_matrix_yz_l_t; + +#define LSM6DSV16X_FSM_EXT_MATRIX_YZ_H 0xCFU +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t fsm_ext_mat_yz : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t fsm_ext_mat_yz : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_fsm_ext_matrix_yz_h_t; + +#define LSM6DSV16X_FSM_EXT_MATRIX_ZZ_L 0xD0U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t fsm_ext_mat_zz : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t fsm_ext_mat_zz : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_fsm_ext_matrix_zz_l_t; + +#define LSM6DSV16X_FSM_EXT_MATRIX_ZZ_H 0xD1U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t fsm_ext_mat_zz : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t fsm_ext_mat_zz : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_fsm_ext_matrix_zz_h_t; + +#define LSM6DSV16X_EXT_CFG_A 0xD4U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t ext_z_axis : 3; + uint8_t not_used0 : 1; + uint8_t ext_y_axis : 3; + uint8_t not_used1 : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t not_used1 : 1; + uint8_t ext_y_axis : 3; + uint8_t not_used0 : 1; + uint8_t ext_z_axis : 3; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_ext_cfg_a_t; + +#define LSM6DSV16X_EXT_CFG_B 0xD5U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t ext_x_axis : 3; + uint8_t not_used0 : 5; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t not_used0 : 5; + uint8_t ext_x_axis : 3; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_ext_cfg_b_t; + +/** + * @} + * + */ + +/** @defgroup bitfields page pg1_emb_adv + * @{ + * + */ +#define LSM6DSV16X_EMB_ADV_PG_1 0x100 + +#define LSM6DSV16X_FSM_LC_TIMEOUT_L 0x7AU +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t fsm_lc_timeout : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t fsm_lc_timeout : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_fsm_lc_timeout_l_t; + +#define LSM6DSV16X_FSM_LC_TIMEOUT_H 0x7BU +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t fsm_lc_timeout : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t fsm_lc_timeout : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_fsm_lc_timeout_h_t; + +#define LSM6DSV16X_FSM_PROGRAMS 0x7CU +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t fsm_n_prog : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t fsm_n_prog : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_fsm_programs_t; + +#define LSM6DSV16X_FSM_START_ADD_L 0x7EU +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t fsm_start : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t fsm_start : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_fsm_start_add_l_t; + +#define LSM6DSV16X_FSM_START_ADD_H 0x7FU +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t fsm_start : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t fsm_start : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_fsm_start_add_h_t; + +#define LSM6DSV16X_PEDO_CMD_REG 0x83U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t not_used0 : 2; + uint8_t fp_rejection_en : 1; + uint8_t carry_count_en : 1; + uint8_t not_used1 : 4; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t not_used1 : 4; + uint8_t carry_count_en : 1; + uint8_t fp_rejection_en : 1; + uint8_t not_used0 : 2; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_pedo_cmd_reg_t; + +#define LSM6DSV16X_PEDO_DEB_STEPS_CONF 0x84U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t deb_step : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t deb_step : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_pedo_deb_steps_conf_t; + +#define LSM6DSV16X_PEDO_SC_DELTAT_L 0xD0U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t pd_sc : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t pd_sc : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_pedo_sc_deltat_l_t; + +#define LSM6DSV16X_PEDO_SC_DELTAT_H 0xD1U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t pd_sc : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t pd_sc : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_pedo_sc_deltat_h_t; + +#define LSM6DSV16X_MLC_EXT_SENSITIVITY_L 0xE8U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t mlc_ext_s : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t mlc_ext_s : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_mlc_ext_sensitivity_l_t; + +#define LSM6DSV16X_MLC_EXT_SENSITIVITY_H 0xE9U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t mlc_ext_s : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t mlc_ext_s : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_mlc_ext_sensitivity_h_t; + +/** @defgroup bitfields page pg2_emb_adv + * @{ + * + */ +#define LSM6DSV16X_EMB_ADV_PG_2 0x200 + +#define LSM6DSV16X_EXT_FORMAT 0x00 +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t not_used0 : 2; + uint8_t ext_format_sel : 1; + uint8_t not_used1 : 5; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t not_used1 : 5; + uint8_t ext_format_sel : 1; + uint8_t not_used0 : 2; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_ext_format_t; + +#define LSM6DSV16X_EXT_3BYTE_SENSITIVITY_L 0x02U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t ext_3byte_s : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t ext_3byte_s : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_ext_3byte_sensitivity_l_t; + +#define LSM6DSV16X_EXT_3BYTE_SENSITIVITY_H 0x03U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t ext_3byte_s : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t ext_3byte_s : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_ext_3byte_sensitivity_h_t; + +#define LSM6DSV16X_EXT_3BYTE_OFFSET_XL 0x06U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t ext_3byte_off : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t ext_3byte_off : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_ext_3byte_offset_xl_t; + +#define LSM6DSV16X_EXT_3BYTE_OFFSET_L 0x07U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t ext_3byte_off : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t ext_3byte_off : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_ext_3byte_offset_l_t; + +#define LSM6DSV16X_EXT_3BYTE_OFFSET_H 0x08U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t ext_3byte_off : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t ext_3byte_off : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_ext_3byte_offset_h_t; + +/** + * @} + * + */ + +/** @defgroup bitfields page sensor_hub + * @{ + * + */ + +#define LSM6DSV16X_SENSOR_HUB_1 0x2U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t sensorhub1 : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t sensorhub1 : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_sensor_hub_1_t; + +#define LSM6DSV16X_SENSOR_HUB_2 0x3U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t sensorhub2 : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t sensorhub2 : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_sensor_hub_2_t; + +#define LSM6DSV16X_SENSOR_HUB_3 0x4U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t sensorhub3 : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t sensorhub3 : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_sensor_hub_3_t; + +#define LSM6DSV16X_SENSOR_HUB_4 0x5U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t sensorhub4 : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t sensorhub4 : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_sensor_hub_4_t; + +#define LSM6DSV16X_SENSOR_HUB_5 0x6U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t sensorhub5 : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t sensorhub5 : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_sensor_hub_5_t; + +#define LSM6DSV16X_SENSOR_HUB_6 0x7U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t sensorhub6 : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t sensorhub6 : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_sensor_hub_6_t; + +#define LSM6DSV16X_SENSOR_HUB_7 0x8U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t sensorhub7 : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t sensorhub7 : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_sensor_hub_7_t; + +#define LSM6DSV16X_SENSOR_HUB_8 0x9U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t sensorhub8 : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t sensorhub8 : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_sensor_hub_8_t; + +#define LSM6DSV16X_SENSOR_HUB_9 0x0AU +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t sensorhub9 : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t sensorhub9 : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_sensor_hub_9_t; + +#define LSM6DSV16X_SENSOR_HUB_10 0x0BU +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t sensorhub10 : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t sensorhub10 : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_sensor_hub_10_t; + +#define LSM6DSV16X_SENSOR_HUB_11 0x0CU +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t sensorhub11 : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t sensorhub11 : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_sensor_hub_11_t; + +#define LSM6DSV16X_SENSOR_HUB_12 0x0DU +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t sensorhub12 : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t sensorhub12 : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_sensor_hub_12_t; + +#define LSM6DSV16X_SENSOR_HUB_13 0x0EU +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t sensorhub13 : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t sensorhub13 : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_sensor_hub_13_t; + +#define LSM6DSV16X_SENSOR_HUB_14 0x0FU +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t sensorhub14 : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t sensorhub14 : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_sensor_hub_14_t; + +#define LSM6DSV16X_SENSOR_HUB_15 0x10U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t sensorhub15 : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t sensorhub15 : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_sensor_hub_15_t; + +#define LSM6DSV16X_SENSOR_HUB_16 0x11U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t sensorhub16 : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t sensorhub16 : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_sensor_hub_16_t; + +#define LSM6DSV16X_SENSOR_HUB_17 0x12U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t sensorhub17 : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t sensorhub17 : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_sensor_hub_17_t; + +#define LSM6DSV16X_SENSOR_HUB_18 0x13U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t sensorhub18 : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t sensorhub18 : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_sensor_hub_18_t; + +#define LSM6DSV16X_MASTER_CONFIG 0x14U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t aux_sens_on : 2; + uint8_t master_on : 1; + uint8_t not_used0 : 1; + uint8_t pass_through_mode : 1; + uint8_t start_config : 1; + uint8_t write_once : 1; + uint8_t rst_master_regs : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t rst_master_regs : 1; + uint8_t write_once : 1; + uint8_t start_config : 1; + uint8_t pass_through_mode : 1; + uint8_t not_used0 : 1; + uint8_t master_on : 1; + uint8_t aux_sens_on : 2; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_master_config_t; + +#define LSM6DSV16X_SLV0_ADD 0x15U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t rw_0 : 1; + uint8_t slave0_add : 7; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t slave0_add : 7; + uint8_t rw_0 : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_slv0_add_t; + +#define LSM6DSV16X_SLV0_SUBADD 0x16U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t slave0_reg : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t slave0_reg : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_slv0_subadd_t; + +#define LSM6DSV16X_SLV0_CONFIG 0x17U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t slave0_numop : 3; + uint8_t batch_ext_sens_0_en : 1; + uint8_t not_used0 : 1; + uint8_t shub_odr : 3; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t shub_odr : 3; + uint8_t not_used0 : 1; + uint8_t batch_ext_sens_0_en : 1; + uint8_t slave0_numop : 3; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_slv0_config_t; + +#define LSM6DSV16X_SLV1_ADD 0x18U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t r_1 : 1; + uint8_t slave1_add : 7; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t slave1_add : 7; + uint8_t r_1 : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_slv1_add_t; + +#define LSM6DSV16X_SLV1_SUBADD 0x19U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t slave1_reg : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t slave1_reg : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_slv1_subadd_t; + +#define LSM6DSV16X_SLV1_CONFIG 0x1AU +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t slave1_numop : 3; + uint8_t batch_ext_sens_1_en : 1; + uint8_t not_used0 : 4; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t not_used0 : 4; + uint8_t batch_ext_sens_1_en : 1; + uint8_t slave1_numop : 3; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_slv1_config_t; + +#define LSM6DSV16X_SLV2_ADD 0x1BU +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t r_2 : 1; + uint8_t slave2_add : 7; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t slave2_add : 7; + uint8_t r_2 : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_slv2_add_t; + +#define LSM6DSV16X_SLV2_SUBADD 0x1CU +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t slave2_reg : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t slave2_reg : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_slv2_subadd_t; + +#define LSM6DSV16X_SLV2_CONFIG 0x1DU +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t slave2_numop : 3; + uint8_t batch_ext_sens_2_en : 1; + uint8_t not_used0 : 4; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t not_used0 : 4; + uint8_t batch_ext_sens_2_en : 1; + uint8_t slave2_numop : 3; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_slv2_config_t; + +#define LSM6DSV16X_SLV3_ADD 0x1EU +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t r_3 : 1; + uint8_t slave3_add : 7; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t slave3_add : 7; + uint8_t r_3 : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_slv3_add_t; + +#define LSM6DSV16X_SLV3_SUBADD 0x1FU +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t slave3_reg : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t slave3_reg : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_slv3_subadd_t; + +#define LSM6DSV16X_SLV3_CONFIG 0x20U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t slave3_numop : 3; + uint8_t batch_ext_sens_3_en : 1; + uint8_t not_used0 : 4; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t not_used0 : 4; + uint8_t batch_ext_sens_3_en : 1; + uint8_t slave3_numop : 3; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_slv3_config_t; + +#define LSM6DSV16X_DATAWRITE_SLV0 0x21U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t slave0_dataw : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t slave0_dataw : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_datawrite_slv0_t; + +#define LSM6DSV16X_STATUS_MASTER 0x22U +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t sens_hub_endop : 1; + uint8_t not_used0 : 2; + uint8_t slave0_nack : 1; + uint8_t slave1_nack : 1; + uint8_t slave2_nack : 1; + uint8_t slave3_nack : 1; + uint8_t wr_once_done : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t wr_once_done : 1; + uint8_t slave3_nack : 1; + uint8_t slave2_nack : 1; + uint8_t slave1_nack : 1; + uint8_t slave0_nack : 1; + uint8_t not_used0 : 2; + uint8_t sens_hub_endop : 1; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_status_master_t; + +/** + * @} + * + */ + +typedef union { + lsm6dsv16x_func_cfg_access_t func_cfg_access; + lsm6dsv16x_pin_ctrl_t pin_ctrl; + lsm6dsv16x_if_cfg_t if_cfg; + lsm6dsv16x_s4s_tph_l_t s4s_tph_l; + lsm6dsv16x_s4s_tph_h_t s4s_tph_h; + lsm6dsv16x_s4s_rr_t s4s_rr; + lsm6dsv16x_fifo_ctrl1_t fifo_ctrl1; + lsm6dsv16x_fifo_ctrl2_t fifo_ctrl2; + lsm6dsv16x_fifo_ctrl3_t fifo_ctrl3; + lsm6dsv16x_fifo_ctrl4_t fifo_ctrl4; + lsm6dsv16x_counter_bdr_reg1_t counter_bdr_reg1; + lsm6dsv16x_counter_bdr_reg2_t counter_bdr_reg2; + lsm6dsv16x_int1_ctrl_t int1_ctrl; + lsm6dsv16x_int2_ctrl_t int2_ctrl; + lsm6dsv16x_who_am_i_t who_am_i; + lsm6dsv16x_ctrl1_t ctrl1; + lsm6dsv16x_ctrl2_t ctrl2; + lsm6dsv16x_ctrl3_t ctrl3; + lsm6dsv16x_ctrl4_t ctrl4; + lsm6dsv16x_ctrl5_t ctrl5; + lsm6dsv16x_ctrl6_t ctrl6; + lsm6dsv16x_ctrl7_t ctrl7; + lsm6dsv16x_ctrl8_t ctrl8; + lsm6dsv16x_ctrl9_t ctrl9; + lsm6dsv16x_ctrl10_t ctrl10; + lsm6dsv16x_ctrl_status_t ctrl_status; + lsm6dsv16x_fifo_status1_t fifo_status1; + lsm6dsv16x_fifo_status2_t fifo_status2; + lsm6dsv16x_all_int_src_t all_int_src; + lsm6dsv16x_status_reg_t status_reg; + lsm6dsv16x_out_temp_l_t out_temp_l; + lsm6dsv16x_out_temp_h_t out_temp_h; + lsm6dsv16x_outx_l_g_t outx_l_g; + lsm6dsv16x_outx_h_g_t outx_h_g; + lsm6dsv16x_outy_l_g_t outy_l_g; + lsm6dsv16x_outy_h_g_t outy_h_g; + lsm6dsv16x_outz_l_g_t outz_l_g; + lsm6dsv16x_outz_h_g_t outz_h_g; + lsm6dsv16x_outx_l_a_t outx_l_a; + lsm6dsv16x_outx_h_a_t outx_h_a; + lsm6dsv16x_outy_l_a_t outy_l_a; + lsm6dsv16x_outy_h_a_t outy_h_a; + lsm6dsv16x_outz_l_a_t outz_l_a; + lsm6dsv16x_outz_h_a_t outz_h_a; + lsm6dsv16x_ui_outx_l_g_ois_eis_t ui_outx_l_g_ois_eis; + lsm6dsv16x_ui_outx_h_g_ois_eis_t ui_outx_h_g_ois_eis; + lsm6dsv16x_ui_outy_l_g_ois_eis_t ui_outy_l_g_ois_eis; + lsm6dsv16x_ui_outy_h_g_ois_eis_t ui_outy_h_g_ois_eis; + lsm6dsv16x_ui_outz_l_g_ois_eis_t ui_outz_l_g_ois_eis; + lsm6dsv16x_ui_outz_h_g_ois_eis_t ui_outz_h_g_ois_eis; + lsm6dsv16x_ui_outx_l_a_ois_dualc_t ui_outx_l_a_ois_dualc; + lsm6dsv16x_ui_outx_h_a_ois_dualc_t ui_outx_h_a_ois_dualc; + lsm6dsv16x_ui_outy_l_a_ois_dualc_t ui_outy_l_a_ois_dualc; + lsm6dsv16x_ui_outy_h_a_ois_dualc_t ui_outy_h_a_ois_dualc; + lsm6dsv16x_ui_outz_l_a_ois_dualc_t ui_outz_l_a_ois_dualc; + lsm6dsv16x_ui_outz_h_a_ois_dualc_t ui_outz_h_a_ois_dualc; + lsm6dsv16x_ah_qvar_out_l_t ah_qvar_out_l; + lsm6dsv16x_ah_qvar_out_h_t ah_qvar_out_h; + lsm6dsv16x_timestamp0_t timestamp0; + lsm6dsv16x_timestamp1_t timestamp1; + lsm6dsv16x_timestamp2_t timestamp2; + lsm6dsv16x_timestamp3_t timestamp3; + lsm6dsv16x_ui_status_reg_ois_t ui_status_reg_ois; + lsm6dsv16x_wake_up_src_t wake_up_src; + lsm6dsv16x_tap_src_t tap_src; + lsm6dsv16x_d6d_src_t d6d_src; + lsm6dsv16x_status_master_mainpage_t status_master_mainpage; + lsm6dsv16x_emb_func_status_mainpage_t emb_func_status_mainpage; + lsm6dsv16x_fsm_status_mainpage_t fsm_status_mainpage; + lsm6dsv16x_mlc_status_mainpage_t mlc_status_mainpage; + lsm6dsv16x_internal_freq_t internal_freq; + lsm6dsv16x_functions_enable_t functions_enable; + lsm6dsv16x_den_t den; + lsm6dsv16x_inactivity_dur_t inactivity_dur; + lsm6dsv16x_inactivity_ths_t inactivity_ths; + lsm6dsv16x_tap_cfg0_t tap_cfg0; + lsm6dsv16x_tap_cfg1_t tap_cfg1; + lsm6dsv16x_tap_cfg2_t tap_cfg2; + lsm6dsv16x_tap_ths_6d_t tap_ths_6d; + lsm6dsv16x_int_dur2_t int_dur2; + lsm6dsv16x_wake_up_ths_t wake_up_ths; + lsm6dsv16x_wake_up_dur_t wake_up_dur; + lsm6dsv16x_free_fall_t free_fall; + lsm6dsv16x_md1_cfg_t md1_cfg; + lsm6dsv16x_md2_cfg_t md2_cfg; + lsm6dsv16x_s4s_st_cmd_code_t s4s_st_cmd_code; + lsm6dsv16x_s4s_dt_reg_t s4s_dt_reg; + lsm6dsv16x_emb_func_cfg_t emb_func_cfg; + lsm6dsv16x_ui_handshake_ctrl_t ui_handshake_ctrl; + lsm6dsv16x_ui_spi2_shared_0_t ui_spi2_shared_0; + lsm6dsv16x_ui_spi2_shared_1_t ui_spi2_shared_1; + lsm6dsv16x_ui_spi2_shared_2_t ui_spi2_shared_2; + lsm6dsv16x_ui_spi2_shared_3_t ui_spi2_shared_3; + lsm6dsv16x_ui_spi2_shared_4_t ui_spi2_shared_4; + lsm6dsv16x_ui_spi2_shared_5_t ui_spi2_shared_5; + lsm6dsv16x_ctrl_eis_t ctrl_eis; + lsm6dsv16x_ui_int_ois_t ui_int_ois; + lsm6dsv16x_ui_ctrl1_ois_t ui_ctrl1_ois; + lsm6dsv16x_ui_ctrl2_ois_t ui_ctrl2_ois; + lsm6dsv16x_ui_ctrl3_ois_t ui_ctrl3_ois; + lsm6dsv16x_x_ofs_usr_t x_ofs_usr; + lsm6dsv16x_y_ofs_usr_t y_ofs_usr; + lsm6dsv16x_z_ofs_usr_t z_ofs_usr; + lsm6dsv16x_fifo_data_out_tag_t fifo_data_out_tag; + lsm6dsv16x_fifo_data_out_x_l_t fifo_data_out_x_l; + lsm6dsv16x_fifo_data_out_x_h_t fifo_data_out_x_h; + lsm6dsv16x_fifo_data_out_y_l_t fifo_data_out_y_l; + lsm6dsv16x_fifo_data_out_y_h_t fifo_data_out_y_h; + lsm6dsv16x_fifo_data_out_z_l_t fifo_data_out_z_l; + lsm6dsv16x_fifo_data_out_z_h_t fifo_data_out_z_h; + bitwise_t bitwise; + uint8_t byte; +} prefix_lowmain_t; + +typedef union { + lsm6dsv16x_spi2_who_am_i_t spi2_who_am_i; + lsm6dsv16x_spi2_status_reg_ois_t spi2_status_reg_ois; + lsm6dsv16x_spi2_out_temp_l_t spi2_out_temp_l; + lsm6dsv16x_spi2_out_temp_h_t spi2_out_temp_h; + lsm6dsv16x_spi2_outx_l_g_ois_t spi2_outx_l_g_ois; + lsm6dsv16x_spi2_outx_h_g_ois_t spi2_outx_h_g_ois; + lsm6dsv16x_spi2_outy_l_g_ois_t spi2_outy_l_g_ois; + lsm6dsv16x_spi2_outy_h_g_ois_t spi2_outy_h_g_ois; + lsm6dsv16x_spi2_outz_l_g_ois_t spi2_outz_l_g_ois; + lsm6dsv16x_spi2_outz_h_g_ois_t spi2_outz_h_g_ois; + lsm6dsv16x_spi2_outx_l_a_ois_t spi2_outx_l_a_ois; + lsm6dsv16x_spi2_outx_h_a_ois_t spi2_outx_h_a_ois; + lsm6dsv16x_spi2_outy_l_a_ois_t spi2_outy_l_a_ois; + lsm6dsv16x_spi2_outy_h_a_ois_t spi2_outy_h_a_ois; + lsm6dsv16x_spi2_outz_l_a_ois_t spi2_outz_l_a_ois; + lsm6dsv16x_spi2_outz_h_a_ois_t spi2_outz_h_a_ois; + lsm6dsv16x_spi2_handshake_ctrl_t spi2_handshake_ctrl; + lsm6dsv16x_spi2_int_ois_t spi2_int_ois; + lsm6dsv16x_spi2_ctrl1_ois_t spi2_ctrl1_ois; + lsm6dsv16x_spi2_ctrl2_ois_t spi2_ctrl2_ois; + lsm6dsv16x_spi2_ctrl3_ois_t spi2_ctrl3_ois; + bitwise_t bitwise; + uint8_t byte; +} prefix_lowspi2_t; + +typedef union { + lsm6dsv16x_page_sel_t page_sel; + lsm6dsv16x_emb_func_en_a_t emb_func_en_a; + lsm6dsv16x_emb_func_en_b_t emb_func_en_b; + lsm6dsv16x_emb_func_exec_status_t emb_func_exec_status; + lsm6dsv16x_page_address_t page_address; + lsm6dsv16x_page_value_t page_value; + lsm6dsv16x_emb_func_int1_t emb_func_int1; + lsm6dsv16x_fsm_int1_t fsm_int1; + lsm6dsv16x_mlc_int1_t mlc_int1; + lsm6dsv16x_emb_func_int2_t emb_func_int2; + lsm6dsv16x_fsm_int2_t fsm_int2; + lsm6dsv16x_mlc_int2_t mlc_int2; + lsm6dsv16x_emb_func_status_t emb_func_status; + lsm6dsv16x_fsm_status_t fsm_status; + lsm6dsv16x_mlc_status_t mlc_status; + lsm6dsv16x_page_rw_t page_rw; + lsm6dsv16x_emb_func_fifo_en_a_t emb_func_fifo_en_a; + lsm6dsv16x_emb_func_fifo_en_b_t emb_func_fifo_en_b; + lsm6dsv16x_fsm_enable_t fsm_enable; + lsm6dsv16x_fsm_long_counter_l_t fsm_long_counter_l; + lsm6dsv16x_fsm_long_counter_h_t fsm_long_counter_h; + lsm6dsv16x_int_ack_mask_t int_ack_mask; + lsm6dsv16x_fsm_outs1_t fsm_outs1; + lsm6dsv16x_fsm_outs2_t fsm_outs2; + lsm6dsv16x_fsm_outs3_t fsm_outs3; + lsm6dsv16x_fsm_outs4_t fsm_outs4; + lsm6dsv16x_fsm_outs5_t fsm_outs5; + lsm6dsv16x_fsm_outs6_t fsm_outs6; + lsm6dsv16x_fsm_outs7_t fsm_outs7; + lsm6dsv16x_fsm_outs8_t fsm_outs8; + lsm6dsv16x_fsm_odr_t fsm_odr; + lsm6dsv16x_mlc_odr_t mlc_odr; + lsm6dsv16x_step_counter_l_t step_counter_l; + lsm6dsv16x_step_counter_h_t step_counter_h; + lsm6dsv16x_emb_func_src_t emb_func_src; + lsm6dsv16x_emb_func_init_a_t emb_func_init_a; + lsm6dsv16x_emb_func_init_b_t emb_func_init_b; + lsm6dsv16x_mlc1_src_t mlc1_src; + lsm6dsv16x_mlc2_src_t mlc2_src; + lsm6dsv16x_mlc3_src_t mlc3_src; + lsm6dsv16x_mlc4_src_t mlc4_src; + bitwise_t bitwise; + uint8_t byte; +} prefix_lowembedded_t; + +typedef union { + lsm6dsv16x_fsm_ext_sensitivity_l_t fsm_ext_sensitivity_l; + lsm6dsv16x_fsm_ext_sensitivity_h_t fsm_ext_sensitivity_h; + lsm6dsv16x_fsm_ext_offx_l_t fsm_ext_offx_l; + lsm6dsv16x_fsm_ext_offx_h_t fsm_ext_offx_h; + lsm6dsv16x_fsm_ext_offy_l_t fsm_ext_offy_l; + lsm6dsv16x_fsm_ext_offy_h_t fsm_ext_offy_h; + lsm6dsv16x_fsm_ext_offz_l_t fsm_ext_offz_l; + lsm6dsv16x_fsm_ext_offz_h_t fsm_ext_offz_h; + lsm6dsv16x_fsm_ext_matrix_xx_l_t fsm_ext_matrix_xx_l; + lsm6dsv16x_fsm_ext_matrix_xx_h_t fsm_ext_matrix_xx_h; + lsm6dsv16x_fsm_ext_matrix_xy_l_t fsm_ext_matrix_xy_l; + lsm6dsv16x_fsm_ext_matrix_xy_h_t fsm_ext_matrix_xy_h; + lsm6dsv16x_fsm_ext_matrix_xz_l_t fsm_ext_matrix_xz_l; + lsm6dsv16x_fsm_ext_matrix_xz_h_t fsm_ext_matrix_xz_h; + lsm6dsv16x_fsm_ext_matrix_yy_l_t fsm_ext_matrix_yy_l; + lsm6dsv16x_fsm_ext_matrix_yy_h_t fsm_ext_matrix_yy_h; + lsm6dsv16x_fsm_ext_matrix_yz_l_t fsm_ext_matrix_yz_l; + lsm6dsv16x_fsm_ext_matrix_yz_h_t fsm_ext_matrix_yz_h; + lsm6dsv16x_fsm_ext_matrix_zz_l_t fsm_ext_matrix_zz_l; + lsm6dsv16x_fsm_ext_matrix_zz_h_t fsm_ext_matrix_zz_h; + lsm6dsv16x_ext_cfg_a_t ext_cfg_a; + lsm6dsv16x_ext_cfg_b_t ext_cfg_b; + bitwise_t bitwise; + uint8_t byte; +} prefix_lowpg0_emb_adv_t; + +typedef union { + lsm6dsv16x_fsm_lc_timeout_l_t fsm_lc_timeout_l; + lsm6dsv16x_fsm_lc_timeout_h_t fsm_lc_timeout_h; + lsm6dsv16x_fsm_programs_t fsm_programs; + lsm6dsv16x_fsm_start_add_l_t fsm_start_add_l; + lsm6dsv16x_fsm_start_add_h_t fsm_start_add_h; + lsm6dsv16x_pedo_cmd_reg_t pedo_cmd_reg; + lsm6dsv16x_pedo_deb_steps_conf_t pedo_deb_steps_conf; + lsm6dsv16x_pedo_sc_deltat_l_t pedo_sc_deltat_l; + lsm6dsv16x_pedo_sc_deltat_h_t pedo_sc_deltat_h; + lsm6dsv16x_mlc_ext_sensitivity_l_t mlc_ext_sensitivity_l; + lsm6dsv16x_mlc_ext_sensitivity_h_t mlc_ext_sensitivity_h; + bitwise_t bitwise; + uint8_t byte; +} prefix_lowpg1_emb_adv_t; + +typedef union { + lsm6dsv16x_sensor_hub_1_t sensor_hub_1; + lsm6dsv16x_sensor_hub_2_t sensor_hub_2; + lsm6dsv16x_sensor_hub_3_t sensor_hub_3; + lsm6dsv16x_sensor_hub_4_t sensor_hub_4; + lsm6dsv16x_sensor_hub_5_t sensor_hub_5; + lsm6dsv16x_sensor_hub_6_t sensor_hub_6; + lsm6dsv16x_sensor_hub_7_t sensor_hub_7; + lsm6dsv16x_sensor_hub_8_t sensor_hub_8; + lsm6dsv16x_sensor_hub_9_t sensor_hub_9; + lsm6dsv16x_sensor_hub_10_t sensor_hub_10; + lsm6dsv16x_sensor_hub_11_t sensor_hub_11; + lsm6dsv16x_sensor_hub_12_t sensor_hub_12; + lsm6dsv16x_sensor_hub_13_t sensor_hub_13; + lsm6dsv16x_sensor_hub_14_t sensor_hub_14; + lsm6dsv16x_sensor_hub_15_t sensor_hub_15; + lsm6dsv16x_sensor_hub_16_t sensor_hub_16; + lsm6dsv16x_sensor_hub_17_t sensor_hub_17; + lsm6dsv16x_sensor_hub_18_t sensor_hub_18; + lsm6dsv16x_master_config_t master_config; + lsm6dsv16x_slv0_add_t slv0_add; + lsm6dsv16x_slv0_subadd_t slv0_subadd; + lsm6dsv16x_slv0_config_t slv0_config; + lsm6dsv16x_slv1_add_t slv1_add; + lsm6dsv16x_slv1_subadd_t slv1_subadd; + lsm6dsv16x_slv1_config_t slv1_config; + lsm6dsv16x_slv2_add_t slv2_add; + lsm6dsv16x_slv2_subadd_t slv2_subadd; + lsm6dsv16x_slv2_config_t slv2_config; + lsm6dsv16x_slv3_add_t slv3_add; + lsm6dsv16x_slv3_subadd_t slv3_subadd; + lsm6dsv16x_slv3_config_t slv3_config; + lsm6dsv16x_datawrite_slv0_t datawrite_slv0; + lsm6dsv16x_status_master_t status_master; + bitwise_t bitwise; + uint8_t byte; +} prefix_lowsensor_hub_t; + +/** + * @} + * + */ + +int32_t lsm6dsv16x_read_reg(lsm6dsv16x_ctx_t *ctx, uint8_t reg, + uint8_t *data, + uint16_t len); +int32_t lsm6dsv16x_write_reg(lsm6dsv16x_ctx_t *ctx, uint8_t reg, + uint8_t *data, + uint16_t len); + +float lsm6dsv16x_from_fs2_to_mg(int16_t lsb); +float lsm6dsv16x_from_fs4_to_mg(int16_t lsb); +float lsm6dsv16x_from_fs8_to_mg(int16_t lsb); +float lsm6dsv16x_from_fs16_to_mg(int16_t lsb); + +float lsm6dsv16x_from_fs125_to_mdps(int16_t lsb); +float lsm6dsv16x_from_fs500_to_mdps(int16_t lsb); +float lsm6dsv16x_from_fs250_to_mdps(int16_t lsb); +float lsm6dsv16x_from_fs1000_to_mdps(int16_t lsb); +float lsm6dsv16x_from_fs2000_to_mdps(int16_t lsb); +float lsm6dsv16x_from_fs4000_to_mdps(int16_t lsb); + +float lsm6dsv16x_from_lsb_to_celsius(int16_t lsb); + +float lsm6dsv16x_from_lsb_to_nsec(uint32_t lsb); + +int32_t lsm6dsv16x_xl_offset_on_out_set(lsm6dsv16x_ctx_t *ctx, uint8_t val); +int32_t lsm6dsv16x_xl_offset_on_out_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val); + +typedef struct { + float z_mg; + float y_mg; + float x_mg; +} lsm6dsv16x_xl_offset_mg_t; +int32_t lsm6dsv16x_xl_offset_mg_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_xl_offset_mg_t val); +int32_t lsm6dsv16x_xl_offset_mg_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_xl_offset_mg_t *val); + +typedef enum { + LSM6DSV16X_READY = 0x0, + LSM6DSV16X_GLOBAL_RST = 0x1, + LSM6DSV16X_RESTORE_CAL_PARAM = 0x2, + LSM6DSV16X_RESTORE_CTRL_REGS = 0x4, +} lsm6dsv16x_reset_t; +int32_t lsm6dsv16x_reset_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_reset_t val); +int32_t lsm6dsv16x_reset_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_reset_t *val); + +typedef enum { + LSM6DSV16X_MAIN_MEM_BANK = 0x0, + LSM6DSV16X_EMBED_FUNC_MEM_BANK = 0x1, + LSM6DSV16X_SENSOR_HUB_MEM_BANK = 0x2, +} lsm6dsv16x_mem_bank_t; +int32_t lsm6dsv16x_mem_bank_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_mem_bank_t val); +int32_t lsm6dsv16x_mem_bank_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_mem_bank_t *val); + +int32_t lsm6dsv16x_device_id_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val); + +typedef enum { + LSM6DSV16X_XL_ODR_OFF = 0x0, + LSM6DSV16X_XL_ODR_AT_1Hz875 = 0x1, + LSM6DSV16X_XL_ODR_AT_7Hz5 = 0x2, + LSM6DSV16X_XL_ODR_AT_15Hz = 0x3, + LSM6DSV16X_XL_ODR_AT_30Hz = 0x4, + LSM6DSV16X_XL_ODR_AT_60Hz = 0x5, + LSM6DSV16X_XL_ODR_AT_120Hz = 0x6, + LSM6DSV16X_XL_ODR_AT_240Hz = 0x7, + LSM6DSV16X_XL_ODR_AT_480Hz = 0x8, + LSM6DSV16X_XL_ODR_AT_960Hz = 0x9, + LSM6DSV16X_XL_ODR_AT_1920Hz = 0xA, + LSM6DSV16X_XL_ODR_AT_3840Hz = 0xB, + LSM6DSV16X_XL_ODR_AT_7680Hz = 0xC, +} lsm6dsv16x_xl_data_rate_t; +int32_t lsm6dsv16x_xl_data_rate_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_xl_data_rate_t val); +int32_t lsm6dsv16x_xl_data_rate_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_xl_data_rate_t *val); + +typedef enum { + LSM6DSV16X_XL_HIGH_PERFORMANCE_MD = 0x0, + LSM6DSV16X_XL_HIGH_ACCURANCY_ODR_MD = 0x1, + LSM6DSV16X_XL_LOW_POWER_2_AVG_MD = 0x4, + LSM6DSV16X_XL_LOW_POWER_4_AVG_MD = 0x5, + LSM6DSV16X_XL_LOW_POWER_8_AVG_MD = 0x6, + LSM6DSV16X_XL_NORMAL_MD = 0x7, +} lsm6dsv16x_xl_mode_t; +int32_t lsm6dsv16x_xl_mode_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_xl_mode_t val); +int32_t lsm6dsv16x_xl_mode_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_xl_mode_t *val); + +typedef enum { + LSM6DSV16X_GY_ODR_OFF = 0x0, + LSM6DSV16X_GY_ODR_AT_7Hz5 = 0x2, + LSM6DSV16X_GY_ODR_AT_15Hz = 0x3, + LSM6DSV16X_GY_ODR_AT_30Hz = 0x4, + LSM6DSV16X_GY_ODR_AT_60Hz = 0x5, + LSM6DSV16X_GY_ODR_AT_120Hz = 0x6, + LSM6DSV16X_GY_ODR_AT_240Hz = 0x7, + LSM6DSV16X_GY_ODR_AT_480Hz = 0x8, + LSM6DSV16X_GY_ODR_AT_960Hz = 0x9, + LSM6DSV16X_GY_ODR_AT_1920Hz = 0xa, + LSM6DSV16X_GY_ODR_AT_3840Hz = 0xb, + LSM6DSV16X_GY_ODR_AT_7680Hz = 0xc, +} lsm6dsv16x_gy_data_rate_t; +int32_t lsm6dsv16x_gy_data_rate_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_gy_data_rate_t val); +int32_t lsm6dsv16x_gy_data_rate_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_gy_data_rate_t *val); + +typedef enum { + LSM6DSV16X_GY_HIGH_PERFORMANCE_MD = 0x0, + LSM6DSV16X_GY_HIGH_ACCURANCY_ODR_MD = 0x1, + LSM6DSV16X_GY_SLEEP_MD = 0x4, + LSM6DSV16X_GY_LOW_POWER_MD = 0x5, +} lsm6dsv16x_gy_mode_t; +int32_t lsm6dsv16x_gy_mode_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_gy_mode_t val); +int32_t lsm6dsv16x_gy_mode_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_gy_mode_t *val); + +int32_t lsm6dsv16x_auto_increment_set(lsm6dsv16x_ctx_t *ctx, uint8_t val); +int32_t lsm6dsv16x_auto_increment_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val); + +int32_t lsm6dsv16x_block_data_update_set(lsm6dsv16x_ctx_t *ctx, uint8_t val); +int32_t lsm6dsv16x_block_data_update_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val); + +typedef enum { + LSM6DSV16X_DRDY_LATCHED = 0x0, + LSM6DSV16X_DRDY_PULSED = 0x1, +} lsm6dsv16x_data_ready_mode_t; +int32_t lsm6dsv16x_data_ready_mode_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_data_ready_mode_t val); +int32_t lsm6dsv16x_data_ready_mode_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_data_ready_mode_t *val); + +typedef enum { + LSM6DSV16X_125dps = 0x0, + LSM6DSV16X_250dps = 0x1, + LSM6DSV16X_500dps = 0x2, + LSM6DSV16X_1000dps = 0x3, + LSM6DSV16X_2000dps = 0x4, + LSM6DSV16X_4000dps = 0x5, +} lsm6dsv16x_gy_full_scale_t; +int32_t lsm6dsv16x_gy_full_scale_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_gy_full_scale_t val); +int32_t lsm6dsv16x_gy_full_scale_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_gy_full_scale_t *val); + +typedef enum { + LSM6DSV16X_2g = 0x0, + LSM6DSV16X_4g = 0x1, + LSM6DSV16X_8g = 0x2, + LSM6DSV16X_16g = 0x3, +} lsm6dsv16x_xl_full_scale_t; +int32_t lsm6dsv16x_xl_full_scale_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_xl_full_scale_t val); +int32_t lsm6dsv16x_xl_full_scale_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_xl_full_scale_t *val); + +int32_t lsm6dsv16x_xl_dual_channel_set(lsm6dsv16x_ctx_t *ctx, uint8_t val); +int32_t lsm6dsv16x_xl_dual_channel_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val); + +typedef enum { + LSM6DSV16X_XL_ST_DISABLE = 0x0, + LSM6DSV16X_XL_ST_POSITIVE = 0x1, + LSM6DSV16X_XL_ST_NEGATIVE = 0x2, +} lsm6dsv16x_xl_self_test_t; +int32_t lsm6dsv16x_xl_self_test_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_xl_self_test_t val); +int32_t lsm6dsv16x_xl_self_test_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_xl_self_test_t *val); + +typedef enum { + LSM6DSV16X_OIS_XL_ST_DISABLE = 0x0, + LSM6DSV16X_OIS_XL_ST_POSITIVE = 0x1, + LSM6DSV16X_OIS_XL_ST_NEGATIVE = 0x2, +} lsm6dsv16x_ois_xl_self_test_t; +int32_t lsm6dsv16x_ois_xl_self_test_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ois_xl_self_test_t val); +int32_t lsm6dsv16x_ois_xl_self_test_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ois_xl_self_test_t *val); + +typedef enum { + LSM6DSV16X_GY_ST_DISABLE = 0x0, + LSM6DSV16X_GY_ST_POSITIVE = 0x1, + LSM6DSV16X_GY_ST_NEGATIVE = 0x2, + +} lsm6dsv16x_gy_self_test_t; +int32_t lsm6dsv16x_gy_self_test_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_gy_self_test_t val); +int32_t lsm6dsv16x_gy_self_test_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_gy_self_test_t *val); + +typedef enum { + LSM6DSV16X_OIS_GY_ST_DISABLE = 0x0, + LSM6DSV16X_OIS_GY_ST_POSITIVE = 0x1, + LSM6DSV16X_OIS_GY_ST_NEGATIVE = 0x2, + LSM6DSV16X_OIS_GY_ST_CLAMP_POS = 0x5, + LSM6DSV16X_OIS_GY_ST_CLAMP_NEG = 0x6, + +} lsm6dsv16x_ois_gy_self_test_t; +int32_t lsm6dsv16x_ois_gy_self_test_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ois_gy_self_test_t val); +int32_t lsm6dsv16x_ois_gy_self_test_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ois_gy_self_test_t *val); + +typedef struct { + uint8_t drdy_xl : 1; + uint8_t drdy_gy : 1; + uint8_t drdy_temp : 1; + uint8_t drdy_ah_qvar : 1; + uint8_t drdy_eis : 1; + uint8_t drdy_ois : 1; + uint8_t gy_settling : 1; + uint8_t timestamp : 1; + uint8_t free_fall : 1; + uint8_t wake_up : 1; + uint8_t wake_up_z : 1; + uint8_t wake_up_y : 1; + uint8_t wake_up_x : 1; + uint8_t single_tap : 1; + uint8_t double_tap : 1; + uint8_t tap_z : 1; + uint8_t tap_y : 1; + uint8_t tap_x : 1; + uint8_t tap_sign : 1; + uint8_t six_d : 1; + uint8_t six_d_xl : 1; + uint8_t six_d_xh : 1; + uint8_t six_d_yl : 1; + uint8_t six_d_yh : 1; + uint8_t six_d_zl : 1; + uint8_t six_d_zh : 1; + uint8_t sleep_change : 1; + uint8_t sleep_state : 1; + uint8_t step_detector : 1; + uint8_t step_count_inc : 1; + uint8_t step_count_overflow : 1; + uint8_t step_on_delta_time : 1; + uint8_t emb_func_stand_by : 1; + uint8_t emb_func_time_exceed : 1; + uint8_t tilt : 1; + uint8_t sig_mot : 1; + uint8_t fsm_lc : 1; + uint8_t fsm1 : 1; + uint8_t fsm2 : 1; + uint8_t fsm3 : 1; + uint8_t fsm4 : 1; + uint8_t fsm5 : 1; + uint8_t fsm6 : 1; + uint8_t fsm7 : 1; + uint8_t fsm8 : 1; + uint8_t mlc1 : 1; + uint8_t mlc2 : 1; + uint8_t mlc3 : 1; + uint8_t mlc4 : 1; + uint8_t sh_endop : 1; + uint8_t sh_slave0_nack : 1; + uint8_t sh_slave1_nack : 1; + uint8_t sh_slave2_nack : 1; + uint8_t sh_slave3_nack : 1; + uint8_t sh_wr_once : 1; + uint8_t fifo_bdr : 1; + uint8_t fifo_full : 1; + uint8_t fifo_ovr : 1; + uint8_t fifo_th : 1; +} lsm6dsv16x_all_sources_t; +int32_t lsm6dsv16x_all_sources_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_all_sources_t *val); + +int32_t lsm6dsv16x_int_ack_mask_set(lsm6dsv16x_ctx_t *ctx, uint8_t val); +int32_t lsm6dsv16x_int_ack_mask_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val); + +int32_t lsm6dsv16x_temperature_raw_get(lsm6dsv16x_ctx_t *ctx, int16_t *val); + +int32_t lsm6dsv16x_angular_rate_raw_get(lsm6dsv16x_ctx_t *ctx, int16_t *val); + +int32_t lsm6dsv16x_ois_angular_rate_raw_get(lsm6dsv16x_ctx_t *ctx, int16_t *val); + +int32_t lsm6dsv16x_ois_eis_angular_rate_raw_get(lsm6dsv16x_ctx_t *ctx, int16_t *val); + +int32_t lsm6dsv16x_acceleration_raw_get(lsm6dsv16x_ctx_t *ctx, int16_t *val); + +int32_t lsm6dsv16x_dual_acceleration_raw_get(lsm6dsv16x_ctx_t *ctx, int16_t *val); + +int32_t lsm6dsv16x_ois_dual_acceleration_raw_get(lsm6dsv16x_ctx_t *ctx, int16_t *val); + +int32_t lsm6dsv16x_ah_qvar_raw_get(lsm6dsv16x_ctx_t *ctx, int16_t *val); + +int32_t lsm6dsv16x_odr_cal_reg_get(lsm6dsv16x_ctx_t *ctx, int8_t *val); + +int32_t lsm6dsv16x_ln_pg_write(lsm6dsv16x_ctx_t *ctx, uint16_t address, uint8_t *buf, uint8_t len); +int32_t lsm6dsv16x_ln_pg_read(lsm6dsv16x_ctx_t *ctx, uint16_t address, uint8_t *buf, uint8_t len); + +typedef enum { + LSM6DSV16X_DEN_ACT_LOW = 0x0, + LSM6DSV16X_DEN_ACT_HIGH = 0x1, +} lsm6dsv16x_den_polarity_t; +int32_t lsm6dsv16x_den_polarity_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_den_polarity_t val); +int32_t lsm6dsv16x_den_polarity_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_den_polarity_t *val); + +typedef struct { + uint8_t stamp_in_gy_data : 1; + uint8_t stamp_in_xl_data : 1; + uint8_t den_x : 1; + uint8_t den_y : 1; + uint8_t den_z : 1; + enum { + DEN_NOT_DEFINED = 0x00, + LEVEL_TRIGGER = 0x02, + LEVEL_LETCHED = 0x03, + } mode; +} lsm6dsv16x_den_conf_t; +int32_t lsm6dsv16x_den_conf_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_den_conf_t val); +int32_t lsm6dsv16x_den_conf_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_den_conf_t *val); + +typedef enum { + LSM6DSV16X_EIS_125dps = 0x0, + LSM6DSV16X_EIS_250dps = 0x1, + LSM6DSV16X_EIS_500dps = 0x2, + LSM6DSV16X_EIS_1000dps = 0x3, + LSM6DSV16X_EIS_2000dps = 0x4, +} lsm6dsv16x_eis_gy_full_scale_t; +int32_t lsm6dsv16x_eis_gy_full_scale_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_eis_gy_full_scale_t val); +int32_t lsm6dsv16x_eis_gy_full_scale_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_eis_gy_full_scale_t *val); + +int32_t lsm6dsv16x_eis_gy_on_spi2_set(lsm6dsv16x_ctx_t *ctx, uint8_t val); +int32_t lsm6dsv16x_eis_gy_on_spi2_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val); + +typedef enum { + LSM6DSV16X_EIS_ODR_OFF = 0x0, + LSM6DSV16X_EIS_1920Hz = 0x1, + LSM6DSV16X_EIS_960Hz = 0x2, +} lsm6dsv16x_gy_eis_data_rate_t; +int32_t lsm6dsv16x_gy_eis_data_rate_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_gy_eis_data_rate_t val); +int32_t lsm6dsv16x_gy_eis_data_rate_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_gy_eis_data_rate_t *val); + +int32_t lsm6dsv16x_fifo_watermark_set(lsm6dsv16x_ctx_t *ctx, uint8_t val); +int32_t lsm6dsv16x_fifo_watermark_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val); + +int32_t lsm6dsv16x_fifo_xl_dual_fsm_batch_set(lsm6dsv16x_ctx_t *ctx, uint8_t val); +int32_t lsm6dsv16x_fifo_xl_dual_fsm_batch_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val); + +typedef enum { + LSM6DSV16X_CMP_DISABLE = 0x0, + LSM6DSV16X_CMP_8_TO_1 = 0x1, + LSM6DSV16X_CMP_16_TO_1 = 0x2, + LSM6DSV16X_CMP_32_TO_1 = 0x3, +} lsm6dsv16x_fifo_compress_algo_t; +int32_t lsm6dsv16x_fifo_compress_algo_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fifo_compress_algo_t val); +int32_t lsm6dsv16x_fifo_compress_algo_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fifo_compress_algo_t *val); + +int32_t lsm6dsv16x_fifo_virtual_sens_odr_chg_set(lsm6dsv16x_ctx_t *ctx, uint8_t val); +int32_t lsm6dsv16x_fifo_virtual_sens_odr_chg_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val); + +int32_t lsm6dsv16x_fifo_compress_algo_real_time_set(lsm6dsv16x_ctx_t *ctx, uint8_t val); +int32_t lsm6dsv16x_fifo_compress_algo_real_time_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val); + +int32_t lsm6dsv16x_fifo_stop_on_wtm_set(lsm6dsv16x_ctx_t *ctx, uint8_t val); +int32_t lsm6dsv16x_fifo_stop_on_wtm_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val); + +typedef enum { + LSM6DSV16X_XL_NOT_BATCHED = 0x0, + LSM6DSV16X_XL_BATCHED_AT_1Hz875 = 0x1, + LSM6DSV16X_XL_BATCHED_AT_7Hz5 = 0x2, + LSM6DSV16X_XL_BATCHED_AT_15Hz = 0x3, + LSM6DSV16X_XL_BATCHED_AT_30Hz = 0x4, + LSM6DSV16X_XL_BATCHED_AT_60Hz = 0x5, + LSM6DSV16X_XL_BATCHED_AT_120Hz = 0x6, + LSM6DSV16X_XL_BATCHED_AT_240Hz = 0x7, + LSM6DSV16X_XL_BATCHED_AT_480Hz = 0x8, + LSM6DSV16X_XL_BATCHED_AT_960Hz = 0x9, + LSM6DSV16X_XL_BATCHED_AT_1920Hz = 0xa, + LSM6DSV16X_XL_BATCHED_AT_3840Hz = 0xb, + LSM6DSV16X_XL_BATCHED_AT_7680Hz = 0xc, +} lsm6dsv16x_fifo_xl_batch_t; +int32_t lsm6dsv16x_fifo_xl_batch_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fifo_xl_batch_t val); +int32_t lsm6dsv16x_fifo_xl_batch_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fifo_xl_batch_t *val); + +typedef enum { + LSM6DSV16X_GY_NOT_BATCHED = 0x0, + LSM6DSV16X_GY_BATCHED_AT_1Hz875 = 0x1, + LSM6DSV16X_GY_BATCHED_AT_7Hz5 = 0x2, + LSM6DSV16X_GY_BATCHED_AT_15Hz = 0x3, + LSM6DSV16X_GY_BATCHED_AT_30Hz = 0x4, + LSM6DSV16X_GY_BATCHED_AT_60Hz = 0x5, + LSM6DSV16X_GY_BATCHED_AT_120Hz = 0x6, + LSM6DSV16X_GY_BATCHED_AT_240Hz = 0x7, + LSM6DSV16X_GY_BATCHED_AT_480Hz = 0x8, + LSM6DSV16X_GY_BATCHED_AT_960Hz = 0x9, + LSM6DSV16X_GY_BATCHED_AT_1920Hz = 0xa, + LSM6DSV16X_GY_BATCHED_AT_3840Hz = 0xb, + LSM6DSV16X_GY_BATCHED_AT_7680Hz = 0xc, +} lsm6dsv16x_fifo_gy_batch_t; +int32_t lsm6dsv16x_fifo_gy_batch_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fifo_gy_batch_t val); +int32_t lsm6dsv16x_fifo_gy_batch_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fifo_gy_batch_t *val); + +typedef enum { + LSM6DSV16X_BYPASS_MODE = 0x0, + LSM6DSV16X_FIFO_MODE = 0x1, + LSM6DSV16X_STREAM_WTM_TO_FULL_MODE = 0x2, + LSM6DSV16X_STREAM_TO_FIFO_MODE = 0x3, + LSM6DSV16X_BYPASS_TO_STREAM_MODE = 0x4, + LSM6DSV16X_STREAM_MODE = 0x6, + LSM6DSV16X_BYPASS_TO_FIFO_MODE = 0x7, +} lsm6dsv16x_fifo_mode_t; +int32_t lsm6dsv16x_fifo_mode_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fifo_mode_t val); +int32_t lsm6dsv16x_fifo_mode_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fifo_mode_t *val); + +int32_t lsm6dsv16x_fifo_gy_eis_batch_set(lsm6dsv16x_ctx_t *ctx, uint8_t val); +int32_t lsm6dsv16x_fifo_gy_eis_batch_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val); + +typedef enum { + LSM6DSV16X_TEMP_NOT_BATCHED = 0x0, + LSM6DSV16X_TEMP_BATCHED_AT_1Hz875 = 0x1, + LSM6DSV16X_TEMP_BATCHED_AT_15Hz = 0x2, + LSM6DSV16X_TEMP_BATCHED_AT_60Hz = 0x3, +} lsm6dsv16x_fifo_temp_batch_t; +int32_t lsm6dsv16x_fifo_temp_batch_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fifo_temp_batch_t val); +int32_t lsm6dsv16x_fifo_temp_batch_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fifo_temp_batch_t *val); + +typedef enum { + LSM6DSV16X_TMSTMP_NOT_BATCHED = 0x0, + LSM6DSV16X_TMSTMP_DEC_1 = 0x1, + LSM6DSV16X_TMSTMP_DEC_8 = 0x2, + LSM6DSV16X_TMSTMP_DEC_32 = 0x3, +} lsm6dsv16x_fifo_timestamp_batch_t; +int32_t lsm6dsv16x_fifo_timestamp_batch_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fifo_timestamp_batch_t val); +int32_t lsm6dsv16x_fifo_timestamp_batch_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fifo_timestamp_batch_t *val); + +int32_t lsm6dsv16x_fifo_batch_counter_threshold_set(lsm6dsv16x_ctx_t *ctx, uint16_t val); +int32_t lsm6dsv16x_fifo_batch_counter_threshold_get(lsm6dsv16x_ctx_t *ctx, uint16_t *val); + +typedef enum { + LSM6DSV16X_XL_BATCH_EVENT = 0x0, + LSM6DSV16X_GY_BATCH_EVENT = 0x1, + LSM6DSV16X_GY_EIS_BATCH_EVENT = 0x2, +} lsm6dsv16x_fifo_batch_cnt_event_t; +int32_t lsm6dsv16x_fifo_batch_cnt_event_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fifo_batch_cnt_event_t val); +int32_t lsm6dsv16x_fifo_batch_cnt_event_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fifo_batch_cnt_event_t *val); + +int32_t lsm6dsv16x_fifo_data_level_get(lsm6dsv16x_ctx_t *ctx, uint16_t *val); + +typedef struct { + enum { + LSM6DSV16X_FIFO_EMPTY = 0x0, + LSM6DSV16X_GY_NC_TAG = 0x1, + LSM6DSV16X_XL_NC_TAG = 0x2, + LSM6DSV16X_TEMPERATURE_TAG = 0x3, + LSM6DSV16X_TIMESTAMP_TAG = 0x4, + LSM6DSV16X_CFG_CHANGE_TAG = 0x5, + LSM6DSV16X_XL_NC_T_2_TAG = 0x6, + LSM6DSV16X_XL_NC_T_1_TAG = 0x7, + LSM6DSV16X_XL_2XC_TAG = 0x8, + LSM6DSV16X_XL_3XC_TAG = 0x9, + LSM6DSV16X_GY_NC_T_2_TAG = 0xA, + LSM6DSV16X_GY_NC_T_1_TAG = 0xB, + LSM6DSV16X_GY_2XC_TAG = 0xC, + LSM6DSV16X_GY_3XC_TAG = 0xD, + LSM6DSV16X_SENSORHUB_SLAVE0_TAG = 0xE, + LSM6DSV16X_SENSORHUB_SLAVE1_TAG = 0xF, + LSM6DSV16X_SENSORHUB_SLAVE2_TAG = 0x10, + LSM6DSV16X_SENSORHUB_SLAVE3_TAG = 0x11, + LSM6DSV16X_STEP_CPUNTER_TAG = 0x12, + LSM6DSV16X_SENSORHUB_NACK_TAG = 0x19, + LSM6DSV16X_MLC_RESULT_TAG = 0x1A, + LSM6DSV16X_MLC_FILTER = 0x1B, + LSM6DSV16X_MLC_FEATURE = 0x1C, + LSM6DSV16X_XL_DUAL_CORE = 0x1D, + LSM6DSV16X_AH_QVAR = 0x1F, + } tag; + uint8_t cnt; + uint8_t data[6]; +} lsm6dsv16x_fifo_out_raw_t; +int32_t lsm6dsv16x_fifo_out_raw_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fifo_out_raw_t *val); + +int32_t lsm6dsv16x_fifo_stpcnt_batch_set(lsm6dsv16x_ctx_t *ctx, uint8_t val); +int32_t lsm6dsv16x_fifo_stpcnt_batch_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val); + +int32_t lsm6dsv16x_fifo_mlc_batch_set(lsm6dsv16x_ctx_t *ctx, uint8_t val); +int32_t lsm6dsv16x_fifo_mlc_batch_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val); + +int32_t lsm6dsv16x_fifo_mlc_filt_batch_set(lsm6dsv16x_ctx_t *ctx, uint8_t val); +int32_t lsm6dsv16x_fifo_mlc_filt_batch_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val); + +int32_t lsm6dsv16x_fifo_batch_sh_slave_0_set(lsm6dsv16x_ctx_t *ctx, uint8_t val); +int32_t lsm6dsv16x_fifo_batch_sh_slave_0_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val); + +int32_t lsm6dsv16x_fifo_batch_sh_slave_1_set(lsm6dsv16x_ctx_t *ctx, uint8_t val); +int32_t lsm6dsv16x_fifo_batch_sh_slave_1_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val); + +int32_t lsm6dsv16x_fifo_batch_sh_slave_2_set(lsm6dsv16x_ctx_t *ctx, uint8_t val); +int32_t lsm6dsv16x_fifo_batch_sh_slave_2_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val); + +int32_t lsm6dsv16x_fifo_batch_sh_slave_3_set(lsm6dsv16x_ctx_t *ctx, uint8_t val); +int32_t lsm6dsv16x_fifo_batch_sh_slave_3_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val); + +typedef enum { + LSM6DSV16X_AUTO = 0x0, + LSM6DSV16X_ALWAYS_ACTIVE = 0x1, +} lsm6dsv16x_filt_anti_spike_t; +int32_t lsm6dsv16x_filt_anti_spike_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_filt_anti_spike_t val); +int32_t lsm6dsv16x_filt_anti_spike_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_filt_anti_spike_t *val); + +typedef struct { + uint8_t drdy : 1; + uint8_t ois_drdy : 1; + uint8_t irq_xl : 1; + uint8_t irq_g : 1; +} lsm6dsv16x_filt_settling_mask_t; +int32_t lsm6dsv16x_filt_settling_mask_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_filt_settling_mask_t val); +int32_t lsm6dsv16x_filt_settling_mask_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_filt_settling_mask_t *val); + +typedef struct { + uint8_t ois_drdy : 1; +} lsm6dsv16x_filt_ois_settling_mask_t; +int32_t lsm6dsv16x_filt_ois_settling_mask_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_filt_ois_settling_mask_t val); +int32_t lsm6dsv16x_filt_ois_settling_mask_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_filt_ois_settling_mask_t *val); + +typedef enum { + LSM6DSV16X_GY_ULTRA_LIGHT = 0x0, + LSM6DSV16X_GY_VERY_LIGHT = 0x1, + LSM6DSV16X_GY_LIGHT = 0x2, + LSM6DSV16X_GY_MEDIUM = 0x3, + LSM6DSV16X_GY_STRONG = 0x4, + LSM6DSV16X_GY_VERY_STRONG = 0x5, + LSM6DSV16X_GY_AGGRESSIVE = 0x6, + LSM6DSV16X_GY_XTREME = 0x7, +} lsm6dsv16x_filt_gy_lp1_bandwidth_t; +int32_t lsm6dsv16x_filt_gy_lp1_bandwidth_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_filt_gy_lp1_bandwidth_t val); +int32_t lsm6dsv16x_filt_gy_lp1_bandwidth_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_filt_gy_lp1_bandwidth_t *val); + +int32_t lsm6dsv16x_filt_gy_lp1_set(lsm6dsv16x_ctx_t *ctx, uint8_t val); +int32_t lsm6dsv16x_filt_gy_lp1_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val); + +typedef enum { + LSM6DSV16X_XL_ULTRA_LIGHT = 0x0, + LSM6DSV16X_XL_VERY_LIGHT = 0x1, + LSM6DSV16X_XL_LIGHT = 0x2, + LSM6DSV16X_XL_MEDIUM = 0x3, + LSM6DSV16X_XL_STRONG = 0x4, + LSM6DSV16X_XL_VERY_STRONG = 0x5, + LSM6DSV16X_XL_AGGRESSIVE = 0x6, + LSM6DSV16X_XL_XTREME = 0x7, +} lsm6dsv16x_filt_xl_lp2_bandwidth_t; +int32_t lsm6dsv16x_filt_xl_lp2_bandwidth_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_filt_xl_lp2_bandwidth_t val); +int32_t lsm6dsv16x_filt_xl_lp2_bandwidth_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_filt_xl_lp2_bandwidth_t *val); + +int32_t lsm6dsv16x_filt_xl_lp2_set(lsm6dsv16x_ctx_t *ctx, uint8_t val); +int32_t lsm6dsv16x_filt_xl_lp2_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val); + +int32_t lsm6dsv16x_filt_xl_hp_set(lsm6dsv16x_ctx_t *ctx, uint8_t val); +int32_t lsm6dsv16x_filt_xl_hp_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val); + +int32_t lsm6dsv16x_filt_xl_fast_settling_set(lsm6dsv16x_ctx_t *ctx, uint8_t val); +int32_t lsm6dsv16x_filt_xl_fast_settling_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val); + +typedef enum { + LSM6DSV16X_HP_MD_NORMAL = 0x0, + LSM6DSV16X_HP_MD_REFERENCE = 0x1, +} lsm6dsv16x_filt_xl_hp_mode_t; +int32_t lsm6dsv16x_filt_xl_hp_mode_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_filt_xl_hp_mode_t val); +int32_t lsm6dsv16x_filt_xl_hp_mode_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_filt_xl_hp_mode_t *val); + +typedef enum { + LSM6DSV16X_WK_FEED_SLOPE = 0x0, + LSM6DSV16X_WK_FEED_HIGH_PASS = 0x1, + LSM6DSV16X_WK_FEED_LP_WITH_OFFSET = 0x2, +} lsm6dsv16x_filt_wkup_act_feed_t; +int32_t lsm6dsv16x_filt_wkup_act_feed_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_filt_wkup_act_feed_t val); +int32_t lsm6dsv16x_filt_wkup_act_feed_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_filt_wkup_act_feed_t *val); + +typedef enum { + LSM6DSV16X_SIXD_FEED_ODR_DIV_2 = 0x0, + LSM6DSV16X_SIXD_FEED_LOW_PASS = 0x1, +} lsm6dsv16x_filt_sixd_feed_t; +int32_t lsm6dsv16x_filt_sixd_feed_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_filt_sixd_feed_t val); +int32_t lsm6dsv16x_filt_sixd_feed_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_filt_sixd_feed_t *val); + +typedef enum { + LSM6DSV16X_EIS_LP_NORMAL = 0x0, + LSM6DSV16X_EIS_LP_LIGHT = 0x1, +} lsm6dsv16x_filt_gy_eis_lp_bandwidth_t; +int32_t lsm6dsv16x_filt_gy_eis_lp_bandwidth_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_filt_gy_eis_lp_bandwidth_t val); +int32_t lsm6dsv16x_filt_gy_eis_lp_bandwidth_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_filt_gy_eis_lp_bandwidth_t *val); + +typedef enum { + LSM6DSV16X_OIS_GY_LP_NORMAL = 0x0, + LSM6DSV16X_OIS_GY_LP_STRONG = 0x1, + LSM6DSV16X_OIS_GY_LP_AGGRESSIVE = 0x2, + LSM6DSV16X_OIS_GY_LP_LIGHT = 0x3, +} lsm6dsv16x_filt_gy_ois_lp_bandwidth_t; +int32_t lsm6dsv16x_filt_gy_ois_lp_bandwidth_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_filt_gy_ois_lp_bandwidth_t val); +int32_t lsm6dsv16x_filt_gy_ois_lp_bandwidth_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_filt_gy_ois_lp_bandwidth_t *val); + +typedef enum { + LSM6DSV16X_OIS_XL_LP_ULTRA_LIGHT = 0x0, + LSM6DSV16X_OIS_XL_LP_VERY_LIGHT = 0x1, + LSM6DSV16X_OIS_XL_LP_LIGHT = 0x2, + LSM6DSV16X_OIS_XL_LP_NORMAL = 0x3, + LSM6DSV16X_OIS_XL_LP_STRONG = 0x4, + LSM6DSV16X_OIS_XL_LP_VERY_STRONG = 0x5, + LSM6DSV16X_OIS_XL_LP_AGGRESSIVE = 0x6, + LSM6DSV16X_OIS_XL_LP_XTREME = 0x7, +} lsm6dsv16x_filt_xl_ois_lp_bandwidth_t; +int32_t lsm6dsv16x_filt_xl_ois_lp_bandwidth_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_filt_xl_ois_lp_bandwidth_t val); +int32_t lsm6dsv16x_filt_xl_ois_lp_bandwidth_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_filt_xl_ois_lp_bandwidth_t *val); + +typedef enum { + LSM6DSV16X_PROTECT_CTRL_REGS = 0x0, + LSM6DSV16X_WRITE_CTRL_REG = 0x1, +} lsm6dsv16x_fsm_permission_t; +int32_t lsm6dsv16x_fsm_permission_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fsm_permission_t val); +int32_t lsm6dsv16x_fsm_permission_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fsm_permission_t *val); +int32_t lsm6dsv16x_fsm_permission_status(lsm6dsv16x_ctx_t *ctx, uint8_t *val); + +typedef struct { + uint8_t fsm1_en : 1; + uint8_t fsm2_en : 1; + uint8_t fsm3_en : 1; + uint8_t fsm4_en : 1; + uint8_t fsm5_en : 1; + uint8_t fsm6_en : 1; + uint8_t fsm7_en : 1; + uint8_t fsm8_en : 1; +} lsm6dsv16x_fsm_mode_t; +int32_t lsm6dsv16x_fsm_mode_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fsm_mode_t val); +int32_t lsm6dsv16x_fsm_mode_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fsm_mode_t *val); + +int32_t lsm6dsv16x_fsm_long_cnt_set(lsm6dsv16x_ctx_t *ctx, uint16_t val); +int32_t lsm6dsv16x_fsm_long_cnt_get(lsm6dsv16x_ctx_t *ctx, uint16_t *val); + + +typedef struct { + lsm6dsv16x_fsm_outs1_t fsm_outs1; + lsm6dsv16x_fsm_outs2_t fsm_outs2; + lsm6dsv16x_fsm_outs3_t fsm_outs3; + lsm6dsv16x_fsm_outs4_t fsm_outs4; + lsm6dsv16x_fsm_outs5_t fsm_outs5; + lsm6dsv16x_fsm_outs6_t fsm_outs6; + lsm6dsv16x_fsm_outs7_t fsm_outs7; + lsm6dsv16x_fsm_outs8_t fsm_outs8; +} lsm6dsv16x_fsm_out_t; +int32_t lsm6dsv16x_fsm_out_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fsm_out_t *val); + +typedef enum { + LSM6DSV16X_FSM_15Hz = 0x0, + LSM6DSV16X_FSM_30Hz = 0x1, + LSM6DSV16X_FSM_60Hz = 0x2, + LSM6DSV16X_FSM_120Hz = 0x3, + LSM6DSV16X_FSM_240Hz = 0x4, + LSM6DSV16X_FSM_480Hz = 0x5, + LSM6DSV16X_FSM_960Hz = 0x6, +} lsm6dsv16x_fsm_data_rate_t; +int32_t lsm6dsv16x_fsm_data_rate_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fsm_data_rate_t val); +int32_t lsm6dsv16x_fsm_data_rate_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fsm_data_rate_t *val); + +int32_t lsm6dsv16x_fsm_ext_sens_sensitivity_set(lsm6dsv16x_ctx_t *ctx, uint16_t val); +int32_t lsm6dsv16x_fsm_ext_sens_sensitivity_get(lsm6dsv16x_ctx_t *ctx, uint16_t *val); + +typedef struct { + uint16_t z; + uint16_t y; + uint16_t x; +} lsm6dsv16x_xl_fsm_ext_sens_offset_t; +int32_t lsm6dsv16x_fsm_ext_sens_offset_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_xl_fsm_ext_sens_offset_t val); +int32_t lsm6dsv16x_fsm_ext_sens_offset_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_xl_fsm_ext_sens_offset_t *val); + +typedef struct { + uint16_t xx; + uint16_t xy; + uint16_t xz; + uint16_t yy; + uint16_t yz; + uint16_t zz; +} lsm6dsv16x_xl_fsm_ext_sens_matrix_t; +int32_t lsm6dsv16x_fsm_ext_sens_matrix_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_xl_fsm_ext_sens_matrix_t val); +int32_t lsm6dsv16x_fsm_ext_sens_matrix_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_xl_fsm_ext_sens_matrix_t *val); + +typedef enum { + LSM6DSV16X_Z_EQ_Y = 0x0, + LSM6DSV16X_Z_EQ_MIN_Y = 0x1, + LSM6DSV16X_Z_EQ_X = 0x2, + LSM6DSV16X_Z_EQ_MIN_X = 0x3, + LSM6DSV16X_Z_EQ_MIN_Z = 0x4, + LSM6DSV16X_Z_EQ_Z = 0x5, +} lsm6dsv16x_fsm_ext_sens_z_orient_t; +int32_t lsm6dsv16x_fsm_ext_sens_z_orient_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fsm_ext_sens_z_orient_t val); +int32_t lsm6dsv16x_fsm_ext_sens_z_orient_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fsm_ext_sens_z_orient_t *val); + +typedef enum { + LSM6DSV16X_Y_EQ_Y = 0x0, + LSM6DSV16X_Y_EQ_MIN_Y = 0x1, + LSM6DSV16X_Y_EQ_X = 0x2, + LSM6DSV16X_Y_EQ_MIN_X = 0x3, + LSM6DSV16X_Y_EQ_MIN_Z = 0x4, + LSM6DSV16X_Y_EQ_Z = 0x5, +} lsm6dsv16x_fsm_ext_sens_y_orient_t; +int32_t lsm6dsv16x_fsm_ext_sens_y_orient_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fsm_ext_sens_y_orient_t val); +int32_t lsm6dsv16x_fsm_ext_sens_y_orient_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fsm_ext_sens_y_orient_t *val); + +typedef enum { + LSM6DSV16X_X_EQ_Y = 0x0, + LSM6DSV16X_X_EQ_MIN_Y = 0x1, + LSM6DSV16X_X_EQ_X = 0x2, + LSM6DSV16X_X_EQ_MIN_X = 0x3, + LSM6DSV16X_X_EQ_MIN_Z = 0x4, + LSM6DSV16X_X_EQ_Z = 0x5, +} lsm6dsv16x_fsm_ext_sens_x_orient_t; +int32_t lsm6dsv16x_fsm_ext_sens_x_orient_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fsm_ext_sens_x_orient_t val); +int32_t lsm6dsv16x_fsm_ext_sens_x_orient_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fsm_ext_sens_x_orient_t *val); + +int32_t lsm6dsv16x_fsm_long_cnt_timeout_set(lsm6dsv16x_ctx_t *ctx, uint16_t val); +int32_t lsm6dsv16x_fsm_long_cnt_timeout_get(lsm6dsv16x_ctx_t *ctx, uint16_t *val); + +int32_t lsm6dsv16x_fsm_number_of_programs_set(lsm6dsv16x_ctx_t *ctx, uint8_t val); +int32_t lsm6dsv16x_fsm_number_of_programs_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val); + +int32_t lsm6dsv16x_fsm_start_address_set(lsm6dsv16x_ctx_t *ctx, uint16_t val); +int32_t lsm6dsv16x_fsm_start_address_get(lsm6dsv16x_ctx_t *ctx, uint16_t *val); + +int32_t lsm6dsv16x_ff_time_windows_set(lsm6dsv16x_ctx_t *ctx, uint8_t val); +int32_t lsm6dsv16x_ff_time_windows_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val); + +typedef enum { + LSM6DSV16X_156_mg = 0x0, + LSM6DSV16X_219_mg = 0x1, + LSM6DSV16X_250_mg = 0x2, + LSM6DSV16X_312_mg = 0x3, + LSM6DSV16X_344_mg = 0x4, + LSM6DSV16X_406_mg = 0x5, + LSM6DSV16X_469_mg = 0x6, + LSM6DSV16X_500_mg = 0x7, +} lsm6dsv16x_ff_thresholds_t; +int32_t lsm6dsv16x_ff_thresholds_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ff_thresholds_t val); +int32_t lsm6dsv16x_ff_thresholds_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ff_thresholds_t *val); + +typedef enum { + LSM6DSV16X_DISABLE = 0x0, + LSM6DSV16X_MLC_BEFORE_FSM = 0x1, + LSM6DSV16X_MLC_AFTER_FSM = 0x2, +} lsm6dsv16x_mlc_mode_t; +int32_t lsm6dsv16x_mlc_mode_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_mlc_mode_t val); +int32_t lsm6dsv16x_mlc_mode_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_mlc_mode_t *val); + +typedef enum { + LSM6DSV16X_MLC_15Hz = 0x0, + LSM6DSV16X_MLC_30Hz = 0x1, + LSM6DSV16X_MLC_60Hz = 0x2, + LSM6DSV16X_MLC_120Hz = 0x3, + LSM6DSV16X_MLC_240Hz = 0x4, + LSM6DSV16X_MLC_480Hz = 0x5, + LSM6DSV16X_MLC_960Hz = 0x6, +} lsm6dsv16x_mlc_data_rate_t; +int32_t lsm6dsv16x_mlc_data_rate_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_mlc_data_rate_t val); +int32_t lsm6dsv16x_mlc_data_rate_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_mlc_data_rate_t *val); + +typedef struct { + uint8_t mlc1_src; + uint8_t mlc2_src; + uint8_t mlc3_src; + uint8_t mlc4_src; +} lsm6dsv16x_mlc_out_t; +int32_t lsm6dsv16x_mlc_out_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_mlc_out_t *val); + +int32_t lsm6dsv16x_mlc_ext_sens_sensitivity_set(lsm6dsv16x_ctx_t *ctx, uint16_t val); +int32_t lsm6dsv16x_mlc_ext_sens_sensitivity_get(lsm6dsv16x_ctx_t *ctx, uint16_t *val); + +typedef enum { + LSM6DSV16X_OIS_CTRL_FROM_OIS = 0x0, + LSM6DSV16X_OIS_CTRL_FROM_UI = 0x1, +} lsm6dsv16x_ois_ctrl_mode_t; +int32_t lsm6dsv16x_ois_ctrl_mode_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ois_ctrl_mode_t val); +int32_t lsm6dsv16x_ois_ctrl_mode_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ois_ctrl_mode_t *val); + +int32_t lsm6dsv16x_ois_reset_set(lsm6dsv16x_ctx_t *ctx, int8_t val); +int32_t lsm6dsv16x_ois_reset_get(lsm6dsv16x_ctx_t *ctx, int8_t *val); + +int32_t lsm6dsv16x_ois_interface_pull_up_set(lsm6dsv16x_ctx_t *ctx, uint8_t val); +int32_t lsm6dsv16x_ois_interface_pull_up_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val); + +typedef struct { + uint8_t ack : 1; + uint8_t req : 1; +} lsm6dsv16x_ois_handshake_t; +int32_t lsm6dsv16x_ois_handshake_from_ui_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ois_handshake_t val); +int32_t lsm6dsv16x_ois_handshake_from_ui_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ois_handshake_t *val); +int32_t lsm6dsv16x_ois_handshake_from_ois_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ois_handshake_t val); +int32_t lsm6dsv16x_ois_handshake_from_ois_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ois_handshake_t *val); + +int32_t lsm6dsv16x_ois_shared_set(lsm6dsv16x_ctx_t *ctx, uint8_t val[6]); +int32_t lsm6dsv16x_ois_shared_get(lsm6dsv16x_ctx_t *ctx, uint8_t val[6]); + +int32_t lsm6dsv16x_ois_on_spi2_set(lsm6dsv16x_ctx_t *ctx, uint8_t val); +int32_t lsm6dsv16x_ois_on_spi2_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val); + +typedef struct { + uint8_t gy : 1; + uint8_t xl : 1; +} lsm6dsv16x_ois_chain_t; +int32_t lsm6dsv16x_ois_chain_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ois_chain_t val); +int32_t lsm6dsv16x_ois_chain_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ois_chain_t *val); + +typedef enum { + LSM6DSV16X_OIS_125dps = 0x0, + LSM6DSV16X_OIS_250dps = 0x1, + LSM6DSV16X_OIS_500dps = 0x2, + LSM6DSV16X_OIS_1000dps = 0x3, + LSM6DSV16X_OIS_2000dps = 0x4, +} lsm6dsv16x_ois_gy_full_scale_t; +int32_t lsm6dsv16x_ois_gy_full_scale_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ois_gy_full_scale_t val); +int32_t lsm6dsv16x_ois_gy_full_scale_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ois_gy_full_scale_t *val); + +typedef enum { + LSM6DSV16X_OIS_2g = 0x0, + LSM6DSV16X_OIS_4g = 0x1, + LSM6DSV16X_OIS_8g = 0x2, + LSM6DSV16X_OIS_16g = 0x3, +} lsm6dsv16x_ois_xl_full_scale_t; +int32_t lsm6dsv16x_ois_xl_full_scale_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ois_xl_full_scale_t val); +int32_t lsm6dsv16x_ois_xl_full_scale_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ois_xl_full_scale_t *val); + +typedef enum { + LSM6DSV16X_DEG_80 = 0x0, + LSM6DSV16X_DEG_70 = 0x1, + LSM6DSV16X_DEG_60 = 0x2, + LSM6DSV16X_DEG_50 = 0x3, +} lsm6dsv16x_6d_threshold_t; +int32_t lsm6dsv16x_6d_threshold_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_6d_threshold_t val); +int32_t lsm6dsv16x_6d_threshold_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_6d_threshold_t *val); + +int32_t lsm6dsv16x_4d_mode_set(lsm6dsv16x_ctx_t *ctx, uint8_t val); +int32_t lsm6dsv16x_4d_mode_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val); + +typedef enum { + LSM6DSV16X_2400MOhm = 0x0, + LSM6DSV16X_730MOhm = 0x1, + LSM6DSV16X_300MOhm = 0x2, + LSM6DSV16X_255MOhm = 0x3, +} lsm6dsv16x_ah_qvar_zin_t; +int32_t lsm6dsv16x_ah_qvar_zin_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ah_qvar_zin_t val); +int32_t lsm6dsv16x_ah_qvar_zin_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ah_qvar_zin_t *val); + +typedef struct { + uint8_t ah_qvar_en : 1; +} lsm6dsv16x_ah_qvar_mode_t; +int32_t lsm6dsv16x_ah_qvar_mode_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ah_qvar_mode_t val); +int32_t lsm6dsv16x_ah_qvar_mode_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ah_qvar_mode_t *val); + +typedef enum { + LSM6DSV16X_SW_RST_DYN_ADDRESS_RST = 0x0, + LSM6DSV16X_I3C_GLOBAL_RST = 0x1, +} lsm6dsv16x_i3c_reset_mode_t; +int32_t lsm6dsv16x_i3c_reset_mode_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_i3c_reset_mode_t val); +int32_t lsm6dsv16x_i3c_reset_mode_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_i3c_reset_mode_t *val); + +typedef enum { + LSM6DSV16X_IBI_2us = 0x0, + LSM6DSV16X_IBI_50us = 0x1, + LSM6DSV16X_IBI_1ms = 0x2, + LSM6DSV16X_IBI_25ms = 0x3, +} lsm6dsv16x_i3c_ibi_time_t; +int32_t lsm6dsv16x_i3c_ibi_time_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_i3c_ibi_time_t val); +int32_t lsm6dsv16x_i3c_ibi_time_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_i3c_ibi_time_t *val); + +int32_t lsm6dsv16x_sh_master_interface_pull_up_set(lsm6dsv16x_ctx_t *ctx, uint8_t val); +int32_t lsm6dsv16x_sh_master_interface_pull_up_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val); + +typedef struct { + lsm6dsv16x_sensor_hub_1_t sh_byte_1; + lsm6dsv16x_sensor_hub_2_t sh_byte_2; + lsm6dsv16x_sensor_hub_3_t sh_byte_3; + lsm6dsv16x_sensor_hub_4_t sh_byte_4; + lsm6dsv16x_sensor_hub_5_t sh_byte_5; + lsm6dsv16x_sensor_hub_6_t sh_byte_6; + lsm6dsv16x_sensor_hub_7_t sh_byte_7; + lsm6dsv16x_sensor_hub_8_t sh_byte_8; + lsm6dsv16x_sensor_hub_9_t sh_byte_9; + lsm6dsv16x_sensor_hub_10_t sh_byte_10; + lsm6dsv16x_sensor_hub_11_t sh_byte_11; + lsm6dsv16x_sensor_hub_12_t sh_byte_12; + lsm6dsv16x_sensor_hub_13_t sh_byte_13; + lsm6dsv16x_sensor_hub_14_t sh_byte_14; + lsm6dsv16x_sensor_hub_15_t sh_byte_15; + lsm6dsv16x_sensor_hub_16_t sh_byte_16; + lsm6dsv16x_sensor_hub_17_t sh_byte_17; + lsm6dsv16x_sensor_hub_18_t sh_byte_18; +} lsm6dsv16x_emb_sh_read_t; +int32_t lsm6dsv16x_sh_read_data_raw_get(lsm6dsv16x_ctx_t *ctx, + lsm6dsv16x_emb_sh_read_t *val, + uint8_t len); + +typedef enum { + LSM6DSV16X_SLV_0 = 0x0, + LSM6DSV16X_SLV_0_1 = 0x1, + LSM6DSV16X_SLV_0_1_2 = 0x2, + LSM6DSV16X_SLV_0_1_2_3 = 0x3, +} lsm6dsv16x_sh_slave_connected_t; +int32_t lsm6dsv16x_sh_slave_connected_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_sh_slave_connected_t val); +int32_t lsm6dsv16x_sh_slave_connected_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_sh_slave_connected_t *val); + +int32_t lsm6dsv16x_sh_master_set(lsm6dsv16x_ctx_t *ctx, uint8_t val); +int32_t lsm6dsv16x_sh_master_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val); + +int32_t lsm6dsv16x_sh_pass_through_set(lsm6dsv16x_ctx_t *ctx, uint8_t val); +int32_t lsm6dsv16x_sh_pass_through_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val); + +typedef enum { + LSM6DSV16X_SH_TRG_XL_GY_DRDY = 0x0, + LSM6DSV16X_SH_TRIG_INT2 = 0x1, +} lsm6dsv16x_sh_syncro_mode_t; +int32_t lsm6dsv16x_sh_syncro_mode_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_sh_syncro_mode_t val); +int32_t lsm6dsv16x_sh_syncro_mode_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_sh_syncro_mode_t *val); + +typedef enum { + LSM6DSV16X_EACH_SH_CYCLE = 0x0, + LSM6DSV16X_ONLY_FIRST_CYCLE = 0x1, +} lsm6dsv16x_sh_write_mode_t; +int32_t lsm6dsv16x_sh_write_mode_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_sh_write_mode_t val); +int32_t lsm6dsv16x_sh_write_mode_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_sh_write_mode_t *val); + +int32_t lsm6dsv16x_sh_reset_set(lsm6dsv16x_ctx_t *ctx, uint8_t val); +int32_t lsm6dsv16x_sh_reset_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val); + +typedef struct { + uint8_t slv0_add; + uint8_t slv0_subadd; + uint8_t slv0_data; +} lsm6dsv16x_sh_cfg_write_t; +int32_t lsm6dsv16x_sh_cfg_write(lsm6dsv16x_ctx_t *ctx, + lsm6dsv16x_sh_cfg_write_t *val); +typedef enum { + LSM6DSV16X_SH_15Hz = 0x1, + LSM6DSV16X_SH_30Hz = 0x2, + LSM6DSV16X_SH_60Hz = 0x3, + LSM6DSV16X_SH_120Hz = 0x4, + LSM6DSV16X_SH_240Hz = 0x5, + LSM6DSV16X_SH_480Hz = 0x6, +} lsm6dsv16x_sh_data_rate_t; +int32_t lsm6dsv16x_sh_data_rate_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_sh_data_rate_t val); +int32_t lsm6dsv16x_sh_data_rate_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_sh_data_rate_t *val); + +typedef struct { + uint8_t slv_add; + uint8_t slv_subadd; + uint8_t slv_len; +} lsm6dsv16x_sh_cfg_read_t; +int32_t lsm6dsv16x_sh_slv0_cfg_read(lsm6dsv16x_ctx_t *ctx, + lsm6dsv16x_sh_cfg_read_t *val); +int32_t lsm6dsv16x_sh_slv1_cfg_read(lsm6dsv16x_ctx_t *ctx, + lsm6dsv16x_sh_cfg_read_t *val); +int32_t lsm6dsv16x_sh_slv2_cfg_read(lsm6dsv16x_ctx_t *ctx, + lsm6dsv16x_sh_cfg_read_t *val); +int32_t lsm6dsv16x_sh_slv3_cfg_read(lsm6dsv16x_ctx_t *ctx, + lsm6dsv16x_sh_cfg_read_t *val); + +int32_t lsm6dsv16x_s4s_sync_samples_set(lsm6dsv16x_ctx_t *ctx, uint16_t val); +int32_t lsm6dsv16x_s4s_sync_samples_get(lsm6dsv16x_ctx_t *ctx, uint16_t *val); + +typedef enum { + LSM6DSV16X_S4S_DT_RES_11 = 0x0, + LSM6DSV16X_S4S_DT_RES_12 = 0x1, + LSM6DSV16X_S4S_DT_RES_13 = 0x2, + LSM6DSV16X_S4S_DT_RES_14 = 0x3, +} lsm6dsv16x_s4s_res_ratio_t; +int32_t lsm6dsv16x_s4s_res_ratio_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_s4s_res_ratio_t val); +int32_t lsm6dsv16x_s4s_res_ratio_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_s4s_res_ratio_t *val); + +int32_t lsm6dsv16x_s4s_command_set(lsm6dsv16x_ctx_t *ctx, uint8_t val); +int32_t lsm6dsv16x_s4s_command_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val); + +int32_t lsm6dsv16x_s4s_dt_set(lsm6dsv16x_ctx_t *ctx, uint8_t val); +int32_t lsm6dsv16x_s4s_dt_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val); + +int32_t lsm6dsv16x_ui_sdo_pull_up_set(lsm6dsv16x_ctx_t *ctx, uint8_t val); +int32_t lsm6dsv16x_ui_sdo_pull_up_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val); + +typedef enum { + LSM6DSV16X_I2C_I3C_ENABLE = 0x0, + LSM6DSV16X_I2C_I3C_DISABLE = 0x1, +} lsm6dsv16x_ui_i2c_i3c_mode_t; +int32_t lsm6dsv16x_ui_i2c_i3c_mode_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ui_i2c_i3c_mode_t val); +int32_t lsm6dsv16x_ui_i2c_i3c_mode_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ui_i2c_i3c_mode_t *val); + +typedef enum { + LSM6DSV16X_SPI_4_WIRE = 0x0, + LSM6DSV16X_SPI_3_WIRE = 0x1, +} lsm6dsv16x_spi_mode_t; +int32_t lsm6dsv16x_spi_mode_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_spi_mode_t val); +int32_t lsm6dsv16x_spi_mode_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_spi_mode_t *val); + +int32_t lsm6dsv16x_ui_sda_pull_up_set(lsm6dsv16x_ctx_t *ctx, uint8_t val); +int32_t lsm6dsv16x_ui_sda_pull_up_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val); + +typedef enum { + LSM6DSV16X_SPI2_4_WIRE = 0x0, + LSM6DSV16X_SPI2_3_WIRE = 0x1, +} lsm6dsv16x_spi2_mode_t; +int32_t lsm6dsv16x_spi2_mode_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_spi2_mode_t val); +int32_t lsm6dsv16x_spi2_mode_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_spi2_mode_t *val); + +int32_t lsm6dsv16x_sigmot_mode_set(lsm6dsv16x_ctx_t *ctx, uint8_t val); +int32_t lsm6dsv16x_sigmot_mode_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val); + +typedef struct { + uint8_t step_counter_enable : 1; + uint8_t false_step_rej : 1; +} lsm6dsv16x_stpcnt_mode_t; +int32_t lsm6dsv16x_stpcnt_mode_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_stpcnt_mode_t val); +int32_t lsm6dsv16x_stpcnt_mode_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_stpcnt_mode_t *val); + +int32_t lsm6dsv16x_stpcnt_steps_get(lsm6dsv16x_ctx_t *ctx, uint16_t *val); + +int32_t lsm6dsv16x_stpcnt_rst_step_set(lsm6dsv16x_ctx_t *ctx, uint8_t val); +int32_t lsm6dsv16x_stpcnt_rst_step_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val); + +int32_t lsm6dsv16x_stpcnt_debounce_set(lsm6dsv16x_ctx_t *ctx, uint8_t val); +int32_t lsm6dsv16x_stpcnt_debounce_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val); + +int32_t lsm6dsv16x_stpcnt_period_set(lsm6dsv16x_ctx_t *ctx, uint16_t val); +int32_t lsm6dsv16x_stpcnt_period_get(lsm6dsv16x_ctx_t *ctx, uint16_t *val); + +typedef struct { + uint8_t tap_x_en : 1; + uint8_t tap_y_en : 1; + uint8_t tap_z_en : 1; +} lsm6dsv16x_tap_detection_t; +int32_t lsm6dsv16x_tap_detection_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_tap_detection_t val); +int32_t lsm6dsv16x_tap_detection_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_tap_detection_t *val); + +typedef struct { + uint8_t x : 1; + uint8_t y : 1; + uint8_t z : 1; +} lsm6dsv16x_tap_thresholds_t; +int32_t lsm6dsv16x_tap_thresholds_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_tap_thresholds_t val); +int32_t lsm6dsv16x_tap_thresholds_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_tap_thresholds_t *val); + +typedef enum { + LSM6DSV16X_XYZ = 0x0, + LSM6DSV16X_YXZ = 0x1, + LSM6DSV16X_XZY = 0x2, + LSM6DSV16X_ZYX = 0x3, + LSM6DSV16X_YZX = 0x5, + LSM6DSV16X_ZXY = 0x6, +} lsm6dsv16x_tap_axis_priority_t; +int32_t lsm6dsv16x_tap_axis_priority_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_tap_axis_priority_t val); +int32_t lsm6dsv16x_tap_axis_priority_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_tap_axis_priority_t *val); + +typedef struct { + uint8_t shock : 1; + uint8_t quiet : 1; + uint8_t tap_gap : 1; +} lsm6dsv16x_tap_time_windows_t; +int32_t lsm6dsv16x_tap_time_windows_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_tap_time_windows_t val); +int32_t lsm6dsv16x_tap_time_windows_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_tap_time_windows_t *val); + +typedef enum { + LSM6DSV16X_ONLY_SINGLE = 0x0, + LSM6DSV16X_BOTH_SINGLE_DOUBLE = 0x1, +} lsm6dsv16x_tap_mode_t; +int32_t lsm6dsv16x_tap_mode_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_tap_mode_t val); +int32_t lsm6dsv16x_tap_mode_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_tap_mode_t *val); + +int32_t lsm6dsv16x_tilt_mode_set(lsm6dsv16x_ctx_t *ctx, uint8_t val); +int32_t lsm6dsv16x_tilt_mode_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val); + +int32_t lsm6dsv16x_timestamp_raw_get(lsm6dsv16x_ctx_t *ctx, uint32_t *val); + +int32_t lsm6dsv16x_timestamp_set(lsm6dsv16x_ctx_t *ctx, uint8_t val); +int32_t lsm6dsv16x_timestamp_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val); + +typedef enum { + LSM6DSV16X_XL_AND_GY_NOT_AFFECTED = 0x0, + LSM6DSV16X_XL_LOW_POWER_GY_NOT_AFFECTED = 0x1, + LSM6DSV16X_XL_LOW_POWER_GY_SLEEP = 0x2, + LSM6DSV16X_XL_LOW_POWER_GY_POWER_DOWN = 0x3, +} lsm6dsv16x_act_mode_t; +int32_t lsm6dsv16x_act_mode_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_act_mode_t val); +int32_t lsm6dsv16x_act_mode_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_act_mode_t *val); + +typedef enum { + LSM6DSV16X_SLEEP_TO_ACT_AT_1ST_SAMPLE = 0x0, + LSM6DSV16X_SLEEP_TO_ACT_AT_2ND_SAMPLE = 0x1, + LSM6DSV16X_SLEEP_TO_ACT_AT_3RD_SAMPLE = 0x2, + LSM6DSV16X_SLEEP_TO_ACT_AT_4th_SAMPLE = 0x3, +} lsm6dsv16x_act_from_sleep_to_act_dur_t; +int32_t lsm6dsv16x_act_from_sleep_to_act_dur_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_act_from_sleep_to_act_dur_t val); +int32_t lsm6dsv16x_act_from_sleep_to_act_dur_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_act_from_sleep_to_act_dur_t *val); + +typedef enum { + LSM6DSV16X_1Hz875 = 0x0, + LSM6DSV16X_15Hz = 0x1, + LSM6DSV16X_30Hz = 0x2, + LSM6DSV16X_60Hz = 0x3, +} lsm6dsv16x_act_sleep_xl_odr_t; +int32_t lsm6dsv16x_act_sleep_xl_odr_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_act_sleep_xl_odr_t val); +int32_t lsm6dsv16x_act_sleep_xl_odr_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_act_sleep_xl_odr_t *val); + +typedef struct { + uint32_t wk_ths_mg; + uint32_t inact_ths_mg; +} lsm6dsv16x_act_thresholds_t; +int32_t lsm6dsv16x_act_thresholds_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_act_thresholds_t val); +int32_t lsm6dsv16x_act_thresholds_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_act_thresholds_t *val); + +typedef struct { + uint8_t shock : 2; + uint8_t quiet : 4; +} lsm6dsv16x_act_wkup_time_windows_t; +int32_t lsm6dsv16x_act_wkup_time_windows_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_act_wkup_time_windows_t val); +int32_t lsm6dsv16x_act_wkup_time_windows_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_act_wkup_time_windows_t *val); + +/** + * @} + * + */ + +#ifdef __cplusplus +} +#endif + +#endif /*LSM6DSV16X_DRIVER_H */ + +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ diff --git a/src/sensors/lsmdsv16x.cpp b/src/sensors/lsmdsv16x.cpp new file mode 100644 index 000000000..eb1da2126 --- /dev/null +++ b/src/sensors/lsmdsv16x.cpp @@ -0,0 +1,211 @@ +/* + SlimeVR Code is placed under the MIT license + Copyright (c) 2021 Eiren Rain & SlimeVR contributors + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. +*/ + +#include "sensors/lsmdsv16x.h" +#include "utils.h" +#include "GlobalVars.h" + +void LSMDSV16XSensor::motionSetup() +{ +#ifdef DEBUG_SENSOR + imu.enableDebugging(Serial); +#endif + imu = LSM6DSV16XSensor(&Wire, addr); + if(!imu.begin()) { + m_Logger.fatal("Can't connect to %s at address 0x%02x", getIMUNameByType(sensorType), addr); + ledManager.pattern(50, 50, 200); + return; + } + + //if(imu.ReadID()) + + m_Logger.info("Connected to %s on 0x%02x. " + "Info: SW Version Major: 0x%02x " + "SW Version Minor: 0x%02x " + "SW Part Number: 0x%02x " + "SW Build Number: 0x%02x " + "SW Version Patch: 0x%02x", + getIMUNameByType(sensorType), + addr + ); + + if(!imu.Enable_X()) { //accel + m_Logger.fatal("Error enabling accelerometer on %s at address 0x%02x", getIMUNameByType(sensorType), addr); + ledManager.pattern(50, 50, 200); + return; + } + + imu.Enable_G(); //gyro + //make an interupt + attachInterrupt(m_IntPin, DoSomething, RISING); + imu.Enable_6D_Orientation(LSM6DSV16X_INT1_PIN); + + lastReset = 0; + lastData = millis(); + working = true; + configured = true; +} + +void LSMDSV16XSensor::motionLoop() +{ + //Look for reports from the IMU + while (imu.dataAvailable()) + { + hadData = true; +#if ENABLE_INSPECTION + { + int16_t rX = imu.getRawGyroX(); + int16_t rY = imu.getRawGyroY(); + int16_t rZ = imu.getRawGyroZ(); + uint8_t rA = imu.getGyroAccuracy(); + + int16_t aX = imu.getRawAccelX(); + int16_t aY = imu.getRawAccelY(); + int16_t aZ = imu.getRawAccelZ(); + uint8_t aA = imu.getAccelAccuracy(); + + networkConnection.sendInspectionRawIMUData(sensorId, rX, rY, rZ, rA, aX, aY, aZ, aA, mX, mY, mZ, mA); + } +#endif + + uint8_t xl = 0; + uint8_t xh = 0; + uint8_t yl = 0; + uint8_t yh = 0; + uint8_t zl = 0; + uint8_t zh = 0; + + imu.Get_6D_Orientation_XL(&xl); + imu.Get_6D_Orientation_XH(&xh); + imu.Get_6D_Orientation_YL(&yl); + imu.Get_6D_Orientation_YH(&yh); + imu.Get_6D_Orientation_ZL(&zl); + imu.Get_6D_Orientation_ZH(&zh); + + lastReset = 0; + lastData = millis(); + + if (imu.hasNewGameQuat()) // New quaternion if context + { + imu.getGameQuat(fusedRotation.x, fusedRotation.y, fusedRotation.z, fusedRotation.w, calibrationAccuracy); + fusedRotation *= sensorOffset; + + if (ENABLE_INSPECTION || !OPTIMIZE_UPDATES || !lastFusedRotationSent.equalsWithEpsilon(fusedRotation)) + { + newFusedRotation = true; + lastFusedRotationSent = fusedRotation; + } + // Leave new quaternion if context open, it's closed later + + // Continuation of the new quaternion if context, used for both 6 and 9 axis +#if SEND_ACCELERATION + { + uint8_t acc; + this->imu.getLinAccel(this->acceleration[0], this->acceleration[1], this->acceleration[2], acc); + this->newAcceleration = true; + } +#endif // SEND_ACCELERATION + } // Closing new quaternion if context + + + if (imu.getTapDetected()) + { + tap = imu.getTapDetector(); + } + if (m_IntPin == 255 || imu.I2CTimedOut()) + break; + } + if (lastData + 1000 < millis() && configured) + { + while(true) { + BNO080Error error = imu.readError(); + if(error.error_source == 255) + break; + lastError = error; + m_Logger.error("BNO08X error. Severity: %d, seq: %d, src: %d, err: %d, mod: %d, code: %d", + error.severity, error.error_sequence_number, error.error_source, error.error, error.error_module, error.error_code); + } + statusManager.setStatus(SlimeVR::Status::IMU_ERROR, true); + working = false; + lastData = millis(); + uint8_t rr = imu.resetReason(); + if (rr != lastReset) + { + lastReset = rr; + networkConnection.sendSensorError(this->sensorId, rr); + } + + m_Logger.error("Sensor %d doesn't respond. Last reset reason:", sensorId, lastReset); + m_Logger.error("Last error: %d, seq: %d, src: %d, err: %d, mod: %d, code: %d", + lastError.severity, lastError.error_sequence_number, lastError.error_source, lastError.error, lastError.error_module, lastError.error_code); + } +} + +SensorStatus BNO080Sensor::getSensorState() { + return lastReset > 0 ? SensorStatus::SENSOR_ERROR : isWorking() ? SensorStatus::SENSOR_OK : SensorStatus::SENSOR_OFFLINE; +} + +void LSMDSV16XSensor::sendData() +{ + if (newFusedRotation) + { + newFusedRotation = false; + networkConnection.sendRotationData(sensorId, &fusedRotation, DATA_TYPE_NORMAL, calibrationAccuracy); + +#ifdef DEBUG_SENSOR + m_Logger.trace("Quaternion: %f, %f, %f, %f", UNPACK_QUATERNION(fusedRotation)); +#endif + } + +#if SEND_ACCELERATION + if(newAcceleration) + { + newAcceleration = false; + networkConnection.sendSensorAcceleration(this->sensorId, this->acceleration); + } +#endif + +#if !USE_6_AXIS + networkConnection.sendMagnetometerAccuracy(sensorId, magneticAccuracyEstimate); +#endif + +#if USE_6_AXIS && BNO_USE_MAGNETOMETER_CORRECTION + if (newMagData) + { + newMagData = false; + networkConnection.sendRotationData(sensorId, &magQuaternion, DATA_TYPE_CORRECTION, magCalibrationAccuracy); + networkConnection.sendMagnetometerAccuracy(sensorId, magneticAccuracyEstimate); + } +#endif + + if (tap != 0) + { + networkConnection.sendSensorTap(sensorId, tap); + tap = 0; + } +} + +void LSMDSV16XSensor::startCalibration(int calibrationType) +{ + //TODO: Look up in data sheet, does not look like it is in the lib +} diff --git a/src/sensors/lsmdsv16x.h b/src/sensors/lsmdsv16x.h new file mode 100644 index 000000000..efcc96075 --- /dev/null +++ b/src/sensors/lsmdsv16x.h @@ -0,0 +1,57 @@ +/* + SlimeVR Code is placed under the MIT license + Copyright (c) 2021 Eiren Rain & SlimeVR contributors + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. +*/ + +#ifndef SENSORS_LSMDSV16X_H +#define SENSORS_LSMDSV16X_H + +#include "sensor.h" +#include + +class LSMDSV16XSensor : public Sensor +{ +public: + LSMDSV16XSensor(uint8_t id, uint8_t type, uint8_t address, float rotation, uint8_t sclPin, uint8_t sdaPin, uint8_t intPin) + : Sensor("LSM6DSV16XSensor", type, id, address, rotation, sclPin, sdaPin), m_IntPin(intPin){}; + ~LSMDSV16XSensor(){}; + void motionSetup() override final; + void postSetup() override { + lastData = millis(); + } + + void motionLoop() override final; + void sendData() override final; + void startCalibration(int calibrationType) override final; + SensorStatus getSensorState() override final; + +private: + LSM6DSV16XSensor imu; + + uint8_t m_IntPin; + + uint8_t tap; + unsigned long lastData = 0; + uint8_t lastReset = 0; + LSM6DSV16XStatusTypeDef lastError{}; +}; + +#endif From 8d16e7bfd7e39f223fd5eb6dfb05ab62bb790d0f Mon Sep 17 00:00:00 2001 From: Carter Meyers Date: Tue, 1 Aug 2023 10:27:24 -0700 Subject: [PATCH 02/60] Build Error and quat needs to be fixed --- src/sensors/lsmdsv16x.cpp | 232 ++++++++++++++++++++------------------ src/sensors/lsmdsv16x.h | 12 +- 2 files changed, 131 insertions(+), 113 deletions(-) diff --git a/src/sensors/lsmdsv16x.cpp b/src/sensors/lsmdsv16x.cpp index eb1da2126..c8a0cb782 100644 --- a/src/sensors/lsmdsv16x.cpp +++ b/src/sensors/lsmdsv16x.cpp @@ -1,6 +1,6 @@ /* SlimeVR Code is placed under the MIT license - Copyright (c) 2021 Eiren Rain & SlimeVR contributors + Copyright (c) 2023 Eiren Rain & SlimeVR contributors Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal @@ -25,40 +25,60 @@ #include "utils.h" #include "GlobalVars.h" +volatile bool imuEvent = false; + void LSMDSV16XSensor::motionSetup() { #ifdef DEBUG_SENSOR - imu.enableDebugging(Serial); + //TODO: Can anything go here #endif + errorCounter = 0; //Either subtract to the error counter or handle the error imu = LSM6DSV16XSensor(&Wire, addr); - if(!imu.begin()) { + if(imu.begin() == LSM6DSV16X_ERROR) { m_Logger.fatal("Can't connect to %s at address 0x%02x", getIMUNameByType(sensorType), addr); ledManager.pattern(50, 50, 200); return; } + uint8_t deviceId = 0; + if(imu.ReadID(&deviceId) == LSM6DSV16X_ERROR) { + m_Logger.fatal("The IMU returned an error when reading the device ID of: 0x%02x", deviceId); + ledManager.pattern(50, 50, 200); + return; + } - //if(imu.ReadID()) + if (deviceId != 0x00) { //TODO: Look up device ID + m_Logger.fatal("The IMU returned an invalid device ID of: 0x%02x when it should have been: 0x%02x", deviceId, 0x00); + ledManager.pattern(50, 50, 200); + return; + } - m_Logger.info("Connected to %s on 0x%02x. " - "Info: SW Version Major: 0x%02x " - "SW Version Minor: 0x%02x " - "SW Part Number: 0x%02x " - "SW Build Number: 0x%02x " - "SW Version Patch: 0x%02x", - getIMUNameByType(sensorType), - addr - ); + m_Logger.info("Connected to %s on 0x%02x. ", getIMUNameByType(sensorType), addr); - if(!imu.Enable_X()) { //accel + if(imu.Enable_X() == LSM6DSV16X_ERROR) { //accel m_Logger.fatal("Error enabling accelerometer on %s at address 0x%02x", getIMUNameByType(sensorType), addr); ledManager.pattern(50, 50, 200); return; } - imu.Enable_G(); //gyro - //make an interupt - attachInterrupt(m_IntPin, DoSomething, RISING); - imu.Enable_6D_Orientation(LSM6DSV16X_INT1_PIN); + if(imu.Enable_G() == LSM6DSV16X_ERROR) { //gyro + m_Logger.fatal("Error enabling gyroscope on %s at address 0x%02x", getIMUNameByType(sensorType), addr); + ledManager.pattern(50, 50, 200); + return; + } + + + attachInterrupt(m_IntPin, interruptHandler, RISING); + + + errorCounter -= imu.Enable_6D_Orientation(LSM6DSV16X_INT1_PIN); + errorCounter -= imu.Enable_Single_Tap_Detection(LSM6DSV16X_INT1_PIN); + errorCounter -= imu.Enable_Double_Tap_Detection(LSM6DSV16X_INT1_PIN); + + if (errorCounter) { + m_Logger.fatal("%d Error(s) occured enabling imu features on %s at address 0x%02x", errorCounter, getIMUNameByType(sensorType), addr); + ledManager.pattern(50, 50, 200); + return; + } lastReset = 0; lastData = millis(); @@ -66,108 +86,117 @@ void LSMDSV16XSensor::motionSetup() configured = true; } +void interruptHandler() { + imuEvent = true; +} + void LSMDSV16XSensor::motionLoop() { - //Look for reports from the IMU - while (imu.dataAvailable()) - { - hadData = true; -#if ENABLE_INSPECTION - { - int16_t rX = imu.getRawGyroX(); - int16_t rY = imu.getRawGyroY(); - int16_t rZ = imu.getRawGyroZ(); - uint8_t rA = imu.getGyroAccuracy(); + if (imuEvent) { + imuEvent = false; + LSM6DSV16X_Event_Status_t status; + errorCounter -= imu.Get_X_Event_Status(&status); - int16_t aX = imu.getRawAccelX(); - int16_t aY = imu.getRawAccelY(); - int16_t aZ = imu.getRawAccelZ(); - uint8_t aA = imu.getAccelAccuracy(); - - networkConnection.sendInspectionRawIMUData(sensorId, rX, rY, rZ, rA, aX, aY, aZ, aA, mX, mY, mZ, mA); + if (status.TapStatus) { + tap++; + } + if (status.DoubleTapStatus) { + tap += 2; } + if (status.D6DOrientationStatus) { + hadData = true; +#if ENABLE_INSPECTION + { + int16_t accelerometer[3]; + int16_t gyroscope[3]; + + errorCounter += imu.Get_X_AxesRaw(accelerometer); + errorCounter += imu.Get_G_AxesRaw(gyroscope); + + networkConnection.sendInspectionRawIMUData(sensorId, gyroscope[0], gyroscope[1], gyroscope[2], 0, accelerometer[0], accelerometer[1], accelerometer[2], 0, 0, 0, 0, 0); + } #endif - uint8_t xl = 0; - uint8_t xh = 0; - uint8_t yl = 0; - uint8_t yh = 0; - uint8_t zl = 0; - uint8_t zh = 0; + uint8_t xl = 0; + uint8_t xh = 0; + uint8_t yl = 0; + uint8_t yh = 0; + uint8_t zl = 0; + uint8_t zh = 0; - imu.Get_6D_Orientation_XL(&xl); - imu.Get_6D_Orientation_XH(&xh); - imu.Get_6D_Orientation_YL(&yl); - imu.Get_6D_Orientation_YH(&yh); - imu.Get_6D_Orientation_ZL(&zl); - imu.Get_6D_Orientation_ZH(&zh); + imu.Get_6D_Orientation_XL(&xl); + imu.Get_6D_Orientation_XH(&xh); + imu.Get_6D_Orientation_YL(&yl); + imu.Get_6D_Orientation_YH(&yh); + imu.Get_6D_Orientation_ZL(&zl); + imu.Get_6D_Orientation_ZH(&zh); - lastReset = 0; - lastData = millis(); + lastReset = 0; + lastData = millis(); - if (imu.hasNewGameQuat()) // New quaternion if context - { - imu.getGameQuat(fusedRotation.x, fusedRotation.y, fusedRotation.z, fusedRotation.w, calibrationAccuracy); - fusedRotation *= sensorOffset; + + //We have imu position (x,y,z) in high/low registers, we need to convert this to a float16 and then to a quaternion - if (ENABLE_INSPECTION || !OPTIMIZE_UPDATES || !lastFusedRotationSent.equalsWithEpsilon(fusedRotation)) + + /* bno code + if (imu.hasNewGameQuat()) // New quaternion if context { - newFusedRotation = true; - lastFusedRotationSent = fusedRotation; - } - // Leave new quaternion if context open, it's closed later + imu.getGameQuat(fusedRotation.x, fusedRotation.y, fusedRotation.z, fusedRotation.w, calibrationAccuracy); + fusedRotation *= sensorOffset; + + if (ENABLE_INSPECTION || !OPTIMIZE_UPDATES || !lastFusedRotationSent.equalsWithEpsilon(fusedRotation)) + { + newFusedRotation = true; + lastFusedRotationSent = fusedRotation; + } + // Leave new quaternion if context open, it's closed later + + // Continuation of the new quaternion if context, used for both 6 and 9 axis + } // Closing new quaternion if context + */ + - // Continuation of the new quaternion if context, used for both 6 and 9 axis #if SEND_ACCELERATION { - uint8_t acc; - this->imu.getLinAccel(this->acceleration[0], this->acceleration[1], this->acceleration[2], acc); - this->newAcceleration = true; + int32_t accelerometerInt[3]; + errorCounter -= imu.Get_X_Axes(accelerometerInt); + acceleration[0] = accelerometerInt[0] * 1000.0; //convert from mg to g + acceleration[1] = accelerometerInt[1] * 1000.0; + acceleration[2] = accelerometerInt[2] * 1000.0; + newAcceleration = true; } #endif // SEND_ACCELERATION - } // Closing new quaternion if context + } - if (imu.getTapDetected()) - { - tap = imu.getTapDetector(); - } - if (m_IntPin == 255 || imu.I2CTimedOut()) - break; - } - if (lastData + 1000 < millis() && configured) - { - while(true) { - BNO080Error error = imu.readError(); - if(error.error_source == 255) - break; - lastError = error; - m_Logger.error("BNO08X error. Severity: %d, seq: %d, src: %d, err: %d, mod: %d, code: %d", - error.severity, error.error_sequence_number, error.error_source, error.error, error.error_module, error.error_code); - } - statusManager.setStatus(SlimeVR::Status::IMU_ERROR, true); - working = false; - lastData = millis(); - uint8_t rr = imu.resetReason(); - if (rr != lastReset) + + + + if ((lastData + 1000 < millis() || errorCounter) && configured) //Errors { - lastReset = rr; - networkConnection.sendSensorError(this->sensorId, rr); - } + if (errorCounter) { + m_Logger.error("The %s at address 0x%02x, had %d error(s) in the motion processing loop", getIMUNameByType(sensorType), addr, errorCounter); + } - m_Logger.error("Sensor %d doesn't respond. Last reset reason:", sensorId, lastReset); - m_Logger.error("Last error: %d, seq: %d, src: %d, err: %d, mod: %d, code: %d", - lastError.severity, lastError.error_sequence_number, lastError.error_source, lastError.error, lastError.error_module, lastError.error_code); + if (lastData + 1000 < millis()) { + m_Logger.error("The %s at address 0x%02x, has not responded in the last second", getIMUNameByType(sensorType), addr); + } + + statusManager.setStatus(SlimeVR::Status::IMU_ERROR, true); + working = false; + lastData = millis(); + errorCounter = 0; + } } } -SensorStatus BNO080Sensor::getSensorState() { - return lastReset > 0 ? SensorStatus::SENSOR_ERROR : isWorking() ? SensorStatus::SENSOR_OK : SensorStatus::SENSOR_OFFLINE; +SensorStatus LSMDSV16XSensor::getSensorState() { + return errorCounter > 0 ? SensorStatus::SENSOR_ERROR : isWorking() ? SensorStatus::SENSOR_OK : SensorStatus::SENSOR_OFFLINE; } void LSMDSV16XSensor::sendData() { - if (newFusedRotation) + if (newFusedRotation) //IDK how to get { newFusedRotation = false; networkConnection.sendRotationData(sensorId, &fusedRotation, DATA_TYPE_NORMAL, calibrationAccuracy); @@ -178,27 +207,14 @@ void LSMDSV16XSensor::sendData() } #if SEND_ACCELERATION - if(newAcceleration) + if(newAcceleration) //Returns acceleration in G's { newAcceleration = false; networkConnection.sendSensorAcceleration(this->sensorId, this->acceleration); } #endif -#if !USE_6_AXIS - networkConnection.sendMagnetometerAccuracy(sensorId, magneticAccuracyEstimate); -#endif - -#if USE_6_AXIS && BNO_USE_MAGNETOMETER_CORRECTION - if (newMagData) - { - newMagData = false; - networkConnection.sendRotationData(sensorId, &magQuaternion, DATA_TYPE_CORRECTION, magCalibrationAccuracy); - networkConnection.sendMagnetometerAccuracy(sensorId, magneticAccuracyEstimate); - } -#endif - - if (tap != 0) + if (tap != 0) //chip supports tap and double tap { networkConnection.sendSensorTap(sensorId, tap); tap = 0; diff --git a/src/sensors/lsmdsv16x.h b/src/sensors/lsmdsv16x.h index efcc96075..b749fbf5e 100644 --- a/src/sensors/lsmdsv16x.h +++ b/src/sensors/lsmdsv16x.h @@ -31,7 +31,7 @@ class LSMDSV16XSensor : public Sensor { public: LSMDSV16XSensor(uint8_t id, uint8_t type, uint8_t address, float rotation, uint8_t sclPin, uint8_t sdaPin, uint8_t intPin) - : Sensor("LSM6DSV16XSensor", type, id, address, rotation, sclPin, sdaPin), m_IntPin(intPin){}; + : Sensor("LSM6DSV16XSensor", type, id, address, rotation, sclPin, sdaPin), m_IntPin(intPin) {}; ~LSMDSV16XSensor(){}; void motionSetup() override final; void postSetup() override { @@ -44,14 +44,16 @@ class LSMDSV16XSensor : public Sensor SensorStatus getSensorState() override final; private: - LSM6DSV16XSensor imu; + //void interruptHandler(); + //volatile bool imuEvent; //the interrupt cant be a class function + LSM6DSV16XSensor imu; uint8_t m_IntPin; - - uint8_t tap; + uint8_t errorCounter = 0; //Error is -1, OK is 0 + uint8_t tap = 0; unsigned long lastData = 0; + uint8_t lastReset = 0; - LSM6DSV16XStatusTypeDef lastError{}; }; #endif From 48dda5fca2725709eeccac6b1fca10b2ad7a7130 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Przemys=C5=82aw=20Romaniak?= Date: Wed, 2 Aug 2023 01:32:15 +0200 Subject: [PATCH 03/60] lsmdsv16x: standardize file names (low level lib without word sensor, sensor impl postfixed with sensor. Fix compilation errors --- .../{LSM6DSV16XSensor.cpp => LSM6DSV16X.cpp} | 182 +++++++++--------- .../{LSM6DSV16XSensor.h => LSM6DSV16X.h} | 14 +- .../{lsmdsv16x.cpp => lsmdsv16xsensor.cpp} | 11 +- .../{lsmdsv16x.h => lsmdsv16xsensor.h} | 8 +- 4 files changed, 109 insertions(+), 106 deletions(-) rename lib/LSM6DSV16X/{LSM6DSV16XSensor.cpp => LSM6DSV16X.cpp} (92%) rename lib/LSM6DSV16X/{LSM6DSV16XSensor.h => LSM6DSV16X.h} (97%) rename src/sensors/{lsmdsv16x.cpp => lsmdsv16xsensor.cpp} (99%) rename src/sensors/{lsmdsv16x.h => lsmdsv16xsensor.h} (94%) diff --git a/lib/LSM6DSV16X/LSM6DSV16XSensor.cpp b/lib/LSM6DSV16X/LSM6DSV16X.cpp similarity index 92% rename from lib/LSM6DSV16X/LSM6DSV16XSensor.cpp rename to lib/LSM6DSV16X/LSM6DSV16X.cpp index 59202fe9d..7c52a5c2f 100644 --- a/lib/LSM6DSV16X/LSM6DSV16XSensor.cpp +++ b/lib/LSM6DSV16X/LSM6DSV16X.cpp @@ -1,6 +1,6 @@ /** ****************************************************************************** - * @file LSM6DSV16XSensor.cpp + * @file LSM6DSV16X.cpp * @author SRA * @version V1.5.1 * @date July 2022 @@ -38,7 +38,7 @@ /* Includes ------------------------------------------------------------------*/ -#include "LSM6DSV16XSensor.h" +#include "LSM6DSV16X.h" /* Class Implementation ------------------------------------------------------*/ @@ -47,7 +47,7 @@ * @param i2c object of an helper class which handles the I2C peripheral * @param address the address of the component's instance */ -LSM6DSV16XSensor::LSM6DSV16XSensor(TwoWire *i2c, uint8_t address) : dev_i2c(i2c), address(address) +LSM6DSV16X::LSM6DSV16X(TwoWire *i2c, uint8_t address) : dev_i2c(i2c), address(address) { reg_ctx.write_reg = LSM6DSV16X_io_write; reg_ctx.read_reg = LSM6DSV16X_io_read; @@ -62,7 +62,7 @@ LSM6DSV16XSensor::LSM6DSV16XSensor(TwoWire *i2c, uint8_t address) : dev_i2c(i2c) * @param cs_pin the chip select pin * @param spi_speed the SPI speed */ -LSM6DSV16XSensor::LSM6DSV16XSensor(SPIClass *spi, int cs_pin, uint32_t spi_speed) : dev_spi(spi), cs_pin(cs_pin), spi_speed(spi_speed) +LSM6DSV16X::LSM6DSV16X(SPIClass *spi, int cs_pin, uint32_t spi_speed) : dev_spi(spi), cs_pin(cs_pin), spi_speed(spi_speed) { reg_ctx.write_reg = LSM6DSV16X_io_write; reg_ctx.read_reg = LSM6DSV16X_io_read; @@ -76,7 +76,7 @@ LSM6DSV16XSensor::LSM6DSV16XSensor(SPIClass *spi, int cs_pin, uint32_t spi_speed * @brief Initialize the LSM6DSV16X sensor * @retval 0 in case of success, an error code otherwise */ -LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::begin() +LSM6DSV16XStatusTypeDef LSM6DSV16X::begin() { if (dev_spi) { // Configure CS pin @@ -135,7 +135,7 @@ LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::begin() * @brief Deinitialize the LSM6DSV16X sensor * @retval 0 in case of success, an error code otherwise */ -LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::end() +LSM6DSV16XStatusTypeDef LSM6DSV16X::end() { /* Disable the component */ if (Disable_X() != LSM6DSV16X_OK) { @@ -160,7 +160,7 @@ LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::end() * @param Id the WHO_AM_I value * @retval 0 in case of success, an error code otherwise */ -LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::ReadID(uint8_t *Id) +LSM6DSV16XStatusTypeDef LSM6DSV16X::ReadID(uint8_t *Id) { if (lsm6dsv16x_device_id_get(®_ctx, Id) != LSM6DSV16X_OK) { return LSM6DSV16X_ERROR; @@ -173,7 +173,7 @@ LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::ReadID(uint8_t *Id) * @brief Enable the LSM6DSV16X accelerometer sensor * @retval 0 in case of success, an error code otherwise */ -LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Enable_X() +LSM6DSV16XStatusTypeDef LSM6DSV16X::Enable_X() { /* Check if the component is already enabled */ if (acc_is_enabled == 1U) { @@ -194,7 +194,7 @@ LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Enable_X() * @brief Disable the LSM6DSV16X accelerometer sensor * @retval 0 in case of success, an error code otherwise */ -LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Disable_X() +LSM6DSV16XStatusTypeDef LSM6DSV16X::Disable_X() { /* Check if the component is already disabled */ if (acc_is_enabled == 0U) { @@ -221,7 +221,7 @@ LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Disable_X() * @param Sensitivity pointer * @retval 0 in case of success, an error code otherwise */ -LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Get_X_Sensitivity(float *Sensitivity) +LSM6DSV16XStatusTypeDef LSM6DSV16X::Get_X_Sensitivity(float *Sensitivity) { LSM6DSV16XStatusTypeDef ret = LSM6DSV16X_OK; lsm6dsv16x_xl_full_scale_t full_scale; @@ -262,7 +262,7 @@ LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Get_X_Sensitivity(float *Sensitivity) * @param Odr pointer where the output data rate is written * @retval 0 in case of success, an error code otherwise */ -LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Get_X_ODR(float *Odr) +LSM6DSV16XStatusTypeDef LSM6DSV16X::Get_X_ODR(float *Odr) { LSM6DSV16XStatusTypeDef ret = LSM6DSV16X_OK; lsm6dsv16x_xl_data_rate_t odr_low_level; @@ -338,7 +338,7 @@ LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Get_X_ODR(float *Odr) * @param Acceleration pointer where the values of the axes are written * @retval 0 in case of success, an error code otherwise */ -LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Get_X_Axes(int32_t *Acceleration) +LSM6DSV16XStatusTypeDef LSM6DSV16X::Get_X_Axes(int32_t *Acceleration) { lsm6dsv16x_axis3bit16_t data_raw; float sensitivity = 0.0f; @@ -366,7 +366,7 @@ LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Get_X_Axes(int32_t *Acceleration) * @param Odr the output data rate value to be set * @retval 0 in case of success, an error code otherwise */ -LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Set_X_ODR(float Odr, LSM6DSV16X_ACC_Operating_Mode_t Mode) +LSM6DSV16XStatusTypeDef LSM6DSV16X::Set_X_ODR(float Odr, LSM6DSV16X_ACC_Operating_Mode_t Mode) { switch (Mode) { case LSM6DSV16X_ACC_HIGH_PERFORMANCE_MODE: { @@ -454,7 +454,7 @@ LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Set_X_ODR(float Odr, LSM6DSV16X_ACC_Op * @param Odr the functional output data rate to be set * @retval 0 in case of success, an error code otherwise */ -LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Set_X_ODR_When_Enabled(float Odr) +LSM6DSV16XStatusTypeDef LSM6DSV16X::Set_X_ODR_When_Enabled(float Odr) { lsm6dsv16x_xl_data_rate_t new_odr; @@ -484,7 +484,7 @@ LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Set_X_ODR_When_Enabled(float Odr) * @param Odr the functional output data rate to be set * @retval 0 in case of success, an error code otherwise */ -LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Set_X_ODR_When_Disabled(float Odr) +LSM6DSV16XStatusTypeDef LSM6DSV16X::Set_X_ODR_When_Disabled(float Odr) { acc_odr = (Odr <= 1.875f) ? LSM6DSV16X_XL_ODR_AT_1Hz875 : (Odr <= 7.5f) ? LSM6DSV16X_XL_ODR_AT_7Hz5 @@ -507,7 +507,7 @@ LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Set_X_ODR_When_Disabled(float Odr) * @param FullScale pointer where the full scale is written * @retval 0 in case of success, an error code otherwise */ -LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Get_X_FS(int32_t *FullScale) +LSM6DSV16XStatusTypeDef LSM6DSV16X::Get_X_FS(int32_t *FullScale) { LSM6DSV16XStatusTypeDef ret = LSM6DSV16X_OK; lsm6dsv16x_xl_full_scale_t fs_low_level; @@ -547,7 +547,7 @@ LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Get_X_FS(int32_t *FullScale) * @param FullScale the functional full scale to be set * @retval 0 in case of success, an error code otherwise */ -LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Set_X_FS(int32_t FullScale) +LSM6DSV16XStatusTypeDef LSM6DSV16X::Set_X_FS(int32_t FullScale) { lsm6dsv16x_xl_full_scale_t new_fs; @@ -570,7 +570,7 @@ LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Set_X_FS(int32_t FullScale) * @param Value pointer where the raw values of the axes are written * @retval 0 in case of success, an error code otherwise */ -LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Get_X_AxesRaw(int16_t *Value) +LSM6DSV16XStatusTypeDef LSM6DSV16X::Get_X_AxesRaw(int16_t *Value) { lsm6dsv16x_axis3bit16_t data_raw; @@ -592,7 +592,7 @@ LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Get_X_AxesRaw(int16_t *Value) * @param Status the status of data ready bit * @retval 0 in case of success, an error code otherwise */ -LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Get_X_DRDY_Status(uint8_t *Status) +LSM6DSV16XStatusTypeDef LSM6DSV16X::Get_X_DRDY_Status(uint8_t *Status) { lsm6dsv16x_all_sources_t val; @@ -609,7 +609,7 @@ LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Get_X_DRDY_Status(uint8_t *Status) * @param Status the status of all hardware events * @retval 0 in case of success, an error code otherwise */ -LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Get_X_Event_Status(LSM6DSV16X_Event_Status_t *Status) +LSM6DSV16XStatusTypeDef LSM6DSV16X::Get_X_Event_Status(LSM6DSV16X_Event_Status_t *Status) { lsm6dsv16x_emb_func_status_t emb_func_status; lsm6dsv16x_wake_up_src_t wake_up_src; @@ -719,7 +719,7 @@ LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Get_X_Event_Status(LSM6DSV16X_Event_St * @param PowerMode Value of the powerMode * @retval 0 in case of success, an error code otherwise */ -LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Set_X_Power_Mode(uint8_t PowerMode) +LSM6DSV16XStatusTypeDef LSM6DSV16X::Set_X_Power_Mode(uint8_t PowerMode) { if (lsm6dsv16x_xl_mode_set(®_ctx, (lsm6dsv16x_xl_mode_t)PowerMode) != LSM6DSV16X_OK) { return LSM6DSV16X_ERROR; @@ -734,7 +734,7 @@ LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Set_X_Power_Mode(uint8_t PowerMode) * @param FilterMode Value of the filter Mode * @retval 0 in case of success, an error code otherwise */ -LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Set_X_Filter_Mode(uint8_t LowHighPassFlag, uint8_t FilterMode) +LSM6DSV16XStatusTypeDef LSM6DSV16X::Set_X_Filter_Mode(uint8_t LowHighPassFlag, uint8_t FilterMode) { if (LowHighPassFlag == 0) { /*Set accelerometer low_pass filter-mode*/ @@ -763,7 +763,7 @@ LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Set_X_Filter_Mode(uint8_t LowHighPassF * @param IntPin interrupt pin line to be used * @retval 0 in case of success, an error code otherwise */ -LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Enable_6D_Orientation(LSM6DSV16X_SensorIntPin_t IntPin) +LSM6DSV16XStatusTypeDef LSM6DSV16X::Enable_6D_Orientation(LSM6DSV16X_SensorIntPin_t IntPin) { LSM6DSV16XStatusTypeDef ret = LSM6DSV16X_OK; lsm6dsv16x_md1_cfg_t val1; @@ -842,7 +842,7 @@ LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Enable_6D_Orientation(LSM6DSV16X_Senso * @brief Disable 6D orientation detection * @retval 0 in case of success, an error code otherwise */ -LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Disable_6D_Orientation() +LSM6DSV16XStatusTypeDef LSM6DSV16X::Disable_6D_Orientation() { lsm6dsv16x_md1_cfg_t val1; lsm6dsv16x_md2_cfg_t val2; @@ -881,7 +881,7 @@ LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Disable_6D_Orientation() * @param Threshold 6D Orientation detection threshold * @retval 0 in case of success, an error code otherwise */ -LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Set_6D_Orientation_Threshold(uint8_t Threshold) +LSM6DSV16XStatusTypeDef LSM6DSV16X::Set_6D_Orientation_Threshold(uint8_t Threshold) { LSM6DSV16XStatusTypeDef ret = LSM6DSV16X_OK; lsm6dsv16x_6d_threshold_t newThreshold = LSM6DSV16X_DEG_80; @@ -921,7 +921,7 @@ LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Set_6D_Orientation_Threshold(uint8_t T * @param XLow the status of XLow orientation * @retval 0 in case of success, an error code otherwise */ -LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Get_6D_Orientation_XL(uint8_t *XLow) +LSM6DSV16XStatusTypeDef LSM6DSV16X::Get_6D_Orientation_XL(uint8_t *XLow) { lsm6dsv16x_d6d_src_t data; @@ -939,7 +939,7 @@ LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Get_6D_Orientation_XL(uint8_t *XLow) * @param XHigh the status of XHigh orientation * @retval 0 in case of success, an error code otherwise */ -LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Get_6D_Orientation_XH(uint8_t *XHigh) +LSM6DSV16XStatusTypeDef LSM6DSV16X::Get_6D_Orientation_XH(uint8_t *XHigh) { lsm6dsv16x_d6d_src_t data; @@ -957,7 +957,7 @@ LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Get_6D_Orientation_XH(uint8_t *XHigh) * @param YLow the status of YLow orientation * @retval 0 in case of success, an error code otherwise */ -LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Get_6D_Orientation_YL(uint8_t *YLow) +LSM6DSV16XStatusTypeDef LSM6DSV16X::Get_6D_Orientation_YL(uint8_t *YLow) { lsm6dsv16x_d6d_src_t data; @@ -975,7 +975,7 @@ LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Get_6D_Orientation_YL(uint8_t *YLow) * @param YHigh the status of YHigh orientation * @retval 0 in case of success, an error code otherwise */ -LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Get_6D_Orientation_YH(uint8_t *YHigh) +LSM6DSV16XStatusTypeDef LSM6DSV16X::Get_6D_Orientation_YH(uint8_t *YHigh) { lsm6dsv16x_d6d_src_t data; @@ -993,7 +993,7 @@ LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Get_6D_Orientation_YH(uint8_t *YHigh) * @param ZLow the status of ZLow orientation * @retval 0 in case of success, an error code otherwise */ -LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Get_6D_Orientation_ZL(uint8_t *ZLow) +LSM6DSV16XStatusTypeDef LSM6DSV16X::Get_6D_Orientation_ZL(uint8_t *ZLow) { lsm6dsv16x_d6d_src_t data; @@ -1011,7 +1011,7 @@ LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Get_6D_Orientation_ZL(uint8_t *ZLow) * @param ZHigh the status of ZHigh orientation * @retval 0 in case of success, an error code otherwise */ -LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Get_6D_Orientation_ZH(uint8_t *ZHigh) +LSM6DSV16XStatusTypeDef LSM6DSV16X::Get_6D_Orientation_ZH(uint8_t *ZHigh) { lsm6dsv16x_d6d_src_t data; @@ -1030,7 +1030,7 @@ LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Get_6D_Orientation_ZH(uint8_t *ZHigh) * @param IntPin interrupt pin line to be used * @retval 0 in case of success, an error code otherwise */ -LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Enable_Free_Fall_Detection(LSM6DSV16X_SensorIntPin_t IntPin) +LSM6DSV16XStatusTypeDef LSM6DSV16X::Enable_Free_Fall_Detection(LSM6DSV16X_SensorIntPin_t IntPin) { LSM6DSV16XStatusTypeDef ret = LSM6DSV16X_OK; lsm6dsv16x_md1_cfg_t val1; @@ -1115,7 +1115,7 @@ LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Enable_Free_Fall_Detection(LSM6DSV16X_ * @brief Disable free fall detection * @retval 0 in case of success, an error code otherwise */ -LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Disable_Free_Fall_Detection() +LSM6DSV16XStatusTypeDef LSM6DSV16X::Disable_Free_Fall_Detection() { lsm6dsv16x_md1_cfg_t val1; lsm6dsv16x_md2_cfg_t val2; @@ -1159,7 +1159,7 @@ LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Disable_Free_Fall_Detection() * @param Threshold free fall detection threshold * @retval 0 in case of success, an error code otherwise */ -LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Set_Free_Fall_Threshold(uint8_t Threshold) +LSM6DSV16XStatusTypeDef LSM6DSV16X::Set_Free_Fall_Threshold(uint8_t Threshold) { lsm6dsv16x_ff_thresholds_t val; switch (Threshold) { @@ -1214,7 +1214,7 @@ LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Set_Free_Fall_Threshold(uint8_t Thresh * @param Duration free fall detection duration * @retval 0 in case of success, an error code otherwise */ -LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Set_Free_Fall_Duration(uint8_t Duration) +LSM6DSV16XStatusTypeDef LSM6DSV16X::Set_Free_Fall_Duration(uint8_t Duration) { if (lsm6dsv16x_ff_time_windows_set(®_ctx, Duration) != LSM6DSV16X_OK) { return LSM6DSV16X_ERROR; @@ -1228,7 +1228,7 @@ LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Set_Free_Fall_Duration(uint8_t Duratio * @param IntPin interrupt pin line to be used * @retval 0 in case of success, an error code otherwise */ -LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Enable_Wake_Up_Detection(LSM6DSV16X_SensorIntPin_t IntPin) +LSM6DSV16XStatusTypeDef LSM6DSV16X::Enable_Wake_Up_Detection(LSM6DSV16X_SensorIntPin_t IntPin) { LSM6DSV16XStatusTypeDef ret = LSM6DSV16X_OK; lsm6dsv16x_md1_cfg_t val1; @@ -1314,7 +1314,7 @@ LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Enable_Wake_Up_Detection(LSM6DSV16X_Se * @brief Disable wake up detection * @retval 0 in case of success, an error code otherwise */ -LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Disable_Wake_Up_Detection() +LSM6DSV16XStatusTypeDef LSM6DSV16X::Disable_Wake_Up_Detection() { lsm6dsv16x_md1_cfg_t val1; lsm6dsv16x_md2_cfg_t val2; @@ -1358,7 +1358,7 @@ LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Disable_Wake_Up_Detection() * @param Threshold wake up detection threshold * @retval 0 in case of success, an error code otherwise */ -LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Set_Wake_Up_Threshold(uint32_t Threshold) +LSM6DSV16XStatusTypeDef LSM6DSV16X::Set_Wake_Up_Threshold(uint32_t Threshold) { lsm6dsv16x_act_thresholds_t wake_up_ths; @@ -1380,7 +1380,7 @@ LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Set_Wake_Up_Threshold(uint32_t Thresho * @param Duration wake up detection duration * @retval 0 in case of success, an error code otherwise */ -LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Set_Wake_Up_Duration(uint8_t Duration) +LSM6DSV16XStatusTypeDef LSM6DSV16X::Set_Wake_Up_Duration(uint8_t Duration) { lsm6dsv16x_act_wkup_time_windows_t dur_t; @@ -1402,7 +1402,7 @@ LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Set_Wake_Up_Duration(uint8_t Duration) * @param IntPin interrupt pin line to be used * @retval 0 in case of success, an error code otherwise */ -LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Enable_Single_Tap_Detection(LSM6DSV16X_SensorIntPin_t IntPin) +LSM6DSV16XStatusTypeDef LSM6DSV16X::Enable_Single_Tap_Detection(LSM6DSV16X_SensorIntPin_t IntPin) { LSM6DSV16XStatusTypeDef ret = LSM6DSV16X_OK; lsm6dsv16x_md1_cfg_t val1; @@ -1520,7 +1520,7 @@ LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Enable_Single_Tap_Detection(LSM6DSV16X * @brief Disable single tap detection * @retval 0 in case of success, an error code otherwise */ -LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Disable_Single_Tap_Detection() +LSM6DSV16XStatusTypeDef LSM6DSV16X::Disable_Single_Tap_Detection() { lsm6dsv16x_md1_cfg_t val1; lsm6dsv16x_md2_cfg_t val2; @@ -1592,7 +1592,7 @@ LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Disable_Single_Tap_Detection() * @param IntPin interrupt pin line to be used * @retval 0 in case of success, an error code otherwise */ -LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Enable_Double_Tap_Detection(LSM6DSV16X_SensorIntPin_t IntPin) +LSM6DSV16XStatusTypeDef LSM6DSV16X::Enable_Double_Tap_Detection(LSM6DSV16X_SensorIntPin_t IntPin) { LSM6DSV16XStatusTypeDef ret = LSM6DSV16X_OK; lsm6dsv16x_md1_cfg_t val1; @@ -1712,7 +1712,7 @@ LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Enable_Double_Tap_Detection(LSM6DSV16X * @brief Disable double tap detection * @retval 0 in case of success, an error code otherwise */ -LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Disable_Double_Tap_Detection() +LSM6DSV16XStatusTypeDef LSM6DSV16X::Disable_Double_Tap_Detection() { lsm6dsv16x_md1_cfg_t val1; lsm6dsv16x_md2_cfg_t val2; @@ -1791,7 +1791,7 @@ LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Disable_Double_Tap_Detection() * @param Threshold tap threshold * @retval 0 in case of success, an error code otherwise */ -LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Set_Tap_Threshold(uint8_t Threshold) +LSM6DSV16XStatusTypeDef LSM6DSV16X::Set_Tap_Threshold(uint8_t Threshold) { lsm6dsv16x_tap_ths_6d_t tap_ths_6d; @@ -1814,7 +1814,7 @@ LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Set_Tap_Threshold(uint8_t Threshold) * @param Time tap shock time * @retval 0 in case of success, an error code otherwise */ -LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Set_Tap_Shock_Time(uint8_t Time) +LSM6DSV16XStatusTypeDef LSM6DSV16X::Set_Tap_Shock_Time(uint8_t Time) { lsm6dsv16x_int_dur2_t tap_dur; @@ -1828,6 +1828,8 @@ LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Set_Tap_Shock_Time(uint8_t Time) if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_INT_DUR2, (uint8_t *)&tap_dur, 1) != LSM6DSV16X_OK) { return LSM6DSV16X_ERROR; } + + return LSM6DSV16X_OK; } /** @@ -1835,7 +1837,7 @@ LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Set_Tap_Shock_Time(uint8_t Time) * @param Time tap quiet time * @retval 0 in case of success, an error code otherwise */ -LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Set_Tap_Quiet_Time(uint8_t Time) +LSM6DSV16XStatusTypeDef LSM6DSV16X::Set_Tap_Quiet_Time(uint8_t Time) { lsm6dsv16x_int_dur2_t tap_dur; @@ -1857,7 +1859,7 @@ LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Set_Tap_Quiet_Time(uint8_t Time) * @param Time tap duration time * @retval 0 in case of success, an error code otherwise */ -LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Set_Tap_Duration_Time(uint8_t Time) +LSM6DSV16XStatusTypeDef LSM6DSV16X::Set_Tap_Duration_Time(uint8_t Time) { lsm6dsv16x_int_dur2_t tap_dur; @@ -1878,7 +1880,7 @@ LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Set_Tap_Duration_Time(uint8_t Time) * @brief Enable pedometer * @retval 0 in case of success, an error code otherwise */ -LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Enable_Pedometer(LSM6DSV16X_SensorIntPin_t IntPin) +LSM6DSV16XStatusTypeDef LSM6DSV16X::Enable_Pedometer(LSM6DSV16X_SensorIntPin_t IntPin) { lsm6dsv16x_stpcnt_mode_t mode; lsm6dsv16x_md1_cfg_t val1; @@ -1990,7 +1992,7 @@ LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Enable_Pedometer(LSM6DSV16X_SensorIntP * @brief Disable pedometer * @retval 0 in case of success, an error code otherwise */ -LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Disable_Pedometer() +LSM6DSV16XStatusTypeDef LSM6DSV16X::Disable_Pedometer() { lsm6dsv16x_md1_cfg_t val1; lsm6dsv16x_md2_cfg_t val2; @@ -2076,7 +2078,7 @@ LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Disable_Pedometer() * @param StepCount step counter * @retval 0 in case of success, an error code otherwise */ -LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Get_Step_Count(uint16_t *StepCount) +LSM6DSV16XStatusTypeDef LSM6DSV16X::Get_Step_Count(uint16_t *StepCount) { if (lsm6dsv16x_stpcnt_steps_get(®_ctx, StepCount) != LSM6DSV16X_OK) { return LSM6DSV16X_ERROR; @@ -2089,7 +2091,7 @@ LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Get_Step_Count(uint16_t *StepCount) * @brief Enable step counter reset * @retval 0 in case of success, an error code otherwise */ -LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Step_Counter_Reset() +LSM6DSV16XStatusTypeDef LSM6DSV16X::Step_Counter_Reset() { if (lsm6dsv16x_stpcnt_rst_step_set(®_ctx, PROPERTY_ENABLE) != LSM6DSV16X_OK) { return LSM6DSV16X_ERROR; @@ -2103,7 +2105,7 @@ LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Step_Counter_Reset() * @param IntPin interrupt pin line to be used * @retval 0 in case of success, an error code otherwise */ -LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Enable_Tilt_Detection(LSM6DSV16X_SensorIntPin_t IntPin) +LSM6DSV16XStatusTypeDef LSM6DSV16X::Enable_Tilt_Detection(LSM6DSV16X_SensorIntPin_t IntPin) { LSM6DSV16XStatusTypeDef ret = LSM6DSV16X_OK; lsm6dsv16x_md1_cfg_t val1; @@ -2226,7 +2228,7 @@ LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Enable_Tilt_Detection(LSM6DSV16X_Senso * @brief Disable tilt detection * @retval 0 in case of success, an error code otherwise */ -LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Disable_Tilt_Detection() +LSM6DSV16XStatusTypeDef LSM6DSV16X::Disable_Tilt_Detection() { LSM6DSV16XStatusTypeDef ret = LSM6DSV16X_OK; lsm6dsv16x_md1_cfg_t val1; @@ -2309,7 +2311,7 @@ LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Disable_Tilt_Detection() * @param NumSamples number of samples * @retval 0 in case of success, an error code otherwise */ -LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::FIFO_Get_Num_Samples(uint16_t *NumSamples) +LSM6DSV16XStatusTypeDef LSM6DSV16X::FIFO_Get_Num_Samples(uint16_t *NumSamples) { return (LSM6DSV16XStatusTypeDef) lsm6dsv16x_fifo_data_level_get(®_ctx, NumSamples); } @@ -2320,7 +2322,7 @@ LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::FIFO_Get_Num_Samples(uint16_t *NumSamp * @param Status FIFO full status * @retval 0 in case of success, an error code otherwise */ -LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::FIFO_Get_Full_Status(uint8_t *Status) +LSM6DSV16XStatusTypeDef LSM6DSV16X::FIFO_Get_Full_Status(uint8_t *Status) { lsm6dsv16x_fifo_status2_t val; @@ -2339,7 +2341,7 @@ LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::FIFO_Get_Full_Status(uint8_t *Status) * @param Status FIFO full interrupt on INT1 pin status * @retval 0 in case of success, an error code otherwise */ -LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::FIFO_Set_INT1_FIFO_Full(uint8_t Status) +LSM6DSV16XStatusTypeDef LSM6DSV16X::FIFO_Set_INT1_FIFO_Full(uint8_t Status) { lsm6dsv16x_int1_ctrl_t reg; @@ -2362,7 +2364,7 @@ LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::FIFO_Set_INT1_FIFO_Full(uint8_t Status * @param Status FIFO full interrupt on INT1 pin status * @retval 0 in case of success, an error code otherwise */ -LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::FIFO_Set_INT2_FIFO_Full(uint8_t Status) +LSM6DSV16XStatusTypeDef LSM6DSV16X::FIFO_Set_INT2_FIFO_Full(uint8_t Status) { lsm6dsv16x_int2_ctrl_t reg; @@ -2385,7 +2387,7 @@ LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::FIFO_Set_INT2_FIFO_Full(uint8_t Status * @param Watermark FIFO watermark level * @retval 0 in case of success, an error code otherwise */ -LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::FIFO_Set_Watermark_Level(uint8_t Watermark) +LSM6DSV16XStatusTypeDef LSM6DSV16X::FIFO_Set_Watermark_Level(uint8_t Watermark) { return (LSM6DSV16XStatusTypeDef) lsm6dsv16x_fifo_watermark_set(®_ctx, Watermark); } @@ -2396,7 +2398,7 @@ LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::FIFO_Set_Watermark_Level(uint8_t Water * @param Status FIFO stop on watermark status * @retval 0 in case of success, an error code otherwise */ -LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::FIFO_Set_Stop_On_Fth(uint8_t Status) +LSM6DSV16XStatusTypeDef LSM6DSV16X::FIFO_Set_Stop_On_Fth(uint8_t Status) { return (LSM6DSV16XStatusTypeDef) lsm6dsv16x_fifo_stop_on_wtm_set(®_ctx, Status); } @@ -2407,7 +2409,7 @@ LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::FIFO_Set_Stop_On_Fth(uint8_t Status) * @param Mode FIFO mode * @retval 0 in case of success, an error code otherwise */ -LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::FIFO_Set_Mode(uint8_t Mode) +LSM6DSV16XStatusTypeDef LSM6DSV16X::FIFO_Set_Mode(uint8_t Mode) { LSM6DSV16XStatusTypeDef ret = LSM6DSV16X_OK; lsm6dsv16x_fifo_mode_t newMode = LSM6DSV16X_BYPASS_MODE; @@ -2453,7 +2455,7 @@ LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::FIFO_Set_Mode(uint8_t Mode) * @param Tag FIFO tag * @retval 0 in case of success, an error code otherwise */ -LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::FIFO_Get_Tag(uint8_t *Tag) +LSM6DSV16XStatusTypeDef LSM6DSV16X::FIFO_Get_Tag(uint8_t *Tag) { lsm6dsv16x_fifo_data_out_tag_t tag_local; @@ -2472,7 +2474,7 @@ LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::FIFO_Get_Tag(uint8_t *Tag) * @param Data FIFO raw data array [6] * @retval 0 in case of success, an error code otherwise */ -LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::FIFO_Get_Data(uint8_t *Data) +LSM6DSV16XStatusTypeDef LSM6DSV16X::FIFO_Get_Data(uint8_t *Data) { return (LSM6DSV16XStatusTypeDef) lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_FIFO_DATA_OUT_X_L, Data, 6); } @@ -2482,7 +2484,7 @@ LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::FIFO_Get_Data(uint8_t *Data) * @param Acceleration FIFO accelero axes [mg] * @retval 0 in case of success, an error code otherwise */ -LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::FIFO_Get_X_Axes(int32_t *Acceleration) +LSM6DSV16XStatusTypeDef LSM6DSV16X::FIFO_Get_X_Axes(int32_t *Acceleration) { lsm6dsv16x_axis3bit16_t data_raw; float sensitivity = 0.0f; @@ -2513,7 +2515,7 @@ LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::FIFO_Get_X_Axes(int32_t *Acceleration) * @param Bdr FIFO accelero BDR value * @retval 0 in case of success, an error code otherwise */ -LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::FIFO_Set_X_BDR(float Bdr) +LSM6DSV16XStatusTypeDef LSM6DSV16X::FIFO_Set_X_BDR(float Bdr) { lsm6dsv16x_fifo_xl_batch_t new_bdr; @@ -2540,7 +2542,7 @@ LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::FIFO_Set_X_BDR(float Bdr) * @param AngularVelocity FIFO gyro axes [mDPS] * @retval 0 in case of success, an error code otherwise */ -LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::FIFO_Get_G_Axes(int32_t *AngularVelocity) +LSM6DSV16XStatusTypeDef LSM6DSV16X::FIFO_Get_G_Axes(int32_t *AngularVelocity) { lsm6dsv16x_axis3bit16_t data_raw; float sensitivity = 0.0f; @@ -2571,7 +2573,7 @@ LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::FIFO_Get_G_Axes(int32_t *AngularVeloci * @param Bdr FIFO gyro BDR value * @retval 0 in case of success, an error code otherwise */ -LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::FIFO_Set_G_BDR(float Bdr) +LSM6DSV16XStatusTypeDef LSM6DSV16X::FIFO_Set_G_BDR(float Bdr) { lsm6dsv16x_fifo_gy_batch_t new_bdr; @@ -2596,7 +2598,7 @@ LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::FIFO_Set_G_BDR(float Bdr) * @brief Enable the LSM6DSV16X gyroscope sensor * @retval 0 in case of success, an error code otherwise */ -LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Enable_G() +LSM6DSV16XStatusTypeDef LSM6DSV16X::Enable_G() { /* Check if the component is already enabled */ if (gyro_is_enabled == 1U) { @@ -2617,7 +2619,7 @@ LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Enable_G() * @brief Disable the LSM6DSV16X gyroscope sensor * @retval 0 in case of success, an error code otherwise */ -LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Disable_G() +LSM6DSV16XStatusTypeDef LSM6DSV16X::Disable_G() { /* Check if the component is already disabled */ if (gyro_is_enabled == 0U) { @@ -2644,7 +2646,7 @@ LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Disable_G() * @param Sensitivity pointer * @retval 0 in case of success, an error code otherwise */ -LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Get_G_Sensitivity(float *Sensitivity) +LSM6DSV16XStatusTypeDef LSM6DSV16X::Get_G_Sensitivity(float *Sensitivity) { LSM6DSV16XStatusTypeDef ret = LSM6DSV16X_OK; lsm6dsv16x_gy_full_scale_t full_scale; @@ -2693,7 +2695,7 @@ LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Get_G_Sensitivity(float *Sensitivity) * @param Odr pointer where the output data rate is written * @retval 0 in case of success, an error code otherwise */ -LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Get_G_ODR(float *Odr) +LSM6DSV16XStatusTypeDef LSM6DSV16X::Get_G_ODR(float *Odr) { LSM6DSV16XStatusTypeDef ret = LSM6DSV16X_OK; lsm6dsv16x_gy_data_rate_t odr_low_level; @@ -2766,7 +2768,7 @@ LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Get_G_ODR(float *Odr) * @param Mode the gyroscope operating mode * @retval 0 in case of success, an error code otherwise */ -LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Set_G_ODR(float Odr, LSM6DSV16X_GYRO_Operating_Mode_t Mode) +LSM6DSV16XStatusTypeDef LSM6DSV16X::Set_G_ODR(float Odr, LSM6DSV16X_GYRO_Operating_Mode_t Mode) { switch (Mode) { case LSM6DSV16X_GYRO_HIGH_PERFORMANCE_MODE: { @@ -2820,7 +2822,7 @@ LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Set_G_ODR(float Odr, LSM6DSV16X_GYRO_O * @param Odr the functional output data rate to be set * @retval 0 in case of success, an error code otherwise */ -LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Set_G_ODR_When_Enabled(float Odr) +LSM6DSV16XStatusTypeDef LSM6DSV16X::Set_G_ODR_When_Enabled(float Odr) { lsm6dsv16x_gy_data_rate_t new_odr; @@ -2849,7 +2851,7 @@ LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Set_G_ODR_When_Enabled(float Odr) * @param Odr the functional output data rate to be set * @retval 0 in case of success, an error code otherwise */ -LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Set_G_ODR_When_Disabled(float Odr) +LSM6DSV16XStatusTypeDef LSM6DSV16X::Set_G_ODR_When_Disabled(float Odr) { gyro_odr = (Odr <= 7.5f) ? LSM6DSV16X_GY_ODR_AT_7Hz5 : (Odr <= 15.0f) ? LSM6DSV16X_GY_ODR_AT_15Hz @@ -2871,7 +2873,7 @@ LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Set_G_ODR_When_Disabled(float Odr) * @param FullScale pointer where the full scale is written * @retval 0 in case of success, an error code otherwise */ -LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Get_G_FS(int32_t *FullScale) +LSM6DSV16XStatusTypeDef LSM6DSV16X::Get_G_FS(int32_t *FullScale) { LSM6DSV16XStatusTypeDef ret = LSM6DSV16X_OK; lsm6dsv16x_gy_full_scale_t fs_low_level; @@ -2919,7 +2921,7 @@ LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Get_G_FS(int32_t *FullScale) * @param FullScale the functional full scale to be set * @retval 0 in case of success, an error code otherwise */ -LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Set_G_FS(int32_t FullScale) +LSM6DSV16XStatusTypeDef LSM6DSV16X::Set_G_FS(int32_t FullScale) { lsm6dsv16x_gy_full_scale_t new_fs; @@ -2942,7 +2944,7 @@ LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Set_G_FS(int32_t FullScale) * @param Value pointer where the raw values of the axes are written * @retval 0 in case of success, an error code otherwise */ -LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Get_G_AxesRaw(int16_t *Value) +LSM6DSV16XStatusTypeDef LSM6DSV16X::Get_G_AxesRaw(int16_t *Value) { lsm6dsv16x_axis3bit16_t data_raw; @@ -2964,7 +2966,7 @@ LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Get_G_AxesRaw(int16_t *Value) * @param AngularRate pointer where the values of the axes are written * @retval 0 in case of success, an error code otherwise */ -LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Get_G_Axes(int32_t *AngularRate) +LSM6DSV16XStatusTypeDef LSM6DSV16X::Get_G_Axes(int32_t *AngularRate) { lsm6dsv16x_axis3bit16_t data_raw; float sensitivity; @@ -2992,7 +2994,7 @@ LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Get_G_Axes(int32_t *AngularRate) * @param Status the status of data ready bit * @retval 0 in case of success, an error code otherwise */ -LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Get_G_DRDY_Status(uint8_t *Status) +LSM6DSV16XStatusTypeDef LSM6DSV16X::Get_G_DRDY_Status(uint8_t *Status) { lsm6dsv16x_all_sources_t val; @@ -3009,7 +3011,7 @@ LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Get_G_DRDY_Status(uint8_t *Status) * @param PowerMode Value of the powerMode * @retval 0 in case of success, an error code otherwise */ -LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Set_G_Power_Mode(uint8_t PowerMode) +LSM6DSV16XStatusTypeDef LSM6DSV16X::Set_G_Power_Mode(uint8_t PowerMode) { if (lsm6dsv16x_gy_mode_set(®_ctx, (lsm6dsv16x_gy_mode_t)PowerMode) != LSM6DSV16X_OK) { return LSM6DSV16X_ERROR; @@ -3024,7 +3026,7 @@ LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Set_G_Power_Mode(uint8_t PowerMode) * @param FilterMode Value of the filter Mode * @retval 0 in case of success, an error code otherwise */ -LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Set_G_Filter_Mode(uint8_t LowHighPassFlag, uint8_t FilterMode) +LSM6DSV16XStatusTypeDef LSM6DSV16X::Set_G_Filter_Mode(uint8_t LowHighPassFlag, uint8_t FilterMode) { if (LowHighPassFlag == 0) { /*Set gyroscope low_pass 1 filter-mode*/ @@ -3053,7 +3055,7 @@ LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Set_G_Filter_Mode(uint8_t LowHighPassF * @brief Enable the LSM6DSV16X QVAR sensor * @retval 0 in case of success, an error code otherwise */ -LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::QVAR_Enable() +LSM6DSV16XStatusTypeDef LSM6DSV16X::QVAR_Enable() { lsm6dsv16x_ctrl7_t ctrl7; @@ -3077,7 +3079,7 @@ LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::QVAR_Enable() * @param Data pointer where the value is written * @retval 0 in case of success, an error code otherwise */ -LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::QVAR_GetData(float *Data) +LSM6DSV16XStatusTypeDef LSM6DSV16X::QVAR_GetData(float *Data) { lsm6dsv16x_axis1bit16_t data_raw; (void)memset(data_raw.u8bit, 0x00, sizeof(int16_t)); @@ -3096,7 +3098,7 @@ LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::QVAR_GetData(float *Data) * @param val impedance in MOhm (2400MOhm, 730MOhm, 300MOhm, 255MOhm) * @retval 0 in case of success, an error code otherwise */ -LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::QVAR_SetImpedance(uint16_t val) +LSM6DSV16XStatusTypeDef LSM6DSV16X::QVAR_SetImpedance(uint16_t val) { LSM6DSV16XStatusTypeDef ret = LSM6DSV16X_OK; lsm6dsv16x_ah_qvar_zin_t imp; @@ -3131,7 +3133,7 @@ LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::QVAR_SetImpedance(uint16_t val) * @retval 0 in case of success, an error code otherwise */ -LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::QVAR_GetStatus(uint8_t *val) +LSM6DSV16XStatusTypeDef LSM6DSV16X::QVAR_GetStatus(uint8_t *val) { lsm6dsv16x_status_reg_t status; @@ -3149,7 +3151,7 @@ LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::QVAR_GetStatus(uint8_t *val) * @param status pointer where the MLC status is written * @retval 0 in case of success, an error code otherwise */ -LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Get_MLC_Status(lsm6dsv16x_mlc_status_mainpage_t *status) +LSM6DSV16XStatusTypeDef LSM6DSV16X::Get_MLC_Status(lsm6dsv16x_mlc_status_mainpage_t *status) { if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_MLC_STATUS_MAINPAGE, (uint8_t *)status, 1) != LSM6DSV16X_OK) { return LSM6DSV16X_ERROR; @@ -3163,7 +3165,7 @@ LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Get_MLC_Status(lsm6dsv16x_mlc_status_m * @param output pointer where the MLC output is written * @retval 0 in case of success, an error code otherwise */ -LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Get_MLC_Output(lsm6dsv16x_mlc_out_t *output) +LSM6DSV16XStatusTypeDef LSM6DSV16X::Get_MLC_Output(lsm6dsv16x_mlc_out_t *output) { if (lsm6dsv16x_mlc_out_get(®_ctx, output) != LSM6DSV16X_OK) { return LSM6DSV16X_ERROR; @@ -3178,7 +3180,7 @@ LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Get_MLC_Output(lsm6dsv16x_mlc_out_t *o * @param Data pointer where the value is written * @retval 0 in case of success, an error code otherwise */ -LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Read_Reg(uint8_t Reg, uint8_t *Data) +LSM6DSV16XStatusTypeDef LSM6DSV16X::Read_Reg(uint8_t Reg, uint8_t *Data) { if (lsm6dsv16x_read_reg(®_ctx, Reg, Data, 1) != LSM6DSV16X_OK) { return LSM6DSV16X_ERROR; @@ -3193,7 +3195,7 @@ LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Read_Reg(uint8_t Reg, uint8_t *Data) * @param Data value to be written * @retval 0 in case of success, an error code otherwise */ -LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Write_Reg(uint8_t Reg, uint8_t Data) +LSM6DSV16XStatusTypeDef LSM6DSV16X::Write_Reg(uint8_t Reg, uint8_t Data) { if (lsm6dsv16x_write_reg(®_ctx, Reg, &Data, 1) != LSM6DSV16X_OK) { return LSM6DSV16X_ERROR; @@ -3204,10 +3206,10 @@ LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::Write_Reg(uint8_t Reg, uint8_t Data) int32_t LSM6DSV16X_io_write(void *handle, uint8_t WriteAddr, uint8_t *pBuffer, uint16_t nBytesToWrite) { - return ((LSM6DSV16XSensor *)handle)->IO_Write(pBuffer, WriteAddr, nBytesToWrite); + return ((LSM6DSV16X *)handle)->IO_Write(pBuffer, WriteAddr, nBytesToWrite); } int32_t LSM6DSV16X_io_read(void *handle, uint8_t ReadAddr, uint8_t *pBuffer, uint16_t nBytesToRead) { - return ((LSM6DSV16XSensor *)handle)->IO_Read(pBuffer, ReadAddr, nBytesToRead); + return ((LSM6DSV16X *)handle)->IO_Read(pBuffer, ReadAddr, nBytesToRead); } diff --git a/lib/LSM6DSV16X/LSM6DSV16XSensor.h b/lib/LSM6DSV16X/LSM6DSV16X.h similarity index 97% rename from lib/LSM6DSV16X/LSM6DSV16XSensor.h rename to lib/LSM6DSV16X/LSM6DSV16X.h index b48c8cd04..26ab1a665 100644 --- a/lib/LSM6DSV16X/LSM6DSV16XSensor.h +++ b/lib/LSM6DSV16X/LSM6DSV16X.h @@ -1,6 +1,6 @@ /** ****************************************************************************** - * @file LSM6DSV16XSensor.h + * @file LSM6DSV16X.h * @author SRA * @version V1.5.1 * @date July 2022 @@ -38,8 +38,8 @@ /* Prevent recursive inclusion -----------------------------------------------*/ -#ifndef __LSM6DSV16XSensor_H__ -#define __LSM6DSV16XSensor_H__ +#ifndef __LSM6DSV16X_H__ +#define __LSM6DSV16X_H__ /* Includes ------------------------------------------------------------------*/ @@ -120,10 +120,10 @@ typedef enum { /** * Abstract class of a LSM6DSV16X sensor. */ -class LSM6DSV16XSensor { +class LSM6DSV16X { public: - LSM6DSV16XSensor(TwoWire *i2c, uint8_t address = LSM6DSV16X_I2C_ADD_H); - LSM6DSV16XSensor(SPIClass *spi, int cs_pin, uint32_t spi_speed = 2000000); + LSM6DSV16X(TwoWire *i2c, uint8_t address = LSM6DSV16X_I2C_ADD_H); + LSM6DSV16X(SPIClass *spi, int cs_pin, uint32_t spi_speed = 2000000); LSM6DSV16XStatusTypeDef begin(); LSM6DSV16XStatusTypeDef end(); @@ -341,4 +341,4 @@ int32_t LSM6DSV16X_io_read(void *handle, uint8_t ReadAddr, uint8_t *pBuffer, uin } #endif -#endif /* __LSM6DSV16XSensor_H__ */ +#endif /* __LSM6DSV16X_H__ */ diff --git a/src/sensors/lsmdsv16x.cpp b/src/sensors/lsmdsv16xsensor.cpp similarity index 99% rename from src/sensors/lsmdsv16x.cpp rename to src/sensors/lsmdsv16xsensor.cpp index c8a0cb782..4504f0f59 100644 --- a/src/sensors/lsmdsv16x.cpp +++ b/src/sensors/lsmdsv16xsensor.cpp @@ -21,19 +21,22 @@ THE SOFTWARE. */ -#include "sensors/lsmdsv16x.h" +#include "sensors/lsmdsv16xsensor.h" #include "utils.h" #include "GlobalVars.h" volatile bool imuEvent = false; +void interruptHandler() { + imuEvent = true; +} + void LSMDSV16XSensor::motionSetup() { #ifdef DEBUG_SENSOR //TODO: Can anything go here #endif errorCounter = 0; //Either subtract to the error counter or handle the error - imu = LSM6DSV16XSensor(&Wire, addr); if(imu.begin() == LSM6DSV16X_ERROR) { m_Logger.fatal("Can't connect to %s at address 0x%02x", getIMUNameByType(sensorType), addr); ledManager.pattern(50, 50, 200); @@ -86,10 +89,6 @@ void LSMDSV16XSensor::motionSetup() configured = true; } -void interruptHandler() { - imuEvent = true; -} - void LSMDSV16XSensor::motionLoop() { if (imuEvent) { diff --git a/src/sensors/lsmdsv16x.h b/src/sensors/lsmdsv16xsensor.h similarity index 94% rename from src/sensors/lsmdsv16x.h rename to src/sensors/lsmdsv16xsensor.h index b749fbf5e..741c9d96f 100644 --- a/src/sensors/lsmdsv16x.h +++ b/src/sensors/lsmdsv16xsensor.h @@ -25,13 +25,15 @@ #define SENSORS_LSMDSV16X_H #include "sensor.h" -#include +#include class LSMDSV16XSensor : public Sensor { public: LSMDSV16XSensor(uint8_t id, uint8_t type, uint8_t address, float rotation, uint8_t sclPin, uint8_t sdaPin, uint8_t intPin) - : Sensor("LSM6DSV16XSensor", type, id, address, rotation, sclPin, sdaPin), m_IntPin(intPin) {}; + : Sensor("LSM6DSV16XSensor", type, id, address, rotation, sclPin, sdaPin), + imu(&Wire, addr), + m_IntPin(intPin) {}; ~LSMDSV16XSensor(){}; void motionSetup() override final; void postSetup() override { @@ -47,7 +49,7 @@ class LSMDSV16XSensor : public Sensor //void interruptHandler(); //volatile bool imuEvent; //the interrupt cant be a class function - LSM6DSV16XSensor imu; + LSM6DSV16X imu; uint8_t m_IntPin; uint8_t errorCounter = 0; //Error is -1, OK is 0 uint8_t tap = 0; From d21463e0dc7dd0e94228e9b55326426246f01288 Mon Sep 17 00:00:00 2001 From: Carter Meyers Date: Tue, 1 Aug 2023 18:11:12 -0700 Subject: [PATCH 04/60] Added fusedRotation math --- src/sensors/lsmdsv16xsensor.cpp | 106 ++++++++++++++++++-------------- src/sensors/lsmdsv16xsensor.h | 4 ++ 2 files changed, 65 insertions(+), 45 deletions(-) diff --git a/src/sensors/lsmdsv16xsensor.cpp b/src/sensors/lsmdsv16xsensor.cpp index 4504f0f59..dd8e24726 100644 --- a/src/sensors/lsmdsv16xsensor.cpp +++ b/src/sensors/lsmdsv16xsensor.cpp @@ -27,6 +27,9 @@ volatile bool imuEvent = false; +//Maybe this should go into the math lib +float convertBytesToFloat(uint8_t dataLow, uint8_t dataHigh); + void interruptHandler() { imuEvent = true; } @@ -34,7 +37,7 @@ void interruptHandler() { void LSMDSV16XSensor::motionSetup() { #ifdef DEBUG_SENSOR - //TODO: Can anything go here + //TODO: Should anything go here #endif errorCounter = 0; //Either subtract to the error counter or handle the error if(imu.begin() == LSM6DSV16X_ERROR) { @@ -49,8 +52,8 @@ void LSMDSV16XSensor::motionSetup() return; } - if (deviceId != 0x00) { //TODO: Look up device ID - m_Logger.fatal("The IMU returned an invalid device ID of: 0x%02x when it should have been: 0x%02x", deviceId, 0x00); + if (deviceId != 0x6B) { + m_Logger.fatal("The IMU returned an invalid device ID of: 0x%02x when it should have been: 0x%02x", deviceId, 0x6B); ledManager.pattern(50, 50, 200); return; } @@ -83,6 +86,14 @@ void LSMDSV16XSensor::motionSetup() return; } + //errorCounter -= imu.Set_G_Filter_Mode(0, 0) //Look up filter setup + //errorCounter -= imu.Set_G_FS(8LSM6DSV16X_ACC_SENSITIVITY_FS_8G); + //errorCounter -= imu.Set_G_ODR(120.0F, LSM6DSV16X_GYRO_HIGH_ACCURACY_MODE) //High accuracy mode is not implemented + + //errorCounter -= imu.Set_X_Filter_Mode(0, 0) //Look up filter setup + //errorCounter -= imu.Set_X_FS(8LSM6DSV16X_ACC_SENSITIVITY_FS_8G); + //errorCounter -= imu.Set_X_ODR(120.0F, LSM6DSV16X_ACC_HIGH_ACCURACY_MODE) //High accuracy mode is not implemented + lastReset = 0; lastData = millis(); working = true; @@ -109,59 +120,46 @@ void LSMDSV16XSensor::motionLoop() int16_t accelerometer[3]; int16_t gyroscope[3]; - errorCounter += imu.Get_X_AxesRaw(accelerometer); - errorCounter += imu.Get_G_AxesRaw(gyroscope); + errorCounter -= imu.Get_X_AxesRaw(accelerometer); + errorCounter -= imu.Get_G_AxesRaw(gyroscope); networkConnection.sendInspectionRawIMUData(sensorId, gyroscope[0], gyroscope[1], gyroscope[2], 0, accelerometer[0], accelerometer[1], accelerometer[2], 0, 0, 0, 0, 0); } #endif - uint8_t xl = 0; - uint8_t xh = 0; - uint8_t yl = 0; - uint8_t yh = 0; - uint8_t zl = 0; - uint8_t zh = 0; - - imu.Get_6D_Orientation_XL(&xl); - imu.Get_6D_Orientation_XH(&xh); - imu.Get_6D_Orientation_YL(&yl); - imu.Get_6D_Orientation_YH(&yh); - imu.Get_6D_Orientation_ZL(&zl); - imu.Get_6D_Orientation_ZH(&zh); + uint8_t dataLow = 0; + uint8_t dataHigh = 0; + errorCounter -= imu.Get_6D_Orientation_XL(&dataLow); + errorCounter -= imu.Get_6D_Orientation_XH(&dataHigh); + fusedRotation.x = convertBytesToFloat(dataLow, dataHigh); - lastReset = 0; - lastData = millis(); - - - //We have imu position (x,y,z) in high/low registers, we need to convert this to a float16 and then to a quaternion + errorCounter -= imu.Get_6D_Orientation_YL(&dataLow); + errorCounter -= imu.Get_6D_Orientation_YH(&dataHigh); + fusedRotation.y = convertBytesToFloat(dataLow, dataHigh); + errorCounter -= imu.Get_6D_Orientation_ZL(&dataLow); + errorCounter -= imu.Get_6D_Orientation_ZH(&dataHigh); + fusedRotation.z = convertBytesToFloat(dataLow, dataHigh); - /* bno code - if (imu.hasNewGameQuat()) // New quaternion if context - { - imu.getGameQuat(fusedRotation.x, fusedRotation.y, fusedRotation.z, fusedRotation.w, calibrationAccuracy); - fusedRotation *= sensorOffset; + fusedRotation.w = sqrtf(1.0F - sq(fusedRotation.x) - sq(fusedRotation.y) - sq(fusedRotation.z)); - if (ENABLE_INSPECTION || !OPTIMIZE_UPDATES || !lastFusedRotationSent.equalsWithEpsilon(fusedRotation)) - { - newFusedRotation = true; - lastFusedRotationSent = fusedRotation; - } - // Leave new quaternion if context open, it's closed later + lastReset = 0; + lastData = millis(); - // Continuation of the new quaternion if context, used for both 6 and 9 axis - } // Closing new quaternion if context - */ + if (ENABLE_INSPECTION || !OPTIMIZE_UPDATES || !lastFusedRotationSent.equalsWithEpsilon(fusedRotation)) + { + newFusedRotation = true; + lastFusedRotationSent = fusedRotation; + } #if SEND_ACCELERATION { int32_t accelerometerInt[3]; errorCounter -= imu.Get_X_Axes(accelerometerInt); - acceleration[0] = accelerometerInt[0] * 1000.0; //convert from mg to g - acceleration[1] = accelerometerInt[1] * 1000.0; - acceleration[2] = accelerometerInt[2] * 1000.0; + acceleration[0] = accelerometerInt[0] * 1000.0F; //convert from mg to g + acceleration[1] = accelerometerInt[1] * 1000.0F; + acceleration[2] = accelerometerInt[2] * 1000.0F; newAcceleration = true; } #endif // SEND_ACCELERATION @@ -169,8 +167,6 @@ void LSMDSV16XSensor::motionLoop() - - if ((lastData + 1000 < millis() || errorCounter) && configured) //Errors { if (errorCounter) { @@ -190,12 +186,13 @@ void LSMDSV16XSensor::motionLoop() } SensorStatus LSMDSV16XSensor::getSensorState() { + //TODO: this may need to be redone, errorCounter gets reset at the end of the loop return errorCounter > 0 ? SensorStatus::SENSOR_ERROR : isWorking() ? SensorStatus::SENSOR_OK : SensorStatus::SENSOR_OFFLINE; } void LSMDSV16XSensor::sendData() { - if (newFusedRotation) //IDK how to get + if (newFusedRotation) { newFusedRotation = false; networkConnection.sendRotationData(sensorId, &fusedRotation, DATA_TYPE_NORMAL, calibrationAccuracy); @@ -209,7 +206,7 @@ void LSMDSV16XSensor::sendData() if(newAcceleration) //Returns acceleration in G's { newAcceleration = false; - networkConnection.sendSensorAcceleration(this->sensorId, this->acceleration); + networkConnection.sendSensorAcceleration(sensorId, acceleration); } #endif @@ -222,5 +219,24 @@ void LSMDSV16XSensor::sendData() void LSMDSV16XSensor::startCalibration(int calibrationType) { - //TODO: Look up in data sheet, does not look like it is in the lib + //These IMU are factory calibrated. + //The register might be able to be changed but it could break the device + //I don't think we will need to mess with them +} + + +//Convert 2 bytes in f16 format to a float +//f16: SEEE EEDD DDDD DDDD +//f32: SEEE EEEE EDDD DDDD DDDD DDDD DDDD DDDD +float convertBytesToFloat(uint8_t dataLow, uint8_t dataHigh) { + uint16_t combinedData = ((uint16_t)dataHigh << 8 | dataLow); + uint32_t dataHolder = ((uint32_t)(combinedData & DATA_MASK_F16) << 13); + dataHolder |= ((uint32_t)(combinedData & SIGN_BIT_F16) << 16); + uint8_t exponent = ((combinedData & EXPONENT_MASK_F16) >> 10); + exponent += 112; //-15 + 127 + dataHolder |= (((uint32_t)exponent << 23)); + float dataFloat = 0.0F; + memcpy(&dataFloat, &dataHolder, 4); + // float dataFloat = reinterpret_cast(dataHolder); + return dataFloat; } diff --git a/src/sensors/lsmdsv16xsensor.h b/src/sensors/lsmdsv16xsensor.h index 741c9d96f..e7de1152d 100644 --- a/src/sensors/lsmdsv16xsensor.h +++ b/src/sensors/lsmdsv16xsensor.h @@ -27,6 +27,10 @@ #include "sensor.h" #include +#define SIGN_BIT_F16 0x8000 +#define EXPONENT_MASK_F16 0x7C00 +#define DATA_MASK_F16 0x03FF + class LSMDSV16XSensor : public Sensor { public: From 10508c9752205e170ce060c92bdab406510b265c Mon Sep 17 00:00:00 2001 From: Carter Meyers Date: Tue, 1 Aug 2023 19:44:01 -0700 Subject: [PATCH 05/60] added sensor to the sensor list and cleanup --- lib/math/customConversions.cpp | 45 +++++++++++++++++++ lib/math/customConversions.h | 43 ++++++++++++++++++ src/consts.h | 1 + src/sensors/SensorManager.cpp | 5 +++ ...mdsv16xsensor.cpp => lsm6dsv16xsensor.cpp} | 43 +++++------------- .../{lsmdsv16xsensor.h => lsm6dsv16xsensor.h} | 16 +++---- src/sensors/sensor.cpp | 2 + src/sensors/sensoraddresses.h | 6 +++ 8 files changed, 120 insertions(+), 41 deletions(-) create mode 100644 lib/math/customConversions.cpp create mode 100644 lib/math/customConversions.h rename src/sensors/{lsmdsv16xsensor.cpp => lsm6dsv16xsensor.cpp} (85%) rename src/sensors/{lsmdsv16xsensor.h => lsm6dsv16xsensor.h} (83%) diff --git a/lib/math/customConversions.cpp b/lib/math/customConversions.cpp new file mode 100644 index 000000000..925e6e11b --- /dev/null +++ b/lib/math/customConversions.cpp @@ -0,0 +1,45 @@ +/* + SlimeVR Code is placed under the MIT license + Copyright (c) 2023 Eiren Rain & SlimeVR contributors + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. +*/ + +#include "customConversions.h" + +//Convert 2 bytes in f16 format to a float +//f16: SEEE EEDD DDDD DDDD +//f32: SEEE EEEE EDDD DDDD DDDD DDDD DDDD DDDD +float Conversions::convertBytesToFloat(uint8_t dataLow, uint8_t dataHigh) { + uint16_t combinedData = ((uint16_t)dataHigh << 8 | dataLow); + uint32_t dataHolder = ((uint32_t)(combinedData & DATA_MASK_F16) << 13); + dataHolder |= ((uint32_t)(combinedData & SIGN_BIT_F16) << 16); + uint8_t exponent = ((combinedData & EXPONENT_MASK_F16) >> 10); + if (!exponent) //subnormal + return 0.0F; + if (exponent == 0b00011111) //nan + exponent == 0xff; + else + exponent += 127 - 15; + dataHolder |= (((uint32_t)exponent << 23)); + float dataFloat = 0.0F; + memcpy(&dataFloat, &dataHolder, 4); + // float dataFloat = reinterpret_cast(dataHolder); + return dataFloat; +} \ No newline at end of file diff --git a/lib/math/customConversions.h b/lib/math/customConversions.h new file mode 100644 index 000000000..9eae2b7ef --- /dev/null +++ b/lib/math/customConversions.h @@ -0,0 +1,43 @@ +/* + SlimeVR Code is placed under the MIT license + Copyright (c) 2023 Eiren Rain & SlimeVR contributors + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. +*/ + +#include "Arduino.h" + +#define SIGN_BIT_F16 0x8000 +#define EXPONENT_MASK_F16 0x7C00 +#define DATA_MASK_F16 0x03FF + + +#ifndef _CUSTOM_CONVERSIONS_H_ +#define _CUSTOM_CONVERSIONS_H_ + + + +namespace Conversions { + float convertBytesToFloat(uint8_t dataLow, uint8_t dataHigh); +} + + + +#endif + diff --git a/src/consts.h b/src/consts.h index f2553eebf..564f89d24 100644 --- a/src/consts.h +++ b/src/consts.h @@ -33,6 +33,7 @@ #define IMU_BNO086 7 #define IMU_BMI160 8 #define IMU_ICM20948 9 +#define IMU_LSM6DSV16X 11 #define BOARD_SLIMEVR_LEGACY 1 #define BOARD_SLIMEVR_DEV 2 diff --git a/src/sensors/SensorManager.cpp b/src/sensors/SensorManager.cpp index b3b46f0e9..658ac5d15 100644 --- a/src/sensors/SensorManager.cpp +++ b/src/sensors/SensorManager.cpp @@ -29,6 +29,7 @@ #include "mpu6050sensor.h" #include "bmi160sensor.h" #include "icm20948sensor.h" +#include "lsm6dsv16xsensor.h" #include "ErroneousSensor.h" #include "sensoraddresses.h" #include "GlobalVars.h" @@ -97,6 +98,10 @@ namespace SlimeVR case IMU_ICM20948: sensor = new ICM20948Sensor(sensorID, address, rotation, sclPin, sdaPin); break; + case IMU_LSM6DSV16X: + uint8_t intPin = extraParam; + sensor = new LSM6DSV16XSensor(sensorID, imuType, address, rotation, sclPin, sdaPin, intPin); + break; default: sensor = new ErroneousSensor(sensorID, imuType); break; diff --git a/src/sensors/lsmdsv16xsensor.cpp b/src/sensors/lsm6dsv16xsensor.cpp similarity index 85% rename from src/sensors/lsmdsv16xsensor.cpp rename to src/sensors/lsm6dsv16xsensor.cpp index dd8e24726..6a472d501 100644 --- a/src/sensors/lsmdsv16xsensor.cpp +++ b/src/sensors/lsm6dsv16xsensor.cpp @@ -21,20 +21,19 @@ THE SOFTWARE. */ -#include "sensors/lsmdsv16xsensor.h" +#include "sensors/lsm6dsv16xsensor.h" #include "utils.h" +#include "customConversions.h" #include "GlobalVars.h" -volatile bool imuEvent = false; -//Maybe this should go into the math lib -float convertBytesToFloat(uint8_t dataLow, uint8_t dataHigh); +volatile bool imuEvent = false; void interruptHandler() { imuEvent = true; } -void LSMDSV16XSensor::motionSetup() +void LSM6DSV16XSensor::motionSetup() { #ifdef DEBUG_SENSOR //TODO: Should anything go here @@ -87,7 +86,7 @@ void LSMDSV16XSensor::motionSetup() } //errorCounter -= imu.Set_G_Filter_Mode(0, 0) //Look up filter setup - //errorCounter -= imu.Set_G_FS(8LSM6DSV16X_ACC_SENSITIVITY_FS_8G); + //errorCounter -= imu.Set_G_FS(2000dps); //errorCounter -= imu.Set_G_ODR(120.0F, LSM6DSV16X_GYRO_HIGH_ACCURACY_MODE) //High accuracy mode is not implemented //errorCounter -= imu.Set_X_Filter_Mode(0, 0) //Look up filter setup @@ -100,7 +99,7 @@ void LSMDSV16XSensor::motionSetup() configured = true; } -void LSMDSV16XSensor::motionLoop() +void LSM6DSV16XSensor::motionLoop() { if (imuEvent) { imuEvent = false; @@ -126,20 +125,19 @@ void LSMDSV16XSensor::motionLoop() networkConnection.sendInspectionRawIMUData(sensorId, gyroscope[0], gyroscope[1], gyroscope[2], 0, accelerometer[0], accelerometer[1], accelerometer[2], 0, 0, 0, 0, 0); } #endif - uint8_t dataLow = 0; uint8_t dataHigh = 0; errorCounter -= imu.Get_6D_Orientation_XL(&dataLow); errorCounter -= imu.Get_6D_Orientation_XH(&dataHigh); - fusedRotation.x = convertBytesToFloat(dataLow, dataHigh); + fusedRotation.x = Conversions::convertBytesToFloat(dataLow, dataHigh); errorCounter -= imu.Get_6D_Orientation_YL(&dataLow); errorCounter -= imu.Get_6D_Orientation_YH(&dataHigh); - fusedRotation.y = convertBytesToFloat(dataLow, dataHigh); + fusedRotation.y = Conversions::convertBytesToFloat(dataLow, dataHigh); errorCounter -= imu.Get_6D_Orientation_ZL(&dataLow); errorCounter -= imu.Get_6D_Orientation_ZH(&dataHigh); - fusedRotation.z = convertBytesToFloat(dataLow, dataHigh); + fusedRotation.z = Conversions::convertBytesToFloat(dataLow, dataHigh); fusedRotation.w = sqrtf(1.0F - sq(fusedRotation.x) - sq(fusedRotation.y) - sq(fusedRotation.z)); @@ -185,12 +183,12 @@ void LSMDSV16XSensor::motionLoop() } } -SensorStatus LSMDSV16XSensor::getSensorState() { +SensorStatus LSM6DSV16XSensor::getSensorState() { //TODO: this may need to be redone, errorCounter gets reset at the end of the loop return errorCounter > 0 ? SensorStatus::SENSOR_ERROR : isWorking() ? SensorStatus::SENSOR_OK : SensorStatus::SENSOR_OFFLINE; } -void LSMDSV16XSensor::sendData() +void LSM6DSV16XSensor::sendData() { if (newFusedRotation) { @@ -217,26 +215,9 @@ void LSMDSV16XSensor::sendData() } } -void LSMDSV16XSensor::startCalibration(int calibrationType) +void LSM6DSV16XSensor::startCalibration(int calibrationType) { //These IMU are factory calibrated. //The register might be able to be changed but it could break the device //I don't think we will need to mess with them } - - -//Convert 2 bytes in f16 format to a float -//f16: SEEE EEDD DDDD DDDD -//f32: SEEE EEEE EDDD DDDD DDDD DDDD DDDD DDDD -float convertBytesToFloat(uint8_t dataLow, uint8_t dataHigh) { - uint16_t combinedData = ((uint16_t)dataHigh << 8 | dataLow); - uint32_t dataHolder = ((uint32_t)(combinedData & DATA_MASK_F16) << 13); - dataHolder |= ((uint32_t)(combinedData & SIGN_BIT_F16) << 16); - uint8_t exponent = ((combinedData & EXPONENT_MASK_F16) >> 10); - exponent += 112; //-15 + 127 - dataHolder |= (((uint32_t)exponent << 23)); - float dataFloat = 0.0F; - memcpy(&dataFloat, &dataHolder, 4); - // float dataFloat = reinterpret_cast(dataHolder); - return dataFloat; -} diff --git a/src/sensors/lsmdsv16xsensor.h b/src/sensors/lsm6dsv16xsensor.h similarity index 83% rename from src/sensors/lsmdsv16xsensor.h rename to src/sensors/lsm6dsv16xsensor.h index e7de1152d..aa7f2863c 100644 --- a/src/sensors/lsmdsv16xsensor.h +++ b/src/sensors/lsm6dsv16xsensor.h @@ -1,6 +1,6 @@ /* SlimeVR Code is placed under the MIT license - Copyright (c) 2021 Eiren Rain & SlimeVR contributors + Copyright (c) 2023 Eiren Rain & SlimeVR contributors Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal @@ -21,24 +21,20 @@ THE SOFTWARE. */ -#ifndef SENSORS_LSMDSV16X_H -#define SENSORS_LSMDSV16X_H +#ifndef SENSORS_LSM6DSV16X_H +#define SENSORS_LSM6DSV16X_H #include "sensor.h" #include -#define SIGN_BIT_F16 0x8000 -#define EXPONENT_MASK_F16 0x7C00 -#define DATA_MASK_F16 0x03FF - -class LSMDSV16XSensor : public Sensor +class LSM6DSV16XSensor : public Sensor { public: - LSMDSV16XSensor(uint8_t id, uint8_t type, uint8_t address, float rotation, uint8_t sclPin, uint8_t sdaPin, uint8_t intPin) + LSM6DSV16XSensor(uint8_t id, uint8_t type, uint8_t address, float rotation, uint8_t sclPin, uint8_t sdaPin, uint8_t intPin) : Sensor("LSM6DSV16XSensor", type, id, address, rotation, sclPin, sdaPin), imu(&Wire, addr), m_IntPin(intPin) {}; - ~LSMDSV16XSensor(){}; + ~LSM6DSV16XSensor(){}; void motionSetup() override final; void postSetup() override { lastData = millis(); diff --git a/src/sensors/sensor.cpp b/src/sensors/sensor.cpp index aaad8b9b4..54f1ad893 100644 --- a/src/sensors/sensor.cpp +++ b/src/sensors/sensor.cpp @@ -75,6 +75,8 @@ const char * getIMUNameByType(int imuType) { return "BMI160"; case IMU_ICM20948: return "ICM20948"; + case IMU_LSM6DSV16X: + return "LSM6DSV16X"; } return "Unknown"; } diff --git a/src/sensors/sensoraddresses.h b/src/sensors/sensoraddresses.h index b8cb8f0f3..a603bdb3e 100644 --- a/src/sensors/sensoraddresses.h +++ b/src/sensors/sensoraddresses.h @@ -13,6 +13,9 @@ #elif IMU == IMU_MPU9250 || IMU == IMU_BMI160 || IMU == IMU_MPU6500 || IMU == IMU_MPU6050 || IMU == IMU_ICM20948 #define PRIMARY_IMU_ADDRESS_ONE 0x68 #define PRIMARY_IMU_ADDRESS_TWO 0x69 + #elif IMU == IMU_LSM6DSV16X + #define PRIMARY_IMU_ADDRESS_ONE 0x6A + #define PRIMARY_IMU_ADDRESS_TWO 0x6B #endif #if SECOND_IMU == IMU_BNO080 || SECOND_IMU == IMU_BNO085 || SECOND_IMU == IMU_BNO086 @@ -24,6 +27,9 @@ #elif SECOND_IMU == IMU_MPU9250 || SECOND_IMU == IMU_BMI160 || SECOND_IMU == IMU_MPU6500 || SECOND_IMU == IMU_MPU6050 || SECOND_IMU == IMU_ICM20948 #define SECONDARY_IMU_ADDRESS_ONE 0x68 #define SECONDARY_IMU_ADDRESS_TWO 0x69 + #elif IMU == IMU_LSM6DSV16X + #define SECONDARY_IMU_ADDRESS_ONE 0x6A + #define SECONDARY_IMU_ADDRESS_TWO 0x6B #endif #else From f3158757c6de339ab14418bd9886d318c51985cd Mon Sep 17 00:00:00 2001 From: Carter Meyers Date: Tue, 1 Aug 2023 19:52:57 -0700 Subject: [PATCH 06/60] Forgot curly brackets --- src/sensors/SensorManager.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/sensors/SensorManager.cpp b/src/sensors/SensorManager.cpp index 658ac5d15..497fbafae 100644 --- a/src/sensors/SensorManager.cpp +++ b/src/sensors/SensorManager.cpp @@ -99,8 +99,10 @@ namespace SlimeVR sensor = new ICM20948Sensor(sensorID, address, rotation, sclPin, sdaPin); break; case IMU_LSM6DSV16X: + { uint8_t intPin = extraParam; sensor = new LSM6DSV16XSensor(sensorID, imuType, address, rotation, sclPin, sdaPin, intPin); + } break; default: sensor = new ErroneousSensor(sensorID, imuType); From 0521675142c8ed4b83d16403354819d8bc4dbe1f Mon Sep 17 00:00:00 2001 From: Carter Meyers Date: Wed, 2 Aug 2023 05:55:07 -0700 Subject: [PATCH 07/60] Fix second sensor id values --- src/sensors/sensoraddresses.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/sensors/sensoraddresses.h b/src/sensors/sensoraddresses.h index a603bdb3e..f9ba44460 100644 --- a/src/sensors/sensoraddresses.h +++ b/src/sensors/sensoraddresses.h @@ -27,7 +27,7 @@ #elif SECOND_IMU == IMU_MPU9250 || SECOND_IMU == IMU_BMI160 || SECOND_IMU == IMU_MPU6500 || SECOND_IMU == IMU_MPU6050 || SECOND_IMU == IMU_ICM20948 #define SECONDARY_IMU_ADDRESS_ONE 0x68 #define SECONDARY_IMU_ADDRESS_TWO 0x69 - #elif IMU == IMU_LSM6DSV16X + #elif SECOND_IMU == IMU_LSM6DSV16X #define SECONDARY_IMU_ADDRESS_ONE 0x6A #define SECONDARY_IMU_ADDRESS_TWO 0x6B #endif From 906b9350a995c5bf34f7f5e5a11804dad992615e Mon Sep 17 00:00:00 2001 From: Carter Meyers Date: Wed, 2 Aug 2023 07:56:00 -0700 Subject: [PATCH 08/60] Update i2c address and very basic interrupt free --- src/sensors/SensorManager.cpp | 3 ++- src/sensors/lsm6dsv16xsensor.cpp | 17 ++++++++++++----- 2 files changed, 14 insertions(+), 6 deletions(-) diff --git a/src/sensors/SensorManager.cpp b/src/sensors/SensorManager.cpp index 497fbafae..ebd430b9c 100644 --- a/src/sensors/SensorManager.cpp +++ b/src/sensors/SensorManager.cpp @@ -101,7 +101,8 @@ namespace SlimeVR case IMU_LSM6DSV16X: { uint8_t intPin = extraParam; - sensor = new LSM6DSV16XSensor(sensorID, imuType, address, rotation, sclPin, sdaPin, intPin); + //We shift the address left 1 to work with the library + sensor = new LSM6DSV16XSensor(sensorID, imuType, address << 1, rotation, sclPin, sdaPin, intPin); } break; default: diff --git a/src/sensors/lsm6dsv16xsensor.cpp b/src/sensors/lsm6dsv16xsensor.cpp index 6a472d501..de172a061 100644 --- a/src/sensors/lsm6dsv16xsensor.cpp +++ b/src/sensors/lsm6dsv16xsensor.cpp @@ -26,10 +26,11 @@ #include "customConversions.h" #include "GlobalVars.h" +//#define INTERRUPTFREE //TODO: change based on int pin number (255 = interruptFree) volatile bool imuEvent = false; -void interruptHandler() { +void IRAM_ATTR interruptHandler() { imuEvent = true; } @@ -51,8 +52,8 @@ void LSM6DSV16XSensor::motionSetup() return; } - if (deviceId != 0x6B) { - m_Logger.fatal("The IMU returned an invalid device ID of: 0x%02x when it should have been: 0x%02x", deviceId, 0x6B); + if (deviceId != LSM6DSV16X_ID) { + m_Logger.fatal("The IMU returned an invalid device ID of: 0x%02x when it should have been: 0x%02x", deviceId, LSM6DSV16X_ID); ledManager.pattern(50, 50, 200); return; } @@ -71,9 +72,9 @@ void LSM6DSV16XSensor::motionSetup() return; } - +#ifndef INTERRUPTFREE attachInterrupt(m_IntPin, interruptHandler, RISING); - +#endif errorCounter -= imu.Enable_6D_Orientation(LSM6DSV16X_INT1_PIN); errorCounter -= imu.Enable_Single_Tap_Detection(LSM6DSV16X_INT1_PIN); @@ -101,6 +102,12 @@ void LSM6DSV16XSensor::motionSetup() void LSM6DSV16XSensor::motionLoop() { +#ifdef INTERRUPTFREE + //TODO: Check if data is actually availiable in the fifo instead of just hoping + if (millis() - 100 > lastData) //10Hz refresh rate TODO: make faster + imuEvent = true; +#endif + if (imuEvent) { imuEvent = false; LSM6DSV16X_Event_Status_t status; From 3555bf864ee840088ddc31a80b29412833a55d38 Mon Sep 17 00:00:00 2001 From: Gorbit99 Date: Wed, 2 Aug 2023 22:31:01 +0200 Subject: [PATCH 09/60] FIFO works, no SF data --- lib/LSM6DSV16X/LSM6DSV16X.cpp | 19 ++ lib/LSM6DSV16X/LSM6DSV16X.h | 2 + lib/LSM6DSV16X/lsm6dsv16x_reg.h | 13 ++ src/sensors/lsm6dsv16xsensor.cpp | 351 ++++++++++++++----------------- src/sensors/lsm6dsv16xsensor.h | 96 +++++---- 5 files changed, 239 insertions(+), 242 deletions(-) diff --git a/lib/LSM6DSV16X/LSM6DSV16X.cpp b/lib/LSM6DSV16X/LSM6DSV16X.cpp index 7c52a5c2f..50b4e78b9 100644 --- a/lib/LSM6DSV16X/LSM6DSV16X.cpp +++ b/lib/LSM6DSV16X/LSM6DSV16X.cpp @@ -3050,6 +3050,25 @@ LSM6DSV16XStatusTypeDef LSM6DSV16X::Set_G_Filter_Mode(uint8_t LowHighPassFlag, u return LSM6DSV16X_OK; } +LSM6DSV16XStatusTypeDef LSM6DSV16X::Set_SFLP_ODR(float odr) { + uint8_t bits = odr <= 15 ? 0b000 + : odr <= 30 ? 0b001 + : odr <= 60 ? 0b010 + : odr <= 120 ? 0b011 + : odr <= 240 ? 0b100 + : 0b101; + + lsm6dsv16x_sflp_odr_t value; + value.not_used0 = 0b011; + value.sflp_game_odr = bits; + value.not_used1 = 0b01; + + if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_SFLP_ODR, (uint8_t *)&value, sizeof(value)) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + return LSM6DSV16X_OK; +} + /** * @brief Enable the LSM6DSV16X QVAR sensor diff --git a/lib/LSM6DSV16X/LSM6DSV16X.h b/lib/LSM6DSV16X/LSM6DSV16X.h index 26ab1a665..bbafd940e 100644 --- a/lib/LSM6DSV16X/LSM6DSV16X.h +++ b/lib/LSM6DSV16X/LSM6DSV16X.h @@ -156,6 +156,8 @@ class LSM6DSV16X { LSM6DSV16XStatusTypeDef Set_G_Power_Mode(uint8_t PowerMode); LSM6DSV16XStatusTypeDef Set_G_Filter_Mode(uint8_t LowHighPassFlag, uint8_t FilterMode); + LSM6DSV16XStatusTypeDef Set_SFLP_ODR(float Odr); + LSM6DSV16XStatusTypeDef Enable_6D_Orientation(LSM6DSV16X_SensorIntPin_t IntPin); LSM6DSV16XStatusTypeDef Disable_6D_Orientation(); LSM6DSV16XStatusTypeDef Set_6D_Orientation_Threshold(uint8_t Threshold); diff --git a/lib/LSM6DSV16X/lsm6dsv16x_reg.h b/lib/LSM6DSV16X/lsm6dsv16x_reg.h index 37ec2ae9d..6f5077309 100644 --- a/lib/LSM6DSV16X/lsm6dsv16x_reg.h +++ b/lib/LSM6DSV16X/lsm6dsv16x_reg.h @@ -2394,6 +2394,19 @@ typedef struct { #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_fsm_outs8_t; +#define LSM6DSV16X_SFLP_ODR 0x5EU +typedef struct { +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t not_used0 : 3; + uint8_t sflp_game_odr : 3; + uint8_t not_used1: 2; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t not_used1: 2; + uint8_t sflp_game_odr : 3; + uint8_t not_used0 : 3; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_sflp_odr_t; + #define LSM6DSV16X_FSM_ODR 0x5FU typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN diff --git a/src/sensors/lsm6dsv16xsensor.cpp b/src/sensors/lsm6dsv16xsensor.cpp index de172a061..7fbcdd178 100644 --- a/src/sensors/lsm6dsv16xsensor.cpp +++ b/src/sensors/lsm6dsv16xsensor.cpp @@ -1,230 +1,187 @@ /* - SlimeVR Code is placed under the MIT license - Copyright (c) 2023 Eiren Rain & SlimeVR contributors - - Permission is hereby granted, free of charge, to any person obtaining a copy - of this software and associated documentation files (the "Software"), to deal - in the Software without restriction, including without limitation the rights - to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - copies of the Software, and to permit persons to whom the Software is - furnished to do so, subject to the following conditions: - - The above copyright notice and this permission notice shall be included in - all copies or substantial portions of the Software. - - THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - THE SOFTWARE. + SlimeVR Code is placed under the MIT license + Copyright (c) 2023 Eiren Rain & SlimeVR contributors + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. */ #include "sensors/lsm6dsv16xsensor.h" -#include "utils.h" -#include "customConversions.h" + #include "GlobalVars.h" +#include "customConversions.h" +#include "utils.h" + +#define INTERRUPTFREE // TODO: change based on int pin number (255 = interruptFree) +#define DEBUG_SENSOR -//#define INTERRUPTFREE //TODO: change based on int pin number (255 = interruptFree) +constexpr uint8_t SFLP_GAME_EN_BIT = 1 << 1; volatile bool imuEvent = false; -void IRAM_ATTR interruptHandler() { - imuEvent = true; -} +void IRAM_ATTR interruptHandler() { imuEvent = true; } -void LSM6DSV16XSensor::motionSetup() -{ +void LSM6DSV16XSensor::motionSetup() { #ifdef DEBUG_SENSOR - //TODO: Should anything go here + // TODO: Should anything go here #endif - errorCounter = 0; //Either subtract to the error counter or handle the error - if(imu.begin() == LSM6DSV16X_ERROR) { - m_Logger.fatal("Can't connect to %s at address 0x%02x", getIMUNameByType(sensorType), addr); - ledManager.pattern(50, 50, 200); - return; - } - uint8_t deviceId = 0; - if(imu.ReadID(&deviceId) == LSM6DSV16X_ERROR) { - m_Logger.fatal("The IMU returned an error when reading the device ID of: 0x%02x", deviceId); - ledManager.pattern(50, 50, 200); - return; - } - - if (deviceId != LSM6DSV16X_ID) { - m_Logger.fatal("The IMU returned an invalid device ID of: 0x%02x when it should have been: 0x%02x", deviceId, LSM6DSV16X_ID); - ledManager.pattern(50, 50, 200); - return; - } - - m_Logger.info("Connected to %s on 0x%02x. ", getIMUNameByType(sensorType), addr); - - if(imu.Enable_X() == LSM6DSV16X_ERROR) { //accel - m_Logger.fatal("Error enabling accelerometer on %s at address 0x%02x", getIMUNameByType(sensorType), addr); - ledManager.pattern(50, 50, 200); - return; - } - - if(imu.Enable_G() == LSM6DSV16X_ERROR) { //gyro - m_Logger.fatal("Error enabling gyroscope on %s at address 0x%02x", getIMUNameByType(sensorType), addr); - ledManager.pattern(50, 50, 200); - return; - } + errorCounter = 0; // Either subtract to the error counter or handle the error + if (imu.begin() == LSM6DSV16X_ERROR) { + m_Logger.fatal( + "Can't connect to %s at address 0x%02x", + getIMUNameByType(sensorType), + addr + ); + ledManager.pattern(50, 50, 200); + return; + } + uint8_t deviceId = 0; + if (imu.ReadID(&deviceId) == LSM6DSV16X_ERROR) { + m_Logger.fatal( + "The IMU returned an error when reading the device ID of: 0x%02x", + deviceId + ); + ledManager.pattern(50, 50, 200); + return; + } + + if (deviceId != LSM6DSV16X_ID) { + m_Logger.fatal( + "The IMU returned an invalid device ID of: 0x%02x when it should have " + "been: 0x%02x", + deviceId, + LSM6DSV16X_ID + ); + ledManager.pattern(50, 50, 200); + return; + } + + m_Logger.info("Connected to %s on 0x%02x. ", getIMUNameByType(sensorType), addr); + + uint8_t status = 0; + + // Enable accelerometer + status |= imu.Enable_X(); + // Enable gyro + status |= imu.Enable_G(); + // Enable only low power fusion + status |= imu.Write_Reg(LSM6DSV16X_EMB_FUNC_EN_A, 0b00000010); + status |= imu.Set_X_ODR(120); + status |= imu.Set_G_ODR(120); + // Stream (in datasheet: continuous) mode discards old data as new comes in + status |= imu.FIFO_Set_Mode(LSM6DSV16X_STREAM_MODE); + status |= imu.Set_SFLP_ODR(120); + status |= imu.Write_Reg(LSM6DSV16X_EMB_FUNC_INIT_A, 0b00000010); #ifndef INTERRUPTFREE - attachInterrupt(m_IntPin, interruptHandler, RISING); -#endif - - errorCounter -= imu.Enable_6D_Orientation(LSM6DSV16X_INT1_PIN); - errorCounter -= imu.Enable_Single_Tap_Detection(LSM6DSV16X_INT1_PIN); - errorCounter -= imu.Enable_Double_Tap_Detection(LSM6DSV16X_INT1_PIN); - - if (errorCounter) { - m_Logger.fatal("%d Error(s) occured enabling imu features on %s at address 0x%02x", errorCounter, getIMUNameByType(sensorType), addr); - ledManager.pattern(50, 50, 200); - return; - } - - //errorCounter -= imu.Set_G_Filter_Mode(0, 0) //Look up filter setup - //errorCounter -= imu.Set_G_FS(2000dps); - //errorCounter -= imu.Set_G_ODR(120.0F, LSM6DSV16X_GYRO_HIGH_ACCURACY_MODE) //High accuracy mode is not implemented - - //errorCounter -= imu.Set_X_Filter_Mode(0, 0) //Look up filter setup - //errorCounter -= imu.Set_X_FS(8LSM6DSV16X_ACC_SENSITIVITY_FS_8G); - //errorCounter -= imu.Set_X_ODR(120.0F, LSM6DSV16X_ACC_HIGH_ACCURACY_MODE) //High accuracy mode is not implemented + attachInterrupt(m_IntPin, interruptHandler, RISING); - lastReset = 0; - lastData = millis(); - working = true; - configured = true; -} - -void LSM6DSV16XSensor::motionLoop() -{ -#ifdef INTERRUPTFREE - //TODO: Check if data is actually availiable in the fifo instead of just hoping - if (millis() - 100 > lastData) //10Hz refresh rate TODO: make faster - imuEvent = true; -#endif - - if (imuEvent) { - imuEvent = false; - LSM6DSV16X_Event_Status_t status; - errorCounter -= imu.Get_X_Event_Status(&status); - - if (status.TapStatus) { - tap++; - } - if (status.DoubleTapStatus) { - tap += 2; - } - if (status.D6DOrientationStatus) { - hadData = true; -#if ENABLE_INSPECTION - { - int16_t accelerometer[3]; - int16_t gyroscope[3]; - - errorCounter -= imu.Get_X_AxesRaw(accelerometer); - errorCounter -= imu.Get_G_AxesRaw(gyroscope); - - networkConnection.sendInspectionRawIMUData(sensorId, gyroscope[0], gyroscope[1], gyroscope[2], 0, accelerometer[0], accelerometer[1], accelerometer[2], 0, 0, 0, 0, 0); - } + errorCounter -= imu.Enable_Single_Tap_Detection(LSM6DSV16X_INT1_PIN); + errorCounter -= imu.Enable_Double_Tap_Detection(LSM6DSV16X_INT1_PIN); #endif - uint8_t dataLow = 0; - uint8_t dataHigh = 0; - errorCounter -= imu.Get_6D_Orientation_XL(&dataLow); - errorCounter -= imu.Get_6D_Orientation_XH(&dataHigh); - fusedRotation.x = Conversions::convertBytesToFloat(dataLow, dataHigh); - - errorCounter -= imu.Get_6D_Orientation_YL(&dataLow); - errorCounter -= imu.Get_6D_Orientation_YH(&dataHigh); - fusedRotation.y = Conversions::convertBytesToFloat(dataLow, dataHigh); - - errorCounter -= imu.Get_6D_Orientation_ZL(&dataLow); - errorCounter -= imu.Get_6D_Orientation_ZH(&dataHigh); - fusedRotation.z = Conversions::convertBytesToFloat(dataLow, dataHigh); - - fusedRotation.w = sqrtf(1.0F - sq(fusedRotation.x) - sq(fusedRotation.y) - sq(fusedRotation.z)); - - lastReset = 0; - lastData = millis(); - - if (ENABLE_INSPECTION || !OPTIMIZE_UPDATES || !lastFusedRotationSent.equalsWithEpsilon(fusedRotation)) - { - newFusedRotation = true; - lastFusedRotationSent = fusedRotation; - } + if (status != LSM6DSV16X_OK) { + m_Logger.fatal( + "%d Error(s) occured enabling imu features on %s at address 0x%02x", + errorCounter, + getIMUNameByType(sensorType), + addr + ); + ledManager.pattern(50, 50, 200); + return; + } + + lastReset = 0; + lastData = millis(); + working = true; + configured = true; +} -#if SEND_ACCELERATION - { - int32_t accelerometerInt[3]; - errorCounter -= imu.Get_X_Axes(accelerometerInt); - acceleration[0] = accelerometerInt[0] * 1000.0F; //convert from mg to g - acceleration[1] = accelerometerInt[1] * 1000.0F; - acceleration[2] = accelerometerInt[2] * 1000.0F; - newAcceleration = true; - } -#endif // SEND_ACCELERATION - } - - - - if ((lastData + 1000 < millis() || errorCounter) && configured) //Errors - { - if (errorCounter) { - m_Logger.error("The %s at address 0x%02x, had %d error(s) in the motion processing loop", getIMUNameByType(sensorType), addr, errorCounter); - } - - if (lastData + 1000 < millis()) { - m_Logger.error("The %s at address 0x%02x, has not responded in the last second", getIMUNameByType(sensorType), addr); - } - - statusManager.setStatus(SlimeVR::Status::IMU_ERROR, true); - working = false; - lastData = millis(); - errorCounter = 0; - } - } +void LSM6DSV16XSensor::motionLoop() { + uint16_t fifo_samples; + if (imu.FIFO_Get_Num_Samples(&fifo_samples) == LSM6DSV16X_ERROR) { + m_Logger.error( + "Error getting number of samples in FIFO on %s at address 0x%02x", + getIMUNameByType(sensorType), + addr + ); + errorCounter++; + return; + } + + for (uint16_t i = 0; i < fifo_samples; i++) { + uint8_t tag; + if (imu.FIFO_Get_Tag(&tag) != LSM6DSV16X_OK) { + m_Logger.error( + "Failed to get FIFO data tag on %s at address 0x%02x", + getIMUNameByType(sensorType), + addr + ); + continue; + } + + if (tag == 1 || tag == 2) { + continue; + } + + m_Logger.info("Got tag %d", tag); + } } SensorStatus LSM6DSV16XSensor::getSensorState() { - //TODO: this may need to be redone, errorCounter gets reset at the end of the loop - return errorCounter > 0 ? SensorStatus::SENSOR_ERROR : isWorking() ? SensorStatus::SENSOR_OK : SensorStatus::SENSOR_OFFLINE; + // TODO: this may need to be redone, errorCounter gets reset at the end of the loop + return errorCounter > 0 ? SensorStatus::SENSOR_ERROR + : isWorking() ? SensorStatus::SENSOR_OK + : SensorStatus::SENSOR_OFFLINE; } -void LSM6DSV16XSensor::sendData() -{ - if (newFusedRotation) - { - newFusedRotation = false; - networkConnection.sendRotationData(sensorId, &fusedRotation, DATA_TYPE_NORMAL, calibrationAccuracy); +void LSM6DSV16XSensor::sendData() { + if (newFusedRotation) { + newFusedRotation = false; + networkConnection.sendRotationData( + sensorId, + &fusedRotation, + DATA_TYPE_NORMAL, + calibrationAccuracy + ); #ifdef DEBUG_SENSOR - m_Logger.trace("Quaternion: %f, %f, %f, %f", UNPACK_QUATERNION(fusedRotation)); + m_Logger.trace("Quaternion: %f, %f, %f, %f", UNPACK_QUATERNION(fusedRotation)); #endif - } + } #if SEND_ACCELERATION - if(newAcceleration) //Returns acceleration in G's - { - newAcceleration = false; - networkConnection.sendSensorAcceleration(sensorId, acceleration); - } + if (newAcceleration) // Returns acceleration in G's + { + newAcceleration = false; + networkConnection.sendSensorAcceleration(sensorId, acceleration); + } #endif - if (tap != 0) //chip supports tap and double tap - { - networkConnection.sendSensorTap(sensorId, tap); - tap = 0; - } + if (tap != 0) // chip supports tap and double tap + { + networkConnection.sendSensorTap(sensorId, tap); + tap = 0; + } } -void LSM6DSV16XSensor::startCalibration(int calibrationType) -{ - //These IMU are factory calibrated. - //The register might be able to be changed but it could break the device - //I don't think we will need to mess with them +void LSM6DSV16XSensor::startCalibration(int calibrationType) { + // These IMU are factory calibrated. + // The register might be able to be changed but it could break the device + // I don't think we will need to mess with them } diff --git a/src/sensors/lsm6dsv16xsensor.h b/src/sensors/lsm6dsv16xsensor.h index aa7f2863c..e8b24bce4 100644 --- a/src/sensors/lsm6dsv16xsensor.h +++ b/src/sensors/lsm6dsv16xsensor.h @@ -1,61 +1,67 @@ /* - SlimeVR Code is placed under the MIT license - Copyright (c) 2023 Eiren Rain & SlimeVR contributors - - Permission is hereby granted, free of charge, to any person obtaining a copy - of this software and associated documentation files (the "Software"), to deal - in the Software without restriction, including without limitation the rights - to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - copies of the Software, and to permit persons to whom the Software is - furnished to do so, subject to the following conditions: - - The above copyright notice and this permission notice shall be included in - all copies or substantial portions of the Software. - - THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - THE SOFTWARE. + SlimeVR Code is placed under the MIT license + Copyright (c) 2023 Eiren Rain & SlimeVR contributors + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. */ #ifndef SENSORS_LSM6DSV16X_H #define SENSORS_LSM6DSV16X_H -#include "sensor.h" #include -class LSM6DSV16XSensor : public Sensor -{ +#include "sensor.h" + +class LSM6DSV16XSensor : public Sensor { public: - LSM6DSV16XSensor(uint8_t id, uint8_t type, uint8_t address, float rotation, uint8_t sclPin, uint8_t sdaPin, uint8_t intPin) - : Sensor("LSM6DSV16XSensor", type, id, address, rotation, sclPin, sdaPin), - imu(&Wire, addr), - m_IntPin(intPin) {}; - ~LSM6DSV16XSensor(){}; - void motionSetup() override final; - void postSetup() override { - lastData = millis(); - } - - void motionLoop() override final; - void sendData() override final; - void startCalibration(int calibrationType) override final; - SensorStatus getSensorState() override final; + LSM6DSV16XSensor( + uint8_t id, + uint8_t type, + uint8_t address, + float rotation, + uint8_t sclPin, + uint8_t sdaPin, + uint8_t intPin + ) + : Sensor("LSM6DSV16XSensor", type, id, address, rotation, sclPin, sdaPin) + , imu(&Wire, addr) + , m_IntPin(intPin){}; + ~LSM6DSV16XSensor(){}; + void motionSetup() override final; + void postSetup() override { lastData = millis(); } + + void motionLoop() override final; + void sendData() override final; + void startCalibration(int calibrationType) override final; + SensorStatus getSensorState() override final; private: - //void interruptHandler(); - //volatile bool imuEvent; //the interrupt cant be a class function + // void interruptHandler(); + // volatile bool imuEvent; //the interrupt cant be a class function - LSM6DSV16X imu; - uint8_t m_IntPin; - uint8_t errorCounter = 0; //Error is -1, OK is 0 - uint8_t tap = 0; - unsigned long lastData = 0; + LSM6DSV16X imu; + uint8_t m_IntPin; + uint8_t errorCounter = 0; // Error is -1, OK is 0 + uint8_t tap = 0; + unsigned long lastData = 0; - uint8_t lastReset = 0; + uint8_t lastReset = 0; }; #endif From 9de426e0ac5784401e50917b6e8c2eb17b35f75b Mon Sep 17 00:00:00 2001 From: Gorbit99 Date: Thu, 3 Aug 2023 01:06:30 +0200 Subject: [PATCH 10/60] Updated the outdated st libs --- lib/LSM6DSV16X/LSM6DSV16X.cpp | 257 +- lib/LSM6DSV16X/LSM6DSV16X.h | 21 +- lib/LSM6DSV16X/lsm6dsv16x_reg.c | 4750 +++++++++++++--------- lib/LSM6DSV16X/lsm6dsv16x_reg.h | 6507 ++++++++++++++++-------------- src/sensors/lsm6dsv16xsensor.cpp | 34 +- src/sensors/lsm6dsv16xsensor.h | 16 + 6 files changed, 6563 insertions(+), 5022 deletions(-) diff --git a/lib/LSM6DSV16X/LSM6DSV16X.cpp b/lib/LSM6DSV16X/LSM6DSV16X.cpp index 50b4e78b9..cc4f0b23a 100644 --- a/lib/LSM6DSV16X/LSM6DSV16X.cpp +++ b/lib/LSM6DSV16X/LSM6DSV16X.cpp @@ -101,10 +101,10 @@ LSM6DSV16XStatusTypeDef LSM6DSV16X::begin() } /* Select default output data rate. */ - acc_odr = LSM6DSV16X_XL_ODR_AT_120Hz; + acc_odr = LSM6DSV16X_ODR_AT_120Hz; /* Output data rate selection - power down. */ - if (lsm6dsv16x_xl_data_rate_set(®_ctx, LSM6DSV16X_XL_ODR_OFF) != LSM6DSV16X_OK) { + if (lsm6dsv16x_xl_data_rate_set(®_ctx, LSM6DSV16X_ODR_OFF) != LSM6DSV16X_OK) { return LSM6DSV16X_ERROR; } @@ -114,10 +114,10 @@ LSM6DSV16XStatusTypeDef LSM6DSV16X::begin() } /* Select default output data rate. */ - gyro_odr = LSM6DSV16X_GY_ODR_AT_120Hz; + gyro_odr = LSM6DSV16X_ODR_AT_120Hz; /* Output data rate selection - power down. */ - if (lsm6dsv16x_gy_data_rate_set(®_ctx, LSM6DSV16X_GY_ODR_OFF) != LSM6DSV16X_OK) { + if (lsm6dsv16x_gy_data_rate_set(®_ctx, LSM6DSV16X_ODR_OFF) != LSM6DSV16X_OK) { return LSM6DSV16X_ERROR; } @@ -147,8 +147,8 @@ LSM6DSV16XStatusTypeDef LSM6DSV16X::end() } /* Reset output data rate. */ - acc_odr = LSM6DSV16X_XL_ODR_OFF; - gyro_odr = LSM6DSV16X_GY_ODR_OFF; + acc_odr = LSM6DSV16X_ODR_OFF; + gyro_odr = LSM6DSV16X_ODR_OFF; initialized = 0U; @@ -207,7 +207,7 @@ LSM6DSV16XStatusTypeDef LSM6DSV16X::Disable_X() } /* Output data rate selection - power down. */ - if (lsm6dsv16x_xl_data_rate_set(®_ctx, LSM6DSV16X_XL_ODR_OFF) != LSM6DSV16X_OK) { + if (lsm6dsv16x_xl_data_rate_set(®_ctx, LSM6DSV16X_ODR_OFF) != LSM6DSV16X_OK) { return LSM6DSV16X_ERROR; } @@ -265,7 +265,7 @@ LSM6DSV16XStatusTypeDef LSM6DSV16X::Get_X_Sensitivity(float *Sensitivity) LSM6DSV16XStatusTypeDef LSM6DSV16X::Get_X_ODR(float *Odr) { LSM6DSV16XStatusTypeDef ret = LSM6DSV16X_OK; - lsm6dsv16x_xl_data_rate_t odr_low_level; + lsm6dsv16x_data_rate_t odr_low_level; /* Get current output data rate. */ if (lsm6dsv16x_xl_data_rate_get(®_ctx, &odr_low_level) != LSM6DSV16X_OK) { @@ -273,55 +273,55 @@ LSM6DSV16XStatusTypeDef LSM6DSV16X::Get_X_ODR(float *Odr) } switch (odr_low_level) { - case LSM6DSV16X_XL_ODR_OFF: + case LSM6DSV16X_ODR_OFF: *Odr = 0.0f; break; - case LSM6DSV16X_XL_ODR_AT_1Hz875: + case LSM6DSV16X_ODR_AT_1Hz875: *Odr = 1.875f; break; - case LSM6DSV16X_XL_ODR_AT_7Hz5: + case LSM6DSV16X_ODR_AT_7Hz5: *Odr = 7.5f; break; - case LSM6DSV16X_XL_ODR_AT_15Hz: + case LSM6DSV16X_ODR_AT_15Hz: *Odr = 15.0f; break; - case LSM6DSV16X_XL_ODR_AT_30Hz: + case LSM6DSV16X_ODR_AT_30Hz: *Odr = 30.0f; break; - case LSM6DSV16X_XL_ODR_AT_60Hz: + case LSM6DSV16X_ODR_AT_60Hz: *Odr = 60.0f; break; - case LSM6DSV16X_XL_ODR_AT_120Hz: + case LSM6DSV16X_ODR_AT_120Hz: *Odr = 120.0f; break; - case LSM6DSV16X_XL_ODR_AT_240Hz: + case LSM6DSV16X_ODR_AT_240Hz: *Odr = 240.0f; break; - case LSM6DSV16X_XL_ODR_AT_480Hz: + case LSM6DSV16X_ODR_AT_480Hz: *Odr = 480.0f; break; - case LSM6DSV16X_XL_ODR_AT_960Hz: + case LSM6DSV16X_ODR_AT_960Hz: *Odr = 960.0f; break; - case LSM6DSV16X_XL_ODR_AT_1920Hz: + case LSM6DSV16X_ODR_AT_1920Hz: *Odr = 1920.0f; break; - case LSM6DSV16X_XL_ODR_AT_3840Hz: + case LSM6DSV16X_ODR_AT_3840Hz: *Odr = 3840.0f; break; - case LSM6DSV16X_XL_ODR_AT_7680Hz: + case LSM6DSV16X_ODR_AT_7680Hz: *Odr = 7680.0f; break; @@ -456,20 +456,20 @@ LSM6DSV16XStatusTypeDef LSM6DSV16X::Set_X_ODR(float Odr, LSM6DSV16X_ACC_Operatin */ LSM6DSV16XStatusTypeDef LSM6DSV16X::Set_X_ODR_When_Enabled(float Odr) { - lsm6dsv16x_xl_data_rate_t new_odr; - - new_odr = (Odr <= 1.875f) ? LSM6DSV16X_XL_ODR_AT_1Hz875 - : (Odr <= 7.5f) ? LSM6DSV16X_XL_ODR_AT_7Hz5 - : (Odr <= 15.0f) ? LSM6DSV16X_XL_ODR_AT_15Hz - : (Odr <= 30.0f) ? LSM6DSV16X_XL_ODR_AT_30Hz - : (Odr <= 60.0f) ? LSM6DSV16X_XL_ODR_AT_60Hz - : (Odr <= 120.0f) ? LSM6DSV16X_XL_ODR_AT_120Hz - : (Odr <= 240.0f) ? LSM6DSV16X_XL_ODR_AT_240Hz - : (Odr <= 480.0f) ? LSM6DSV16X_XL_ODR_AT_480Hz - : (Odr <= 960.0f) ? LSM6DSV16X_XL_ODR_AT_960Hz - : (Odr <= 1920.0f) ? LSM6DSV16X_XL_ODR_AT_1920Hz - : (Odr <= 3840.0f) ? LSM6DSV16X_XL_ODR_AT_3840Hz - : LSM6DSV16X_XL_ODR_AT_7680Hz; + lsm6dsv16x_data_rate_t new_odr; + + new_odr = (Odr <= 1.875f) ? LSM6DSV16X_ODR_AT_1Hz875 + : (Odr <= 7.5f) ? LSM6DSV16X_ODR_AT_7Hz5 + : (Odr <= 15.0f) ? LSM6DSV16X_ODR_AT_15Hz + : (Odr <= 30.0f) ? LSM6DSV16X_ODR_AT_30Hz + : (Odr <= 60.0f) ? LSM6DSV16X_ODR_AT_60Hz + : (Odr <= 120.0f) ? LSM6DSV16X_ODR_AT_120Hz + : (Odr <= 240.0f) ? LSM6DSV16X_ODR_AT_240Hz + : (Odr <= 480.0f) ? LSM6DSV16X_ODR_AT_480Hz + : (Odr <= 960.0f) ? LSM6DSV16X_ODR_AT_960Hz + : (Odr <= 1920.0f) ? LSM6DSV16X_ODR_AT_1920Hz + : (Odr <= 3840.0f) ? LSM6DSV16X_ODR_AT_3840Hz + : LSM6DSV16X_ODR_AT_7680Hz; /* Output data rate selection. */ if (lsm6dsv16x_xl_data_rate_set(®_ctx, new_odr) != LSM6DSV16X_OK) { @@ -486,18 +486,18 @@ LSM6DSV16XStatusTypeDef LSM6DSV16X::Set_X_ODR_When_Enabled(float Odr) */ LSM6DSV16XStatusTypeDef LSM6DSV16X::Set_X_ODR_When_Disabled(float Odr) { - acc_odr = (Odr <= 1.875f) ? LSM6DSV16X_XL_ODR_AT_1Hz875 - : (Odr <= 7.5f) ? LSM6DSV16X_XL_ODR_AT_7Hz5 - : (Odr <= 15.0f) ? LSM6DSV16X_XL_ODR_AT_15Hz - : (Odr <= 30.0f) ? LSM6DSV16X_XL_ODR_AT_30Hz - : (Odr <= 60.0f) ? LSM6DSV16X_XL_ODR_AT_60Hz - : (Odr <= 120.0f) ? LSM6DSV16X_XL_ODR_AT_120Hz - : (Odr <= 240.0f) ? LSM6DSV16X_XL_ODR_AT_240Hz - : (Odr <= 480.0f) ? LSM6DSV16X_XL_ODR_AT_480Hz - : (Odr <= 960.0f) ? LSM6DSV16X_XL_ODR_AT_960Hz - : (Odr <= 1920.0f) ? LSM6DSV16X_XL_ODR_AT_1920Hz - : (Odr <= 3840.0f) ? LSM6DSV16X_XL_ODR_AT_3840Hz - : LSM6DSV16X_XL_ODR_AT_7680Hz; + acc_odr = (Odr <= 1.875f) ? LSM6DSV16X_ODR_AT_1Hz875 + : (Odr <= 7.5f) ? LSM6DSV16X_ODR_AT_7Hz5 + : (Odr <= 15.0f) ? LSM6DSV16X_ODR_AT_15Hz + : (Odr <= 30.0f) ? LSM6DSV16X_ODR_AT_30Hz + : (Odr <= 60.0f) ? LSM6DSV16X_ODR_AT_60Hz + : (Odr <= 120.0f) ? LSM6DSV16X_ODR_AT_120Hz + : (Odr <= 240.0f) ? LSM6DSV16X_ODR_AT_240Hz + : (Odr <= 480.0f) ? LSM6DSV16X_ODR_AT_480Hz + : (Odr <= 960.0f) ? LSM6DSV16X_ODR_AT_960Hz + : (Odr <= 1920.0f) ? LSM6DSV16X_ODR_AT_1920Hz + : (Odr <= 3840.0f) ? LSM6DSV16X_ODR_AT_3840Hz + : LSM6DSV16X_ODR_AT_7680Hz; return LSM6DSV16X_OK; } @@ -1366,9 +1366,9 @@ LSM6DSV16XStatusTypeDef LSM6DSV16X::Set_Wake_Up_Threshold(uint32_t Threshold) return LSM6DSV16X_ERROR; } - wake_up_ths.wk_ths_mg = Threshold; + wake_up_ths.threshold = Threshold; - if (lsm6dsv16x_act_thresholds_set(®_ctx, wake_up_ths) != LSM6DSV16X_OK) { + if (lsm6dsv16x_act_thresholds_set(®_ctx, &wake_up_ths) != LSM6DSV16X_OK) { return LSM6DSV16X_ERROR; } @@ -1409,7 +1409,7 @@ LSM6DSV16XStatusTypeDef LSM6DSV16X::Enable_Single_Tap_Detection(LSM6DSV16X_Senso lsm6dsv16x_md2_cfg_t val2; lsm6dsv16x_functions_enable_t functions_enable; - lsm6dsv16x_int_dur2_t tap_dur; + lsm6dsv16x_tap_dur_t tap_dur; lsm6dsv16x_tap_cfg0_t tap_cfg0; lsm6dsv16x_tap_ths_6d_t tap_ths_6d; @@ -1446,14 +1446,14 @@ LSM6DSV16XStatusTypeDef LSM6DSV16X::Enable_Single_Tap_Detection(LSM6DSV16X_Senso } /* Set quiet and shock time windows. */ - if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_INT_DUR2, (uint8_t *)&tap_dur, 1) != LSM6DSV16X_OK) { + if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_TAP_DUR, (uint8_t *)&tap_dur, 1) != LSM6DSV16X_OK) { return LSM6DSV16X_ERROR; } tap_dur.quiet = (uint8_t)0x02U; tap_dur.shock = (uint8_t)0x01U; - if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_INT_DUR2, (uint8_t *)&tap_dur, 1) != LSM6DSV16X_OK) { + if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_TAP_DUR, (uint8_t *)&tap_dur, 1) != LSM6DSV16X_OK) { return LSM6DSV16X_ERROR; } @@ -1524,7 +1524,7 @@ LSM6DSV16XStatusTypeDef LSM6DSV16X::Disable_Single_Tap_Detection() { lsm6dsv16x_md1_cfg_t val1; lsm6dsv16x_md2_cfg_t val2; - lsm6dsv16x_int_dur2_t tap_dur; + lsm6dsv16x_tap_dur_t tap_dur; lsm6dsv16x_tap_cfg0_t tap_cfg0; lsm6dsv16x_tap_ths_6d_t tap_ths_6d; @@ -1573,14 +1573,14 @@ LSM6DSV16XStatusTypeDef LSM6DSV16X::Disable_Single_Tap_Detection() } /* Reset quiet and shock time windows. */ - if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_INT_DUR2, (uint8_t *)&tap_dur, 1) != LSM6DSV16X_OK) { + if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_TAP_DUR, (uint8_t *)&tap_dur, 1) != LSM6DSV16X_OK) { return LSM6DSV16X_ERROR; } tap_dur.quiet = (uint8_t)0x0U; tap_dur.shock = (uint8_t)0x0U; - if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_INT_DUR2, (uint8_t *)&tap_dur, 1) != LSM6DSV16X_OK) { + if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_TAP_DUR, (uint8_t *)&tap_dur, 1) != LSM6DSV16X_OK) { return LSM6DSV16X_ERROR; } @@ -1599,7 +1599,7 @@ LSM6DSV16XStatusTypeDef LSM6DSV16X::Enable_Double_Tap_Detection(LSM6DSV16X_Senso lsm6dsv16x_md2_cfg_t val2; lsm6dsv16x_functions_enable_t functions_enable; - lsm6dsv16x_int_dur2_t tap_dur; + lsm6dsv16x_tap_dur_t tap_dur; lsm6dsv16x_tap_cfg0_t tap_cfg0; lsm6dsv16x_tap_ths_6d_t tap_ths_6d; @@ -1627,7 +1627,7 @@ LSM6DSV16XStatusTypeDef LSM6DSV16X::Enable_Double_Tap_Detection(LSM6DSV16X_Senso } /* Set quiet shock and duration. */ - if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_INT_DUR2, (uint8_t *)&tap_dur, 1) != LSM6DSV16X_OK) { + if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_TAP_DUR, (uint8_t *)&tap_dur, 1) != LSM6DSV16X_OK) { return LSM6DSV16X_ERROR; } @@ -1635,7 +1635,7 @@ LSM6DSV16XStatusTypeDef LSM6DSV16X::Enable_Double_Tap_Detection(LSM6DSV16X_Senso tap_dur.quiet = (uint8_t)0x02U; tap_dur.shock = (uint8_t)0x02U; - if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_INT_DUR2, (uint8_t *)&tap_dur, 1) != LSM6DSV16X_OK) { + if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_TAP_DUR, (uint8_t *)&tap_dur, 1) != LSM6DSV16X_OK) { return LSM6DSV16X_ERROR; } @@ -1717,7 +1717,7 @@ LSM6DSV16XStatusTypeDef LSM6DSV16X::Disable_Double_Tap_Detection() lsm6dsv16x_md1_cfg_t val1; lsm6dsv16x_md2_cfg_t val2; - lsm6dsv16x_int_dur2_t tap_dur; + lsm6dsv16x_tap_dur_t tap_dur; lsm6dsv16x_tap_cfg0_t tap_cfg0; lsm6dsv16x_tap_ths_6d_t tap_ths_6d; @@ -1765,7 +1765,7 @@ LSM6DSV16XStatusTypeDef LSM6DSV16X::Disable_Double_Tap_Detection() } /* Reset quiet shock and duratio. */ - if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_INT_DUR2, (uint8_t *)&tap_dur, 1) != LSM6DSV16X_OK) { + if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_TAP_DUR, (uint8_t *)&tap_dur, 1) != LSM6DSV16X_OK) { return LSM6DSV16X_ERROR; } @@ -1773,7 +1773,7 @@ LSM6DSV16XStatusTypeDef LSM6DSV16X::Disable_Double_Tap_Detection() tap_dur.quiet = (uint8_t)0x0U; tap_dur.shock = (uint8_t)0x0U; - if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_INT_DUR2, (uint8_t *)&tap_dur, 1) != LSM6DSV16X_OK) { + if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_TAP_DUR, (uint8_t *)&tap_dur, 1) != LSM6DSV16X_OK) { return LSM6DSV16X_ERROR; } @@ -1816,16 +1816,16 @@ LSM6DSV16XStatusTypeDef LSM6DSV16X::Set_Tap_Threshold(uint8_t Threshold) */ LSM6DSV16XStatusTypeDef LSM6DSV16X::Set_Tap_Shock_Time(uint8_t Time) { - lsm6dsv16x_int_dur2_t tap_dur; + lsm6dsv16x_tap_dur_t tap_dur; /* Set shock time */ - if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_INT_DUR2, (uint8_t *)&tap_dur, 1) != LSM6DSV16X_OK) { + if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_TAP_DUR, (uint8_t *)&tap_dur, 1) != LSM6DSV16X_OK) { return LSM6DSV16X_ERROR; } tap_dur.shock = Time; - if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_INT_DUR2, (uint8_t *)&tap_dur, 1) != LSM6DSV16X_OK) { + if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_TAP_DUR, (uint8_t *)&tap_dur, 1) != LSM6DSV16X_OK) { return LSM6DSV16X_ERROR; } @@ -1839,16 +1839,16 @@ LSM6DSV16XStatusTypeDef LSM6DSV16X::Set_Tap_Shock_Time(uint8_t Time) */ LSM6DSV16XStatusTypeDef LSM6DSV16X::Set_Tap_Quiet_Time(uint8_t Time) { - lsm6dsv16x_int_dur2_t tap_dur; + lsm6dsv16x_tap_dur_t tap_dur; /* Set quiet time */ - if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_INT_DUR2, (uint8_t *)&tap_dur, 1) != LSM6DSV16X_OK) { + if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_TAP_DUR, (uint8_t *)&tap_dur, 1) != LSM6DSV16X_OK) { return LSM6DSV16X_ERROR; } tap_dur.quiet = Time; - if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_INT_DUR2, (uint8_t *)&tap_dur, 1) != LSM6DSV16X_OK) { + if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_TAP_DUR, (uint8_t *)&tap_dur, 1) != LSM6DSV16X_OK) { return LSM6DSV16X_ERROR; } return LSM6DSV16X_OK; @@ -1861,16 +1861,16 @@ LSM6DSV16XStatusTypeDef LSM6DSV16X::Set_Tap_Quiet_Time(uint8_t Time) */ LSM6DSV16XStatusTypeDef LSM6DSV16X::Set_Tap_Duration_Time(uint8_t Time) { - lsm6dsv16x_int_dur2_t tap_dur; + lsm6dsv16x_tap_dur_t tap_dur; /* Set duration time */ - if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_INT_DUR2, (uint8_t *)&tap_dur, 1) != LSM6DSV16X_OK) { + if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_TAP_DUR, (uint8_t *)&tap_dur, 1) != LSM6DSV16X_OK) { return LSM6DSV16X_ERROR; } tap_dur.dur = Time; - if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_INT_DUR2, (uint8_t *)&tap_dur, 1) != LSM6DSV16X_OK) { + if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_TAP_DUR, (uint8_t *)&tap_dur, 1) != LSM6DSV16X_OK) { return LSM6DSV16X_ERROR; } return LSM6DSV16X_OK; @@ -2313,7 +2313,11 @@ LSM6DSV16XStatusTypeDef LSM6DSV16X::Disable_Tilt_Detection() */ LSM6DSV16XStatusTypeDef LSM6DSV16X::FIFO_Get_Num_Samples(uint16_t *NumSamples) { - return (LSM6DSV16XStatusTypeDef) lsm6dsv16x_fifo_data_level_get(®_ctx, NumSamples); + lsm6dsv16x_fifo_status_t fifo_status; + LSM6DSV16XStatusTypeDef result = + (LSM6DSV16XStatusTypeDef) lsm6dsv16x_fifo_status_get(®_ctx, &fifo_status); + *NumSamples = fifo_status.fifo_level; + return result; } /** @@ -2594,6 +2598,18 @@ LSM6DSV16XStatusTypeDef LSM6DSV16X::FIFO_Set_G_BDR(float Bdr) return (LSM6DSV16XStatusTypeDef) lsm6dsv16x_fifo_gy_batch_set(®_ctx, new_bdr); } +LSM6DSV16XStatusTypeDef LSM6DSV16X::FIFO_Set_SFLP_Batch(bool GameRotation, bool Gravity, bool gBias) +{ + lsm6dsv16x_fifo_sflp_raw_t fifo_sflp; + fifo_sflp.game_rotation = 1; + fifo_sflp.gravity = 1; + fifo_sflp.gbias = 1; + if (lsm6dsv16x_fifo_sflp_batch_set(®_ctx, fifo_sflp) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + return LSM6DSV16X_OK; +} + /** * @brief Enable the LSM6DSV16X gyroscope sensor * @retval 0 in case of success, an error code otherwise @@ -2632,7 +2648,7 @@ LSM6DSV16XStatusTypeDef LSM6DSV16X::Disable_G() } /* Output data rate selection - power down. */ - if (lsm6dsv16x_gy_data_rate_set(®_ctx, LSM6DSV16X_GY_ODR_OFF) != LSM6DSV16X_OK) { + if (lsm6dsv16x_gy_data_rate_set(®_ctx, LSM6DSV16X_ODR_OFF) != LSM6DSV16X_OK) { return LSM6DSV16X_ERROR; } @@ -2698,7 +2714,7 @@ LSM6DSV16XStatusTypeDef LSM6DSV16X::Get_G_Sensitivity(float *Sensitivity) LSM6DSV16XStatusTypeDef LSM6DSV16X::Get_G_ODR(float *Odr) { LSM6DSV16XStatusTypeDef ret = LSM6DSV16X_OK; - lsm6dsv16x_gy_data_rate_t odr_low_level; + lsm6dsv16x_data_rate_t odr_low_level; /* Get current output data rate. */ if (lsm6dsv16x_gy_data_rate_get(®_ctx, &odr_low_level) != LSM6DSV16X_OK) { @@ -2706,51 +2722,51 @@ LSM6DSV16XStatusTypeDef LSM6DSV16X::Get_G_ODR(float *Odr) } switch (odr_low_level) { - case LSM6DSV16X_GY_ODR_OFF: + case LSM6DSV16X_ODR_OFF: *Odr = 0.0f; break; - case LSM6DSV16X_GY_ODR_AT_7Hz5: + case LSM6DSV16X_ODR_AT_7Hz5: *Odr = 7.5f; break; - case LSM6DSV16X_GY_ODR_AT_15Hz: + case LSM6DSV16X_ODR_AT_15Hz: *Odr = 15.0f; break; - case LSM6DSV16X_GY_ODR_AT_30Hz: + case LSM6DSV16X_ODR_AT_30Hz: *Odr = 30.0f; break; - case LSM6DSV16X_GY_ODR_AT_60Hz: + case LSM6DSV16X_ODR_AT_60Hz: *Odr = 60.0f; break; - case LSM6DSV16X_GY_ODR_AT_120Hz: + case LSM6DSV16X_ODR_AT_120Hz: *Odr = 120.0f; break; - case LSM6DSV16X_GY_ODR_AT_240Hz: + case LSM6DSV16X_ODR_AT_240Hz: *Odr = 240.0f; break; - case LSM6DSV16X_GY_ODR_AT_480Hz: + case LSM6DSV16X_ODR_AT_480Hz: *Odr = 480.0f; break; - case LSM6DSV16X_GY_ODR_AT_960Hz: + case LSM6DSV16X_ODR_AT_960Hz: *Odr = 960.0f; break; - case LSM6DSV16X_GY_ODR_AT_1920Hz: + case LSM6DSV16X_ODR_AT_1920Hz: *Odr = 1920.0f; break; - case LSM6DSV16X_GY_ODR_AT_3840Hz: + case LSM6DSV16X_ODR_AT_3840Hz: *Odr = 3840.0f; break; - case LSM6DSV16X_GY_ODR_AT_7680Hz: + case LSM6DSV16X_ODR_AT_7680Hz: *Odr = 7680.0f; break; @@ -2824,19 +2840,19 @@ LSM6DSV16XStatusTypeDef LSM6DSV16X::Set_G_ODR(float Odr, LSM6DSV16X_GYRO_Operati */ LSM6DSV16XStatusTypeDef LSM6DSV16X::Set_G_ODR_When_Enabled(float Odr) { - lsm6dsv16x_gy_data_rate_t new_odr; + lsm6dsv16x_data_rate_t new_odr; - new_odr = (Odr <= 7.5f) ? LSM6DSV16X_GY_ODR_AT_7Hz5 - : (Odr <= 15.0f) ? LSM6DSV16X_GY_ODR_AT_15Hz - : (Odr <= 30.0f) ? LSM6DSV16X_GY_ODR_AT_30Hz - : (Odr <= 60.0f) ? LSM6DSV16X_GY_ODR_AT_60Hz - : (Odr <= 120.0f) ? LSM6DSV16X_GY_ODR_AT_120Hz - : (Odr <= 240.0f) ? LSM6DSV16X_GY_ODR_AT_240Hz - : (Odr <= 480.0f) ? LSM6DSV16X_GY_ODR_AT_480Hz - : (Odr <= 960.0f) ? LSM6DSV16X_GY_ODR_AT_960Hz - : (Odr <= 1920.0f) ? LSM6DSV16X_GY_ODR_AT_1920Hz - : (Odr <= 3840.0f) ? LSM6DSV16X_GY_ODR_AT_3840Hz - : LSM6DSV16X_GY_ODR_AT_7680Hz; + new_odr = (Odr <= 7.5f) ? LSM6DSV16X_ODR_AT_7Hz5 + : (Odr <= 15.0f) ? LSM6DSV16X_ODR_AT_15Hz + : (Odr <= 30.0f) ? LSM6DSV16X_ODR_AT_30Hz + : (Odr <= 60.0f) ? LSM6DSV16X_ODR_AT_60Hz + : (Odr <= 120.0f) ? LSM6DSV16X_ODR_AT_120Hz + : (Odr <= 240.0f) ? LSM6DSV16X_ODR_AT_240Hz + : (Odr <= 480.0f) ? LSM6DSV16X_ODR_AT_480Hz + : (Odr <= 960.0f) ? LSM6DSV16X_ODR_AT_960Hz + : (Odr <= 1920.0f) ? LSM6DSV16X_ODR_AT_1920Hz + : (Odr <= 3840.0f) ? LSM6DSV16X_ODR_AT_3840Hz + : LSM6DSV16X_ODR_AT_7680Hz; /* Output data rate selection. */ if (lsm6dsv16x_gy_data_rate_set(®_ctx, new_odr) != LSM6DSV16X_OK) { @@ -2853,17 +2869,17 @@ LSM6DSV16XStatusTypeDef LSM6DSV16X::Set_G_ODR_When_Enabled(float Odr) */ LSM6DSV16XStatusTypeDef LSM6DSV16X::Set_G_ODR_When_Disabled(float Odr) { - gyro_odr = (Odr <= 7.5f) ? LSM6DSV16X_GY_ODR_AT_7Hz5 - : (Odr <= 15.0f) ? LSM6DSV16X_GY_ODR_AT_15Hz - : (Odr <= 30.0f) ? LSM6DSV16X_GY_ODR_AT_30Hz - : (Odr <= 60.0f) ? LSM6DSV16X_GY_ODR_AT_60Hz - : (Odr <= 120.0f) ? LSM6DSV16X_GY_ODR_AT_120Hz - : (Odr <= 240.0f) ? LSM6DSV16X_GY_ODR_AT_240Hz - : (Odr <= 480.0f) ? LSM6DSV16X_GY_ODR_AT_480Hz - : (Odr <= 960.0f) ? LSM6DSV16X_GY_ODR_AT_960Hz - : (Odr <= 1920.0f) ? LSM6DSV16X_GY_ODR_AT_1920Hz - : (Odr <= 3840.0f) ? LSM6DSV16X_GY_ODR_AT_3840Hz - : LSM6DSV16X_GY_ODR_AT_7680Hz; + gyro_odr = (Odr <= 7.5f) ? LSM6DSV16X_ODR_AT_7Hz5 + : (Odr <= 15.0f) ? LSM6DSV16X_ODR_AT_15Hz + : (Odr <= 30.0f) ? LSM6DSV16X_ODR_AT_30Hz + : (Odr <= 60.0f) ? LSM6DSV16X_ODR_AT_60Hz + : (Odr <= 120.0f) ? LSM6DSV16X_ODR_AT_120Hz + : (Odr <= 240.0f) ? LSM6DSV16X_ODR_AT_240Hz + : (Odr <= 480.0f) ? LSM6DSV16X_ODR_AT_480Hz + : (Odr <= 960.0f) ? LSM6DSV16X_ODR_AT_960Hz + : (Odr <= 1920.0f) ? LSM6DSV16X_ODR_AT_1920Hz + : (Odr <= 3840.0f) ? LSM6DSV16X_ODR_AT_3840Hz + : LSM6DSV16X_ODR_AT_7680Hz; return LSM6DSV16X_OK; } @@ -3223,7 +3239,7 @@ LSM6DSV16XStatusTypeDef LSM6DSV16X::Write_Reg(uint8_t Reg, uint8_t Data) return LSM6DSV16X_OK; } -int32_t LSM6DSV16X_io_write(void *handle, uint8_t WriteAddr, uint8_t *pBuffer, uint16_t nBytesToWrite) +int32_t LSM6DSV16X_io_write(void *handle, uint8_t WriteAddr, const uint8_t *pBuffer, uint16_t nBytesToWrite) { return ((LSM6DSV16X *)handle)->IO_Write(pBuffer, WriteAddr, nBytesToWrite); } @@ -3232,3 +3248,26 @@ int32_t LSM6DSV16X_io_read(void *handle, uint8_t ReadAddr, uint8_t *pBuffer, uin { return ((LSM6DSV16X *)handle)->IO_Read(pBuffer, ReadAddr, nBytesToRead); } + +LSM6DSV16XStatusTypeDef LSM6DSV16X::Reset_Set(uint8_t flags) +{ + if (lsm6dsv16x_reset_set(®_ctx, (lsm6dsv16x_reset_t)flags) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + lsm6dsv16x_reset_t rst; + do { + if (lsm6dsv16x_reset_get(®_ctx, &rst) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + } while (rst != LSM6DSV16X_READY); + return LSM6DSV16X_OK; +} + +LSM6DSV16XStatusTypeDef LSM6DSV16X::Enable_Game_Rotation(bool enable) +{ + if (lsm6dsv16x_sflp_game_rotation_set(®_ctx, enable ? PROPERTY_ENABLE : PROPERTY_DISABLE) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + return LSM6DSV16X_OK; +} diff --git a/lib/LSM6DSV16X/LSM6DSV16X.h b/lib/LSM6DSV16X/LSM6DSV16X.h index bbafd940e..a5e6639ca 100644 --- a/lib/LSM6DSV16X/LSM6DSV16X.h +++ b/lib/LSM6DSV16X/LSM6DSV16X.h @@ -98,6 +98,12 @@ typedef union { uint8_t u8bit[2]; } lsm6dsv16x_axis1bit16_t; +enum LSM6DSV16X_Reset { + LSM6DSV16X_RESET_GLOBAL = 0x1, + LSM6DSV16X_RESET_CAL_PARAM = 0x2, + LSM6DSV16X_RESET_CTRL_REGS = 0x4, +}; + typedef enum { LSM6DSV16X_ACC_HIGH_PERFORMANCE_MODE, LSM6DSV16X_ACC_HIGH_ACCURACY_MODE, @@ -208,6 +214,7 @@ class LSM6DSV16X { LSM6DSV16XStatusTypeDef FIFO_Set_X_BDR(float Bdr); LSM6DSV16XStatusTypeDef FIFO_Get_G_Axes(int32_t *AngularVelocity); LSM6DSV16XStatusTypeDef FIFO_Set_G_BDR(float Bdr); + LSM6DSV16XStatusTypeDef FIFO_Set_SFLP_Batch(bool GameRotation, bool Gravity, bool gBias); LSM6DSV16XStatusTypeDef QVAR_Enable(); LSM6DSV16XStatusTypeDef QVAR_GetStatus(uint8_t *val); @@ -220,6 +227,10 @@ class LSM6DSV16X { LSM6DSV16XStatusTypeDef Read_Reg(uint8_t Reg, uint8_t *Data); LSM6DSV16XStatusTypeDef Write_Reg(uint8_t Reg, uint8_t Data); + LSM6DSV16XStatusTypeDef Reset_Set(uint8_t flags); + + LSM6DSV16XStatusTypeDef Enable_Game_Rotation(bool enable = true); + /** * @brief Utility function to read data. * @param pBuffer: pointer to data to be read. @@ -274,7 +285,7 @@ class LSM6DSV16X { * @param NumByteToWrite: number of bytes to write. * @retval 0 if ok, an error code otherwise. */ - uint8_t IO_Write(uint8_t *pBuffer, uint8_t RegisterAddr, uint16_t NumByteToWrite) + uint8_t IO_Write(const uint8_t *pBuffer, uint8_t RegisterAddr, uint16_t NumByteToWrite) { if (dev_spi) { dev_spi->beginTransaction(SPISettings(spi_speed, MSBFIRST, SPI_MODE3)); @@ -326,18 +337,18 @@ class LSM6DSV16X { int cs_pin; uint32_t spi_speed; - lsm6dsv16x_xl_data_rate_t acc_odr; - lsm6dsv16x_gy_data_rate_t gyro_odr; + lsm6dsv16x_data_rate_t acc_odr; + lsm6dsv16x_data_rate_t gyro_odr; uint8_t acc_is_enabled; uint8_t gyro_is_enabled; uint8_t initialized; - lsm6dsv16x_ctx_t reg_ctx; + stmdev_ctx_t reg_ctx; }; #ifdef __cplusplus extern "C" { #endif -int32_t LSM6DSV16X_io_write(void *handle, uint8_t WriteAddr, uint8_t *pBuffer, uint16_t nBytesToWrite); +int32_t LSM6DSV16X_io_write(void *handle, uint8_t WriteAddr, const uint8_t *pBuffer, uint16_t nBytesToWrite); int32_t LSM6DSV16X_io_read(void *handle, uint8_t ReadAddr, uint8_t *pBuffer, uint16_t nBytesToRead); #ifdef __cplusplus } diff --git a/lib/LSM6DSV16X/lsm6dsv16x_reg.c b/lib/LSM6DSV16X/lsm6dsv16x_reg.c index d77236c48..030f80909 100644 --- a/lib/LSM6DSV16X/lsm6dsv16x_reg.c +++ b/lib/LSM6DSV16X/lsm6dsv16x_reg.c @@ -6,7 +6,7 @@ ****************************************************************************** * @attention * - *

© Copyright (c) 2021 STMicroelectronics. + *

© Copyright (c) 2022 STMicroelectronics. * All rights reserved.

* * This software component is licensed by ST under BSD 3-Clause license, @@ -46,9 +46,9 @@ * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_read_reg(lsm6dsv16x_ctx_t *ctx, uint8_t reg, - uint8_t *data, - uint16_t len) +int32_t __weak lsm6dsv16x_read_reg(stmdev_ctx_t *ctx, uint8_t reg, + uint8_t *data, + uint16_t len) { int32_t ret; @@ -67,9 +67,9 @@ int32_t lsm6dsv16x_read_reg(lsm6dsv16x_ctx_t *ctx, uint8_t reg, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_write_reg(lsm6dsv16x_ctx_t *ctx, uint8_t reg, - uint8_t *data, - uint16_t len) +int32_t __weak lsm6dsv16x_write_reg(stmdev_ctx_t *ctx, uint8_t reg, + uint8_t *data, + uint16_t len) { int32_t ret; @@ -92,7 +92,8 @@ int32_t lsm6dsv16x_write_reg(lsm6dsv16x_ctx_t *ctx, uint8_t reg, static void bytecpy(uint8_t *target, uint8_t *source) { - if ((target != NULL) && (source != NULL)) { + if ((target != NULL) && (source != NULL)) + { *target = *source; } } @@ -108,64 +109,74 @@ static void bytecpy(uint8_t *target, uint8_t *source) * @{ * */ -float lsm6dsv16x_from_fs2_to_mg(int16_t lsb) +float_t lsm6dsv16x_from_sflp_to_mg(int16_t lsb) { - return ((float)lsb) * 0.061f; + return ((float_t)lsb) * 0.061f; } -float lsm6dsv16x_from_fs4_to_mg(int16_t lsb) +float_t lsm6dsv16x_from_fs2_to_mg(int16_t lsb) { - return ((float)lsb) * 0.122f; + return ((float_t)lsb) * 0.061f; } -float lsm6dsv16x_from_fs8_to_mg(int16_t lsb) +float_t lsm6dsv16x_from_fs4_to_mg(int16_t lsb) { - return ((float)lsb) * 0.244f; + return ((float_t)lsb) * 0.122f; } -float lsm6dsv16x_from_fs16_to_mg(int16_t lsb) +float_t lsm6dsv16x_from_fs8_to_mg(int16_t lsb) { - return ((float)lsb) * 0.488f; + return ((float_t)lsb) * 0.244f; } -float lsm6dsv16x_from_fs125_to_mdps(int16_t lsb) +float_t lsm6dsv16x_from_fs16_to_mg(int16_t lsb) { - return ((float)lsb) * 4.375f; + return ((float_t)lsb) * 0.488f; } -float lsm6dsv16x_from_fs250_to_mdps(int16_t lsb) +float_t lsm6dsv16x_from_fs125_to_mdps(int16_t lsb) { - return ((float)lsb) * 8.750f; + return ((float_t)lsb) * 4.375f; } -float lsm6dsv16x_from_fs500_to_mdps(int16_t lsb) +float_t lsm6dsv16x_from_fs250_to_mdps(int16_t lsb) { - return ((float)lsb) * 17.50f; + return ((float_t)lsb) * 8.750f; } -float lsm6dsv16x_from_fs1000_to_mdps(int16_t lsb) +float_t lsm6dsv16x_from_fs500_to_mdps(int16_t lsb) { - return ((float)lsb) * 35.0f; + return ((float_t)lsb) * 17.50f; } -float lsm6dsv16x_from_fs2000_to_mdps(int16_t lsb) +float_t lsm6dsv16x_from_fs1000_to_mdps(int16_t lsb) { - return ((float)lsb) * 70.0f; + return ((float_t)lsb) * 35.0f; } -float lsm6dsv16x_from_fs4000_to_mdps(int16_t lsb) +float_t lsm6dsv16x_from_fs2000_to_mdps(int16_t lsb) { - return ((float)lsb) * 140.0f; + return ((float_t)lsb) * 70.0f; } -float lsm6dsv16x_from_lsb_to_celsius(int16_t lsb) +float_t lsm6dsv16x_from_fs4000_to_mdps(int16_t lsb) { - return (((float)lsb / 256.0f) + 25.0f); + return ((float_t)lsb) * 140.0f; } -float lsm6dsv16x_from_lsb_to_nsec(uint32_t lsb) +float_t lsm6dsv16x_from_lsb_to_celsius(int16_t lsb) { - return ((float)lsb * 21750.0f); + return (((float_t)lsb / 256.0f) + 25.0f); +} + +float_t lsm6dsv16x_from_lsb_to_nsec(uint32_t lsb) +{ + return ((float_t)lsb * 21750.0f); +} + +float_t lsm6dsv16x_from_lsb_to_mv(int16_t lsb) +{ + return ((float_t)lsb) / 78.0f; } /** @@ -189,13 +200,14 @@ float lsm6dsv16x_from_lsb_to_nsec(uint32_t lsb) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_xl_offset_on_out_set(lsm6dsv16x_ctx_t *ctx, uint8_t val) +int32_t lsm6dsv16x_xl_offset_on_out_set(stmdev_ctx_t *ctx, uint8_t val) { lsm6dsv16x_ctrl9_t ctrl9; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL9, (uint8_t *)&ctrl9, 1); - if (ret == 0) { + if (ret == 0) + { ctrl9.usr_off_on_out = val; ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_CTRL9, (uint8_t *)&ctrl9, 1); } @@ -211,7 +223,7 @@ int32_t lsm6dsv16x_xl_offset_on_out_set(lsm6dsv16x_ctx_t *ctx, uint8_t val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_xl_offset_on_out_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val) +int32_t lsm6dsv16x_xl_offset_on_out_get(stmdev_ctx_t *ctx, uint8_t *val) { lsm6dsv16x_ctrl9_t ctrl9; int32_t ret; @@ -230,27 +242,26 @@ int32_t lsm6dsv16x_xl_offset_on_out_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_xl_offset_mg_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_xl_offset_mg_t val) +int32_t lsm6dsv16x_xl_offset_mg_set(stmdev_ctx_t *ctx, + lsm6dsv16x_xl_offset_mg_t val) { lsm6dsv16x_z_ofs_usr_t z_ofs_usr; lsm6dsv16x_y_ofs_usr_t y_ofs_usr; lsm6dsv16x_x_ofs_usr_t x_ofs_usr; lsm6dsv16x_ctrl9_t ctrl9; int32_t ret; - float tmp; + float_t tmp; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_Z_OFS_USR, (uint8_t *)&z_ofs_usr, 1); - if (ret == 0) { - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_Y_OFS_USR, (uint8_t *)&y_ofs_usr, 1); - } - if (ret == 0) { - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_X_OFS_USR, (uint8_t *)&x_ofs_usr, 1); - } + ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_Y_OFS_USR, (uint8_t *)&y_ofs_usr, 1); + ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_X_OFS_USR, (uint8_t *)&x_ofs_usr, 1); + if (ret != 0) { return ret; } if ((val.x_mg < (0.0078125f * 127.0f)) && (val.x_mg > (0.0078125f * -127.0f)) && (val.y_mg < (0.0078125f * 127.0f)) && (val.y_mg > (0.0078125f * -127.0f)) && - (val.z_mg < (0.0078125f * 127.0f)) && (val.z_mg > (0.0078125f * -127.0f))) { + (val.z_mg < (0.0078125f * 127.0f)) && (val.z_mg > (0.0078125f * -127.0f))) + { ctrl9.usr_off_w = 0; tmp = val.z_mg / 0.0078125f; @@ -261,9 +272,11 @@ int32_t lsm6dsv16x_xl_offset_mg_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_xl_offset_ tmp = val.x_mg / 0.0078125f; x_ofs_usr.x_ofs_usr = (uint8_t)tmp; - } else if ((val.x_mg < (0.125f * 127.0f)) && (val.x_mg > (0.125f * -127.0f)) && - (val.y_mg < (0.125f * 127.0f)) && (val.y_mg > (0.125f * -127.0f)) && - (val.z_mg < (0.125f * 127.0f)) && (val.z_mg > (0.125f * -127.0f))) { + } + else if ((val.x_mg < (0.125f * 127.0f)) && (val.x_mg > (0.125f * -127.0f)) && + (val.y_mg < (0.125f * 127.0f)) && (val.y_mg > (0.125f * -127.0f)) && + (val.z_mg < (0.125f * 127.0f)) && (val.z_mg > (0.125f * -127.0f))) + { ctrl9.usr_off_w = 1; tmp = val.z_mg / 0.125f; @@ -274,25 +287,20 @@ int32_t lsm6dsv16x_xl_offset_mg_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_xl_offset_ tmp = val.x_mg / 0.125f; x_ofs_usr.x_ofs_usr = (uint8_t)tmp; - } else { // out of limit + } + else // out of limit + { ctrl9.usr_off_w = 1; z_ofs_usr.z_ofs_usr = 0xFFU; y_ofs_usr.y_ofs_usr = 0xFFU; x_ofs_usr.x_ofs_usr = 0xFFU; } - if (ret == 0) { - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_Z_OFS_USR, (uint8_t *)&z_ofs_usr, 1); - } - if (ret == 0) { - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_Y_OFS_USR, (uint8_t *)&y_ofs_usr, 1); - } - if (ret == 0) { - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_X_OFS_USR, (uint8_t *)&x_ofs_usr, 1); - } - if (ret == 0) { - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_CTRL9, (uint8_t *)&ctrl9, 1); - } + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_Z_OFS_USR, (uint8_t *)&z_ofs_usr, 1); + ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_Y_OFS_USR, (uint8_t *)&y_ofs_usr, 1); + ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_X_OFS_USR, (uint8_t *)&x_ofs_usr, 1); + ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_CTRL9, (uint8_t *)&ctrl9, 1); + return ret; } @@ -304,7 +312,8 @@ int32_t lsm6dsv16x_xl_offset_mg_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_xl_offset_ * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_xl_offset_mg_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_xl_offset_mg_t *val) +int32_t lsm6dsv16x_xl_offset_mg_get(stmdev_ctx_t *ctx, + lsm6dsv16x_xl_offset_mg_t *val) { lsm6dsv16x_z_ofs_usr_t z_ofs_usr; lsm6dsv16x_y_ofs_usr_t y_ofs_usr; @@ -313,24 +322,22 @@ int32_t lsm6dsv16x_xl_offset_mg_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_xl_offset_ int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL9, (uint8_t *)&ctrl9, 1); - if (ret == 0) { - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_Z_OFS_USR, (uint8_t *)&z_ofs_usr, 1); - } - if (ret == 0) { - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_Y_OFS_USR, (uint8_t *)&y_ofs_usr, 1); - } - if (ret == 0) { - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_X_OFS_USR, (uint8_t *)&x_ofs_usr, 1); - } + ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_Z_OFS_USR, (uint8_t *)&z_ofs_usr, 1); + ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_Y_OFS_USR, (uint8_t *)&y_ofs_usr, 1); + ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_X_OFS_USR, (uint8_t *)&x_ofs_usr, 1); + if (ret != 0) { return ret; } - if (ctrl9.usr_off_w == PROPERTY_DISABLE) { - val->z_mg = ((float)z_ofs_usr.z_ofs_usr * 0.0078125f); - val->y_mg = ((float)y_ofs_usr.y_ofs_usr * 0.0078125f); - val->x_mg = ((float)x_ofs_usr.x_ofs_usr * 0.0078125f); - } else { - val->z_mg = ((float)z_ofs_usr.z_ofs_usr * 0.125f); - val->y_mg = ((float)y_ofs_usr.y_ofs_usr * 0.125f); - val->x_mg = ((float)x_ofs_usr.x_ofs_usr * 0.125f); + if (ctrl9.usr_off_w == PROPERTY_DISABLE) + { + val->z_mg = ((float_t)z_ofs_usr.z_ofs_usr * 0.0078125f); + val->y_mg = ((float_t)y_ofs_usr.y_ofs_usr * 0.0078125f); + val->x_mg = ((float_t)x_ofs_usr.x_ofs_usr * 0.0078125f); + } + else + { + val->z_mg = ((float_t)z_ofs_usr.z_ofs_usr * 0.125f); + val->y_mg = ((float_t)y_ofs_usr.y_ofs_usr * 0.125f); + val->x_mg = ((float_t)x_ofs_usr.x_ofs_usr * 0.125f); } return ret; @@ -349,27 +356,22 @@ int32_t lsm6dsv16x_xl_offset_mg_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_xl_offset_ * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_reset_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_reset_t val) +int32_t lsm6dsv16x_reset_set(stmdev_ctx_t *ctx, lsm6dsv16x_reset_t val) { lsm6dsv16x_func_cfg_access_t func_cfg_access; lsm6dsv16x_ctrl3_t ctrl3; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL3, (uint8_t *)&ctrl3, 1); - if (ret == 0) { - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FUNC_CFG_ACCESS, (uint8_t *)&func_cfg_access, 1); - } + ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FUNC_CFG_ACCESS, (uint8_t *)&func_cfg_access, 1); + if (ret != 0) { return ret; } ctrl3.boot = ((uint8_t)val & 0x04U) >> 2; ctrl3.sw_reset = ((uint8_t)val & 0x02U) >> 1; func_cfg_access.sw_por = (uint8_t)val & 0x01U; - if (ret == 0) { - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_CTRL3, (uint8_t *)&ctrl3, 1); - } - if (ret == 0) { - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_FUNC_CFG_ACCESS, (uint8_t *)&func_cfg_access, 1); - } + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_CTRL3, (uint8_t *)&ctrl3, 1); + ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_FUNC_CFG_ACCESS, (uint8_t *)&func_cfg_access, 1); return ret; } @@ -382,18 +384,18 @@ int32_t lsm6dsv16x_reset_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_reset_t val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_reset_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_reset_t *val) +int32_t lsm6dsv16x_reset_get(stmdev_ctx_t *ctx, lsm6dsv16x_reset_t *val) { lsm6dsv16x_func_cfg_access_t func_cfg_access; lsm6dsv16x_ctrl3_t ctrl3; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL3, (uint8_t *)&ctrl3, 1); - if (ret == 0) { - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FUNC_CFG_ACCESS, (uint8_t *)&func_cfg_access, 1); - } + ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FUNC_CFG_ACCESS, (uint8_t *)&func_cfg_access, 1); + if (ret != 0) { return ret; } - switch ((ctrl3.sw_reset << 2) + (ctrl3.boot << 1) + func_cfg_access.sw_por) { + switch ((ctrl3.sw_reset << 2) + (ctrl3.boot << 1) + func_cfg_access.sw_por) + { case LSM6DSV16X_READY: *val = LSM6DSV16X_READY; break; @@ -414,6 +416,7 @@ int32_t lsm6dsv16x_reset_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_reset_t *val) *val = LSM6DSV16X_GLOBAL_RST; break; } + return ret; } @@ -425,18 +428,17 @@ int32_t lsm6dsv16x_reset_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_reset_t *val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_mem_bank_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_mem_bank_t val) +int32_t lsm6dsv16x_mem_bank_set(stmdev_ctx_t *ctx, lsm6dsv16x_mem_bank_t val) { lsm6dsv16x_func_cfg_access_t func_cfg_access; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FUNC_CFG_ACCESS, (uint8_t *)&func_cfg_access, 1); + if (ret != 0) { return ret; } - if (ret == 0) { - func_cfg_access.shub_reg_access = ((uint8_t)val & 0x02U) >> 1; - func_cfg_access.emb_func_reg_access = (uint8_t)val & 0x01U; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_FUNC_CFG_ACCESS, (uint8_t *)&func_cfg_access, 1); - } + func_cfg_access.shub_reg_access = ((uint8_t)val & 0x02U) >> 1; + func_cfg_access.emb_func_reg_access = (uint8_t)val & 0x01U; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_FUNC_CFG_ACCESS, (uint8_t *)&func_cfg_access, 1); return ret; } @@ -449,14 +451,16 @@ int32_t lsm6dsv16x_mem_bank_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_mem_bank_t val * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_mem_bank_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_mem_bank_t *val) +int32_t lsm6dsv16x_mem_bank_get(stmdev_ctx_t *ctx, lsm6dsv16x_mem_bank_t *val) { lsm6dsv16x_func_cfg_access_t func_cfg_access; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FUNC_CFG_ACCESS, (uint8_t *)&func_cfg_access, 1); + if (ret != 0) { return ret; } - switch ((func_cfg_access.shub_reg_access << 1) + func_cfg_access.emb_func_reg_access) { + switch ((func_cfg_access.shub_reg_access << 1) + func_cfg_access.emb_func_reg_access) + { case LSM6DSV16X_MAIN_MEM_BANK: *val = LSM6DSV16X_MAIN_MEM_BANK; break; @@ -473,6 +477,7 @@ int32_t lsm6dsv16x_mem_bank_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_mem_bank_t *va *val = LSM6DSV16X_MAIN_MEM_BANK; break; } + return ret; } @@ -485,7 +490,7 @@ int32_t lsm6dsv16x_mem_bank_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_mem_bank_t *va * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_device_id_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val) +int32_t lsm6dsv16x_device_id_get(stmdev_ctx_t *ctx, uint8_t *val) { int32_t ret; @@ -498,19 +503,31 @@ int32_t lsm6dsv16x_device_id_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val) * @brief Accelerometer output data rate (ODR) selection.[set] * * @param ctx read / write interface definitions - * @param val XL_ODR_OFF, XL_ODR_AT_1Hz875, XL_ODR_AT_7Hz5, XL_ODR_AT_15Hz, XL_ODR_AT_30Hz, XL_ODR_AT_60Hz, XL_ODR_AT_120Hz, XL_ODR_AT_240Hz, XL_ODR_AT_480Hz, XL_ODR_AT_960Hz, XL_ODR_AT_1920Hz, XL_ODR_AT_3840Hz, XL_ODR_AT_7680Hz, + * @param val lsm6dsv16x_data_rate_t enum * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_xl_data_rate_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_xl_data_rate_t val) +int32_t lsm6dsv16x_xl_data_rate_set(stmdev_ctx_t *ctx, + lsm6dsv16x_data_rate_t val) { lsm6dsv16x_ctrl1_t ctrl1; + lsm6dsv16x_haodr_cfg_t haodr; + uint8_t sel; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL1, (uint8_t *)&ctrl1, 1); - if (ret == 0) { - ctrl1.odr_xl = (uint8_t)val & 0x0Fu; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_CTRL1, (uint8_t *)&ctrl1, 1); + if (ret != 0) { return ret; } + + ctrl1.odr_xl = (uint8_t)val & 0x0Fu; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_CTRL1, (uint8_t *)&ctrl1, 1); + if (ret != 0) { return ret; } + + sel = ((uint8_t)val >> 4) & 0xFU; + if (sel != 0U) + { + ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_HAODR_CFG, (uint8_t *)&haodr, 1); + haodr.haodr_sel = sel; + ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_HAODR_CFG, (uint8_t *)&haodr, 1); } return ret; @@ -520,74 +537,193 @@ int32_t lsm6dsv16x_xl_data_rate_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_xl_data_ra * @brief Accelerometer output data rate (ODR) selection.[get] * * @param ctx read / write interface definitions - * @param val XL_ODR_OFF, XL_ODR_AT_1Hz875, XL_ODR_AT_7Hz5, XL_ODR_AT_15Hz, XL_ODR_AT_30Hz, XL_ODR_AT_60Hz, XL_ODR_AT_120Hz, XL_ODR_AT_240Hz, XL_ODR_AT_480Hz, XL_ODR_AT_960Hz, XL_ODR_AT_1920Hz, XL_ODR_AT_3840Hz, XL_ODR_AT_7680Hz, + * @param val lsm6dsv16x_data_rate_t enum * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_xl_data_rate_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_xl_data_rate_t *val) +int32_t lsm6dsv16x_xl_data_rate_get(stmdev_ctx_t *ctx, + lsm6dsv16x_data_rate_t *val) { lsm6dsv16x_ctrl1_t ctrl1; + lsm6dsv16x_haodr_cfg_t haodr; + uint8_t sel; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL1, (uint8_t *)&ctrl1, 1); + ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_HAODR_CFG, (uint8_t *)&haodr, 1); + if (ret != 0) { return ret; } + + sel = haodr.haodr_sel; - switch (ctrl1.odr_xl) { - case LSM6DSV16X_XL_ODR_OFF: - *val = LSM6DSV16X_XL_ODR_OFF; + switch (ctrl1.odr_xl) + { + case LSM6DSV16X_ODR_OFF: + *val = LSM6DSV16X_ODR_OFF; break; - case LSM6DSV16X_XL_ODR_AT_1Hz875: - *val = LSM6DSV16X_XL_ODR_AT_1Hz875; + case LSM6DSV16X_ODR_AT_1Hz875: + *val = LSM6DSV16X_ODR_AT_1Hz875; break; - case LSM6DSV16X_XL_ODR_AT_7Hz5: - *val = LSM6DSV16X_XL_ODR_AT_7Hz5; + case LSM6DSV16X_ODR_AT_7Hz5: + *val = LSM6DSV16X_ODR_AT_7Hz5; break; - case LSM6DSV16X_XL_ODR_AT_15Hz: - *val = LSM6DSV16X_XL_ODR_AT_15Hz; + case LSM6DSV16X_ODR_AT_15Hz: + switch (sel) { + default: + case 0: + *val = LSM6DSV16X_ODR_AT_15Hz; + break; + case 1: + *val = LSM6DSV16X_ODR_HA01_AT_15Hz625; + break; + case 2: + *val = LSM6DSV16X_ODR_HA02_AT_12Hz5; + break; + } break; - case LSM6DSV16X_XL_ODR_AT_30Hz: - *val = LSM6DSV16X_XL_ODR_AT_30Hz; + case LSM6DSV16X_ODR_AT_30Hz: + switch (sel) { + default: + case 0: + *val = LSM6DSV16X_ODR_AT_30Hz; + break; + case 1: + *val = LSM6DSV16X_ODR_HA01_AT_31Hz25; + break; + case 2: + *val = LSM6DSV16X_ODR_HA02_AT_25Hz; + break; + } break; - case LSM6DSV16X_XL_ODR_AT_60Hz: - *val = LSM6DSV16X_XL_ODR_AT_60Hz; + case LSM6DSV16X_ODR_AT_60Hz: + switch (sel) { + default: + case 0: + *val = LSM6DSV16X_ODR_AT_60Hz; + break; + case 1: + *val = LSM6DSV16X_ODR_HA01_AT_62Hz5; + break; + case 2: + *val = LSM6DSV16X_ODR_HA02_AT_50Hz; + break; + } break; - case LSM6DSV16X_XL_ODR_AT_120Hz: - *val = LSM6DSV16X_XL_ODR_AT_120Hz; + case LSM6DSV16X_ODR_AT_120Hz: + switch (sel) { + default: + case 0: + *val = LSM6DSV16X_ODR_AT_120Hz; + break; + case 1: + *val = LSM6DSV16X_ODR_HA01_AT_125Hz; + break; + case 2: + *val = LSM6DSV16X_ODR_HA02_AT_100Hz; + break; + } break; - case LSM6DSV16X_XL_ODR_AT_240Hz: - *val = LSM6DSV16X_XL_ODR_AT_240Hz; + case LSM6DSV16X_ODR_AT_240Hz: + switch (sel) { + default: + case 0: + *val = LSM6DSV16X_ODR_AT_240Hz; + break; + case 1: + *val = LSM6DSV16X_ODR_HA01_AT_250Hz; + break; + case 2: + *val = LSM6DSV16X_ODR_HA02_AT_200Hz; + break; + } break; - case LSM6DSV16X_XL_ODR_AT_480Hz: - *val = LSM6DSV16X_XL_ODR_AT_480Hz; + case LSM6DSV16X_ODR_AT_480Hz: + switch (sel) { + default: + case 0: + *val = LSM6DSV16X_ODR_AT_480Hz; + break; + case 1: + *val = LSM6DSV16X_ODR_HA01_AT_500Hz; + break; + case 2: + *val = LSM6DSV16X_ODR_HA02_AT_400Hz; + break; + } break; - case LSM6DSV16X_XL_ODR_AT_960Hz: - *val = LSM6DSV16X_XL_ODR_AT_960Hz; + case LSM6DSV16X_ODR_AT_960Hz: + switch (sel) { + default: + case 0: + *val = LSM6DSV16X_ODR_AT_960Hz; + break; + case 1: + *val = LSM6DSV16X_ODR_HA01_AT_1000Hz; + break; + case 2: + *val = LSM6DSV16X_ODR_HA02_AT_800Hz; + break; + } break; - case LSM6DSV16X_XL_ODR_AT_1920Hz: - *val = LSM6DSV16X_XL_ODR_AT_1920Hz; + case LSM6DSV16X_ODR_AT_1920Hz: + switch (sel) { + default: + case 0: + *val = LSM6DSV16X_ODR_AT_1920Hz; + break; + case 1: + *val = LSM6DSV16X_ODR_HA01_AT_2000Hz; + break; + case 2: + *val = LSM6DSV16X_ODR_HA02_AT_1600Hz; + break; + } break; - case LSM6DSV16X_XL_ODR_AT_3840Hz: - *val = LSM6DSV16X_XL_ODR_AT_3840Hz; + case LSM6DSV16X_ODR_AT_3840Hz: + switch (sel) { + default: + case 0: + *val = LSM6DSV16X_ODR_AT_3840Hz; + break; + case 1: + *val = LSM6DSV16X_ODR_HA01_AT_4000Hz; + break; + case 2: + *val = LSM6DSV16X_ODR_HA02_AT_3200Hz; + break; + } break; - case LSM6DSV16X_XL_ODR_AT_7680Hz: - *val = LSM6DSV16X_XL_ODR_AT_7680Hz; + case LSM6DSV16X_ODR_AT_7680Hz: + switch (sel) { + default: + case 0: + *val = LSM6DSV16X_ODR_AT_7680Hz; + break; + case 1: + *val = LSM6DSV16X_ODR_HA01_AT_8000Hz; + break; + case 2: + *val = LSM6DSV16X_ODR_HA02_AT_6400Hz; + break; + } break; default: - *val = LSM6DSV16X_XL_ODR_OFF; + *val = LSM6DSV16X_ODR_OFF; break; } + return ret; } @@ -595,18 +731,19 @@ int32_t lsm6dsv16x_xl_data_rate_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_xl_data_ra * @brief Accelerometer operating mode selection.[set] * * @param ctx read / write interface definitions - * @param val XL_HIGH_PERFORMANCE_MD, XL_HIGH_ACCURANCY_ODR_MD, XL_LOW_POWER_2_AVG_MD, XL_LOW_POWER_4_AVG_MD, XL_LOW_POWER_8_AVG_MD, XL_NORMAL_MD, + * @param val XL_HIGH_PERFORMANCE_MD, XL_HIGH_ACCURACY_ODR_MD, XL_LOW_POWER_2_AVG_MD, XL_LOW_POWER_4_AVG_MD, XL_LOW_POWER_8_AVG_MD, XL_NORMAL_MD, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_xl_mode_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_xl_mode_t val) +int32_t lsm6dsv16x_xl_mode_set(stmdev_ctx_t *ctx, lsm6dsv16x_xl_mode_t val) { lsm6dsv16x_ctrl1_t ctrl1; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL1, (uint8_t *)&ctrl1, 1); - if (ret == 0) { + if (ret == 0) + { ctrl1.op_mode_xl = (uint8_t)val & 0x07U; ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_CTRL1, (uint8_t *)&ctrl1, 1); } @@ -618,24 +755,30 @@ int32_t lsm6dsv16x_xl_mode_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_xl_mode_t val) * @brief Accelerometer operating mode selection.[get] * * @param ctx read / write interface definitions - * @param val XL_HIGH_PERFORMANCE_MD, XL_HIGH_ACCURANCY_ODR_MD, XL_LOW_POWER_2_AVG_MD, XL_LOW_POWER_4_AVG_MD, XL_LOW_POWER_8_AVG_MD, XL_NORMAL_MD, + * @param val XL_HIGH_PERFORMANCE_MD, XL_HIGH_ACCURACY_ODR_MD, XL_LOW_POWER_2_AVG_MD, XL_LOW_POWER_4_AVG_MD, XL_LOW_POWER_8_AVG_MD, XL_NORMAL_MD, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_xl_mode_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_xl_mode_t *val) +int32_t lsm6dsv16x_xl_mode_get(stmdev_ctx_t *ctx, lsm6dsv16x_xl_mode_t *val) { lsm6dsv16x_ctrl1_t ctrl1; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL1, (uint8_t *)&ctrl1, 1); + if (ret != 0) { return ret; } - switch (ctrl1.op_mode_xl) { + switch (ctrl1.op_mode_xl) + { case LSM6DSV16X_XL_HIGH_PERFORMANCE_MD: *val = LSM6DSV16X_XL_HIGH_PERFORMANCE_MD; break; - case LSM6DSV16X_XL_HIGH_ACCURANCY_ODR_MD: - *val = LSM6DSV16X_XL_HIGH_ACCURANCY_ODR_MD; + case LSM6DSV16X_XL_HIGH_ACCURACY_ODR_MD: + *val = LSM6DSV16X_XL_HIGH_ACCURACY_ODR_MD; + break; + + case LSM6DSV16X_XL_ODR_TRIGGERED_MD: + *val = LSM6DSV16X_XL_ODR_TRIGGERED_MD; break; case LSM6DSV16X_XL_LOW_POWER_2_AVG_MD: @@ -658,6 +801,7 @@ int32_t lsm6dsv16x_xl_mode_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_xl_mode_t *val) *val = LSM6DSV16X_XL_HIGH_PERFORMANCE_MD; break; } + return ret; } @@ -665,20 +809,29 @@ int32_t lsm6dsv16x_xl_mode_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_xl_mode_t *val) * @brief Gyroscope output data rate (ODR) selection.[set] * * @param ctx read / write interface definitions - * @param val GY_ODR_OFF, GY_ODR_AT_7Hz5, GY_ODR_AT_15Hz, GY_ODR_AT_30Hz, GY_ODR_AT_60Hz, GY_ODR_AT_120Hz, GY_ODR_AT_240Hz, GY_ODR_AT_480Hz, GY_ODR_AT_960Hz, GY_ODR_AT_1920Hz, GY_ODR_AT_3840Hz, GY_ODR_AT_7680Hz, + * @param val lsm6dsv16x_data_rate_t enum * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_gy_data_rate_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_gy_data_rate_t val) +int32_t lsm6dsv16x_gy_data_rate_set(stmdev_ctx_t *ctx, + lsm6dsv16x_data_rate_t val) { lsm6dsv16x_ctrl2_t ctrl2; + lsm6dsv16x_haodr_cfg_t haodr; + uint8_t sel; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL2, (uint8_t *)&ctrl2, 1); + ctrl2.odr_g = (uint8_t)val & 0x0Fu; + ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_CTRL2, (uint8_t *)&ctrl2, 1); + if (ret != 0) { return ret; } - if (ret == 0) { - ctrl2.odr_g = (uint8_t)val & 0x0Fu; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_CTRL2, (uint8_t *)&ctrl2, 1); + sel = ((uint8_t)val >> 4) & 0xFU; + if (sel != 0U) + { + ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_HAODR_CFG, (uint8_t *)&haodr, 1); + haodr.haodr_sel = sel; + ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_HAODR_CFG, (uint8_t *)&haodr, 1); } return ret; @@ -688,70 +841,193 @@ int32_t lsm6dsv16x_gy_data_rate_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_gy_data_ra * @brief Gyroscope output data rate (ODR) selection.[get] * * @param ctx read / write interface definitions - * @param val GY_ODR_OFF, GY_ODR_AT_7Hz5, GY_ODR_AT_15Hz, GY_ODR_AT_30Hz, GY_ODR_AT_60Hz, GY_ODR_AT_120Hz, GY_ODR_AT_240Hz, GY_ODR_AT_480Hz, GY_ODR_AT_960Hz, GY_ODR_AT_1920Hz, GY_ODR_AT_3840Hz, GY_ODR_AT_7680Hz, + * @param val lsm6dsv16x_data_rate_t enum * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_gy_data_rate_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_gy_data_rate_t *val) +int32_t lsm6dsv16x_gy_data_rate_get(stmdev_ctx_t *ctx, + lsm6dsv16x_data_rate_t *val) { lsm6dsv16x_ctrl2_t ctrl2; + lsm6dsv16x_haodr_cfg_t haodr; + uint8_t sel; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL2, (uint8_t *)&ctrl2, 1); + ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_HAODR_CFG, (uint8_t *)&haodr, 1); + if (ret != 0) { return ret; } + + sel = haodr.haodr_sel; + + switch (ctrl2.odr_g) + { + case LSM6DSV16X_ODR_OFF: + *val = LSM6DSV16X_ODR_OFF; + break; - switch (ctrl2.odr_g) { - case LSM6DSV16X_GY_ODR_OFF: - *val = LSM6DSV16X_GY_ODR_OFF; + case LSM6DSV16X_ODR_AT_1Hz875: + *val = LSM6DSV16X_ODR_AT_1Hz875; break; - case LSM6DSV16X_GY_ODR_AT_7Hz5: - *val = LSM6DSV16X_GY_ODR_AT_7Hz5; + case LSM6DSV16X_ODR_AT_7Hz5: + *val = LSM6DSV16X_ODR_AT_7Hz5; break; - case LSM6DSV16X_GY_ODR_AT_15Hz: - *val = LSM6DSV16X_GY_ODR_AT_15Hz; + case LSM6DSV16X_ODR_AT_15Hz: + switch (sel) { + default: + case 0: + *val = LSM6DSV16X_ODR_AT_15Hz; + break; + case 1: + *val = LSM6DSV16X_ODR_HA01_AT_15Hz625; + break; + case 2: + *val = LSM6DSV16X_ODR_HA02_AT_12Hz5; + break; + } break; - case LSM6DSV16X_GY_ODR_AT_30Hz: - *val = LSM6DSV16X_GY_ODR_AT_30Hz; + case LSM6DSV16X_ODR_AT_30Hz: + switch (sel) { + default: + case 0: + *val = LSM6DSV16X_ODR_AT_30Hz; + break; + case 1: + *val = LSM6DSV16X_ODR_HA01_AT_31Hz25; + break; + case 2: + *val = LSM6DSV16X_ODR_HA02_AT_25Hz; + break; + } break; - case LSM6DSV16X_GY_ODR_AT_60Hz: - *val = LSM6DSV16X_GY_ODR_AT_60Hz; + case LSM6DSV16X_ODR_AT_60Hz: + switch (sel) { + default: + case 0: + *val = LSM6DSV16X_ODR_AT_60Hz; + break; + case 1: + *val = LSM6DSV16X_ODR_HA01_AT_62Hz5; + break; + case 2: + *val = LSM6DSV16X_ODR_HA02_AT_50Hz; + break; + } break; - case LSM6DSV16X_GY_ODR_AT_120Hz: - *val = LSM6DSV16X_GY_ODR_AT_120Hz; + case LSM6DSV16X_ODR_AT_120Hz: + switch (sel) { + default: + case 0: + *val = LSM6DSV16X_ODR_AT_120Hz; + break; + case 1: + *val = LSM6DSV16X_ODR_HA01_AT_125Hz; + break; + case 2: + *val = LSM6DSV16X_ODR_HA02_AT_100Hz; + break; + } break; - case LSM6DSV16X_GY_ODR_AT_240Hz: - *val = LSM6DSV16X_GY_ODR_AT_240Hz; + case LSM6DSV16X_ODR_AT_240Hz: + switch (sel) { + default: + case 0: + *val = LSM6DSV16X_ODR_AT_240Hz; + break; + case 1: + *val = LSM6DSV16X_ODR_HA01_AT_250Hz; + break; + case 2: + *val = LSM6DSV16X_ODR_HA02_AT_200Hz; + break; + } break; - case LSM6DSV16X_GY_ODR_AT_480Hz: - *val = LSM6DSV16X_GY_ODR_AT_480Hz; + case LSM6DSV16X_ODR_AT_480Hz: + switch (sel) { + default: + case 0: + *val = LSM6DSV16X_ODR_AT_480Hz; + break; + case 1: + *val = LSM6DSV16X_ODR_HA01_AT_500Hz; + break; + case 2: + *val = LSM6DSV16X_ODR_HA02_AT_400Hz; + break; + } break; - case LSM6DSV16X_GY_ODR_AT_960Hz: - *val = LSM6DSV16X_GY_ODR_AT_960Hz; + case LSM6DSV16X_ODR_AT_960Hz: + switch (sel) { + default: + case 0: + *val = LSM6DSV16X_ODR_AT_960Hz; + break; + case 1: + *val = LSM6DSV16X_ODR_HA01_AT_1000Hz; + break; + case 2: + *val = LSM6DSV16X_ODR_HA02_AT_800Hz; + break; + } break; - case LSM6DSV16X_GY_ODR_AT_1920Hz: - *val = LSM6DSV16X_GY_ODR_AT_1920Hz; + case LSM6DSV16X_ODR_AT_1920Hz: + switch (sel) { + default: + case 0: + *val = LSM6DSV16X_ODR_AT_1920Hz; + break; + case 1: + *val = LSM6DSV16X_ODR_HA01_AT_2000Hz; + break; + case 2: + *val = LSM6DSV16X_ODR_HA02_AT_1600Hz; + break; + } break; - case LSM6DSV16X_GY_ODR_AT_3840Hz: - *val = LSM6DSV16X_GY_ODR_AT_3840Hz; + case LSM6DSV16X_ODR_AT_3840Hz: + switch (sel) { + default: + case 0: + *val = LSM6DSV16X_ODR_AT_3840Hz; + break; + case 1: + *val = LSM6DSV16X_ODR_HA01_AT_4000Hz; + break; + case 2: + *val = LSM6DSV16X_ODR_HA02_AT_3200Hz; + break; + } break; - case LSM6DSV16X_GY_ODR_AT_7680Hz: - *val = LSM6DSV16X_GY_ODR_AT_7680Hz; + case LSM6DSV16X_ODR_AT_7680Hz: + switch (sel) { + default: + case 0: + *val = LSM6DSV16X_ODR_AT_7680Hz; + break; + case 1: + *val = LSM6DSV16X_ODR_HA01_AT_8000Hz; + break; + case 2: + *val = LSM6DSV16X_ODR_HA02_AT_6400Hz; + break; + } break; default: - *val = LSM6DSV16X_GY_ODR_OFF; + *val = LSM6DSV16X_ODR_OFF; break; } + return ret; } @@ -759,17 +1035,18 @@ int32_t lsm6dsv16x_gy_data_rate_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_gy_data_ra * @brief Gyroscope operating mode selection.[set] * * @param ctx read / write interface definitions - * @param val GY_HIGH_PERFORMANCE_MD, GY_HIGH_ACCURANCY_ODR_MD, GY_SLEEP_MD, GY_LOW_POWER_MD, + * @param val GY_HIGH_PERFORMANCE_MD, GY_HIGH_ACCURACY_ODR_MD, GY_SLEEP_MD, GY_LOW_POWER_MD, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_gy_mode_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_gy_mode_t val) +int32_t lsm6dsv16x_gy_mode_set(stmdev_ctx_t *ctx, lsm6dsv16x_gy_mode_t val) { lsm6dsv16x_ctrl2_t ctrl2; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL2, (uint8_t *)&ctrl2, 1); - if (ret == 0) { + if (ret == 0) + { ctrl2.op_mode_g = (uint8_t)val & 0x07U; ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_CTRL2, (uint8_t *)&ctrl2, 1); } @@ -781,23 +1058,26 @@ int32_t lsm6dsv16x_gy_mode_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_gy_mode_t val) * @brief Gyroscope operating mode selection.[get] * * @param ctx read / write interface definitions - * @param val GY_HIGH_PERFORMANCE_MD, GY_HIGH_ACCURANCY_ODR_MD, GY_SLEEP_MD, GY_LOW_POWER_MD, + * @param val GY_HIGH_PERFORMANCE_MD, GY_HIGH_ACCURACY_ODR_MD, GY_SLEEP_MD, GY_LOW_POWER_MD, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_gy_mode_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_gy_mode_t *val) +int32_t lsm6dsv16x_gy_mode_get(stmdev_ctx_t *ctx, lsm6dsv16x_gy_mode_t *val) { lsm6dsv16x_ctrl2_t ctrl2; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL2, (uint8_t *)&ctrl2, 1); - switch (ctrl2.op_mode_g) { + if (ret != 0) { return ret; } + + switch (ctrl2.op_mode_g) + { case LSM6DSV16X_GY_HIGH_PERFORMANCE_MD: *val = LSM6DSV16X_GY_HIGH_PERFORMANCE_MD; break; - case LSM6DSV16X_GY_HIGH_ACCURANCY_ODR_MD: - *val = LSM6DSV16X_GY_HIGH_ACCURANCY_ODR_MD; + case LSM6DSV16X_GY_HIGH_ACCURACY_ODR_MD: + *val = LSM6DSV16X_GY_HIGH_ACCURACY_ODR_MD; break; case LSM6DSV16X_GY_SLEEP_MD: @@ -812,6 +1092,7 @@ int32_t lsm6dsv16x_gy_mode_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_gy_mode_t *val) *val = LSM6DSV16X_GY_HIGH_PERFORMANCE_MD; break; } + return ret; } @@ -823,13 +1104,14 @@ int32_t lsm6dsv16x_gy_mode_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_gy_mode_t *val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_auto_increment_set(lsm6dsv16x_ctx_t *ctx, uint8_t val) +int32_t lsm6dsv16x_auto_increment_set(stmdev_ctx_t *ctx, uint8_t val) { lsm6dsv16x_ctrl3_t ctrl3; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL3, (uint8_t *)&ctrl3, 1); - if (ret == 0) { + if (ret == 0) + { ctrl3.if_inc = val; ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_CTRL3, (uint8_t *)&ctrl3, 1); } @@ -845,7 +1127,7 @@ int32_t lsm6dsv16x_auto_increment_set(lsm6dsv16x_ctx_t *ctx, uint8_t val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_auto_increment_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val) +int32_t lsm6dsv16x_auto_increment_get(stmdev_ctx_t *ctx, uint8_t *val) { lsm6dsv16x_ctrl3_t ctrl3; int32_t ret; @@ -853,7 +1135,6 @@ int32_t lsm6dsv16x_auto_increment_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val) ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL3, (uint8_t *)&ctrl3, 1); *val = ctrl3.if_inc; - return ret; } @@ -865,14 +1146,15 @@ int32_t lsm6dsv16x_auto_increment_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_block_data_update_set(lsm6dsv16x_ctx_t *ctx, uint8_t val) +int32_t lsm6dsv16x_block_data_update_set(stmdev_ctx_t *ctx, uint8_t val) { lsm6dsv16x_ctrl3_t ctrl3; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL3, (uint8_t *)&ctrl3, 1); - if (ret == 0) { + if (ret == 0) + { ctrl3.bdu = val; ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_CTRL3, (uint8_t *)&ctrl3, 1); } @@ -888,7 +1170,7 @@ int32_t lsm6dsv16x_block_data_update_set(lsm6dsv16x_ctx_t *ctx, uint8_t val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_block_data_update_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val) +int32_t lsm6dsv16x_block_data_update_get(stmdev_ctx_t *ctx, uint8_t *val) { lsm6dsv16x_ctrl3_t ctrl3; int32_t ret; @@ -899,6 +1181,53 @@ int32_t lsm6dsv16x_block_data_update_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val) return ret; } +/** + * @brief Configure ODR trigger. [set] + * + * @param ctx read / write interface definitions + * @param val number of data in the reference period. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_odr_trig_cfg_set(stmdev_ctx_t *ctx, uint8_t val) +{ + lsm6dsv16x_odr_trig_cfg_t odr_trig; + int32_t ret; + + if (val >= 1U && val <= 3U) { + return -1; + } + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_ODR_TRIG_CFG, (uint8_t *)&odr_trig, 1); + + if (ret == 0) + { + odr_trig.odr_trig_nodr = val; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_ODR_TRIG_CFG, (uint8_t *)&odr_trig, 1); + } + + return ret; +} + +/** + * @brief Configure ODR trigger. [get] + * + * @param ctx read / write interface definitions + * @param val number of data in the reference period. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_odr_trig_cfg_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + lsm6dsv16x_odr_trig_cfg_t odr_trig; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_ODR_TRIG_CFG, (uint8_t *)&odr_trig, 1); + *val = odr_trig.odr_trig_nodr; + + return ret; +} + /** * @brief Enables pulsed data-ready mode (~75 us).[set] * @@ -907,14 +1236,16 @@ int32_t lsm6dsv16x_block_data_update_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_data_ready_mode_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_data_ready_mode_t val) +int32_t lsm6dsv16x_data_ready_mode_set(stmdev_ctx_t *ctx, + lsm6dsv16x_data_ready_mode_t val) { lsm6dsv16x_ctrl4_t ctrl4; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL4, (uint8_t *)&ctrl4, 1); - if (ret == 0) { + if (ret == 0) + { ctrl4.drdy_pulsed = (uint8_t)val & 0x1U; ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_CTRL4, (uint8_t *)&ctrl4, 1); } @@ -930,14 +1261,16 @@ int32_t lsm6dsv16x_data_ready_mode_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_data_re * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_data_ready_mode_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_data_ready_mode_t *val) +int32_t lsm6dsv16x_data_ready_mode_get(stmdev_ctx_t *ctx, + lsm6dsv16x_data_ready_mode_t *val) { lsm6dsv16x_ctrl4_t ctrl4; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL4, (uint8_t *)&ctrl4, 1); - switch (ctrl4.drdy_pulsed) { + switch (ctrl4.drdy_pulsed) + { case LSM6DSV16X_DRDY_LATCHED: *val = LSM6DSV16X_DRDY_LATCHED; break; @@ -950,6 +1283,59 @@ int32_t lsm6dsv16x_data_ready_mode_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_data_re *val = LSM6DSV16X_DRDY_LATCHED; break; } + + return ret; +} + +/** + * @brief Enables interrupt.[set] + * + * @param ctx read / write interface definitions + * @param val enable/disable, latched/pulsed + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_interrupt_enable_set(stmdev_ctx_t *ctx, + lsm6dsv16x_interrupt_mode_t val) +{ + lsm6dsv16x_tap_cfg0_t cfg; + lsm6dsv16x_functions_enable_t func; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FUNCTIONS_ENABLE, (uint8_t *)&func, 1); + func.interrupts_enable = val.enable; + ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_FUNCTIONS_ENABLE, (uint8_t *)&func, 1); + if (ret != 0) { return ret; } + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_TAP_CFG0, (uint8_t *)&cfg, 1); + cfg.lir = val.lir; + ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_TAP_CFG0, (uint8_t *)&cfg, 1); + + return ret; +} + +/** + * @brief Enables latched interrupt mode.[get] + * + * @param ctx read / write interface definitions + * @param val enable/disable, latched/pulsed + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_interrupt_enable_get(stmdev_ctx_t *ctx, + lsm6dsv16x_interrupt_mode_t *val) +{ + lsm6dsv16x_tap_cfg0_t cfg; + lsm6dsv16x_functions_enable_t func; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FUNCTIONS_ENABLE, (uint8_t *)&func, 1); + ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_TAP_CFG0, (uint8_t *)&cfg, 1); + if (ret != 0) { return ret; } + + val->enable = func.interrupts_enable; + val->lir = cfg.lir; + return ret; } @@ -961,14 +1347,16 @@ int32_t lsm6dsv16x_data_ready_mode_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_data_re * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_gy_full_scale_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_gy_full_scale_t val) +int32_t lsm6dsv16x_gy_full_scale_set(stmdev_ctx_t *ctx, + lsm6dsv16x_gy_full_scale_t val) { lsm6dsv16x_ctrl6_t ctrl6; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL6, (uint8_t *)&ctrl6, 1); - if (ret == 0) { + if (ret == 0) + { ctrl6.fs_g = (uint8_t)val & 0xfu; ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_CTRL6, (uint8_t *)&ctrl6, 1); } @@ -984,14 +1372,17 @@ int32_t lsm6dsv16x_gy_full_scale_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_gy_full_s * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_gy_full_scale_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_gy_full_scale_t *val) +int32_t lsm6dsv16x_gy_full_scale_get(stmdev_ctx_t *ctx, + lsm6dsv16x_gy_full_scale_t *val) { lsm6dsv16x_ctrl6_t ctrl6; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL6, (uint8_t *)&ctrl6, 1); + if (ret != 0) { return ret; } - switch (ctrl6.fs_g) { + switch (ctrl6.fs_g) + { case LSM6DSV16X_125dps: *val = LSM6DSV16X_125dps; break; @@ -1020,6 +1411,7 @@ int32_t lsm6dsv16x_gy_full_scale_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_gy_full_s *val = LSM6DSV16X_125dps; break; } + return ret; } @@ -1031,14 +1423,16 @@ int32_t lsm6dsv16x_gy_full_scale_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_gy_full_s * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_xl_full_scale_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_xl_full_scale_t val) +int32_t lsm6dsv16x_xl_full_scale_set(stmdev_ctx_t *ctx, + lsm6dsv16x_xl_full_scale_t val) { lsm6dsv16x_ctrl8_t ctrl8; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL8, (uint8_t *)&ctrl8, 1); - if (ret == 0) { + if (ret == 0) + { ctrl8.fs_xl = (uint8_t)val & 0x3U; ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_CTRL8, (uint8_t *)&ctrl8, 1); } @@ -1054,14 +1448,17 @@ int32_t lsm6dsv16x_xl_full_scale_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_xl_full_s * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_xl_full_scale_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_xl_full_scale_t *val) +int32_t lsm6dsv16x_xl_full_scale_get(stmdev_ctx_t *ctx, + lsm6dsv16x_xl_full_scale_t *val) { lsm6dsv16x_ctrl8_t ctrl8; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL8, (uint8_t *)&ctrl8, 1); + if (ret != 0) { return ret; } - switch (ctrl8.fs_xl) { + switch (ctrl8.fs_xl) + { case LSM6DSV16X_2g: *val = LSM6DSV16X_2g; break; @@ -1082,6 +1479,7 @@ int32_t lsm6dsv16x_xl_full_scale_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_xl_full_s *val = LSM6DSV16X_2g; break; } + return ret; } @@ -1093,14 +1491,15 @@ int32_t lsm6dsv16x_xl_full_scale_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_xl_full_s * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_xl_dual_channel_set(lsm6dsv16x_ctx_t *ctx, uint8_t val) +int32_t lsm6dsv16x_xl_dual_channel_set(stmdev_ctx_t *ctx, uint8_t val) { lsm6dsv16x_ctrl8_t ctrl8; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL8, (uint8_t *)&ctrl8, 1); - if (ret == 0) { + if (ret == 0) + { ctrl8.xl_dualc_en = val; ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_CTRL8, (uint8_t *)&ctrl8, 1); } @@ -1116,7 +1515,7 @@ int32_t lsm6dsv16x_xl_dual_channel_set(lsm6dsv16x_ctx_t *ctx, uint8_t val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_xl_dual_channel_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val) +int32_t lsm6dsv16x_xl_dual_channel_get(stmdev_ctx_t *ctx, uint8_t *val) { lsm6dsv16x_ctrl8_t ctrl8; int32_t ret; @@ -1135,14 +1534,16 @@ int32_t lsm6dsv16x_xl_dual_channel_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_xl_self_test_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_xl_self_test_t val) +int32_t lsm6dsv16x_xl_self_test_set(stmdev_ctx_t *ctx, + lsm6dsv16x_xl_self_test_t val) { lsm6dsv16x_ctrl10_t ctrl10; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL10, (uint8_t *)&ctrl10, 1); - if (ret == 0) { + if (ret == 0) + { ctrl10.st_xl = (uint8_t)val & 0x3U; ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_CTRL10, (uint8_t *)&ctrl10, 1); } @@ -1158,14 +1559,17 @@ int32_t lsm6dsv16x_xl_self_test_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_xl_self_te * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_xl_self_test_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_xl_self_test_t *val) +int32_t lsm6dsv16x_xl_self_test_get(stmdev_ctx_t *ctx, + lsm6dsv16x_xl_self_test_t *val) { lsm6dsv16x_ctrl10_t ctrl10; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL10, (uint8_t *)&ctrl10, 1); + if (ret != 0) { return ret; } - switch (ctrl10.st_xl) { + switch (ctrl10.st_xl) + { case LSM6DSV16X_XL_ST_DISABLE: *val = LSM6DSV16X_XL_ST_DISABLE; break; @@ -1182,6 +1586,7 @@ int32_t lsm6dsv16x_xl_self_test_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_xl_self_te *val = LSM6DSV16X_XL_ST_DISABLE; break; } + return ret; } @@ -1193,14 +1598,16 @@ int32_t lsm6dsv16x_xl_self_test_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_xl_self_te * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_gy_self_test_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_gy_self_test_t val) +int32_t lsm6dsv16x_gy_self_test_set(stmdev_ctx_t *ctx, + lsm6dsv16x_gy_self_test_t val) { lsm6dsv16x_ctrl10_t ctrl10; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL10, (uint8_t *)&ctrl10, 1); - if (ret == 0) { + if (ret == 0) + { ctrl10.st_g = (uint8_t)val & 0x3U; ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_CTRL10, (uint8_t *)&ctrl10, 1); } @@ -1216,14 +1623,17 @@ int32_t lsm6dsv16x_gy_self_test_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_gy_self_te * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_gy_self_test_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_gy_self_test_t *val) +int32_t lsm6dsv16x_gy_self_test_get(stmdev_ctx_t *ctx, + lsm6dsv16x_gy_self_test_t *val) { lsm6dsv16x_ctrl10_t ctrl10; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL10, (uint8_t *)&ctrl10, 1); + if (ret != 0) { return ret; } - switch (ctrl10.st_g) { + switch (ctrl10.st_g) + { case LSM6DSV16X_GY_ST_DISABLE: *val = LSM6DSV16X_GY_ST_DISABLE; break; @@ -1240,6 +1650,7 @@ int32_t lsm6dsv16x_gy_self_test_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_gy_self_te *val = LSM6DSV16X_GY_ST_DISABLE; break; } + return ret; } @@ -1251,14 +1662,16 @@ int32_t lsm6dsv16x_gy_self_test_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_gy_self_te * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_ois_xl_self_test_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ois_xl_self_test_t val) +int32_t lsm6dsv16x_ois_xl_self_test_set(stmdev_ctx_t *ctx, + lsm6dsv16x_ois_xl_self_test_t val) { lsm6dsv16x_spi2_int_ois_t spi2_int_ois; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_SPI2_INT_OIS, (uint8_t *)&spi2_int_ois, 1); - if (ret == 0) { + if (ret == 0) + { spi2_int_ois.st_xl_ois = ((uint8_t)val & 0x3U); ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_SPI2_INT_OIS, (uint8_t *)&spi2_int_ois, 1); } @@ -1274,14 +1687,17 @@ int32_t lsm6dsv16x_ois_xl_self_test_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ois_xl * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_ois_xl_self_test_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ois_xl_self_test_t *val) +int32_t lsm6dsv16x_ois_xl_self_test_get(stmdev_ctx_t *ctx, + lsm6dsv16x_ois_xl_self_test_t *val) { lsm6dsv16x_spi2_int_ois_t spi2_int_ois; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_SPI2_INT_OIS, (uint8_t *)&spi2_int_ois, 1); + if (ret != 0) { return ret; } - switch (spi2_int_ois.st_xl_ois) { + switch (spi2_int_ois.st_xl_ois) + { case LSM6DSV16X_OIS_XL_ST_DISABLE: *val = LSM6DSV16X_OIS_XL_ST_DISABLE; break; @@ -1298,6 +1714,7 @@ int32_t lsm6dsv16x_ois_xl_self_test_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ois_xl *val = LSM6DSV16X_OIS_XL_ST_DISABLE; break; } + return ret; } @@ -1309,14 +1726,16 @@ int32_t lsm6dsv16x_ois_xl_self_test_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ois_xl * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_ois_gy_self_test_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ois_gy_self_test_t val) +int32_t lsm6dsv16x_ois_gy_self_test_set(stmdev_ctx_t *ctx, + lsm6dsv16x_ois_gy_self_test_t val) { lsm6dsv16x_spi2_int_ois_t spi2_int_ois; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_SPI2_INT_OIS, (uint8_t *)&spi2_int_ois, 1); - if (ret == 0) { + if (ret == 0) + { spi2_int_ois.st_g_ois = ((uint8_t)val & 0x3U); spi2_int_ois.st_ois_clampdis = ((uint8_t)val & 0x04U) >> 2; ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_SPI2_INT_OIS, (uint8_t *)&spi2_int_ois, 1); @@ -1333,14 +1752,17 @@ int32_t lsm6dsv16x_ois_gy_self_test_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ois_gy * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_ois_gy_self_test_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ois_gy_self_test_t *val) +int32_t lsm6dsv16x_ois_gy_self_test_get(stmdev_ctx_t *ctx, + lsm6dsv16x_ois_gy_self_test_t *val) { lsm6dsv16x_spi2_int_ois_t spi2_int_ois; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_SPI2_INT_OIS, (uint8_t *)&spi2_int_ois, 1); + if (ret != 0) { return ret; } - switch (spi2_int_ois.st_g_ois) { + switch (spi2_int_ois.st_g_ois) + { case LSM6DSV16X_OIS_GY_ST_DISABLE: *val = LSM6DSV16X_OIS_GY_ST_DISABLE; break; @@ -1357,9 +1779,197 @@ int32_t lsm6dsv16x_ois_gy_self_test_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ois_gy *val = LSM6DSV16X_OIS_GY_ST_DISABLE; break; } + + return ret; +} + +/** + * @defgroup interrupt_pins + * @brief This section groups all the functions that manage + * interrupt pins + * @{ + * + */ + +/** + * @brief Select the signal that need to route on int1 pad[set] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val the signals to route on int1 pin. + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6dsv16x_pin_int1_route_set(stmdev_ctx_t *ctx, + lsm6dsv16x_pin_int_route_t *val) +{ + lsm6dsv16x_int1_ctrl_t int1_ctrl; + lsm6dsv16x_md1_cfg_t md1_cfg; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_INT1_CTRL, (uint8_t *)&int1_ctrl, 1); + if (ret != 0) { return ret; } + + int1_ctrl.int1_drdy_xl = val->drdy_xl; + int1_ctrl.int1_drdy_g = val->drdy_g; + int1_ctrl.int1_fifo_th = val->fifo_th; + int1_ctrl.int1_fifo_ovr = val->fifo_ovr; + int1_ctrl.int1_fifo_full = val->fifo_full; + int1_ctrl.int1_cnt_bdr = val->cnt_bdr; + + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_INT1_CTRL, (uint8_t *)&int1_ctrl, 1); + if (ret != 0) { return ret; } + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_MD1_CFG, (uint8_t *)&md1_cfg, 1); + if (ret != 0) { return ret; } + + md1_cfg.int1_shub = val->shub; + md1_cfg.int1_emb_func = val->emb_func; + md1_cfg.int1_6d = val->sixd; + md1_cfg.int1_single_tap = val->single_tap; + md1_cfg.int1_double_tap = val->double_tap; + md1_cfg.int1_wu = val->wakeup; + md1_cfg.int1_ff = val->freefall; + md1_cfg.int1_sleep_change = val->sleep_change; + + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_MD1_CFG, (uint8_t *)&md1_cfg, 1); + + return ret; +} + +/** + * @brief Select the signal that need to route on int1 pad.[get] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val the signals that are routed on int1 pin.(ptr) + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6dsv16x_pin_int1_route_get(stmdev_ctx_t *ctx, + lsm6dsv16x_pin_int_route_t *val) +{ + lsm6dsv16x_int1_ctrl_t int1_ctrl; + lsm6dsv16x_md1_cfg_t md1_cfg; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_INT1_CTRL, (uint8_t *)&int1_ctrl, 1); + if (ret != 0) { return ret; } + + val->drdy_xl = int1_ctrl.int1_drdy_xl; + val->drdy_g = int1_ctrl.int1_drdy_g; + val->fifo_th = int1_ctrl.int1_fifo_th; + val->fifo_ovr = int1_ctrl.int1_fifo_ovr; + val->fifo_full = int1_ctrl.int1_fifo_full; + val->cnt_bdr = int1_ctrl.int1_cnt_bdr; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_MD1_CFG, (uint8_t *)&md1_cfg, 1); + if (ret != 0) { return ret; } + + val->shub = md1_cfg.int1_shub; + val->emb_func = md1_cfg.int1_emb_func; + val->sixd = md1_cfg.int1_6d; + val->single_tap = md1_cfg.int1_single_tap; + val->double_tap = md1_cfg.int1_double_tap; + val->wakeup = md1_cfg.int1_wu; + val->freefall = md1_cfg.int1_ff; + val->sleep_change = md1_cfg.int1_sleep_change; + + return ret; +} + +/** + * @brief Select the signal that need to route on int2 pad[set] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val the signals to route on int1 pin. + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6dsv16x_pin_int2_route_set(stmdev_ctx_t *ctx, + lsm6dsv16x_pin_int_route_t *val) +{ + lsm6dsv16x_int2_ctrl_t int2_ctrl; + lsm6dsv16x_md2_cfg_t md2_cfg; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_INT2_CTRL, (uint8_t *)&int2_ctrl, 1); + if (ret != 0) { return ret; } + + int2_ctrl.int2_drdy_xl = val->drdy_xl; + int2_ctrl.int2_drdy_g = val->drdy_g; + int2_ctrl.int2_fifo_th = val->fifo_th; + int2_ctrl.int2_fifo_ovr = val->fifo_ovr; + int2_ctrl.int2_fifo_full = val->fifo_full; + int2_ctrl.int2_cnt_bdr = val->cnt_bdr; + int2_ctrl.int2_drdy_g_eis = val->drdy_g_eis; + int2_ctrl.int2_emb_func_endop = val->emb_func_endop; + + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_INT2_CTRL, (uint8_t *)&int2_ctrl, 1); + if (ret != 0) { return ret; } + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_MD2_CFG, (uint8_t *)&md2_cfg, 1); + if (ret != 0) { return ret; } + + md2_cfg.int2_timestamp = val->timestamp; + md2_cfg.int2_emb_func = val->emb_func; + md2_cfg.int2_6d = val->sixd; + md2_cfg.int2_single_tap = val->single_tap; + md2_cfg.int2_double_tap = val->double_tap; + md2_cfg.int2_wu = val->wakeup; + md2_cfg.int2_ff = val->freefall; + md2_cfg.int2_sleep_change = val->sleep_change; + + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_MD2_CFG, (uint8_t *)&md2_cfg, 1); + + return ret; +} + +/** + * @brief Select the signal that need to route on int2 pad.[get] + * + * @param ctx Read / write interface definitions.(ptr) + * @param val the signals that are routed on int1 pin.(ptr) + * @retval Interface status (MANDATORY: return 0 -> no Error). + * + */ +int32_t lsm6dsv16x_pin_int2_route_get(stmdev_ctx_t *ctx, + lsm6dsv16x_pin_int_route_t *val) +{ + lsm6dsv16x_int2_ctrl_t int2_ctrl; + lsm6dsv16x_md2_cfg_t md2_cfg; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_INT2_CTRL, (uint8_t *)&int2_ctrl, 1); + if (ret != 0) { return ret; } + + val->drdy_xl = int2_ctrl.int2_drdy_xl; + val->drdy_g = int2_ctrl.int2_drdy_g; + val->fifo_th = int2_ctrl.int2_fifo_th; + val->fifo_ovr = int2_ctrl.int2_fifo_ovr; + val->fifo_full = int2_ctrl.int2_fifo_full; + val->cnt_bdr = int2_ctrl.int2_cnt_bdr; + val->drdy_g_eis = int2_ctrl.int2_drdy_g_eis; + val->emb_func_endop = int2_ctrl.int2_emb_func_endop; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_MD2_CFG, (uint8_t *)&md2_cfg, 1); + if (ret != 0) { return ret; } + + val->timestamp = md2_cfg.int2_timestamp; + val->emb_func = md2_cfg.int2_emb_func; + val->sixd = md2_cfg.int2_6d; + val->single_tap = md2_cfg.int2_single_tap; + val->double_tap = md2_cfg.int2_double_tap; + val->wakeup = md2_cfg.int2_wu; + val->freefall = md2_cfg.int2_ff; + val->sleep_change = md2_cfg.int2_sleep_change; + return ret; } +/** + * @} + * + */ + /** * @brief Get the status of all the interrupt sources.[get] * @@ -1368,7 +1978,8 @@ int32_t lsm6dsv16x_ois_gy_self_test_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ois_gy * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_all_sources_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_all_sources_t *val) +int32_t lsm6dsv16x_all_sources_get(stmdev_ctx_t *ctx, + lsm6dsv16x_all_sources_t *val) { lsm6dsv16x_emb_func_status_mainpage_t emb_func_status_mainpage; lsm6dsv16x_emb_func_exec_status_t emb_func_exec_status; @@ -1388,14 +1999,13 @@ int32_t lsm6dsv16x_all_sources_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_all_sources int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FUNCTIONS_ENABLE, (uint8_t *)&functions_enable, 1); - if (ret == 0) { - functions_enable.dis_rst_lir_all_int = PROPERTY_ENABLE; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_FUNCTIONS_ENABLE, (uint8_t *)&functions_enable, 1); - } + functions_enable.dis_rst_lir_all_int = PROPERTY_ENABLE; + ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_FUNCTIONS_ENABLE, (uint8_t *)&functions_enable, 1); + if (ret != 0) { return ret; } + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FIFO_STATUS1, (uint8_t *)&buff, 4); + if (ret != 0) { return ret; } - if (ret == 0) { - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FIFO_STATUS1, (uint8_t *)&buff, 4); - } bytecpy((uint8_t *)&fifo_status2, &buff[1]); bytecpy((uint8_t *)&all_int_src, &buff[2]); bytecpy((uint8_t *)&status_reg, &buff[3]); @@ -1417,17 +2027,13 @@ int32_t lsm6dsv16x_all_sources_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_all_sources val->drdy_ois = status_reg.ois_drdy; val->timestamp = status_reg.timestamp_endcount; - if (ret == 0) { - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FUNCTIONS_ENABLE, (uint8_t *)&functions_enable, 1); - } - if (ret == 0) { - functions_enable.dis_rst_lir_all_int = PROPERTY_DISABLE; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_FUNCTIONS_ENABLE, (uint8_t *)&functions_enable, 1); - } + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FUNCTIONS_ENABLE, (uint8_t *)&functions_enable, 1); + functions_enable.dis_rst_lir_all_int = PROPERTY_DISABLE; + ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_FUNCTIONS_ENABLE, (uint8_t *)&functions_enable, 1); + if (ret != 0) { return ret; } - if (ret == 0) { - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_UI_STATUS_REG_OIS, (uint8_t *)&buff, 8); - } + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_UI_STATUS_REG_OIS, (uint8_t *)&buff, 8); + if (ret != 0) { return ret; } bytecpy((uint8_t *)&status_reg_ois, &buff[0]); bytecpy((uint8_t *)&wake_up_src, &buff[1]); @@ -1478,18 +2084,12 @@ int32_t lsm6dsv16x_all_sources_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_all_sources val->mlc4 = mlc_status_mainpage.is_mlc4; - if (ret == 0) { - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK); - } - if (ret == 0) { - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_EMB_FUNC_EXEC_STATUS, (uint8_t *)&emb_func_exec_status, 1); - } - if (ret == 0) { - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_EMB_FUNC_SRC, (uint8_t *)&emb_func_src, 1); - } - if (ret == 0) { - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); - } + /* embedded func */ + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK); + ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_EMB_FUNC_EXEC_STATUS, (uint8_t *)&emb_func_exec_status, 1); + ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_EMB_FUNC_SRC, (uint8_t *)&emb_func_src, 1); + ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + if (ret != 0) { return ret; } val->emb_func_stand_by = emb_func_exec_status.emb_func_endop; val->emb_func_time_exceed = emb_func_exec_status.emb_func_exec_ovr; @@ -1500,15 +2100,8 @@ int32_t lsm6dsv16x_all_sources_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_all_sources val->step_detector = emb_func_src.step_detected; /* sensor hub */ - if (ret == 0) { - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_SENSOR_HUB_MEM_BANK); - } - if (ret == 0) { - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_STATUS_MASTER, (uint8_t *)&status_shub, 1); - } - if (ret == 0) { - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); - } + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_STATUS_MASTER_MAINPAGE, (uint8_t *)&status_shub, 1); + if (ret != 0) { return ret; } val->sh_endop = status_shub.sens_hub_endop; val->sh_wr_once = status_shub.wr_once_done; @@ -1520,6 +2113,22 @@ int32_t lsm6dsv16x_all_sources_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_all_sources return ret; } +int32_t lsm6dsv16x_flag_data_ready_get(stmdev_ctx_t *ctx, + lsm6dsv16x_data_ready_t *val) +{ + lsm6dsv16x_status_reg_t status; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_STATUS_REG, (uint8_t *)&status, 1); + if (ret != 0) { return ret; } + + val->drdy_xl = status.xlda; + val->drdy_gy = status.gda; + val->drdy_temp = status.tda; + + return ret; +} + /** * @brief Mask status bit reset[set] * @@ -1528,7 +2137,7 @@ int32_t lsm6dsv16x_all_sources_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_all_sources * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_int_ack_mask_set(lsm6dsv16x_ctx_t *ctx, uint8_t val) +int32_t lsm6dsv16x_int_ack_mask_set(stmdev_ctx_t *ctx, uint8_t val) { int32_t ret; @@ -1545,7 +2154,7 @@ int32_t lsm6dsv16x_int_ack_mask_set(lsm6dsv16x_ctx_t *ctx, uint8_t val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_int_ack_mask_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val) +int32_t lsm6dsv16x_int_ack_mask_get(stmdev_ctx_t *ctx, uint8_t *val) { int32_t ret; @@ -1562,12 +2171,14 @@ int32_t lsm6dsv16x_int_ack_mask_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_temperature_raw_get(lsm6dsv16x_ctx_t *ctx, int16_t *val) +int32_t lsm6dsv16x_temperature_raw_get(stmdev_ctx_t *ctx, int16_t *val) { uint8_t buff[2]; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_OUT_TEMP_L, &buff[0], 2); + if (ret != 0) { return ret; } + *val = (int16_t)buff[1]; *val = (*val * 256) + (int16_t)buff[0]; @@ -1582,12 +2193,14 @@ int32_t lsm6dsv16x_temperature_raw_get(lsm6dsv16x_ctx_t *ctx, int16_t *val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_angular_rate_raw_get(lsm6dsv16x_ctx_t *ctx, int16_t *val) +int32_t lsm6dsv16x_angular_rate_raw_get(stmdev_ctx_t *ctx, int16_t *val) { uint8_t buff[6]; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_OUTX_L_G, &buff[0], 6); + if (ret != 0) { return ret; } + val[0] = (int16_t)buff[1]; val[0] = (val[0] * 256) + (int16_t)buff[0]; val[1] = (int16_t)buff[3]; @@ -1606,12 +2219,14 @@ int32_t lsm6dsv16x_angular_rate_raw_get(lsm6dsv16x_ctx_t *ctx, int16_t *val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_ois_angular_rate_raw_get(lsm6dsv16x_ctx_t *ctx, int16_t *val) +int32_t lsm6dsv16x_ois_angular_rate_raw_get(stmdev_ctx_t *ctx, int16_t *val) { uint8_t buff[6]; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_SPI2_OUTX_L_G_OIS, &buff[0], 6); + if (ret != 0) { return ret; } + val[0] = (int16_t)buff[1]; val[0] = (*val * 256) + (int16_t)buff[0]; val[1] = (int16_t)buff[3]; @@ -1630,12 +2245,14 @@ int32_t lsm6dsv16x_ois_angular_rate_raw_get(lsm6dsv16x_ctx_t *ctx, int16_t *val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_ois_eis_angular_rate_raw_get(lsm6dsv16x_ctx_t *ctx, int16_t *val) +int32_t lsm6dsv16x_ois_eis_angular_rate_raw_get(stmdev_ctx_t *ctx, int16_t *val) { uint8_t buff[6]; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_UI_OUTX_L_G_OIS_EIS, &buff[0], 6); + if (ret != 0) { return ret; } + val[0] = (int16_t)buff[1]; val[0] = (*val * 256) + (int16_t)buff[0]; val[1] = (int16_t)buff[3]; @@ -1654,12 +2271,14 @@ int32_t lsm6dsv16x_ois_eis_angular_rate_raw_get(lsm6dsv16x_ctx_t *ctx, int16_t * * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_acceleration_raw_get(lsm6dsv16x_ctx_t *ctx, int16_t *val) +int32_t lsm6dsv16x_acceleration_raw_get(stmdev_ctx_t *ctx, int16_t *val) { uint8_t buff[6]; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_OUTX_L_A, &buff[0], 6); + if (ret != 0) { return ret; } + val[0] = (int16_t)buff[1]; val[0] = (val[0] * 256) + (int16_t)buff[0]; val[1] = (int16_t)buff[3]; @@ -1678,12 +2297,14 @@ int32_t lsm6dsv16x_acceleration_raw_get(lsm6dsv16x_ctx_t *ctx, int16_t *val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_dual_acceleration_raw_get(lsm6dsv16x_ctx_t *ctx, int16_t *val) +int32_t lsm6dsv16x_dual_acceleration_raw_get(stmdev_ctx_t *ctx, int16_t *val) { uint8_t buff[6]; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_UI_OUTX_L_A_OIS_DUALC, &buff[0], 6); + if (ret != 0) { return ret; } + val[0] = (int16_t)buff[1]; val[0] = (val[0] * 256) + (int16_t)buff[0]; val[1] = (int16_t)buff[3]; @@ -1702,12 +2323,14 @@ int32_t lsm6dsv16x_dual_acceleration_raw_get(lsm6dsv16x_ctx_t *ctx, int16_t *val * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_ah_qvar_raw_get(lsm6dsv16x_ctx_t *ctx, int16_t *val) +int32_t lsm6dsv16x_ah_qvar_raw_get(stmdev_ctx_t *ctx, int16_t *val) { uint8_t buff[2]; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_AH_QVAR_OUT_L, &buff[0], 2); + if (ret != 0) { return ret; } + *val = (int16_t)buff[1]; *val = (*val * 256) + (int16_t)buff[0]; @@ -1722,7 +2345,7 @@ int32_t lsm6dsv16x_ah_qvar_raw_get(lsm6dsv16x_ctx_t *ctx, int16_t *val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_odr_cal_reg_get(lsm6dsv16x_ctx_t *ctx, int8_t *val) +int32_t lsm6dsv16x_odr_cal_reg_get(stmdev_ctx_t *ctx, int8_t *val) { lsm6dsv16x_internal_freq_t internal_freq; int32_t ret; @@ -1741,7 +2364,8 @@ int32_t lsm6dsv16x_odr_cal_reg_get(lsm6dsv16x_ctx_t *ctx, int8_t *val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_ln_pg_write(lsm6dsv16x_ctx_t *ctx, uint16_t address, uint8_t *buf, uint8_t len) +int32_t lsm6dsv16x_ln_pg_write(stmdev_ctx_t *ctx, uint16_t address, + uint8_t *buf, uint8_t len) { lsm6dsv16x_page_address_t page_address; lsm6dsv16x_page_sel_t page_sel; @@ -1755,66 +2379,62 @@ int32_t lsm6dsv16x_ln_pg_write(lsm6dsv16x_ctx_t *ctx, uint16_t address, uint8_t lsb = (uint8_t)address & 0xFFU; ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK); - - if (ret == 0) { - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_PAGE_RW, (uint8_t *)&page_rw, 1); - } - - if (ret == 0) { - page_rw.page_read = PROPERTY_DISABLE; - page_rw.page_write = PROPERTY_ENABLE; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_PAGE_RW, (uint8_t *)&page_rw, 1); - } - if (ret == 0) { - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_PAGE_SEL, (uint8_t *)&page_sel, 1); - } - if (ret == 0) { - page_sel.page_sel = msb; - page_sel.not_used0 = 1; // Default value - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_PAGE_SEL, (uint8_t *)&page_sel, 1); - } - if (ret == 0) { - page_address.page_addr = lsb; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_PAGE_ADDRESS, (uint8_t *)&page_address, 1); - } - - if (ret == 0) { - for (i = 0; ((i < len) && (ret == 0)); i++) { - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_PAGE_RW, &buf[i], 1); - lsb++; - - /* Check if page wrap */ - if (((lsb & 0xFFU) == 0x00U) && (ret == 0)) { - msb++; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_PAGE_SEL, (uint8_t *)&page_sel, 1); - - if (ret == 0) { - page_sel.page_sel = msb; - page_sel.not_used0 = 1; // Default value - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_PAGE_SEL, (uint8_t *)&page_sel, 1); - } - } + if (ret != 0) { return ret; } + + /* set page write */ + ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_PAGE_RW, (uint8_t *)&page_rw, 1); + page_rw.page_read = PROPERTY_DISABLE; + page_rw.page_write = PROPERTY_ENABLE; + ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_PAGE_RW, (uint8_t *)&page_rw, 1); + if (ret != 0) { goto exit; } + + /* select page */ + ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_PAGE_SEL, (uint8_t *)&page_sel, 1); + page_sel.page_sel = msb; + page_sel.not_used0 = 1; // Default value + ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_PAGE_SEL, (uint8_t *)&page_sel, 1); + if (ret != 0) { goto exit; } + + /* set page addr */ + page_address.page_addr = lsb; + ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_PAGE_ADDRESS, + (uint8_t *)&page_address, 1); + if (ret != 0) { goto exit; } + + for (i = 0; ((i < len) && (ret == 0)); i++) + { + ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_PAGE_VALUE, &buf[i], 1); + if (ret != 0) { goto exit; } + + lsb++; + + /* Check if page wrap */ + if (((lsb & 0xFFU) == 0x00U) && (ret == 0)) + { + msb++; + ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_PAGE_SEL, (uint8_t *)&page_sel, 1); + if (ret != 0) { goto exit; } + + page_sel.page_sel = msb; + page_sel.not_used0 = 1; // Default value + ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_PAGE_SEL, (uint8_t *)&page_sel, 1); + if (ret != 0) { goto exit; } } } - if (ret == 0) { - page_sel.page_sel = 0; - page_sel.not_used0 = 1;// Default value - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_PAGE_SEL, (uint8_t *)&page_sel, 1); - } + page_sel.page_sel = 0; + page_sel.not_used0 = 1;// Default value + ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_PAGE_SEL, (uint8_t *)&page_sel, 1); + if (ret != 0) { goto exit; } - if (ret == 0) { - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_PAGE_RW, (uint8_t *)&page_rw, 1); - } - if (ret == 0) { - page_rw.page_read = PROPERTY_DISABLE; - page_rw.page_write = PROPERTY_DISABLE; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_PAGE_RW, (uint8_t *)&page_rw, 1); - } + /* unset page write */ + ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_PAGE_RW, (uint8_t *)&page_rw, 1); + page_rw.page_read = PROPERTY_DISABLE; + page_rw.page_write = PROPERTY_DISABLE; + ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_PAGE_RW, (uint8_t *)&page_rw, 1); - if (ret == 0) { - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); - } +exit: + ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); return ret; } @@ -1827,14 +2447,15 @@ int32_t lsm6dsv16x_ln_pg_write(lsm6dsv16x_ctx_t *ctx, uint16_t address, uint8_t */ /** - * @brief Write buffer in a page.[set] + * @brief Read buffer in a page.[set] * * @param ctx read / write interface definitions * @param val Write buffer in a page. * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_ln_pg_read(lsm6dsv16x_ctx_t *ctx, uint16_t address, uint8_t *buf, uint8_t len) +int32_t lsm6dsv16x_ln_pg_read(stmdev_ctx_t *ctx, uint16_t address, uint8_t *buf, + uint8_t len) { lsm6dsv16x_page_address_t page_address; lsm6dsv16x_page_sel_t page_sel; @@ -1848,70 +2469,111 @@ int32_t lsm6dsv16x_ln_pg_read(lsm6dsv16x_ctx_t *ctx, uint16_t address, uint8_t * lsb = (uint8_t)address & 0xFFU; ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK); - - if (ret == 0) { - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_PAGE_RW, (uint8_t *)&page_rw, 1); + if (ret != 0) { return ret; } + + /* set page write */ + ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_PAGE_RW, (uint8_t *)&page_rw, 1); + page_rw.page_read = PROPERTY_ENABLE; + page_rw.page_write = PROPERTY_DISABLE; + ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_PAGE_RW, (uint8_t *)&page_rw, 1); + if (ret != 0) { goto exit; } + + /* select page */ + ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_PAGE_SEL, (uint8_t *)&page_sel, 1); + page_sel.page_sel = msb; + page_sel.not_used0 = 1; // Default value + ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_PAGE_SEL, (uint8_t *)&page_sel, 1); + if (ret != 0) { goto exit; } + + /* set page addr */ + page_address.page_addr = lsb; + ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_PAGE_ADDRESS, + (uint8_t *)&page_address, 1); + if (ret != 0) { goto exit; } + + for (i = 0; ((i < len) && (ret == 0)); i++) + { + ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_PAGE_VALUE, &buf[i], 1); + if (ret != 0) { goto exit; } + + lsb++; + + /* Check if page wrap */ + if (((lsb & 0xFFU) == 0x00U) && (ret == 0)) + { + msb++; + ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_PAGE_SEL, (uint8_t *)&page_sel, 1); + if (ret != 0) { goto exit; } + + page_sel.page_sel = msb; + page_sel.not_used0 = 1; // Default value + ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_PAGE_SEL, (uint8_t *)&page_sel, 1); + if (ret != 0) { goto exit; } + } } - if (ret == 0) { - page_rw.page_read = PROPERTY_ENABLE; - page_rw.page_write = PROPERTY_DISABLE; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_PAGE_RW, (uint8_t *)&page_rw, 1); - } - if (ret == 0) { - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_PAGE_SEL, (uint8_t *)&page_sel, 1); - } - if (ret == 0) { - page_sel.page_sel = msb; - page_sel.not_used0 = 1; // Default value - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_PAGE_SEL, (uint8_t *)&page_sel, 1); - } - if (ret == 0) { - page_address.page_addr = lsb; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_PAGE_ADDRESS, (uint8_t *)&page_address, 1); - } + page_sel.page_sel = 0; + page_sel.not_used0 = 1;// Default value + ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_PAGE_SEL, (uint8_t *)&page_sel, 1); + if (ret != 0) { goto exit; } - if (ret == 0) { - for (i = 0; ((i < len) && (ret == 0)); i++) { - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_PAGE_RW, &buf[i], 1); - lsb++; + /* unset page write */ + ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_PAGE_RW, (uint8_t *)&page_rw, 1); + page_rw.page_read = PROPERTY_DISABLE; + page_rw.page_write = PROPERTY_DISABLE; + ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_PAGE_RW, (uint8_t *)&page_rw, 1); - /* Check if page wrap */ - if (((lsb & 0xFFU) == 0x00U) && (ret == 0)) { - msb++; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_PAGE_SEL, (uint8_t *)&page_sel, 1); +exit: + ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); - if (ret == 0) { - page_sel.page_sel = msb; - page_sel.not_used0 = 1; // Default value - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_PAGE_SEL, (uint8_t *)&page_sel, 1); - } - } - } - } + return ret; +} - if (ret == 0) { - page_sel.page_sel = 0; - page_sel.not_used0 = 1;// Default value - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_PAGE_SEL, (uint8_t *)&page_sel, 1); - } +/** + * @brief Enable debug mode for embedded functions [set] + * + * @param ctx read / write interface definitions + * @param val 0, 1 + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_emb_function_dbg_set(stmdev_ctx_t *ctx, uint8_t val) +{ + lsm6dsv16x_ctrl10_t ctrl10; + int32_t ret; - if (ret == 0) { - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_PAGE_RW, (uint8_t *)&page_rw, 1); - } - if (ret == 0) { - page_rw.page_read = PROPERTY_DISABLE; - page_rw.page_write = PROPERTY_DISABLE; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_PAGE_RW, (uint8_t *)&page_rw, 1); - } + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL10, (uint8_t *)&ctrl10, 1); - if (ret == 0) { - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + if (ret == 0) + { + ctrl10.emb_func_debug = val; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_CTRL10, (uint8_t *)&ctrl10, 1); } return ret; } +/** + * @brief Enable debug mode for embedded functions [get] + * + * @param ctx read / write interface definitions + * @param val 0, 1 + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_emb_function_dbg_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + lsm6dsv16x_ctrl10_t ctrl10; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL10, (uint8_t *)&ctrl10, 1); + if (ret != 0) { return ret; } + + *val = ctrl10.emb_func_debug; + + return ret; +} + /** * @} * @@ -1933,14 +2595,16 @@ int32_t lsm6dsv16x_ln_pg_read(lsm6dsv16x_ctx_t *ctx, uint16_t address, uint8_t * * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_den_polarity_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_den_polarity_t val) +int32_t lsm6dsv16x_den_polarity_set(stmdev_ctx_t *ctx, + lsm6dsv16x_den_polarity_t val) { lsm6dsv16x_ctrl4_t ctrl4; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL4, (uint8_t *)&ctrl4, 1); - if (ret == 0) { + if (ret == 0) + { ctrl4.int2_in_lh = (uint8_t)val & 0x1U; ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_CTRL4, (uint8_t *)&ctrl4, 1); } @@ -1956,13 +2620,17 @@ int32_t lsm6dsv16x_den_polarity_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_den_polari * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_den_polarity_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_den_polarity_t *val) +int32_t lsm6dsv16x_den_polarity_get(stmdev_ctx_t *ctx, + lsm6dsv16x_den_polarity_t *val) { lsm6dsv16x_ctrl4_t ctrl4; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL4, (uint8_t *)&ctrl4, 1); - switch (ctrl4.int2_in_lh) { + if (ret != 0) { return ret; } + + switch (ctrl4.int2_in_lh) + { case LSM6DSV16X_DEN_ACT_LOW: *val = LSM6DSV16X_DEN_ACT_LOW; break; @@ -1975,6 +2643,7 @@ int32_t lsm6dsv16x_den_polarity_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_den_polari *val = LSM6DSV16X_DEN_ACT_LOW; break; } + return ret; } @@ -1986,42 +2655,49 @@ int32_t lsm6dsv16x_den_polarity_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_den_polari * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_den_conf_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_den_conf_t val) +int32_t lsm6dsv16x_den_conf_set(stmdev_ctx_t *ctx, lsm6dsv16x_den_conf_t val) { lsm6dsv16x_den_t den; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_DEN, (uint8_t *)&den, 1); - if (ret == 0) { - den.den_z = val.den_z; - den.den_y = val.den_y; - den.den_x = val.den_x; - - den.lvl2_en = (uint8_t)val.mode & 0x1U; - den.lvl1_en = ((uint8_t)val.mode & 0x2U) >> 1; - - if (val.stamp_in_gy_data == PROPERTY_ENABLE && val.stamp_in_xl_data == PROPERTY_ENABLE) { - den.den_xl_g = PROPERTY_DISABLE; - den.den_xl_en = PROPERTY_ENABLE; - } else if (val.stamp_in_gy_data == PROPERTY_ENABLE && val.stamp_in_xl_data == PROPERTY_DISABLE) { - den.den_xl_g = PROPERTY_DISABLE; - den.den_xl_en = PROPERTY_DISABLE; - } else if (val.stamp_in_gy_data == PROPERTY_DISABLE && val.stamp_in_xl_data == PROPERTY_ENABLE) { - den.den_xl_g = PROPERTY_ENABLE; - den.den_xl_en = PROPERTY_DISABLE; - } else { - den.den_xl_g = PROPERTY_DISABLE; - den.den_xl_en = PROPERTY_DISABLE; - den.den_z = PROPERTY_DISABLE; - den.den_y = PROPERTY_DISABLE; - den.den_x = PROPERTY_DISABLE; - den.lvl2_en = PROPERTY_DISABLE; - den.lvl1_en = PROPERTY_DISABLE; - } + if (ret != 0) { return ret; } + + den.den_z = val.den_z; + den.den_y = val.den_y; + den.den_x = val.den_x; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_DEN, (uint8_t *)&den, 1); + den.lvl2_en = (uint8_t)val.mode & 0x1U; + den.lvl1_en = ((uint8_t)val.mode & 0x2U) >> 1; + + if (val.stamp_in_gy_data == PROPERTY_ENABLE && val.stamp_in_xl_data == PROPERTY_ENABLE) + { + den.den_xl_g = PROPERTY_DISABLE; + den.den_xl_en = PROPERTY_ENABLE; + } + else if (val.stamp_in_gy_data == PROPERTY_ENABLE && val.stamp_in_xl_data == PROPERTY_DISABLE) + { + den.den_xl_g = PROPERTY_DISABLE; + den.den_xl_en = PROPERTY_DISABLE; + } + else if (val.stamp_in_gy_data == PROPERTY_DISABLE && val.stamp_in_xl_data == PROPERTY_ENABLE) + { + den.den_xl_g = PROPERTY_ENABLE; + den.den_xl_en = PROPERTY_DISABLE; + } + else + { + den.den_xl_g = PROPERTY_DISABLE; + den.den_xl_en = PROPERTY_DISABLE; + den.den_z = PROPERTY_DISABLE; + den.den_y = PROPERTY_DISABLE; + den.den_x = PROPERTY_DISABLE; + den.lvl2_en = PROPERTY_DISABLE; + den.lvl1_en = PROPERTY_DISABLE; } + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_DEN, (uint8_t *)&den, 1); + return ret; } @@ -2034,40 +2710,50 @@ int32_t lsm6dsv16x_den_conf_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_den_conf_t val * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_den_conf_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_den_conf_t *val) +int32_t lsm6dsv16x_den_conf_get(stmdev_ctx_t *ctx, lsm6dsv16x_den_conf_t *val) { lsm6dsv16x_den_t den; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_DEN, (uint8_t *)&den, 1); + if (ret != 0) { return ret; } val->den_z = den.den_z; val->den_y = den.den_y; val->den_x = den.den_x; - if ((den.den_z | den.den_z | den.den_z) == PROPERTY_ENABLE) { - if (den.den_xl_g == PROPERTY_DISABLE && den.den_xl_en == PROPERTY_ENABLE) { + if ((den.den_z | den.den_z | den.den_z) == PROPERTY_ENABLE) + { + if (den.den_xl_g == PROPERTY_DISABLE && den.den_xl_en == PROPERTY_ENABLE) + { val->stamp_in_gy_data = PROPERTY_ENABLE; val->stamp_in_xl_data = PROPERTY_ENABLE; - } else if (den.den_xl_g == PROPERTY_DISABLE && den.den_xl_en == PROPERTY_DISABLE) { + } + else if (den.den_xl_g == PROPERTY_DISABLE && den.den_xl_en == PROPERTY_DISABLE) + { val->stamp_in_gy_data = PROPERTY_ENABLE; val->stamp_in_xl_data = PROPERTY_DISABLE; - } else { // ( (den.den_xl_g & !den.den_xl_en) == PROPERTY_ENABLE ) + } + else // ( (den.den_xl_g & !den.den_xl_en) == PROPERTY_ENABLE ) + { val->stamp_in_gy_data = PROPERTY_DISABLE; val->stamp_in_xl_data = PROPERTY_ENABLE; } - } else { + } + else + { val->stamp_in_gy_data = PROPERTY_DISABLE; val->stamp_in_xl_data = PROPERTY_DISABLE; } - switch ((den.lvl1_en << 1) + den.lvl2_en) { + switch ((den.lvl1_en << 1) + den.lvl2_en) + { case LEVEL_TRIGGER: val->mode = LEVEL_TRIGGER; break; - case LEVEL_LETCHED: - val->mode = LEVEL_LETCHED; + case LEVEL_LATCHED: + val->mode = LEVEL_LATCHED; break; default: @@ -2098,14 +2784,16 @@ int32_t lsm6dsv16x_den_conf_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_den_conf_t *va * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_eis_gy_full_scale_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_eis_gy_full_scale_t val) +int32_t lsm6dsv16x_eis_gy_full_scale_set(stmdev_ctx_t *ctx, + lsm6dsv16x_eis_gy_full_scale_t val) { lsm6dsv16x_ctrl_eis_t ctrl_eis; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL_EIS, (uint8_t *)&ctrl_eis, 1); - if (ret == 0) { + if (ret == 0) + { ctrl_eis.fs_g_eis = (uint8_t)val & 0x7U; ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_CTRL_EIS, (uint8_t *)&ctrl_eis, 1); } @@ -2121,14 +2809,17 @@ int32_t lsm6dsv16x_eis_gy_full_scale_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_eis_g * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_eis_gy_full_scale_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_eis_gy_full_scale_t *val) +int32_t lsm6dsv16x_eis_gy_full_scale_get(stmdev_ctx_t *ctx, + lsm6dsv16x_eis_gy_full_scale_t *val) { lsm6dsv16x_ctrl_eis_t ctrl_eis; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL_EIS, (uint8_t *)&ctrl_eis, 1); + if (ret != 0) { return ret; } - switch (ctrl_eis.fs_g_eis) { + switch (ctrl_eis.fs_g_eis) + { case LSM6DSV16X_EIS_125dps: *val = LSM6DSV16X_EIS_125dps; break; @@ -2164,14 +2855,15 @@ int32_t lsm6dsv16x_eis_gy_full_scale_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_eis_g * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_eis_gy_on_spi2_set(lsm6dsv16x_ctx_t *ctx, uint8_t val) +int32_t lsm6dsv16x_eis_gy_on_spi2_set(stmdev_ctx_t *ctx, uint8_t val) { lsm6dsv16x_ctrl_eis_t ctrl_eis; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL_EIS, (uint8_t *)&ctrl_eis, 1); - if (ret == 0) { + if (ret == 0) + { ctrl_eis.g_eis_on_g_ois_out_reg = val; ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_CTRL_EIS, (uint8_t *)&ctrl_eis, 1); } @@ -2187,7 +2879,7 @@ int32_t lsm6dsv16x_eis_gy_on_spi2_set(lsm6dsv16x_ctx_t *ctx, uint8_t val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_eis_gy_on_spi2_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val) +int32_t lsm6dsv16x_eis_gy_on_spi2_get(stmdev_ctx_t *ctx, uint8_t *val) { lsm6dsv16x_ctrl_eis_t ctrl_eis; int32_t ret; @@ -2206,14 +2898,16 @@ int32_t lsm6dsv16x_eis_gy_on_spi2_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_gy_eis_data_rate_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_gy_eis_data_rate_t val) +int32_t lsm6dsv16x_gy_eis_data_rate_set(stmdev_ctx_t *ctx, + lsm6dsv16x_gy_eis_data_rate_t val) { lsm6dsv16x_ctrl_eis_t ctrl_eis; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL_EIS, (uint8_t *)&ctrl_eis, 1); - if (ret == 0) { + if (ret == 0) + { ctrl_eis.odr_g_eis = (uint8_t)val & 0x03U; ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_CTRL_EIS, (uint8_t *)&ctrl_eis, 1); } @@ -2229,14 +2923,17 @@ int32_t lsm6dsv16x_gy_eis_data_rate_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_gy_eis * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_gy_eis_data_rate_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_gy_eis_data_rate_t *val) +int32_t lsm6dsv16x_gy_eis_data_rate_get(stmdev_ctx_t *ctx, + lsm6dsv16x_gy_eis_data_rate_t *val) { lsm6dsv16x_ctrl_eis_t ctrl_eis; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL_EIS, (uint8_t *)&ctrl_eis, 1); + if (ret != 0) { return ret; } - switch (ctrl_eis.odr_g_eis) { + switch (ctrl_eis.odr_g_eis) + { case LSM6DSV16X_EIS_ODR_OFF: *val = LSM6DSV16X_EIS_ODR_OFF; break; @@ -2253,6 +2950,7 @@ int32_t lsm6dsv16x_gy_eis_data_rate_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_gy_eis *val = LSM6DSV16X_EIS_1920Hz; break; } + return ret; } @@ -2276,14 +2974,15 @@ int32_t lsm6dsv16x_gy_eis_data_rate_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_gy_eis * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fifo_watermark_set(lsm6dsv16x_ctx_t *ctx, uint8_t val) +int32_t lsm6dsv16x_fifo_watermark_set(stmdev_ctx_t *ctx, uint8_t val) { lsm6dsv16x_fifo_ctrl1_t fifo_ctrl1; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FIFO_CTRL1, (uint8_t *)&fifo_ctrl1, 1); - if (ret == 0) { + if (ret == 0) + { fifo_ctrl1.wtm = val; ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_FIFO_CTRL1, (uint8_t *)&fifo_ctrl1, 1); } @@ -2299,7 +2998,7 @@ int32_t lsm6dsv16x_fifo_watermark_set(lsm6dsv16x_ctx_t *ctx, uint8_t val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fifo_watermark_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val) +int32_t lsm6dsv16x_fifo_watermark_get(stmdev_ctx_t *ctx, uint8_t *val) { lsm6dsv16x_fifo_ctrl1_t fifo_ctrl1; int32_t ret; @@ -2318,13 +3017,14 @@ int32_t lsm6dsv16x_fifo_watermark_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fifo_xl_dual_fsm_batch_set(lsm6dsv16x_ctx_t *ctx, uint8_t val) +int32_t lsm6dsv16x_fifo_xl_dual_fsm_batch_set(stmdev_ctx_t *ctx, uint8_t val) { lsm6dsv16x_fifo_ctrl2_t fifo_ctrl2; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FIFO_CTRL2, (uint8_t *)&fifo_ctrl2, 1); - if (ret == 0) { + if (ret == 0) + { fifo_ctrl2.xl_dualc_batch_from_fsm = val; ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_FIFO_CTRL2, (uint8_t *)&fifo_ctrl2, 1); } @@ -2340,7 +3040,7 @@ int32_t lsm6dsv16x_fifo_xl_dual_fsm_batch_set(lsm6dsv16x_ctx_t *ctx, uint8_t val * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fifo_xl_dual_fsm_batch_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val) +int32_t lsm6dsv16x_fifo_xl_dual_fsm_batch_get(stmdev_ctx_t *ctx, uint8_t *val) { lsm6dsv16x_fifo_ctrl2_t fifo_ctrl2; int32_t ret; @@ -2348,7 +3048,6 @@ int32_t lsm6dsv16x_fifo_xl_dual_fsm_batch_get(lsm6dsv16x_ctx_t *ctx, uint8_t *va ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FIFO_CTRL2, (uint8_t *)&fifo_ctrl2, 1); *val = fifo_ctrl2.xl_dualc_batch_from_fsm; - return ret; } @@ -2360,13 +3059,15 @@ int32_t lsm6dsv16x_fifo_xl_dual_fsm_batch_get(lsm6dsv16x_ctx_t *ctx, uint8_t *va * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fifo_compress_algo_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fifo_compress_algo_t val) +int32_t lsm6dsv16x_fifo_compress_algo_set(stmdev_ctx_t *ctx, + lsm6dsv16x_fifo_compress_algo_t val) { lsm6dsv16x_fifo_ctrl2_t fifo_ctrl2; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FIFO_CTRL2, (uint8_t *)&fifo_ctrl2, 1); - if (ret == 0) { + if (ret == 0) + { fifo_ctrl2.uncompr_rate = (uint8_t)val & 0x03U; ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_FIFO_CTRL2, (uint8_t *)&fifo_ctrl2, 1); } @@ -2382,13 +3083,17 @@ int32_t lsm6dsv16x_fifo_compress_algo_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fifo * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fifo_compress_algo_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fifo_compress_algo_t *val) +int32_t lsm6dsv16x_fifo_compress_algo_get(stmdev_ctx_t *ctx, + lsm6dsv16x_fifo_compress_algo_t *val) { lsm6dsv16x_fifo_ctrl2_t fifo_ctrl2; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FIFO_CTRL2, (uint8_t *)&fifo_ctrl2, 1); - switch (fifo_ctrl2.uncompr_rate) { + if (ret != 0) { return ret; } + + switch (fifo_ctrl2.uncompr_rate) + { case LSM6DSV16X_CMP_DISABLE: *val = LSM6DSV16X_CMP_DISABLE; break; @@ -2420,13 +3125,14 @@ int32_t lsm6dsv16x_fifo_compress_algo_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fifo * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fifo_virtual_sens_odr_chg_set(lsm6dsv16x_ctx_t *ctx, uint8_t val) +int32_t lsm6dsv16x_fifo_virtual_sens_odr_chg_set(stmdev_ctx_t *ctx, uint8_t val) { lsm6dsv16x_fifo_ctrl2_t fifo_ctrl2; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FIFO_CTRL2, (uint8_t *)&fifo_ctrl2, 1); - if (ret == 0) { + if (ret == 0) + { fifo_ctrl2.odr_chg_en = val; ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_FIFO_CTRL2, (uint8_t *)&fifo_ctrl2, 1); } @@ -2442,7 +3148,8 @@ int32_t lsm6dsv16x_fifo_virtual_sens_odr_chg_set(lsm6dsv16x_ctx_t *ctx, uint8_t * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fifo_virtual_sens_odr_chg_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val) +int32_t lsm6dsv16x_fifo_virtual_sens_odr_chg_get(stmdev_ctx_t *ctx, + uint8_t *val) { lsm6dsv16x_fifo_ctrl2_t fifo_ctrl2; int32_t ret; @@ -2461,7 +3168,8 @@ int32_t lsm6dsv16x_fifo_virtual_sens_odr_chg_get(lsm6dsv16x_ctx_t *ctx, uint8_t * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fifo_compress_algo_real_time_set(lsm6dsv16x_ctx_t *ctx, uint8_t val) +int32_t lsm6dsv16x_fifo_compress_algo_real_time_set(stmdev_ctx_t *ctx, + uint8_t val) { lsm6dsv16x_emb_func_en_b_t emb_func_en_b; lsm6dsv16x_fifo_ctrl2_t fifo_ctrl2; @@ -2469,24 +3177,17 @@ int32_t lsm6dsv16x_fifo_compress_algo_real_time_set(lsm6dsv16x_ctx_t *ctx, uint8 int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FIFO_CTRL2, (uint8_t *)&fifo_ctrl2, 1); - if (ret == 0) { - fifo_ctrl2.fifo_compr_rt_en = val; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_FIFO_CTRL2, (uint8_t *)&fifo_ctrl2, 1); - } + fifo_ctrl2.fifo_compr_rt_en = val; + ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_FIFO_CTRL2, (uint8_t *)&fifo_ctrl2, 1); + if (ret != 0) { return ret; } - if (ret == 0) { - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK); - } - if (ret == 0) { - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_EMB_FUNC_EN_B, (uint8_t *)&emb_func_en_b, 1); - } - if (ret == 0) { - emb_func_en_b.fifo_compr_en = val; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_EMB_FUNC_EN_B, (uint8_t *)&emb_func_en_b, 1); - } - if (ret == 0) { - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); - } + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK); + if (ret != 0) { return ret; } + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_EMB_FUNC_EN_B, (uint8_t *)&emb_func_en_b, 1); + emb_func_en_b.fifo_compr_en = val; + ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_EMB_FUNC_EN_B, (uint8_t *)&emb_func_en_b, 1); + ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); return ret; } @@ -2499,7 +3200,8 @@ int32_t lsm6dsv16x_fifo_compress_algo_real_time_set(lsm6dsv16x_ctx_t *ctx, uint8 * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fifo_compress_algo_real_time_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val) +int32_t lsm6dsv16x_fifo_compress_algo_real_time_get(stmdev_ctx_t *ctx, + uint8_t *val) { lsm6dsv16x_fifo_ctrl2_t fifo_ctrl2; int32_t ret; @@ -2519,13 +3221,14 @@ int32_t lsm6dsv16x_fifo_compress_algo_real_time_get(lsm6dsv16x_ctx_t *ctx, uint8 * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fifo_stop_on_wtm_set(lsm6dsv16x_ctx_t *ctx, uint8_t val) +int32_t lsm6dsv16x_fifo_stop_on_wtm_set(stmdev_ctx_t *ctx, uint8_t val) { lsm6dsv16x_fifo_ctrl2_t fifo_ctrl2; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FIFO_CTRL2, (uint8_t *)&fifo_ctrl2, 1); - if (ret == 0) { + if (ret == 0) + { fifo_ctrl2.stop_on_wtm = val; ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_FIFO_CTRL2, (uint8_t *)&fifo_ctrl2, 1); } @@ -2541,7 +3244,7 @@ int32_t lsm6dsv16x_fifo_stop_on_wtm_set(lsm6dsv16x_ctx_t *ctx, uint8_t val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fifo_stop_on_wtm_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val) +int32_t lsm6dsv16x_fifo_stop_on_wtm_get(stmdev_ctx_t *ctx, uint8_t *val) { lsm6dsv16x_fifo_ctrl2_t fifo_ctrl2; int32_t ret; @@ -2560,13 +3263,15 @@ int32_t lsm6dsv16x_fifo_stop_on_wtm_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fifo_xl_batch_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fifo_xl_batch_t val) +int32_t lsm6dsv16x_fifo_xl_batch_set(stmdev_ctx_t *ctx, + lsm6dsv16x_fifo_xl_batch_t val) { lsm6dsv16x_fifo_ctrl3_t fifo_ctrl3; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FIFO_CTRL3, (uint8_t *)&fifo_ctrl3, 1); - if (ret == 0) { + if (ret == 0) + { fifo_ctrl3.bdr_xl = (uint8_t)val & 0xFu; ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_FIFO_CTRL3, (uint8_t *)&fifo_ctrl3, 1); } @@ -2582,13 +3287,17 @@ int32_t lsm6dsv16x_fifo_xl_batch_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fifo_xl_b * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fifo_xl_batch_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fifo_xl_batch_t *val) +int32_t lsm6dsv16x_fifo_xl_batch_get(stmdev_ctx_t *ctx, + lsm6dsv16x_fifo_xl_batch_t *val) { lsm6dsv16x_fifo_ctrl3_t fifo_ctrl3; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FIFO_CTRL3, (uint8_t *)&fifo_ctrl3, 1); - switch (fifo_ctrl3.bdr_xl) { + if (ret != 0) { return ret; } + + switch (fifo_ctrl3.bdr_xl) + { case LSM6DSV16X_XL_NOT_BATCHED: *val = LSM6DSV16X_XL_NOT_BATCHED; break; @@ -2645,6 +3354,7 @@ int32_t lsm6dsv16x_fifo_xl_batch_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fifo_xl_b *val = LSM6DSV16X_XL_NOT_BATCHED; break; } + return ret; } @@ -2656,13 +3366,15 @@ int32_t lsm6dsv16x_fifo_xl_batch_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fifo_xl_b * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fifo_gy_batch_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fifo_gy_batch_t val) +int32_t lsm6dsv16x_fifo_gy_batch_set(stmdev_ctx_t *ctx, + lsm6dsv16x_fifo_gy_batch_t val) { lsm6dsv16x_fifo_ctrl3_t fifo_ctrl3; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FIFO_CTRL3, (uint8_t *)&fifo_ctrl3, 1); - if (ret == 0) { + if (ret == 0) + { fifo_ctrl3.bdr_gy = (uint8_t)val & 0x0Fu; ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_FIFO_CTRL3, (uint8_t *)&fifo_ctrl3, 1); } @@ -2678,13 +3390,17 @@ int32_t lsm6dsv16x_fifo_gy_batch_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fifo_gy_b * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fifo_gy_batch_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fifo_gy_batch_t *val) +int32_t lsm6dsv16x_fifo_gy_batch_get(stmdev_ctx_t *ctx, + lsm6dsv16x_fifo_gy_batch_t *val) { lsm6dsv16x_fifo_ctrl3_t fifo_ctrl3; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FIFO_CTRL3, (uint8_t *)&fifo_ctrl3, 1); - switch (fifo_ctrl3.bdr_gy) { + if (ret != 0) { return ret; } + + switch (fifo_ctrl3.bdr_gy) + { case LSM6DSV16X_GY_NOT_BATCHED: *val = LSM6DSV16X_GY_NOT_BATCHED; break; @@ -2749,17 +3465,18 @@ int32_t lsm6dsv16x_fifo_gy_batch_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fifo_gy_b * @brief FIFO mode selection.[set] * * @param ctx read / write interface definitions - * @param val BYPASS_MODE, FIFO_MODE, STREAM_TO_FIFO_MODE, BYPASS_TO_STREAM_MODE, STREAM_MODE, BYPASS_TO_FIFO_MODE, + * @param val BYPASS_MODE, FIFO_MODE, STREAM_WTM_TO_FULL_MODE, STREAM_TO_FIFO_MODE, BYPASS_TO_STREAM_MODE, STREAM_MODE, BYPASS_TO_FIFO_MODE, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fifo_mode_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fifo_mode_t val) +int32_t lsm6dsv16x_fifo_mode_set(stmdev_ctx_t *ctx, lsm6dsv16x_fifo_mode_t val) { lsm6dsv16x_fifo_ctrl4_t fifo_ctrl4; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FIFO_CTRL4, (uint8_t *)&fifo_ctrl4, 1); - if (ret == 0) { + if (ret == 0) + { fifo_ctrl4.fifo_mode = (uint8_t)val & 0x07U; ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_FIFO_CTRL4, (uint8_t *)&fifo_ctrl4, 1); } @@ -2771,17 +3488,20 @@ int32_t lsm6dsv16x_fifo_mode_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fifo_mode_t v * @brief FIFO mode selection.[get] * * @param ctx read / write interface definitions - * @param val BYPASS_MODE, FIFO_MODE, STREAM_TO_FIFO_MODE, BYPASS_TO_STREAM_MODE, STREAM_MODE, BYPASS_TO_FIFO_MODE, + * @param val BYPASS_MODE, FIFO_MODE, STREAM_WTM_TO_FULL_MODE, STREAM_TO_FIFO_MODE, BYPASS_TO_STREAM_MODE, STREAM_MODE, BYPASS_TO_FIFO_MODE, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fifo_mode_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fifo_mode_t *val) +int32_t lsm6dsv16x_fifo_mode_get(stmdev_ctx_t *ctx, lsm6dsv16x_fifo_mode_t *val) { lsm6dsv16x_fifo_ctrl4_t fifo_ctrl4; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FIFO_CTRL4, (uint8_t *)&fifo_ctrl4, 1); - switch (fifo_ctrl4.fifo_mode) { + if (ret != 0) { return ret; } + + switch (fifo_ctrl4.fifo_mode) + { case LSM6DSV16X_BYPASS_MODE: *val = LSM6DSV16X_BYPASS_MODE; break; @@ -2825,13 +3545,14 @@ int32_t lsm6dsv16x_fifo_mode_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fifo_mode_t * * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fifo_gy_eis_batch_set(lsm6dsv16x_ctx_t *ctx, uint8_t val) +int32_t lsm6dsv16x_fifo_gy_eis_batch_set(stmdev_ctx_t *ctx, uint8_t val) { lsm6dsv16x_fifo_ctrl4_t fifo_ctrl4; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FIFO_CTRL4, (uint8_t *)&fifo_ctrl4, 1); - if (ret == 0) { + if (ret == 0) + { fifo_ctrl4.g_eis_fifo_en = val; ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_FIFO_CTRL4, (uint8_t *)&fifo_ctrl4, 1); } @@ -2847,7 +3568,7 @@ int32_t lsm6dsv16x_fifo_gy_eis_batch_set(lsm6dsv16x_ctx_t *ctx, uint8_t val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fifo_gy_eis_batch_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val) +int32_t lsm6dsv16x_fifo_gy_eis_batch_get(stmdev_ctx_t *ctx, uint8_t *val) { lsm6dsv16x_fifo_ctrl4_t fifo_ctrl4; int32_t ret; @@ -2866,13 +3587,15 @@ int32_t lsm6dsv16x_fifo_gy_eis_batch_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fifo_temp_batch_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fifo_temp_batch_t val) +int32_t lsm6dsv16x_fifo_temp_batch_set(stmdev_ctx_t *ctx, + lsm6dsv16x_fifo_temp_batch_t val) { lsm6dsv16x_fifo_ctrl4_t fifo_ctrl4; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FIFO_CTRL4, (uint8_t *)&fifo_ctrl4, 1); - if (ret == 0) { + if (ret == 0) + { fifo_ctrl4.odr_t_batch = (uint8_t)val & 0x03U; ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_FIFO_CTRL4, (uint8_t *)&fifo_ctrl4, 1); } @@ -2888,13 +3611,17 @@ int32_t lsm6dsv16x_fifo_temp_batch_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fifo_te * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fifo_temp_batch_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fifo_temp_batch_t *val) +int32_t lsm6dsv16x_fifo_temp_batch_get(stmdev_ctx_t *ctx, + lsm6dsv16x_fifo_temp_batch_t *val) { lsm6dsv16x_fifo_ctrl4_t fifo_ctrl4; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FIFO_CTRL4, (uint8_t *)&fifo_ctrl4, 1); - switch (fifo_ctrl4.odr_t_batch) { + if (ret != 0) { return ret; } + + switch (fifo_ctrl4.odr_t_batch) + { case LSM6DSV16X_TEMP_NOT_BATCHED: *val = LSM6DSV16X_TEMP_NOT_BATCHED; break; @@ -2926,13 +3653,15 @@ int32_t lsm6dsv16x_fifo_temp_batch_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fifo_te * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fifo_timestamp_batch_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fifo_timestamp_batch_t val) +int32_t lsm6dsv16x_fifo_timestamp_batch_set(stmdev_ctx_t *ctx, + lsm6dsv16x_fifo_timestamp_batch_t val) { lsm6dsv16x_fifo_ctrl4_t fifo_ctrl4; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FIFO_CTRL4, (uint8_t *)&fifo_ctrl4, 1); - if (ret == 0) { + if (ret == 0) + { fifo_ctrl4.dec_ts_batch = (uint8_t)val & 0x03U; ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_FIFO_CTRL4, (uint8_t *)&fifo_ctrl4, 1); } @@ -2948,13 +3677,17 @@ int32_t lsm6dsv16x_fifo_timestamp_batch_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fi * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fifo_timestamp_batch_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fifo_timestamp_batch_t *val) +int32_t lsm6dsv16x_fifo_timestamp_batch_get(stmdev_ctx_t *ctx, + lsm6dsv16x_fifo_timestamp_batch_t *val) { lsm6dsv16x_fifo_ctrl4_t fifo_ctrl4; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FIFO_CTRL4, (uint8_t *)&fifo_ctrl4, 1); - switch (fifo_ctrl4.dec_ts_batch) { + if (ret != 0) { return ret; } + + switch (fifo_ctrl4.dec_ts_batch) + { case LSM6DSV16X_TMSTMP_NOT_BATCHED: *val = LSM6DSV16X_TMSTMP_NOT_BATCHED; break; @@ -2975,6 +3708,7 @@ int32_t lsm6dsv16x_fifo_timestamp_batch_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fi *val = LSM6DSV16X_TMSTMP_NOT_BATCHED; break; } + return ret; } @@ -2986,7 +3720,8 @@ int32_t lsm6dsv16x_fifo_timestamp_batch_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fi * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fifo_batch_counter_threshold_set(lsm6dsv16x_ctx_t *ctx, uint16_t val) +int32_t lsm6dsv16x_fifo_batch_counter_threshold_set(stmdev_ctx_t *ctx, + uint16_t val) { uint8_t buff[2]; int32_t ret; @@ -3006,12 +3741,15 @@ int32_t lsm6dsv16x_fifo_batch_counter_threshold_set(lsm6dsv16x_ctx_t *ctx, uint1 * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fifo_batch_counter_threshold_get(lsm6dsv16x_ctx_t *ctx, uint16_t *val) +int32_t lsm6dsv16x_fifo_batch_counter_threshold_get(stmdev_ctx_t *ctx, + uint16_t *val) { uint8_t buff[2]; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_COUNTER_BDR_REG1, &buff[0], 2); + if (ret != 0) { return ret; } + *val = buff[1]; *val = (*val * 256U) + buff[0]; @@ -3026,13 +3764,15 @@ int32_t lsm6dsv16x_fifo_batch_counter_threshold_get(lsm6dsv16x_ctx_t *ctx, uint1 * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fifo_batch_cnt_event_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fifo_batch_cnt_event_t val) +int32_t lsm6dsv16x_fifo_batch_cnt_event_set(stmdev_ctx_t *ctx, + lsm6dsv16x_fifo_batch_cnt_event_t val) { lsm6dsv16x_counter_bdr_reg1_t counter_bdr_reg1; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_COUNTER_BDR_REG1, (uint8_t *)&counter_bdr_reg1, 1); - if (ret == 0) { + if (ret == 0) + { counter_bdr_reg1.trig_counter_bdr = (uint8_t)val & 0x03U; ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_COUNTER_BDR_REG1, (uint8_t *)&counter_bdr_reg1, 1); } @@ -3048,13 +3788,17 @@ int32_t lsm6dsv16x_fifo_batch_cnt_event_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fi * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fifo_batch_cnt_event_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fifo_batch_cnt_event_t *val) +int32_t lsm6dsv16x_fifo_batch_cnt_event_get(stmdev_ctx_t *ctx, + lsm6dsv16x_fifo_batch_cnt_event_t *val) { lsm6dsv16x_counter_bdr_reg1_t counter_bdr_reg1; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_COUNTER_BDR_REG1, (uint8_t *)&counter_bdr_reg1, 1); - switch (counter_bdr_reg1.trig_counter_bdr) { + if (ret != 0) { return ret; } + + switch (counter_bdr_reg1.trig_counter_bdr) + { case LSM6DSV16X_XL_BATCH_EVENT: *val = LSM6DSV16X_XL_BATCH_EVENT; break; @@ -3071,47 +3815,63 @@ int32_t lsm6dsv16x_fifo_batch_cnt_event_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fi *val = LSM6DSV16X_XL_BATCH_EVENT; break; } + return ret; } -/** - * @brief Number of unread sensor data (TAG + 6 bytes) stored in FIFO.[get] - * - * @param ctx read / write interface definitions - * @param val Number of unread sensor data (TAG + 6 bytes) stored in FIFO. - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6dsv16x_fifo_data_level_get(lsm6dsv16x_ctx_t *ctx, uint16_t *val) +int32_t lsm6dsv16x_fifo_status_get(stmdev_ctx_t *ctx, + lsm6dsv16x_fifo_status_t *val) { uint8_t buff[2]; + lsm6dsv16x_fifo_status2_t status; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FIFO_STATUS1, &buff[0], 2); - *val = (uint16_t)buff[1] & 0x01U; - *val = (*val * 256U) + buff[0]; + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FIFO_STATUS1, (uint8_t *)&buff[0], 2); + if (ret != 0) { return ret; } + + bytecpy((uint8_t *)&status, &buff[1]); + + val->fifo_bdr = status.counter_bdr_ia; + val->fifo_ovr = status.fifo_ovr_ia; + val->fifo_full = status.fifo_full_ia; + val->fifo_th = status.fifo_wtm_ia; + + val->fifo_level = (uint16_t)buff[1] & 0x01U; + val->fifo_level = (val->fifo_level * 256U) + buff[0]; return ret; } + /** * @brief FIFO data output[get] * * @param ctx read / write interface definitions - * @param val FIFO_EMPTY, GY_NC_TAG, XL_NC_TAG, TIMESTAMP_TAG, TEMPERATURE_TAG, CFG_CHANGE_TAG, XL_NC_T_2_TAG, XL_NC_T_1_TAG, XL_2XC_TAG, XL_3XC_TAG, GY_NC_T_2_TAG, GY_NC_T_1_TAG, GY_2XC_TAG, GY_3XC_TAG, SENSORHUB_SLAVE0_TAG, SENSORHUB_SLAVE1_TAG, SENSORHUB_SLAVE2_TAG, SENSORHUB_SLAVE3_TAG, STEP_CPUNTER_TAG, SENSORHUB_NACK_TAG, MLC_RESULT_TAG, MLC_FILTER, MLC_FEATURE, XL_DUAL_CORE, AH_QVAR, + * @param val FIFO_EMPTY, GY_NC_TAG, XL_NC_TAG, TIMESTAMP_TAG, + TEMPERATURE_TAG, CFG_CHANGE_TAG, XL_NC_T_2_TAG, + XL_NC_T_1_TAG, XL_2XC_TAG, XL_3XC_TAG, GY_NC_T_2_TAG, + GY_NC_T_1_TAG, GY_2XC_TAG, GY_3XC_TAG, SENSORHUB_SLAVE0_TAG, + SENSORHUB_SLAVE1_TAG, SENSORHUB_SLAVE2_TAG, SENSORHUB_SLAVE3_TAG, + STEP_COUNTER_TAG, SFLP_GAME_ROTATION_VECTOR_TAG, SFLP_GYROSCOPE_BIAS_TAG, + SFLP_GRAVITY_VECTOR_TAG, SENSORHUB_NACK_TAG, MLC_RESULT_TAG, + MLC_FILTER, MLC_FEATURE, XL_DUAL_CORE, GY_ENHANCED_EIS, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fifo_out_raw_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fifo_out_raw_t *val) +int32_t lsm6dsv16x_fifo_out_raw_get(stmdev_ctx_t *ctx, + lsm6dsv16x_fifo_out_raw_t *val) { lsm6dsv16x_fifo_data_out_tag_t fifo_data_out_tag; uint8_t buff[7]; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FIFO_DATA_OUT_TAG, buff, 7); + if (ret != 0) { return ret; } + bytecpy((uint8_t *)&fifo_data_out_tag, &buff[0]); - switch (fifo_data_out_tag.tag_sensor) { + switch (fifo_data_out_tag.tag_sensor) + { case LSM6DSV16X_FIFO_EMPTY: val->tag = LSM6DSV16X_FIFO_EMPTY; break; @@ -3184,8 +3944,20 @@ int32_t lsm6dsv16x_fifo_out_raw_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fifo_out_r val->tag = LSM6DSV16X_SENSORHUB_SLAVE3_TAG; break; - case LSM6DSV16X_STEP_CPUNTER_TAG: - val->tag = LSM6DSV16X_STEP_CPUNTER_TAG; + case LSM6DSV16X_STEP_COUNTER_TAG: + val->tag = LSM6DSV16X_STEP_COUNTER_TAG; + break; + + case LSM6DSV16X_SFLP_GAME_ROTATION_VECTOR_TAG: + val->tag = LSM6DSV16X_SFLP_GAME_ROTATION_VECTOR_TAG; + break; + + case LSM6DSV16X_SFLP_GYROSCOPE_BIAS_TAG: + val->tag = LSM6DSV16X_SFLP_GYROSCOPE_BIAS_TAG; + break; + + case LSM6DSV16X_SFLP_GRAVITY_VECTOR_TAG: + val->tag = LSM6DSV16X_SFLP_GRAVITY_VECTOR_TAG; break; case LSM6DSV16X_SENSORHUB_NACK_TAG: @@ -3208,8 +3980,8 @@ int32_t lsm6dsv16x_fifo_out_raw_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fifo_out_r val->tag = LSM6DSV16X_XL_DUAL_CORE; break; - case LSM6DSV16X_AH_QVAR: - val->tag = LSM6DSV16X_AH_QVAR; + case LSM6DSV16X_GY_ENHANCED_EIS: + val->tag = LSM6DSV16X_GY_ENHANCED_EIS; break; default: @@ -3237,23 +4009,19 @@ int32_t lsm6dsv16x_fifo_out_raw_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fifo_out_r * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fifo_stpcnt_batch_set(lsm6dsv16x_ctx_t *ctx, uint8_t val) +int32_t lsm6dsv16x_fifo_stpcnt_batch_set(stmdev_ctx_t *ctx, uint8_t val) { lsm6dsv16x_emb_func_fifo_en_a_t emb_func_fifo_en_a; int32_t ret; ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK); - if (ret == 0) { - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_EMB_FUNC_FIFO_EN_A, (uint8_t *)&emb_func_fifo_en_a, 1); - } + if (ret != 0) { return ret; } - if (ret == 0) { - emb_func_fifo_en_a.step_counter_fifo_en = val; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_EMB_FUNC_FIFO_EN_A, (uint8_t *)&emb_func_fifo_en_a, 1); - } - if (ret == 0) { - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); - } + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_EMB_FUNC_FIFO_EN_A, (uint8_t *)&emb_func_fifo_en_a, 1); + emb_func_fifo_en_a.step_counter_fifo_en = val; + ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_EMB_FUNC_FIFO_EN_A, (uint8_t *)&emb_func_fifo_en_a, 1); + + ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); return ret; } @@ -3266,21 +4034,18 @@ int32_t lsm6dsv16x_fifo_stpcnt_batch_set(lsm6dsv16x_ctx_t *ctx, uint8_t val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fifo_stpcnt_batch_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val) +int32_t lsm6dsv16x_fifo_stpcnt_batch_get(stmdev_ctx_t *ctx, uint8_t *val) { lsm6dsv16x_emb_func_fifo_en_a_t emb_func_fifo_en_a; int32_t ret; ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK); - if (ret == 0) { - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_EMB_FUNC_FIFO_EN_A, (uint8_t *)&emb_func_fifo_en_a, 1); - } + if (ret != 0) { return ret; } + ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_EMB_FUNC_FIFO_EN_A, (uint8_t *)&emb_func_fifo_en_a, 1); *val = emb_func_fifo_en_a.step_counter_fifo_en; - if (ret == 0) { - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); - } + ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); return ret; } @@ -3293,23 +4058,19 @@ int32_t lsm6dsv16x_fifo_stpcnt_batch_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fifo_mlc_batch_set(lsm6dsv16x_ctx_t *ctx, uint8_t val) +int32_t lsm6dsv16x_fifo_mlc_batch_set(stmdev_ctx_t *ctx, uint8_t val) { lsm6dsv16x_emb_func_fifo_en_a_t emb_func_fifo_en_a; int32_t ret; ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK); - if (ret == 0) { - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_EMB_FUNC_FIFO_EN_A, (uint8_t *)&emb_func_fifo_en_a, 1); - } + if (ret != 0) { return ret; } - if (ret == 0) { - emb_func_fifo_en_a.mlc_fifo_en = val; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_EMB_FUNC_FIFO_EN_A, (uint8_t *)&emb_func_fifo_en_a, 1); - } - if (ret == 0) { - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); - } + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_EMB_FUNC_FIFO_EN_A, (uint8_t *)&emb_func_fifo_en_a, 1); + emb_func_fifo_en_a.mlc_fifo_en = val; + ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_EMB_FUNC_FIFO_EN_A, (uint8_t *)&emb_func_fifo_en_a, 1); + + ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); return ret; } @@ -3322,21 +4083,18 @@ int32_t lsm6dsv16x_fifo_mlc_batch_set(lsm6dsv16x_ctx_t *ctx, uint8_t val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fifo_mlc_batch_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val) +int32_t lsm6dsv16x_fifo_mlc_batch_get(stmdev_ctx_t *ctx, uint8_t *val) { lsm6dsv16x_emb_func_fifo_en_a_t emb_func_fifo_en_a; int32_t ret; ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK); - if (ret == 0) { - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_EMB_FUNC_FIFO_EN_A, (uint8_t *)&emb_func_fifo_en_a, 1); - } + if (ret != 0) { return ret; } + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_EMB_FUNC_FIFO_EN_A, (uint8_t *)&emb_func_fifo_en_a, 1); *val = emb_func_fifo_en_a.mlc_fifo_en; - if (ret == 0) { - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); - } + ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); return ret; } @@ -3349,23 +4107,19 @@ int32_t lsm6dsv16x_fifo_mlc_batch_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fifo_mlc_filt_batch_set(lsm6dsv16x_ctx_t *ctx, uint8_t val) +int32_t lsm6dsv16x_fifo_mlc_filt_batch_set(stmdev_ctx_t *ctx, uint8_t val) { lsm6dsv16x_emb_func_fifo_en_b_t emb_func_fifo_en_b; int32_t ret; ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK); - if (ret == 0) { - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_EMB_FUNC_FIFO_EN_B, (uint8_t *)&emb_func_fifo_en_b, 1); - } + if (ret != 0) { return ret; } - if (ret == 0) { - emb_func_fifo_en_b.mlc_filter_feature_fifo_en = val; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_EMB_FUNC_FIFO_EN_B, (uint8_t *)&emb_func_fifo_en_b, 1); - } - if (ret == 0) { - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); - } + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_EMB_FUNC_FIFO_EN_B, (uint8_t *)&emb_func_fifo_en_b, 1); + emb_func_fifo_en_b.mlc_filter_feature_fifo_en = val; + ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_EMB_FUNC_FIFO_EN_B, (uint8_t *)&emb_func_fifo_en_b, 1); + + ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); return ret; } @@ -3378,245 +4132,126 @@ int32_t lsm6dsv16x_fifo_mlc_filt_batch_set(lsm6dsv16x_ctx_t *ctx, uint8_t val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fifo_mlc_filt_batch_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val) +int32_t lsm6dsv16x_fifo_mlc_filt_batch_get(stmdev_ctx_t *ctx, uint8_t *val) { lsm6dsv16x_emb_func_fifo_en_b_t emb_func_fifo_en_b; int32_t ret; ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK); - if (ret == 0) { - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_EMB_FUNC_FIFO_EN_B, (uint8_t *)&emb_func_fifo_en_b, 1); - } + if (ret != 0) { return ret; } + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_EMB_FUNC_FIFO_EN_B, (uint8_t *)&emb_func_fifo_en_b, 1); *val = emb_func_fifo_en_b.mlc_filter_feature_fifo_en; - if (ret == 0) { - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); - } - - return ret; -} - -/** - * @brief Enable FIFO data batching of first slave.[set] - * - * @param ctx read / write interface definitions - * @param val Enable FIFO data batching of first slave. - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6dsv16x_fifo_batch_sh_slave_0_set(lsm6dsv16x_ctx_t *ctx, uint8_t val) -{ - lsm6dsv16x_slv0_config_t slv0_config; - int32_t ret; - - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_SENSOR_HUB_MEM_BANK); - if (ret == 0) { - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_SLV0_CONFIG, (uint8_t *)&slv0_config, 1); - } - - if (ret == 0) { - slv0_config.batch_ext_sens_0_en = val; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_SLV0_CONFIG, (uint8_t *)&slv0_config, 1); - } - if (ret == 0) { - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); - } + ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); return ret; } /** - * @brief Enable FIFO data batching of first slave.[get] + * @brief Enable FIFO data batching of slave idx.[set] * * @param ctx read / write interface definitions - * @param val Enable FIFO data batching of first slave. + * @param val Enable FIFO data batching of slave idx. * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fifo_batch_sh_slave_0_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val) +int32_t lsm6dsv16x_fifo_sh_batch_slave_set(stmdev_ctx_t *ctx, uint8_t idx, uint8_t val) { - lsm6dsv16x_slv0_config_t slv0_config; + lsm6dsv16x_slv0_config_t slv_config; int32_t ret; ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_SENSOR_HUB_MEM_BANK); - if (ret == 0) { - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_SLV0_CONFIG, (uint8_t *)&slv0_config, 1); - } - - *val = slv0_config.batch_ext_sens_0_en; - - if (ret == 0) { - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); - } - - return ret; -} - -/** - * @brief Enable FIFO data batching of second slave.[set] - * - * @param ctx read / write interface definitions - * @param val Enable FIFO data batching of second slave. - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6dsv16x_fifo_batch_sh_slave_1_set(lsm6dsv16x_ctx_t *ctx, uint8_t val) -{ - lsm6dsv16x_slv1_config_t slv1_config; - int32_t ret; + if (ret != 0) { return ret; } - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_SENSOR_HUB_MEM_BANK); - if (ret == 0) { - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_SLV1_CONFIG, (uint8_t *)&slv1_config, 1); - } + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_SLV0_CONFIG + idx*3U, (uint8_t *)&slv_config, 1); + slv_config.batch_ext_sens_0_en = val; + ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_SLV0_CONFIG + idx*3U, (uint8_t *)&slv_config, 1); - if (ret == 0) { - slv1_config.batch_ext_sens_1_en = val; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_SLV1_CONFIG, (uint8_t *)&slv1_config, 1); - } - if (ret == 0) { - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); - } + ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); return ret; } /** - * @brief Enable FIFO data batching of second slave.[get] + * @brief Enable FIFO data batching of slave idx.[get] * * @param ctx read / write interface definitions - * @param val Enable FIFO data batching of second slave. + * @param val Enable FIFO data batching of slave idx. * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fifo_batch_sh_slave_1_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val) +int32_t lsm6dsv16x_fifo_sh_batch_slave_get(stmdev_ctx_t *ctx, uint8_t idx, uint8_t *val) { - lsm6dsv16x_slv1_config_t slv1_config; + lsm6dsv16x_slv0_config_t slv_config; int32_t ret; ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_SENSOR_HUB_MEM_BANK); - if (ret == 0) { - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_SLV1_CONFIG, (uint8_t *)&slv1_config, 1); - } - - *val = slv1_config.batch_ext_sens_1_en; - - if (ret == 0) { - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); - } - - return ret; -} - -/** - * @brief Enable FIFO data batching of third slave.[set] - * - * @param ctx read / write interface definitions - * @param val Enable FIFO data batching of third slave. - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6dsv16x_fifo_batch_sh_slave_2_set(lsm6dsv16x_ctx_t *ctx, uint8_t val) -{ - lsm6dsv16x_slv2_config_t slv2_config; - int32_t ret; + if (ret != 0) { return ret; } - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_SENSOR_HUB_MEM_BANK); - if (ret == 0) { - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_SLV2_CONFIG, (uint8_t *)&slv2_config, 1); - } + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_SLV0_CONFIG + idx*3U, (uint8_t *)&slv_config, 1); + *val = slv_config.batch_ext_sens_0_en; - if (ret == 0) { - slv2_config.batch_ext_sens_2_en = val; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_SLV2_CONFIG, (uint8_t *)&slv2_config, 1); - } - if (ret == 0) { - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); - } + ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); return ret; } /** - * @brief Enable FIFO data batching of third slave.[get] + * @brief Batching in FIFO buffer of SFLP.[set] * * @param ctx read / write interface definitions - * @param val Enable FIFO data batching of third slave. + * @param val Batching in FIFO buffer of SFLP values. * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fifo_batch_sh_slave_2_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val) +int32_t lsm6dsv16x_fifo_sflp_batch_set(stmdev_ctx_t *ctx, + lsm6dsv16x_fifo_sflp_raw_t val) { - lsm6dsv16x_slv2_config_t slv2_config; + lsm6dsv16x_emb_func_fifo_en_a_t emb_func_fifo_en_a; int32_t ret; - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_SENSOR_HUB_MEM_BANK); - if (ret == 0) { - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_SLV2_CONFIG, (uint8_t *)&slv2_config, 1); + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK); + if (ret == 0) + { + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_EMB_FUNC_FIFO_EN_A, (uint8_t *)&emb_func_fifo_en_a, 1); + emb_func_fifo_en_a.sflp_game_fifo_en = val.game_rotation; + emb_func_fifo_en_a.sflp_gravity_fifo_en = val.gravity; + emb_func_fifo_en_a.sflp_gbias_fifo_en = val.gbias; + ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_EMB_FUNC_FIFO_EN_A, + (uint8_t *)&emb_func_fifo_en_a, 1); } - *val = slv2_config.batch_ext_sens_2_en; - - if (ret == 0) { - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); - } + ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); return ret; } /** - * @brief Enable FIFO data batching of fourth slave.[set] + * @brief Batching in FIFO buffer of SFLP.[get] * * @param ctx read / write interface definitions - * @param val Enable FIFO data batching of fourth slave. + * @param val Batching in FIFO buffer of SFLP values. * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fifo_batch_sh_slave_3_set(lsm6dsv16x_ctx_t *ctx, uint8_t val) +int32_t lsm6dsv16x_fifo_sflp_batch_get(stmdev_ctx_t *ctx, + lsm6dsv16x_fifo_sflp_raw_t *val) { - lsm6dsv16x_slv3_config_t slv3_config; + lsm6dsv16x_emb_func_fifo_en_a_t emb_func_fifo_en_a; int32_t ret; - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_SENSOR_HUB_MEM_BANK); - if (ret == 0) { - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_SLV3_CONFIG, (uint8_t *)&slv3_config, 1); - } - - if (ret == 0) { - slv3_config.batch_ext_sens_3_en = val; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_SLV3_CONFIG, (uint8_t *)&slv3_config, 1); - } - if (ret == 0) { - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); - } - - return ret; -} - -/** - * @brief Enable FIFO data batching of fourth slave.[get] - * - * @param ctx read / write interface definitions - * @param val Enable FIFO data batching of fourth slave. - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6dsv16x_fifo_batch_sh_slave_3_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val) -{ - lsm6dsv16x_slv3_config_t slv3_config; - int32_t ret; + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK); + if (ret == 0) + { + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_EMB_FUNC_FIFO_EN_A, (uint8_t *)&emb_func_fifo_en_a, 1); - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_SENSOR_HUB_MEM_BANK); - if (ret == 0) { - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_SLV3_CONFIG, (uint8_t *)&slv3_config, 1); + val->game_rotation = emb_func_fifo_en_a.sflp_game_fifo_en; + val->gravity = emb_func_fifo_en_a.sflp_gravity_fifo_en; + val->gbias = emb_func_fifo_en_a.sflp_gbias_fifo_en; } - *val = slv3_config.batch_ext_sens_3_en; - - if (ret == 0) { - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); - } + ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); return ret; } @@ -3642,14 +4277,16 @@ int32_t lsm6dsv16x_fifo_batch_sh_slave_3_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_filt_anti_spike_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_filt_anti_spike_t val) +int32_t lsm6dsv16x_filt_anti_spike_set(stmdev_ctx_t *ctx, + lsm6dsv16x_filt_anti_spike_t val) { lsm6dsv16x_if_cfg_t if_cfg; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_IF_CFG, (uint8_t *)&if_cfg, 1); - if (ret == 0) { + if (ret == 0) + { if_cfg.asf_ctrl = (uint8_t)val & 0x01U; ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_IF_CFG, (uint8_t *)&if_cfg, 1); } @@ -3665,13 +4302,17 @@ int32_t lsm6dsv16x_filt_anti_spike_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_filt_an * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_filt_anti_spike_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_filt_anti_spike_t *val) +int32_t lsm6dsv16x_filt_anti_spike_get(stmdev_ctx_t *ctx, + lsm6dsv16x_filt_anti_spike_t *val) { lsm6dsv16x_if_cfg_t if_cfg; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_IF_CFG, (uint8_t *)&if_cfg, 1); - switch (if_cfg.asf_ctrl) { + if (ret != 0) { return ret; } + + switch (if_cfg.asf_ctrl) + { case LSM6DSV16X_AUTO: *val = LSM6DSV16X_AUTO; break; @@ -3684,6 +4325,7 @@ int32_t lsm6dsv16x_filt_anti_spike_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_filt_an *val = LSM6DSV16X_AUTO; break; } + return ret; } @@ -3695,7 +4337,8 @@ int32_t lsm6dsv16x_filt_anti_spike_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_filt_an * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_filt_settling_mask_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_filt_settling_mask_t val) +int32_t lsm6dsv16x_filt_settling_mask_set(stmdev_ctx_t *ctx, + lsm6dsv16x_filt_settling_mask_t val) { lsm6dsv16x_emb_func_cfg_t emb_func_cfg; lsm6dsv16x_ui_int_ois_t ui_int_ois; @@ -3703,31 +4346,19 @@ int32_t lsm6dsv16x_filt_settling_mask_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_filt int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL4, (uint8_t *)&ctrl4, 1); + ctrl4.drdy_mask = val.drdy; + ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_CTRL4, (uint8_t *)&ctrl4, 1); + if (ret != 0) { return ret; } - if (ret == 0) { - ctrl4.drdy_mask = val.drdy; - - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_CTRL4, (uint8_t *)&ctrl4, 1); - } - - if (ret == 0) { - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_EMB_FUNC_CFG, (uint8_t *)&emb_func_cfg, 1); - } - - if (ret == 0) { - emb_func_cfg.emb_func_irq_mask_xl_settl = val.irq_xl; - emb_func_cfg.emb_func_irq_mask_g_settl = val.irq_g; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_EMB_FUNC_CFG, (uint8_t *)&emb_func_cfg, 1); - } - - if (ret == 0) { - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_UI_INT_OIS, (uint8_t *)&ui_int_ois, 1); - } + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_EMB_FUNC_CFG, (uint8_t *)&emb_func_cfg, 1); + emb_func_cfg.emb_func_irq_mask_xl_settl = val.irq_xl; + emb_func_cfg.emb_func_irq_mask_g_settl = val.irq_g; + ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_EMB_FUNC_CFG, (uint8_t *)&emb_func_cfg, 1); + if (ret != 0) { return ret; } - if (ret == 0) { - ui_int_ois.drdy_mask_ois = val.ois_drdy; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_UI_INT_OIS, (uint8_t *)&ui_int_ois, 1); - } + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_UI_INT_OIS, (uint8_t *)&ui_int_ois, 1); + ui_int_ois.drdy_mask_ois = val.ois_drdy; + ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_UI_INT_OIS, (uint8_t *)&ui_int_ois, 1); return ret; } @@ -3740,7 +4371,8 @@ int32_t lsm6dsv16x_filt_settling_mask_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_filt * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_filt_settling_mask_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_filt_settling_mask_t *val) +int32_t lsm6dsv16x_filt_settling_mask_get(stmdev_ctx_t *ctx, + lsm6dsv16x_filt_settling_mask_t *val) { lsm6dsv16x_emb_func_cfg_t emb_func_cfg; lsm6dsv16x_ui_int_ois_t ui_int_ois; @@ -3748,12 +4380,8 @@ int32_t lsm6dsv16x_filt_settling_mask_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_filt int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL4, (uint8_t *)&ctrl4, 1); - if (ret == 0) { - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_EMB_FUNC_CFG, (uint8_t *)&emb_func_cfg, 1); - } - if (ret == 0) { - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_UI_INT_OIS, (uint8_t *)&ui_int_ois, 1); - } + ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_EMB_FUNC_CFG, (uint8_t *)&emb_func_cfg, 1); + ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_UI_INT_OIS, (uint8_t *)&ui_int_ois, 1); val->irq_xl = emb_func_cfg.emb_func_irq_mask_xl_settl; val->irq_g = emb_func_cfg.emb_func_irq_mask_g_settl; @@ -3770,14 +4398,16 @@ int32_t lsm6dsv16x_filt_settling_mask_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_filt * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_filt_ois_settling_mask_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_filt_ois_settling_mask_t val) +int32_t lsm6dsv16x_filt_ois_settling_mask_set(stmdev_ctx_t *ctx, + lsm6dsv16x_filt_ois_settling_mask_t val) { lsm6dsv16x_spi2_int_ois_t spi2_int_ois; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_SPI2_INT_OIS, (uint8_t *)&spi2_int_ois, 1); - if (ret == 0) { + if (ret == 0) + { spi2_int_ois.drdy_mask_ois = val.ois_drdy; ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_SPI2_INT_OIS, (uint8_t *)&spi2_int_ois, 1); } @@ -3793,7 +4423,8 @@ int32_t lsm6dsv16x_filt_ois_settling_mask_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_filt_ois_settling_mask_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_filt_ois_settling_mask_t *val) +int32_t lsm6dsv16x_filt_ois_settling_mask_get(stmdev_ctx_t *ctx, + lsm6dsv16x_filt_ois_settling_mask_t *val) { lsm6dsv16x_spi2_int_ois_t spi2_int_ois; @@ -3813,13 +4444,15 @@ int32_t lsm6dsv16x_filt_ois_settling_mask_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_filt_gy_lp1_bandwidth_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_filt_gy_lp1_bandwidth_t val) +int32_t lsm6dsv16x_filt_gy_lp1_bandwidth_set(stmdev_ctx_t *ctx, + lsm6dsv16x_filt_gy_lp1_bandwidth_t val) { lsm6dsv16x_ctrl6_t ctrl6; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL6, (uint8_t *)&ctrl6, 1); - if (ret == 0) { + if (ret == 0) + { ctrl6.lpf1_g_bw = (uint8_t)val & 0x0Fu; ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_CTRL6, (uint8_t *)&ctrl6, 1); } @@ -3835,13 +4468,17 @@ int32_t lsm6dsv16x_filt_gy_lp1_bandwidth_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_f * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_filt_gy_lp1_bandwidth_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_filt_gy_lp1_bandwidth_t *val) +int32_t lsm6dsv16x_filt_gy_lp1_bandwidth_get(stmdev_ctx_t *ctx, + lsm6dsv16x_filt_gy_lp1_bandwidth_t *val) { lsm6dsv16x_ctrl6_t ctrl6; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL6, (uint8_t *)&ctrl6, 1); - switch (ctrl6.lpf1_g_bw) { + if (ret != 0) { return ret; } + + switch (ctrl6.lpf1_g_bw) + { case LSM6DSV16X_GY_ULTRA_LIGHT: *val = LSM6DSV16X_GY_ULTRA_LIGHT; break; @@ -3878,6 +4515,7 @@ int32_t lsm6dsv16x_filt_gy_lp1_bandwidth_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_f *val = LSM6DSV16X_GY_ULTRA_LIGHT; break; } + return ret; } @@ -3889,13 +4527,14 @@ int32_t lsm6dsv16x_filt_gy_lp1_bandwidth_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_f * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_filt_gy_lp1_set(lsm6dsv16x_ctx_t *ctx, uint8_t val) +int32_t lsm6dsv16x_filt_gy_lp1_set(stmdev_ctx_t *ctx, uint8_t val) { lsm6dsv16x_ctrl7_t ctrl7; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL7, (uint8_t *)&ctrl7, 1); - if (ret == 0) { + if (ret == 0) + { ctrl7.lpf1_g_en = val; ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_CTRL7, (uint8_t *)&ctrl7, 1); } @@ -3912,7 +4551,7 @@ int32_t lsm6dsv16x_filt_gy_lp1_set(lsm6dsv16x_ctx_t *ctx, uint8_t val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_filt_gy_lp1_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val) +int32_t lsm6dsv16x_filt_gy_lp1_get(stmdev_ctx_t *ctx, uint8_t *val) { lsm6dsv16x_ctrl7_t ctrl7; int32_t ret; @@ -3931,13 +4570,15 @@ int32_t lsm6dsv16x_filt_gy_lp1_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_filt_xl_lp2_bandwidth_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_filt_xl_lp2_bandwidth_t val) +int32_t lsm6dsv16x_filt_xl_lp2_bandwidth_set(stmdev_ctx_t *ctx, + lsm6dsv16x_filt_xl_lp2_bandwidth_t val) { lsm6dsv16x_ctrl8_t ctrl8; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL8, (uint8_t *)&ctrl8, 1); - if (ret == 0) { + if (ret == 0) + { ctrl8.hp_lpf2_xl_bw = (uint8_t)val & 0x07U; ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_CTRL8, (uint8_t *)&ctrl8, 1); } @@ -3953,13 +4594,17 @@ int32_t lsm6dsv16x_filt_xl_lp2_bandwidth_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_f * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_filt_xl_lp2_bandwidth_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_filt_xl_lp2_bandwidth_t *val) +int32_t lsm6dsv16x_filt_xl_lp2_bandwidth_get(stmdev_ctx_t *ctx, + lsm6dsv16x_filt_xl_lp2_bandwidth_t *val) { lsm6dsv16x_ctrl8_t ctrl8; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL8, (uint8_t *)&ctrl8, 1); - switch (ctrl8.hp_lpf2_xl_bw) { + if (ret != 0) { return ret; } + + switch (ctrl8.hp_lpf2_xl_bw) + { case LSM6DSV16X_XL_ULTRA_LIGHT: *val = LSM6DSV16X_XL_ULTRA_LIGHT; break; @@ -3996,6 +4641,7 @@ int32_t lsm6dsv16x_filt_xl_lp2_bandwidth_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_f *val = LSM6DSV16X_XL_ULTRA_LIGHT; break; } + return ret; } @@ -4007,13 +4653,14 @@ int32_t lsm6dsv16x_filt_xl_lp2_bandwidth_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_f * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_filt_xl_lp2_set(lsm6dsv16x_ctx_t *ctx, uint8_t val) +int32_t lsm6dsv16x_filt_xl_lp2_set(stmdev_ctx_t *ctx, uint8_t val) { lsm6dsv16x_ctrl9_t ctrl9; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL9, (uint8_t *)&ctrl9, 1); - if (ret == 0) { + if (ret == 0) + { ctrl9.lpf2_xl_en = val; ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_CTRL9, (uint8_t *)&ctrl9, 1); } @@ -4029,7 +4676,7 @@ int32_t lsm6dsv16x_filt_xl_lp2_set(lsm6dsv16x_ctx_t *ctx, uint8_t val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_filt_xl_lp2_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val) +int32_t lsm6dsv16x_filt_xl_lp2_get(stmdev_ctx_t *ctx, uint8_t *val) { lsm6dsv16x_ctrl9_t ctrl9; int32_t ret; @@ -4048,13 +4695,14 @@ int32_t lsm6dsv16x_filt_xl_lp2_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_filt_xl_hp_set(lsm6dsv16x_ctx_t *ctx, uint8_t val) +int32_t lsm6dsv16x_filt_xl_hp_set(stmdev_ctx_t *ctx, uint8_t val) { lsm6dsv16x_ctrl9_t ctrl9; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL9, (uint8_t *)&ctrl9, 1); - if (ret == 0) { + if (ret == 0) + { ctrl9.hp_slope_xl_en = val; ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_CTRL9, (uint8_t *)&ctrl9, 1); } @@ -4070,7 +4718,7 @@ int32_t lsm6dsv16x_filt_xl_hp_set(lsm6dsv16x_ctx_t *ctx, uint8_t val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_filt_xl_hp_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val) +int32_t lsm6dsv16x_filt_xl_hp_get(stmdev_ctx_t *ctx, uint8_t *val) { lsm6dsv16x_ctrl9_t ctrl9; int32_t ret; @@ -4089,13 +4737,14 @@ int32_t lsm6dsv16x_filt_xl_hp_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_filt_xl_fast_settling_set(lsm6dsv16x_ctx_t *ctx, uint8_t val) +int32_t lsm6dsv16x_filt_xl_fast_settling_set(stmdev_ctx_t *ctx, uint8_t val) { lsm6dsv16x_ctrl9_t ctrl9; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL9, (uint8_t *)&ctrl9, 1); - if (ret == 0) { + if (ret == 0) + { ctrl9.xl_fastsettl_mode = val; ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_CTRL9, (uint8_t *)&ctrl9, 1); } @@ -4111,7 +4760,7 @@ int32_t lsm6dsv16x_filt_xl_fast_settling_set(lsm6dsv16x_ctx_t *ctx, uint8_t val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_filt_xl_fast_settling_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val) +int32_t lsm6dsv16x_filt_xl_fast_settling_get(stmdev_ctx_t *ctx, uint8_t *val) { lsm6dsv16x_ctrl9_t ctrl9; int32_t ret; @@ -4130,13 +4779,15 @@ int32_t lsm6dsv16x_filt_xl_fast_settling_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_filt_xl_hp_mode_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_filt_xl_hp_mode_t val) +int32_t lsm6dsv16x_filt_xl_hp_mode_set(stmdev_ctx_t *ctx, + lsm6dsv16x_filt_xl_hp_mode_t val) { lsm6dsv16x_ctrl9_t ctrl9; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL9, (uint8_t *)&ctrl9, 1); - if (ret == 0) { + if (ret == 0) + { ctrl9.hp_ref_mode_xl = (uint8_t)val & 0x01U; ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_CTRL9, (uint8_t *)&ctrl9, 1); } @@ -4152,13 +4803,17 @@ int32_t lsm6dsv16x_filt_xl_hp_mode_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_filt_xl * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_filt_xl_hp_mode_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_filt_xl_hp_mode_t *val) +int32_t lsm6dsv16x_filt_xl_hp_mode_get(stmdev_ctx_t *ctx, + lsm6dsv16x_filt_xl_hp_mode_t *val) { lsm6dsv16x_ctrl9_t ctrl9; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL9, (uint8_t *)&ctrl9, 1); - switch (ctrl9.hp_ref_mode_xl) { + if (ret != 0) { return ret; } + + switch (ctrl9.hp_ref_mode_xl) + { case LSM6DSV16X_HP_MD_NORMAL: *val = LSM6DSV16X_HP_MD_NORMAL; break; @@ -4171,6 +4826,7 @@ int32_t lsm6dsv16x_filt_xl_hp_mode_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_filt_xl *val = LSM6DSV16X_HP_MD_NORMAL; break; } + return ret; } @@ -4182,25 +4838,23 @@ int32_t lsm6dsv16x_filt_xl_hp_mode_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_filt_xl * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_filt_wkup_act_feed_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_filt_wkup_act_feed_t val) +int32_t lsm6dsv16x_filt_wkup_act_feed_set(stmdev_ctx_t *ctx, + lsm6dsv16x_filt_wkup_act_feed_t val) { lsm6dsv16x_wake_up_ths_t wake_up_ths; lsm6dsv16x_tap_cfg0_t tap_cfg0; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_WAKE_UP_THS, (uint8_t *)&wake_up_ths, 1); - if (ret == 0) { - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_TAP_CFG0, (uint8_t *)&tap_cfg0, 1); - } + ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_TAP_CFG0, (uint8_t *)&tap_cfg0, 1); + if (ret != 0) { return ret; } - if (ret == 0) { - tap_cfg0.slope_fds = (uint8_t)val & 0x01U; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_TAP_CFG0, (uint8_t *)&tap_cfg0, 1); - } - if (ret == 0) { - wake_up_ths.usr_off_on_wu = ((uint8_t)val & 0x02U) >> 1; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_WAKE_UP_THS, (uint8_t *)&wake_up_ths, 1); - } + tap_cfg0.slope_fds = (uint8_t)val & 0x01U; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_TAP_CFG0, (uint8_t *)&tap_cfg0, 1); + if (ret != 0) { return ret; } + + wake_up_ths.usr_off_on_wu = ((uint8_t)val & 0x02U) >> 1; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_WAKE_UP_THS, (uint8_t *)&wake_up_ths, 1); return ret; } @@ -4213,18 +4867,19 @@ int32_t lsm6dsv16x_filt_wkup_act_feed_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_filt * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_filt_wkup_act_feed_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_filt_wkup_act_feed_t *val) +int32_t lsm6dsv16x_filt_wkup_act_feed_get(stmdev_ctx_t *ctx, + lsm6dsv16x_filt_wkup_act_feed_t *val) { lsm6dsv16x_wake_up_ths_t wake_up_ths; lsm6dsv16x_tap_cfg0_t tap_cfg0; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_WAKE_UP_THS, (uint8_t *)&wake_up_ths, 1); - if (ret == 0) { - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_TAP_CFG0, (uint8_t *)&tap_cfg0, 1); - } + ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_TAP_CFG0, (uint8_t *)&tap_cfg0, 1); + if (ret != 0) { return ret; } - switch ((wake_up_ths.usr_off_on_wu << 1) + tap_cfg0.slope_fds) { + switch ((wake_up_ths.usr_off_on_wu << 1) + tap_cfg0.slope_fds) + { case LSM6DSV16X_WK_FEED_SLOPE: *val = LSM6DSV16X_WK_FEED_SLOPE; break; @@ -4241,26 +4896,28 @@ int32_t lsm6dsv16x_filt_wkup_act_feed_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_filt *val = LSM6DSV16X_WK_FEED_SLOPE; break; } + return ret; } /** - * @brief LPF2 filter on 6D (sixd) function selection.[set] + * @brief Mask hw function triggers when xl is settling.[set] * * @param ctx read / write interface definitions - * @param val SIXD_FEED_ODR_DIV_2, SIXD_FEED_LOW_PASS, + * @param val 0 or 1, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_filt_sixd_feed_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_filt_sixd_feed_t val) +int32_t lsm6dsv16x_mask_trigger_xl_settl_set(stmdev_ctx_t *ctx, uint8_t val) { lsm6dsv16x_tap_cfg0_t tap_cfg0; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_TAP_CFG0, (uint8_t *)&tap_cfg0, 1); - if (ret == 0) { - tap_cfg0.low_pass_on_6d = (uint8_t)val & 0x01U; + if (ret == 0) + { + tap_cfg0.hw_func_mask_xl_settl = val & 0x01U; ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_TAP_CFG0, (uint8_t *)&tap_cfg0, 1); } @@ -4268,75 +4925,128 @@ int32_t lsm6dsv16x_filt_sixd_feed_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_filt_six } /** - * @brief LPF2 filter on 6D (sixd) function selection.[get] + * @brief Mask hw function triggers when xl is settling.[get] * * @param ctx read / write interface definitions - * @param val SIXD_FEED_ODR_DIV_2, SIXD_FEED_LOW_PASS, + * @param val 0 or 1, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_filt_sixd_feed_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_filt_sixd_feed_t *val) +int32_t lsm6dsv16x_mask_trigger_xl_settl_get(stmdev_ctx_t *ctx, uint8_t *val) { lsm6dsv16x_tap_cfg0_t tap_cfg0; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_TAP_CFG0, (uint8_t *)&tap_cfg0, 1); + *val = tap_cfg0.hw_func_mask_xl_settl; - switch (tap_cfg0.low_pass_on_6d) { - case LSM6DSV16X_SIXD_FEED_ODR_DIV_2: - *val = LSM6DSV16X_SIXD_FEED_ODR_DIV_2; - break; - - case LSM6DSV16X_SIXD_FEED_LOW_PASS: - *val = LSM6DSV16X_SIXD_FEED_LOW_PASS; - break; - - default: - *val = LSM6DSV16X_SIXD_FEED_ODR_DIV_2; - break; - } return ret; } /** - * @brief Gyroscope digital LPF_EIS filter bandwidth selection.[set] + * @brief LPF2 filter on 6D (sixd) function selection.[set] * * @param ctx read / write interface definitions - * @param val EIS_LP_NORMAL, EIS_LP_LIGHT, + * @param val SIXD_FEED_ODR_DIV_2, SIXD_FEED_LOW_PASS, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_filt_gy_eis_lp_bandwidth_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_filt_gy_eis_lp_bandwidth_t val) +int32_t lsm6dsv16x_filt_sixd_feed_set(stmdev_ctx_t *ctx, + lsm6dsv16x_filt_sixd_feed_t val) { - lsm6dsv16x_ctrl_eis_t ctrl_eis; + lsm6dsv16x_tap_cfg0_t tap_cfg0; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL_EIS, (uint8_t *)&ctrl_eis, 1); + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_TAP_CFG0, (uint8_t *)&tap_cfg0, 1); - if (ret == 0) { - ctrl_eis.lpf_g_eis_bw = (uint8_t)val & 0x01U; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_CTRL_EIS, (uint8_t *)&ctrl_eis, 1); + if (ret == 0) + { + tap_cfg0.low_pass_on_6d = (uint8_t)val & 0x01U; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_TAP_CFG0, (uint8_t *)&tap_cfg0, 1); } return ret; } /** - * @brief Gyroscope digital LPF_EIS filter bandwidth selection.[get] + * @brief LPF2 filter on 6D (sixd) function selection.[get] + * + * @param ctx read / write interface definitions + * @param val SIXD_FEED_ODR_DIV_2, SIXD_FEED_LOW_PASS, + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_filt_sixd_feed_get(stmdev_ctx_t *ctx, + lsm6dsv16x_filt_sixd_feed_t *val) +{ + lsm6dsv16x_tap_cfg0_t tap_cfg0; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_TAP_CFG0, (uint8_t *)&tap_cfg0, 1); + if (ret != 0) { return ret; } + + switch (tap_cfg0.low_pass_on_6d) + { + case LSM6DSV16X_SIXD_FEED_ODR_DIV_2: + *val = LSM6DSV16X_SIXD_FEED_ODR_DIV_2; + break; + + case LSM6DSV16X_SIXD_FEED_LOW_PASS: + *val = LSM6DSV16X_SIXD_FEED_LOW_PASS; + break; + + default: + *val = LSM6DSV16X_SIXD_FEED_ODR_DIV_2; + break; + } + + return ret; +} + +/** + * @brief Gyroscope digital LPF_EIS filter bandwidth selection.[set] + * + * @param ctx read / write interface definitions + * @param val EIS_LP_NORMAL, EIS_LP_LIGHT, + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_filt_gy_eis_lp_bandwidth_set(stmdev_ctx_t *ctx, + lsm6dsv16x_filt_gy_eis_lp_bandwidth_t val) +{ + lsm6dsv16x_ctrl_eis_t ctrl_eis; + int32_t ret; + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL_EIS, (uint8_t *)&ctrl_eis, 1); + + if (ret == 0) + { + ctrl_eis.lpf_g_eis_bw = (uint8_t)val & 0x01U; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_CTRL_EIS, (uint8_t *)&ctrl_eis, 1); + } + + return ret; +} + +/** + * @brief Gyroscope digital LPF_EIS filter bandwidth selection.[get] * * @param ctx read / write interface definitions * @param val EIS_LP_NORMAL, EIS_LP_LIGHT, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_filt_gy_eis_lp_bandwidth_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_filt_gy_eis_lp_bandwidth_t *val) +int32_t lsm6dsv16x_filt_gy_eis_lp_bandwidth_get(stmdev_ctx_t *ctx, + lsm6dsv16x_filt_gy_eis_lp_bandwidth_t *val) { lsm6dsv16x_ctrl_eis_t ctrl_eis; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL_EIS, (uint8_t *)&ctrl_eis, 1); + if (ret != 0) { return ret; } - switch (ctrl_eis.lpf_g_eis_bw) { + switch (ctrl_eis.lpf_g_eis_bw) + { case LSM6DSV16X_EIS_LP_NORMAL: *val = LSM6DSV16X_EIS_LP_NORMAL; break; @@ -4349,6 +5059,7 @@ int32_t lsm6dsv16x_filt_gy_eis_lp_bandwidth_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16 *val = LSM6DSV16X_EIS_LP_NORMAL; break; } + return ret; } @@ -4360,14 +5071,16 @@ int32_t lsm6dsv16x_filt_gy_eis_lp_bandwidth_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16 * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_filt_gy_ois_lp_bandwidth_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_filt_gy_ois_lp_bandwidth_t val) +int32_t lsm6dsv16x_filt_gy_ois_lp_bandwidth_set(stmdev_ctx_t *ctx, + lsm6dsv16x_filt_gy_ois_lp_bandwidth_t val) { lsm6dsv16x_ui_ctrl2_ois_t ui_ctrl2_ois; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_UI_CTRL2_OIS, (uint8_t *)&ui_ctrl2_ois, 1); - if (ret == 0) { + if (ret == 0) + { ui_ctrl2_ois.lpf1_g_ois_bw = (uint8_t)val & 0x03U; ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_UI_CTRL2_OIS, (uint8_t *)&ui_ctrl2_ois, 1); } @@ -4383,15 +5096,18 @@ int32_t lsm6dsv16x_filt_gy_ois_lp_bandwidth_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16 * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_filt_gy_ois_lp_bandwidth_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_filt_gy_ois_lp_bandwidth_t *val) +int32_t lsm6dsv16x_filt_gy_ois_lp_bandwidth_get(stmdev_ctx_t *ctx, + lsm6dsv16x_filt_gy_ois_lp_bandwidth_t *val) { lsm6dsv16x_ui_ctrl2_ois_t ui_ctrl2_ois; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_UI_CTRL2_OIS, (uint8_t *)&ui_ctrl2_ois, 1); + if (ret != 0) { return ret; } - switch (ui_ctrl2_ois.lpf1_g_ois_bw) { + switch (ui_ctrl2_ois.lpf1_g_ois_bw) + { case LSM6DSV16X_OIS_GY_LP_NORMAL: *val = LSM6DSV16X_OIS_GY_LP_NORMAL; break; @@ -4412,6 +5128,7 @@ int32_t lsm6dsv16x_filt_gy_ois_lp_bandwidth_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16 *val = LSM6DSV16X_OIS_GY_LP_NORMAL; break; } + return ret; } @@ -4423,14 +5140,16 @@ int32_t lsm6dsv16x_filt_gy_ois_lp_bandwidth_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16 * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_filt_xl_ois_lp_bandwidth_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_filt_xl_ois_lp_bandwidth_t val) +int32_t lsm6dsv16x_filt_xl_ois_lp_bandwidth_set(stmdev_ctx_t *ctx, + lsm6dsv16x_filt_xl_ois_lp_bandwidth_t val) { lsm6dsv16x_ui_ctrl3_ois_t ui_ctrl3_ois; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_UI_CTRL3_OIS, (uint8_t *)&ui_ctrl3_ois, 1); - if (ret == 0) { + if (ret == 0) + { ui_ctrl3_ois.lpf_xl_ois_bw = (uint8_t)val & 0x07U; ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_UI_CTRL3_OIS, (uint8_t *)&ui_ctrl3_ois, 1); } @@ -4446,14 +5165,17 @@ int32_t lsm6dsv16x_filt_xl_ois_lp_bandwidth_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16 * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_filt_xl_ois_lp_bandwidth_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_filt_xl_ois_lp_bandwidth_t *val) +int32_t lsm6dsv16x_filt_xl_ois_lp_bandwidth_get(stmdev_ctx_t *ctx, + lsm6dsv16x_filt_xl_ois_lp_bandwidth_t *val) { lsm6dsv16x_ui_ctrl3_ois_t ui_ctrl3_ois; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_UI_CTRL3_OIS, (uint8_t *)&ui_ctrl3_ois, 1); + if (ret != 0) { return ret; } - switch (ui_ctrl3_ois.lpf_xl_ois_bw) { + switch (ui_ctrl3_ois.lpf_xl_ois_bw) + { case LSM6DSV16X_OIS_XL_LP_ULTRA_LIGHT: *val = LSM6DSV16X_OIS_XL_LP_ULTRA_LIGHT; break; @@ -4490,6 +5212,7 @@ int32_t lsm6dsv16x_filt_xl_ois_lp_bandwidth_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16 *val = LSM6DSV16X_OIS_XL_LP_ULTRA_LIGHT; break; } + return ret; } @@ -4514,14 +5237,16 @@ int32_t lsm6dsv16x_filt_xl_ois_lp_bandwidth_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16 * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fsm_permission_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fsm_permission_t val) +int32_t lsm6dsv16x_fsm_permission_set(stmdev_ctx_t *ctx, + lsm6dsv16x_fsm_permission_t val) { lsm6dsv16x_func_cfg_access_t func_cfg_access; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FUNC_CFG_ACCESS, (uint8_t *)&func_cfg_access, 1); - if (ret == 0) { + if (ret == 0) + { func_cfg_access.fsm_wr_ctrl_en = (uint8_t)val & 0x01U; ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_FUNC_CFG_ACCESS, (uint8_t *)&func_cfg_access, 1); } @@ -4537,14 +5262,17 @@ int32_t lsm6dsv16x_fsm_permission_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fsm_perm * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fsm_permission_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fsm_permission_t *val) +int32_t lsm6dsv16x_fsm_permission_get(stmdev_ctx_t *ctx, + lsm6dsv16x_fsm_permission_t *val) { lsm6dsv16x_func_cfg_access_t func_cfg_access; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FUNC_CFG_ACCESS, (uint8_t *)&func_cfg_access, 1); + if (ret != 0) { return ret; } - switch (func_cfg_access.fsm_wr_ctrl_en) { + switch (func_cfg_access.fsm_wr_ctrl_en) + { case LSM6DSV16X_PROTECT_CTRL_REGS: *val = LSM6DSV16X_PROTECT_CTRL_REGS; break; @@ -4557,6 +5285,7 @@ int32_t lsm6dsv16x_fsm_permission_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fsm_perm *val = LSM6DSV16X_PROTECT_CTRL_REGS; break; } + return ret; } @@ -4568,7 +5297,7 @@ int32_t lsm6dsv16x_fsm_permission_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fsm_perm * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fsm_permission_status(lsm6dsv16x_ctx_t *ctx, uint8_t *val) +int32_t lsm6dsv16x_fsm_permission_status(stmdev_ctx_t *ctx, uint8_t *val) { lsm6dsv16x_ctrl_status_t ctrl_status; int32_t ret; @@ -4588,43 +5317,44 @@ int32_t lsm6dsv16x_fsm_permission_status(lsm6dsv16x_ctx_t *ctx, uint8_t *val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fsm_mode_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fsm_mode_t val) +int32_t lsm6dsv16x_fsm_mode_set(stmdev_ctx_t *ctx, lsm6dsv16x_fsm_mode_t val) { lsm6dsv16x_emb_func_en_b_t emb_func_en_b; lsm6dsv16x_fsm_enable_t fsm_enable; int32_t ret; ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK); - if (ret == 0) { - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_EMB_FUNC_EN_B, (uint8_t *)&emb_func_en_b, 1); - } - if (ret == 0) { - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FSM_ENABLE, (uint8_t *)&fsm_enable, 1); - } + if (ret != 0) { return ret; } + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_EMB_FUNC_EN_B, (uint8_t *)&emb_func_en_b, 1); + ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FSM_ENABLE, (uint8_t *)&fsm_enable, 1); + if (ret != 0) { goto exit; } + if ((val.fsm1_en | val.fsm2_en | val.fsm1_en | val.fsm1_en - | val.fsm1_en | val.fsm2_en | val.fsm1_en | val.fsm1_en) == PROPERTY_ENABLE) { + | val.fsm1_en | val.fsm2_en | val.fsm1_en | val.fsm1_en) == PROPERTY_ENABLE) + { emb_func_en_b.fsm_en = PROPERTY_ENABLE; - } else { - emb_func_en_b.fsm_en = PROPERTY_DISABLE; - } - if (ret == 0) { - fsm_enable.fsm1_en = val.fsm1_en; - fsm_enable.fsm2_en = val.fsm2_en; - fsm_enable.fsm3_en = val.fsm3_en; - fsm_enable.fsm4_en = val.fsm4_en; - fsm_enable.fsm5_en = val.fsm5_en; - fsm_enable.fsm6_en = val.fsm6_en; - fsm_enable.fsm7_en = val.fsm7_en; - fsm_enable.fsm8_en = val.fsm8_en; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_FSM_ENABLE, (uint8_t *)&fsm_enable, 1); } - if (ret == 0) { - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_EMB_FUNC_EN_B, (uint8_t *)&emb_func_en_b, 1); - } - if (ret == 0) { - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + else + { + emb_func_en_b.fsm_en = PROPERTY_DISABLE; } + fsm_enable.fsm1_en = val.fsm1_en; + fsm_enable.fsm2_en = val.fsm2_en; + fsm_enable.fsm3_en = val.fsm3_en; + fsm_enable.fsm4_en = val.fsm4_en; + fsm_enable.fsm5_en = val.fsm5_en; + fsm_enable.fsm6_en = val.fsm6_en; + fsm_enable.fsm7_en = val.fsm7_en; + fsm_enable.fsm8_en = val.fsm8_en; + + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_FSM_ENABLE, (uint8_t *)&fsm_enable, 1); + ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_EMB_FUNC_EN_B, (uint8_t *)&emb_func_en_b, 1); + +exit: + ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + return ret; } @@ -4636,18 +5366,15 @@ int32_t lsm6dsv16x_fsm_mode_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fsm_mode_t val * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fsm_mode_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fsm_mode_t *val) +int32_t lsm6dsv16x_fsm_mode_get(stmdev_ctx_t *ctx, lsm6dsv16x_fsm_mode_t *val) { lsm6dsv16x_fsm_enable_t fsm_enable; int32_t ret; ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK); - if (ret == 0) { - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FSM_ENABLE, (uint8_t *)&fsm_enable, 1); - } - if (ret == 0) { - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); - } + ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FSM_ENABLE, (uint8_t *)&fsm_enable, 1); + ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + if (ret != 0) { return ret; } val->fsm1_en = fsm_enable.fsm1_en; val->fsm2_en = fsm_enable.fsm2_en; @@ -4669,7 +5396,7 @@ int32_t lsm6dsv16x_fsm_mode_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fsm_mode_t *va * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fsm_long_cnt_set(lsm6dsv16x_ctx_t *ctx, uint16_t val) +int32_t lsm6dsv16x_fsm_long_cnt_set(stmdev_ctx_t *ctx, uint16_t val) { uint8_t buff[2]; int32_t ret; @@ -4677,10 +5404,9 @@ int32_t lsm6dsv16x_fsm_long_cnt_set(lsm6dsv16x_ctx_t *ctx, uint16_t val) buff[1] = (uint8_t)(val / 256U); buff[0] = (uint8_t)(val - (buff[1] * 256U)); - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_FSM_LONG_COUNTER_L, (uint8_t *)&buff[0], 2); - if (ret == 0) { - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); - } + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK); + ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_FSM_LONG_COUNTER_L, (uint8_t *)&buff[0], 2); + ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); return ret; } @@ -4693,18 +5419,15 @@ int32_t lsm6dsv16x_fsm_long_cnt_set(lsm6dsv16x_ctx_t *ctx, uint16_t val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fsm_long_cnt_get(lsm6dsv16x_ctx_t *ctx, uint16_t *val) +int32_t lsm6dsv16x_fsm_long_cnt_get(stmdev_ctx_t *ctx, uint16_t *val) { uint8_t buff[2]; int32_t ret; ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK); - if (ret == 0) { - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FSM_LONG_COUNTER_L, &buff[0], 2); - } - if (ret == 0) { - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); - } + ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FSM_LONG_COUNTER_L, &buff[0], 2); + ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + if (ret != 0) { return ret; } *val = buff[1]; *val = (*val * 256U) + buff[0]; @@ -4720,17 +5443,13 @@ int32_t lsm6dsv16x_fsm_long_cnt_get(lsm6dsv16x_ctx_t *ctx, uint16_t *val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fsm_out_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fsm_out_t *val) +int32_t lsm6dsv16x_fsm_out_get(stmdev_ctx_t *ctx, lsm6dsv16x_fsm_out_t *val) { int32_t ret; ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK); - if (ret == 0) { - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FSM_OUTS1, (uint8_t *)val, 8); - } - if (ret == 0) { - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); - } + ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FSM_OUTS1, (uint8_t *)val, 8); + ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); return ret; } @@ -4743,23 +5462,21 @@ int32_t lsm6dsv16x_fsm_out_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fsm_out_t *val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fsm_data_rate_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fsm_data_rate_t val) +int32_t lsm6dsv16x_fsm_data_rate_set(stmdev_ctx_t *ctx, + lsm6dsv16x_fsm_data_rate_t val) { lsm6dsv16x_fsm_odr_t fsm_odr; int32_t ret; ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK); - if (ret == 0) { - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FSM_ODR, (uint8_t *)&fsm_odr, 1); - } + ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FSM_ODR, (uint8_t *)&fsm_odr, 1); + if (ret != 0) { goto exit; } - if (ret == 0) { - fsm_odr.fsm_odr = (uint8_t)val & 0x07U; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_FSM_ODR, (uint8_t *)&fsm_odr, 1); - } - if (ret == 0) { - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); - } + fsm_odr.fsm_odr = (uint8_t)val & 0x07U; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_FSM_ODR, (uint8_t *)&fsm_odr, 1); + +exit: + ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); return ret; } @@ -4772,21 +5489,19 @@ int32_t lsm6dsv16x_fsm_data_rate_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fsm_data_ * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fsm_data_rate_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fsm_data_rate_t *val) +int32_t lsm6dsv16x_fsm_data_rate_get(stmdev_ctx_t *ctx, + lsm6dsv16x_fsm_data_rate_t *val) { lsm6dsv16x_fsm_odr_t fsm_odr; int32_t ret; ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK); - if (ret == 0) { - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FSM_ODR, (uint8_t *)&fsm_odr, 1); - } - if (ret == 0) { - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); - } - + ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FSM_ODR, (uint8_t *)&fsm_odr, 1); + ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + if (ret != 0) { return ret; } - switch (fsm_odr.fsm_odr) { + switch (fsm_odr.fsm_odr) + { case LSM6DSV16X_FSM_15Hz: *val = LSM6DSV16X_FSM_15Hz; break; @@ -4819,6 +5534,325 @@ int32_t lsm6dsv16x_fsm_data_rate_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fsm_data_ *val = LSM6DSV16X_FSM_15Hz; break; } + + return ret; +} + +/* + * Original conversion routines taken from: https://github.com/numpy/numpy + * + * uint16_t npy_floatbits_to_halfbits(uint32_t f); + * uint16_t npy_float_to_half(float_t f); + * + * Released under BSD-3-Clause License + */ +static uint16_t npy_floatbits_to_halfbits(uint32_t f) +{ + uint32_t f_exp, f_sig; + uint16_t h_sgn, h_exp, h_sig; + + h_sgn = (uint16_t)((f & 0x80000000u) >> 16); + f_exp = (f & 0x7f800000u); + + /* Exponent overflow/NaN converts to signed inf/NaN */ + if (f_exp >= 0x47800000u) + { + if (f_exp == 0x7f800000u) + { + /* Inf or NaN */ + f_sig = (f & 0x007fffffu); + if (f_sig != 0U) + { + /* NaN - propagate the flag in the significand... */ + uint16_t ret = (uint16_t)(0x7c00u + (f_sig >> 13)); + /* ...but make sure it stays a NaN */ + if (ret == 0x7c00u) + { + ret++; + } + return h_sgn + ret; + } + else + { + /* signed inf */ + return (uint16_t)(h_sgn + 0x7c00u); + } + } + else + { + /* overflow to signed inf */ +#if NPY_HALF_GENERATE_OVERFLOW + npy_set_floatstatus_overflow(); +#endif + return (uint16_t)(h_sgn + 0x7c00u); + } + } + + /* Exponent underflow converts to a subnormal half or signed zero */ + if (f_exp <= 0x38000000u) + { + /* + * Signed zeros, subnormal floats, and floats with small + * exponents all convert to signed zero half-floats. + */ + if (f_exp < 0x33000000u) + { +#if NPY_HALF_GENERATE_UNDERFLOW + /* If f != 0, it underflowed to 0 */ + if ((f & 0x7fffffff) != 0) + { + npy_set_floatstatus_underflow(); + } +#endif + return h_sgn; + } + /* Make the subnormal significand */ + f_exp >>= 23; + f_sig = (0x00800000u + (f & 0x007fffffu)); +#if NPY_HALF_GENERATE_UNDERFLOW + /* If it's not exactly represented, it underflowed */ + if ((f_sig & (((uint32_t)1 << (126 - f_exp)) - 1)) != 0) + { + npy_set_floatstatus_underflow(); + } +#endif + /* + * Usually the significand is shifted by 13. For subnormals an + * additional shift needs to occur. This shift is one for the largest + * exponent giving a subnormal `f_exp = 0x38000000 >> 23 = 112`, which + * offsets the new first bit. At most the shift can be 1+10 bits. + */ + f_sig >>= (113U - f_exp); + /* Handle rounding by adding 1 to the bit beyond half precision */ +#if NPY_HALF_ROUND_TIES_TO_EVEN + /* + * If the last bit in the half significand is 0 (already even), and + * the remaining bit pattern is 1000...0, then we do not add one + * to the bit after the half significand. However, the (113 - f_exp) + * shift can lose up to 11 bits, so the || checks them in the original. + * In all other cases, we can just add one. + */ + if (((f_sig & 0x00003fffu) != 0x00001000u) || (f & 0x000007ffu)) + { + f_sig += 0x00001000u; + } +#else + f_sig += 0x00001000u; +#endif + h_sig = (uint16_t)(f_sig >> 13); + /* + * If the rounding causes a bit to spill into h_exp, it will + * increment h_exp from zero to one and h_sig will be zero. + * This is the correct result. + */ + return (uint16_t)(h_sgn + h_sig); + } + + /* Regular case with no overflow or underflow */ + h_exp = (uint16_t)((f_exp - 0x38000000u) >> 13); + /* Handle rounding by adding 1 to the bit beyond half precision */ + f_sig = (f & 0x007fffffu); +#if NPY_HALF_ROUND_TIES_TO_EVEN + /* + * If the last bit in the half significand is 0 (already even), and + * the remaining bit pattern is 1000...0, then we do not add one + * to the bit after the half significand. In all other cases, we do. + */ + if ((f_sig & 0x00003fffu) != 0x00001000u) + { + f_sig += 0x00001000u; + } +#else + f_sig += 0x00001000u; +#endif + h_sig = (uint16_t)(f_sig >> 13); + /* + * If the rounding causes a bit to spill into h_exp, it will + * increment h_exp by one and h_sig will be zero. This is the + * correct result. h_exp may increment to 15, at greatest, in + * which case the result overflows to a signed inf. + */ +#if NPY_HALF_GENERATE_OVERFLOW + h_sig += h_exp; + if (h_sig == 0x7c00u) + { + npy_set_floatstatus_overflow(); + } + return h_sgn + h_sig; +#else + return h_sgn + h_exp + h_sig; +#endif +} + +static uint16_t npy_float_to_half(float_t f) +{ + union + { + float_t f; + uint32_t fbits; + } conv; + conv.f = f; + return npy_floatbits_to_halfbits(conv.fbits); +} + +/** + * @brief SFLP GBIAS value. The register value is expressed as half-precision + * floating-point format: SEEEEEFFFFFFFFFF (S: 1 sign bit; E: 5 exponent + * bits; F: 10 fraction bits).[set] + * + * @param ctx read / write interface definitions + * @param val GBIAS x/y/z val. + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_sflp_game_gbias_set(stmdev_ctx_t *ctx, + lsm6dsv16x_sflp_gbias_t *val) +{ + lsm6dsv16x_sflp_data_rate_t sflp_odr; + lsm6dsv16x_emb_func_exec_status_t emb_func_sts; + lsm6dsv16x_data_ready_t drdy; + lsm6dsv16x_xl_full_scale_t xl_fs; + lsm6dsv16x_ctrl10_t ctrl10; + uint8_t master_config; + uint8_t emb_func_en_saved[2]; + uint8_t conf_saved[2]; + uint8_t reg_zero[2] = {0x0, 0x0}; + uint16_t gbias_hf[3]; + float_t k = 0.005f; + int16_t xl_data[3]; + int32_t data_tmp; + uint8_t *data_ptr = (uint8_t *)&data_tmp; + uint8_t i, j; + int32_t ret; + + ret = lsm6dsv16x_sflp_data_rate_get(ctx, &sflp_odr); + if (ret != 0) { return ret; } + + /* Calculate k factor */ + switch (sflp_odr) + { + default: + case LSM6DSV16X_SFLP_15Hz: + k = 0.04f; + break; + case LSM6DSV16X_SFLP_30Hz: + k = 0.02f; + break; + case LSM6DSV16X_SFLP_60Hz: + k = 0.01f; + break; + case LSM6DSV16X_SFLP_120Hz: + k = 0.005f; + break; + case LSM6DSV16X_SFLP_240Hz: + k = 0.0025f; + break; + case LSM6DSV16X_SFLP_480Hz: + k = 0.00125f; + break; + } + + /* compute gbias as half precision float in order to be put in embedded advanced feature register */ + gbias_hf[0] = npy_float_to_half(val->gbias_x * (3.14159265358979323846f / 180.0f) / k); + gbias_hf[1] = npy_float_to_half(val->gbias_y * (3.14159265358979323846f / 180.0f) / k); + gbias_hf[2] = npy_float_to_half(val->gbias_z * (3.14159265358979323846f / 180.0f) / k); + + /* Save sensor configuration and set high-performance mode (if the sensor is in power-down mode, turn it on) */ + ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL1, conf_saved, 2); + ret += lsm6dsv16x_xl_mode_set(ctx, LSM6DSV16X_XL_HIGH_PERFORMANCE_MD); + ret += lsm6dsv16x_gy_mode_set(ctx, LSM6DSV16X_GY_HIGH_PERFORMANCE_MD); + if (((uint8_t)conf_saved[0] & 0x0FU) == (uint8_t)LSM6DSV16X_ODR_OFF) + { + ret += lsm6dsv16x_xl_data_rate_set(ctx, LSM6DSV16X_ODR_AT_120Hz); + } + + /* Make sure to turn the sensor-hub master off */ + ret += lsm6dsv16x_sh_master_get(ctx, &master_config); + ret += lsm6dsv16x_sh_master_set(ctx, 0); + + /* disable algos */ + ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK); + ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_EMB_FUNC_EN_A, emb_func_en_saved, 2); + ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_EMB_FUNC_EN_A, reg_zero, 2); + do + { + ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_EMB_FUNC_EXEC_STATUS, + (uint8_t *)&emb_func_sts, 1); + } while (emb_func_sts.emb_func_endop != 1U); + ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + + // enable gbias setting + ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL10, (uint8_t *)&ctrl10, 1); + ctrl10.emb_func_debug = 1; + ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_CTRL10, (uint8_t *)&ctrl10, 1); + + /* enable algos */ + ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK); + emb_func_en_saved[0] |= 0x02U; /* force SFLP GAME en */ + ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_EMB_FUNC_EN_A, emb_func_en_saved, + 2); + ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + + ret += lsm6dsv16x_xl_full_scale_get(ctx, &xl_fs); + + /* Read XL data */ + do + { + ret += lsm6dsv16x_flag_data_ready_get(ctx, &drdy); + } while (drdy.drdy_xl != 1U); + ret += lsm6dsv16x_acceleration_raw_get(ctx, xl_data); + + /* force sflp initialization */ + ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_SENSOR_HUB_MEM_BANK); + for (i = 0; i < 3U; i++) + { + j = 0; + data_tmp = (int32_t)xl_data[i]; + data_tmp <<= xl_fs; // shift based on current fs + ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_SENSOR_HUB_1 + 3U * i, + &data_ptr[j++], 1); + ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_SENSOR_HUB_2 + 3U * i, + &data_ptr[j++], 1); + ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_SENSOR_HUB_3 + 3U * i, &data_ptr[j], + 1); + } + for (i = 0; i < 3U; i++) + { + j = 0; + data_tmp = 0; + ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_SENSOR_HUB_10 + 3U * i, + &data_ptr[j++], 1); + ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_SENSOR_HUB_11 + 3U * i, + &data_ptr[j++], 1); + ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_SENSOR_HUB_12 + 3U * i, &data_ptr[j], + 1); + } + ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + + // wait end_op (and at least 30 us) + ctx->mdelay(1); + ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK); + do + { + ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_EMB_FUNC_EXEC_STATUS, + (uint8_t *)&emb_func_sts, 1); + } while (emb_func_sts.emb_func_endop != 1U); + ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + + /* write gbias in embedded advanced features registers */ + ret += lsm6dsv16x_ln_pg_write(ctx, LSM6DSV16X_SFLP_GAME_GBIASX_L, + (uint8_t *)gbias_hf, 6); + + /* reload previous sensor configuration */ + ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_CTRL1, conf_saved, 2); + + // disable gbias setting + ctrl10.emb_func_debug = 0; + ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_CTRL10, (uint8_t *)&ctrl10, 1); + + /* reload previous master configuration */ + ret += lsm6dsv16x_sh_master_set(ctx, master_config); + return ret; } @@ -4830,7 +5864,7 @@ int32_t lsm6dsv16x_fsm_data_rate_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fsm_data_ * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fsm_ext_sens_sensitivity_set(lsm6dsv16x_ctx_t *ctx, uint16_t val) +int32_t lsm6dsv16x_fsm_ext_sens_sensitivity_set(stmdev_ctx_t *ctx, uint16_t val) { uint8_t buff[2]; int32_t ret; @@ -4850,12 +5884,15 @@ int32_t lsm6dsv16x_fsm_ext_sens_sensitivity_set(lsm6dsv16x_ctx_t *ctx, uint16_t * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fsm_ext_sens_sensitivity_get(lsm6dsv16x_ctx_t *ctx, uint16_t *val) +int32_t lsm6dsv16x_fsm_ext_sens_sensitivity_get(stmdev_ctx_t *ctx, + uint16_t *val) { uint8_t buff[2]; int32_t ret; ret = lsm6dsv16x_ln_pg_read(ctx, LSM6DSV16X_FSM_EXT_SENSITIVITY_L, &buff[0], 2); + if (ret != 0) { return ret; } + *val = buff[1]; *val = (*val * 256U) + buff[0]; @@ -4870,7 +5907,8 @@ int32_t lsm6dsv16x_fsm_ext_sens_sensitivity_get(lsm6dsv16x_ctx_t *ctx, uint16_t * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fsm_ext_sens_offset_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_xl_fsm_ext_sens_offset_t val) +int32_t lsm6dsv16x_fsm_ext_sens_offset_set(stmdev_ctx_t *ctx, + lsm6dsv16x_xl_fsm_ext_sens_offset_t val) { uint8_t buff[6]; int32_t ret; @@ -4894,12 +5932,14 @@ int32_t lsm6dsv16x_fsm_ext_sens_offset_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_xl_ * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fsm_ext_sens_offset_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_xl_fsm_ext_sens_offset_t *val) +int32_t lsm6dsv16x_fsm_ext_sens_offset_get(stmdev_ctx_t *ctx, + lsm6dsv16x_xl_fsm_ext_sens_offset_t *val) { uint8_t buff[6]; int32_t ret; ret = lsm6dsv16x_ln_pg_read(ctx, LSM6DSV16X_FSM_EXT_OFFX_L, &buff[0], 6); + if (ret != 0) { return ret; } val->x = buff[1]; val->x = (val->x * 256U) + buff[0]; @@ -4919,7 +5959,8 @@ int32_t lsm6dsv16x_fsm_ext_sens_offset_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_xl_ * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fsm_ext_sens_matrix_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_xl_fsm_ext_sens_matrix_t val) +int32_t lsm6dsv16x_fsm_ext_sens_matrix_set(stmdev_ctx_t *ctx, + lsm6dsv16x_xl_fsm_ext_sens_matrix_t val) { uint8_t buff[12]; int32_t ret; @@ -4949,12 +5990,14 @@ int32_t lsm6dsv16x_fsm_ext_sens_matrix_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_xl_ * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fsm_ext_sens_matrix_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_xl_fsm_ext_sens_matrix_t *val) +int32_t lsm6dsv16x_fsm_ext_sens_matrix_get(stmdev_ctx_t *ctx, + lsm6dsv16x_xl_fsm_ext_sens_matrix_t *val) { uint8_t buff[12]; int32_t ret; ret = lsm6dsv16x_ln_pg_read(ctx, LSM6DSV16X_FSM_EXT_MATRIX_XX_L, &buff[0], 12); + if (ret != 0) { return ret; } val->xx = buff[1]; val->xx = (val->xx * 256U) + buff[0]; @@ -4980,16 +6023,15 @@ int32_t lsm6dsv16x_fsm_ext_sens_matrix_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_xl_ * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fsm_ext_sens_z_orient_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fsm_ext_sens_z_orient_t val) +int32_t lsm6dsv16x_fsm_ext_sens_z_orient_set(stmdev_ctx_t *ctx, + lsm6dsv16x_fsm_ext_sens_z_orient_t val) { lsm6dsv16x_ext_cfg_a_t ext_cfg_a; int32_t ret; ret = lsm6dsv16x_ln_pg_read(ctx, LSM6DSV16X_EXT_CFG_A, (uint8_t *)&ext_cfg_a, 1); - if (ret == 0) { - ext_cfg_a.ext_z_axis = (uint8_t)val & 0x07U; - ret = lsm6dsv16x_ln_pg_write(ctx, LSM6DSV16X_EXT_CFG_A, (uint8_t *)&ext_cfg_a, 1); - } + ext_cfg_a.ext_z_axis = (uint8_t)val & 0x07U; + ret += lsm6dsv16x_ln_pg_write(ctx, LSM6DSV16X_EXT_CFG_A, (uint8_t *)&ext_cfg_a, 1); return ret; } @@ -5002,13 +6044,17 @@ int32_t lsm6dsv16x_fsm_ext_sens_z_orient_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_f * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fsm_ext_sens_z_orient_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fsm_ext_sens_z_orient_t *val) +int32_t lsm6dsv16x_fsm_ext_sens_z_orient_get(stmdev_ctx_t *ctx, + lsm6dsv16x_fsm_ext_sens_z_orient_t *val) { lsm6dsv16x_ext_cfg_a_t ext_cfg_a; int32_t ret; ret = lsm6dsv16x_ln_pg_read(ctx, LSM6DSV16X_EXT_CFG_A, (uint8_t *)&ext_cfg_a, 1); - switch (ext_cfg_a.ext_z_axis) { + if (ret != 0) { return ret; } + + switch (ext_cfg_a.ext_z_axis) + { case LSM6DSV16X_Z_EQ_Y: *val = LSM6DSV16X_Z_EQ_Y; break; @@ -5037,6 +6083,7 @@ int32_t lsm6dsv16x_fsm_ext_sens_z_orient_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_f *val = LSM6DSV16X_Z_EQ_Y; break; } + return ret; } @@ -5048,13 +6095,15 @@ int32_t lsm6dsv16x_fsm_ext_sens_z_orient_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_f * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fsm_ext_sens_y_orient_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fsm_ext_sens_y_orient_t val) +int32_t lsm6dsv16x_fsm_ext_sens_y_orient_set(stmdev_ctx_t *ctx, + lsm6dsv16x_fsm_ext_sens_y_orient_t val) { lsm6dsv16x_ext_cfg_a_t ext_cfg_a; int32_t ret; ret = lsm6dsv16x_ln_pg_read(ctx, LSM6DSV16X_EXT_CFG_A, (uint8_t *)&ext_cfg_a, 1); - if (ret == 0) { + if (ret == 0) + { ext_cfg_a.ext_y_axis = (uint8_t)val & 0x7U; ret = lsm6dsv16x_ln_pg_write(ctx, LSM6DSV16X_EXT_CFG_A, (uint8_t *)&ext_cfg_a, 1); } @@ -5070,13 +6119,17 @@ int32_t lsm6dsv16x_fsm_ext_sens_y_orient_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_f * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fsm_ext_sens_y_orient_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fsm_ext_sens_y_orient_t *val) +int32_t lsm6dsv16x_fsm_ext_sens_y_orient_get(stmdev_ctx_t *ctx, + lsm6dsv16x_fsm_ext_sens_y_orient_t *val) { lsm6dsv16x_ext_cfg_a_t ext_cfg_a; int32_t ret; ret = lsm6dsv16x_ln_pg_read(ctx, LSM6DSV16X_EXT_CFG_A, (uint8_t *)&ext_cfg_a, 1); - switch (ext_cfg_a.ext_y_axis) { + if (ret != 0) { return ret; } + + switch (ext_cfg_a.ext_y_axis) + { case LSM6DSV16X_Y_EQ_Y: *val = LSM6DSV16X_Y_EQ_Y; break; @@ -5105,6 +6158,7 @@ int32_t lsm6dsv16x_fsm_ext_sens_y_orient_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_f *val = LSM6DSV16X_Y_EQ_Y; break; } + return ret; } @@ -5116,13 +6170,15 @@ int32_t lsm6dsv16x_fsm_ext_sens_y_orient_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_f * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fsm_ext_sens_x_orient_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fsm_ext_sens_x_orient_t val) +int32_t lsm6dsv16x_fsm_ext_sens_x_orient_set(stmdev_ctx_t *ctx, + lsm6dsv16x_fsm_ext_sens_x_orient_t val) { lsm6dsv16x_ext_cfg_b_t ext_cfg_b; int32_t ret; ret = lsm6dsv16x_ln_pg_read(ctx, LSM6DSV16X_EXT_CFG_B, (uint8_t *)&ext_cfg_b, 1); - if (ret == 0) { + if (ret == 0) + { ext_cfg_b.ext_x_axis = (uint8_t)val & 0x7U; ret = lsm6dsv16x_ln_pg_write(ctx, LSM6DSV16X_EXT_CFG_B, (uint8_t *)&ext_cfg_b, 1); } @@ -5138,13 +6194,17 @@ int32_t lsm6dsv16x_fsm_ext_sens_x_orient_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_f * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fsm_ext_sens_x_orient_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fsm_ext_sens_x_orient_t *val) +int32_t lsm6dsv16x_fsm_ext_sens_x_orient_get(stmdev_ctx_t *ctx, + lsm6dsv16x_fsm_ext_sens_x_orient_t *val) { lsm6dsv16x_ext_cfg_b_t ext_cfg_b; int32_t ret; ret = lsm6dsv16x_ln_pg_read(ctx, LSM6DSV16X_EXT_CFG_B, (uint8_t *)&ext_cfg_b, 1); - switch (ext_cfg_b.ext_x_axis) { + if (ret != 0) { return ret; } + + switch (ext_cfg_b.ext_x_axis) + { case LSM6DSV16X_X_EQ_Y: *val = LSM6DSV16X_X_EQ_Y; break; @@ -5173,6 +6233,7 @@ int32_t lsm6dsv16x_fsm_ext_sens_x_orient_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_f *val = LSM6DSV16X_X_EQ_Y; break; } + return ret; } @@ -5184,7 +6245,7 @@ int32_t lsm6dsv16x_fsm_ext_sens_x_orient_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_f * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fsm_long_cnt_timeout_set(lsm6dsv16x_ctx_t *ctx, uint16_t val) +int32_t lsm6dsv16x_fsm_long_cnt_timeout_set(stmdev_ctx_t *ctx, uint16_t val) { uint8_t buff[2]; int32_t ret; @@ -5204,12 +6265,14 @@ int32_t lsm6dsv16x_fsm_long_cnt_timeout_set(lsm6dsv16x_ctx_t *ctx, uint16_t val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fsm_long_cnt_timeout_get(lsm6dsv16x_ctx_t *ctx, uint16_t *val) +int32_t lsm6dsv16x_fsm_long_cnt_timeout_get(stmdev_ctx_t *ctx, uint16_t *val) { uint8_t buff[2]; int32_t ret; ret = lsm6dsv16x_ln_pg_read(ctx, LSM6DSV16X_EMB_ADV_PG_1 + LSM6DSV16X_FSM_LC_TIMEOUT_L, &buff[0], 2); + if (ret != 0) { return ret; } + *val = buff[1]; *val = (*val * 256U) + buff[0]; @@ -5224,13 +6287,14 @@ int32_t lsm6dsv16x_fsm_long_cnt_timeout_get(lsm6dsv16x_ctx_t *ctx, uint16_t *val * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fsm_number_of_programs_set(lsm6dsv16x_ctx_t *ctx, uint8_t val) +int32_t lsm6dsv16x_fsm_number_of_programs_set(stmdev_ctx_t *ctx, uint8_t val) { lsm6dsv16x_fsm_programs_t fsm_programs; int32_t ret; ret = lsm6dsv16x_ln_pg_read(ctx, LSM6DSV16X_EMB_ADV_PG_1 + LSM6DSV16X_FSM_PROGRAMS, (uint8_t *)&fsm_programs, 1); - if (ret == 0) { + if (ret == 0) + { fsm_programs.fsm_n_prog = val; ret = lsm6dsv16x_ln_pg_write(ctx, LSM6DSV16X_EMB_ADV_PG_1 + LSM6DSV16X_FSM_PROGRAMS, (uint8_t *)&fsm_programs, 1); } @@ -5246,7 +6310,7 @@ int32_t lsm6dsv16x_fsm_number_of_programs_set(lsm6dsv16x_ctx_t *ctx, uint8_t val * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fsm_number_of_programs_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val) +int32_t lsm6dsv16x_fsm_number_of_programs_get(stmdev_ctx_t *ctx, uint8_t *val) { lsm6dsv16x_fsm_programs_t fsm_programs; int32_t ret; @@ -5265,7 +6329,7 @@ int32_t lsm6dsv16x_fsm_number_of_programs_get(lsm6dsv16x_ctx_t *ctx, uint8_t *va * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fsm_start_address_set(lsm6dsv16x_ctx_t *ctx, uint16_t val) +int32_t lsm6dsv16x_fsm_start_address_set(stmdev_ctx_t *ctx, uint16_t val) { uint8_t buff[2]; int32_t ret; @@ -5285,12 +6349,14 @@ int32_t lsm6dsv16x_fsm_start_address_set(lsm6dsv16x_ctx_t *ctx, uint16_t val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fsm_start_address_get(lsm6dsv16x_ctx_t *ctx, uint16_t *val) +int32_t lsm6dsv16x_fsm_start_address_get(stmdev_ctx_t *ctx, uint16_t *val) { uint8_t buff[2]; int32_t ret; ret = lsm6dsv16x_ln_pg_read(ctx, LSM6DSV16X_EMB_ADV_PG_1 + LSM6DSV16X_FSM_START_ADD_L, &buff[0], 2); + if (ret != 0) { return ret; } + *val = buff[1]; *val = (*val * 256U) + buff[0]; @@ -5318,25 +6384,20 @@ int32_t lsm6dsv16x_fsm_start_address_get(lsm6dsv16x_ctx_t *ctx, uint16_t *val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_ff_time_windows_set(lsm6dsv16x_ctx_t *ctx, uint8_t val) +int32_t lsm6dsv16x_ff_time_windows_set(stmdev_ctx_t *ctx, uint8_t val) { lsm6dsv16x_wake_up_dur_t wake_up_dur; lsm6dsv16x_free_fall_t free_fall; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_WAKE_UP_DUR, (uint8_t *)&wake_up_dur, 1); - if (ret == 0) { - wake_up_dur.ff_dur = ((uint8_t)val & 0x20U) >> 5; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_WAKE_UP_DUR, (uint8_t *)&wake_up_dur, 1); - } - if (ret == 0) { - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FREE_FALL, (uint8_t *)&free_fall, 1); - } + wake_up_dur.ff_dur = ((uint8_t)val & 0x20U) >> 5; + ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_WAKE_UP_DUR, (uint8_t *)&wake_up_dur, 1); + if (ret != 0) { return ret; } - if (ret == 0) { - free_fall.ff_dur = (uint8_t)val & 0x1FU; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_FREE_FALL, (uint8_t *)&free_fall, 1); - } + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FREE_FALL, (uint8_t *)&free_fall, 1); + free_fall.ff_dur = (uint8_t)val & 0x1FU; + ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_FREE_FALL, (uint8_t *)&free_fall, 1); return ret; } @@ -5349,16 +6410,14 @@ int32_t lsm6dsv16x_ff_time_windows_set(lsm6dsv16x_ctx_t *ctx, uint8_t val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_ff_time_windows_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val) +int32_t lsm6dsv16x_ff_time_windows_get(stmdev_ctx_t *ctx, uint8_t *val) { lsm6dsv16x_wake_up_dur_t wake_up_dur; lsm6dsv16x_free_fall_t free_fall; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_WAKE_UP_DUR, (uint8_t *)&wake_up_dur, 1); - if (ret == 0) { - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FREE_FALL, (uint8_t *)&free_fall, 1); - } + ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FREE_FALL, (uint8_t *)&free_fall, 1); *val = (wake_up_dur.ff_dur << 5) + free_fall.ff_dur; @@ -5373,13 +6432,15 @@ int32_t lsm6dsv16x_ff_time_windows_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_ff_thresholds_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ff_thresholds_t val) +int32_t lsm6dsv16x_ff_thresholds_set(stmdev_ctx_t *ctx, + lsm6dsv16x_ff_thresholds_t val) { lsm6dsv16x_free_fall_t free_fall; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FREE_FALL, (uint8_t *)&free_fall, 1); - if (ret == 0) { + if (ret == 0) + { free_fall.ff_ths = (uint8_t)val & 0x7U; ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_FREE_FALL, (uint8_t *)&free_fall, 1); } @@ -5395,13 +6456,17 @@ int32_t lsm6dsv16x_ff_thresholds_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ff_thresh * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_ff_thresholds_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ff_thresholds_t *val) +int32_t lsm6dsv16x_ff_thresholds_get(stmdev_ctx_t *ctx, + lsm6dsv16x_ff_thresholds_t *val) { lsm6dsv16x_free_fall_t free_fall; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FREE_FALL, (uint8_t *)&free_fall, 1); - switch (free_fall.ff_ths) { + if (ret != 0) { return ret; } + + switch (free_fall.ff_ths) + { case LSM6DSV16X_156_mg: *val = LSM6DSV16X_156_mg; break; @@ -5438,6 +6503,7 @@ int32_t lsm6dsv16x_ff_thresholds_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ff_thresh *val = LSM6DSV16X_156_mg; break; } + return ret; } @@ -5458,36 +6524,45 @@ int32_t lsm6dsv16x_ff_thresholds_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ff_thresh * @brief It enables Machine Learning Core feature (MLC). When the Machine Learning Core is enabled the Finite State Machine (FSM) programs are executed before executing the MLC algorithms.[set] * * @param ctx read / write interface definitions - * @param val DISABLE, MLC_BEFORE_FSM, MLC_AFTER_FSM, + * @param val MLC_OFF, MLC_ON, MLC_BEFORE_FSM, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_mlc_mode_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_mlc_mode_t val) +int32_t lsm6dsv16x_mlc_set(stmdev_ctx_t *ctx, lsm6dsv16x_mlc_mode_t val) { - lsm6dsv16x_emb_func_en_b_t emb_func_en_b; - lsm6dsv16x_emb_func_en_a_t emb_func_en_a; + lsm6dsv16x_emb_func_en_b_t emb_en_b; + lsm6dsv16x_emb_func_en_a_t emb_en_a; int32_t ret; ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK); - if (ret == 0) { - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_EMB_FUNC_EN_A, (uint8_t *)&emb_func_en_a, 1); - } - if (ret == 0) { - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_EMB_FUNC_EN_B, (uint8_t *)&emb_func_en_b, 1); - } - emb_func_en_a.mlc_before_fsm_en = (uint8_t)val & 0x01U; - emb_func_en_b.mlc_en = ((uint8_t)val & 0x02U) >> 1; - if (ret == 0) { - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_EMB_FUNC_EN_A, (uint8_t *)&emb_func_en_a, 1); - } - if (ret == 0) { + ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_EMB_FUNC_EN_A, (uint8_t *)&emb_en_a, 1); + ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_EMB_FUNC_EN_B, (uint8_t *)&emb_en_b, 1); + if (ret != 0) { goto exit; } - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_EMB_FUNC_EN_B, (uint8_t *)&emb_func_en_b, 1); - } - if (ret == 0) { - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + switch(val) + { + case LSM6DSV16X_MLC_OFF: + emb_en_a.mlc_before_fsm_en = 0; + emb_en_b.mlc_en = 0; + break; + case LSM6DSV16X_MLC_ON: + emb_en_a.mlc_before_fsm_en = 0; + emb_en_b.mlc_en = 1; + break; + case LSM6DSV16X_MLC_ON_BEFORE_FSM: + emb_en_a.mlc_before_fsm_en = 1; + emb_en_b.mlc_en = 0; + break; + default: + break; } + ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_EMB_FUNC_EN_A, (uint8_t *)&emb_en_a, 1); + ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_EMB_FUNC_EN_B, (uint8_t *)&emb_en_b, 1); + +exit: + ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + return ret; } @@ -5495,44 +6570,41 @@ int32_t lsm6dsv16x_mlc_mode_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_mlc_mode_t val * @brief It enables Machine Learning Core feature (MLC). When the Machine Learning Core is enabled the Finite State Machine (FSM) programs are executed before executing the MLC algorithms.[get] * * @param ctx read / write interface definitions - * @param val DISABLE, MLC_BEFORE_FSM, MLC_AFTER_FSM, + * @param val MLC_OFF, MLC_ON, MLC_BEFORE_FSM, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_mlc_mode_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_mlc_mode_t *val) +int32_t lsm6dsv16x_mlc_get(stmdev_ctx_t *ctx, lsm6dsv16x_mlc_mode_t *val) { - lsm6dsv16x_emb_func_en_a_t emb_func_en_a; - lsm6dsv16x_emb_func_en_b_t emb_func_en_b; + lsm6dsv16x_emb_func_en_b_t emb_en_b; + lsm6dsv16x_emb_func_en_a_t emb_en_a; int32_t ret; ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK); - if (ret == 0) { - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_EMB_FUNC_EN_A, (uint8_t *)&emb_func_en_a, 1); + ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_EMB_FUNC_EN_A, (uint8_t *)&emb_en_a, 1); + ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_EMB_FUNC_EN_B, (uint8_t *)&emb_en_b, 1); + if (ret != 0) { goto exit; } + + if (emb_en_a.mlc_before_fsm_en == 0U && emb_en_b.mlc_en == 0U) + { + *val = LSM6DSV16X_MLC_OFF; } - if (ret == 0) { - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_EMB_FUNC_EN_B, (uint8_t *)&emb_func_en_b, 1); + else if (emb_en_a.mlc_before_fsm_en == 0U && emb_en_b.mlc_en == 1U) + { + *val = LSM6DSV16X_MLC_ON; } - if (ret == 0) { - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + else if (emb_en_a.mlc_before_fsm_en == 1U) + { + *val = LSM6DSV16X_MLC_ON_BEFORE_FSM; + } + else + { + /* Do nothing */ } - switch ((emb_func_en_b.mlc_en << 1) + emb_func_en_a.mlc_before_fsm_en) { - case LSM6DSV16X_DISABLE: - *val = LSM6DSV16X_DISABLE; - break; - - case LSM6DSV16X_MLC_BEFORE_FSM: - *val = LSM6DSV16X_MLC_BEFORE_FSM; - break; - - case LSM6DSV16X_MLC_AFTER_FSM: - *val = LSM6DSV16X_MLC_AFTER_FSM; - break; +exit: + ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); - default: - *val = LSM6DSV16X_DISABLE; - break; - } return ret; } @@ -5544,23 +6616,21 @@ int32_t lsm6dsv16x_mlc_mode_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_mlc_mode_t *va * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_mlc_data_rate_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_mlc_data_rate_t val) +int32_t lsm6dsv16x_mlc_data_rate_set(stmdev_ctx_t *ctx, + lsm6dsv16x_mlc_data_rate_t val) { lsm6dsv16x_mlc_odr_t mlc_odr; int32_t ret; ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK); - if (ret == 0) { - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_MLC_ODR, (uint8_t *)&mlc_odr, 1); - } + ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_MLC_ODR, (uint8_t *)&mlc_odr, 1); + if (ret != 0) { goto exit; } - if (ret == 0) { - mlc_odr.mlc_odr = (uint8_t)val & 0x07U; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_MLC_ODR, (uint8_t *)&mlc_odr, 1); - } - if (ret == 0) { - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); - } + mlc_odr.mlc_odr = (uint8_t)val & 0x07U; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_MLC_ODR, (uint8_t *)&mlc_odr, 1); + +exit: + ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); return ret; } @@ -5573,20 +6643,19 @@ int32_t lsm6dsv16x_mlc_data_rate_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_mlc_data_ * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_mlc_data_rate_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_mlc_data_rate_t *val) +int32_t lsm6dsv16x_mlc_data_rate_get(stmdev_ctx_t *ctx, + lsm6dsv16x_mlc_data_rate_t *val) { lsm6dsv16x_mlc_odr_t mlc_odr; int32_t ret; ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK); - if (ret == 0) { - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_MLC_ODR, (uint8_t *)&mlc_odr, 1); - } - if (ret == 0) { - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); - } + ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_MLC_ODR, (uint8_t *)&mlc_odr, 1); + ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + if (ret != 0) { return ret; } - switch (mlc_odr.mlc_odr) { + switch (mlc_odr.mlc_odr) + { case LSM6DSV16X_MLC_15Hz: *val = LSM6DSV16X_MLC_15Hz; break; @@ -5619,6 +6688,7 @@ int32_t lsm6dsv16x_mlc_data_rate_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_mlc_data_ *val = LSM6DSV16X_MLC_15Hz; break; } + return ret; } @@ -5630,17 +6700,17 @@ int32_t lsm6dsv16x_mlc_data_rate_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_mlc_data_ * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_mlc_out_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_mlc_out_t *val) +int32_t lsm6dsv16x_mlc_out_get(stmdev_ctx_t *ctx, lsm6dsv16x_mlc_out_t *val) { int32_t ret; ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK); - if (ret == 0) { + if (ret == 0) + { ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_MLC1_SRC, (uint8_t *)val, 4); } - if (ret == 0) { - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); - } + ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + return ret; } @@ -5652,7 +6722,7 @@ int32_t lsm6dsv16x_mlc_out_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_mlc_out_t *val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_mlc_ext_sens_sensitivity_set(lsm6dsv16x_ctx_t *ctx, uint16_t val) +int32_t lsm6dsv16x_mlc_ext_sens_sensitivity_set(stmdev_ctx_t *ctx, uint16_t val) { uint8_t buff[2]; int32_t ret; @@ -5673,12 +6743,15 @@ int32_t lsm6dsv16x_mlc_ext_sens_sensitivity_set(lsm6dsv16x_ctx_t *ctx, uint16_t * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_mlc_ext_sens_sensitivity_get(lsm6dsv16x_ctx_t *ctx, uint16_t *val) +int32_t lsm6dsv16x_mlc_ext_sens_sensitivity_get(stmdev_ctx_t *ctx, + uint16_t *val) { uint8_t buff[2]; int32_t ret; ret = lsm6dsv16x_ln_pg_read(ctx, LSM6DSV16X_EMB_ADV_PG_1 + LSM6DSV16X_MLC_EXT_SENSITIVITY_L, &buff[0], 2); + if (ret != 0) { return ret; } + *val = buff[1]; *val = (*val * 256U) + buff[0]; @@ -5706,13 +6779,15 @@ int32_t lsm6dsv16x_mlc_ext_sens_sensitivity_get(lsm6dsv16x_ctx_t *ctx, uint16_t * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_ois_ctrl_mode_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ois_ctrl_mode_t val) +int32_t lsm6dsv16x_ois_ctrl_mode_set(stmdev_ctx_t *ctx, + lsm6dsv16x_ois_ctrl_mode_t val) { lsm6dsv16x_func_cfg_access_t func_cfg_access; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FUNC_CFG_ACCESS, (uint8_t *)&func_cfg_access, 1); - if (ret == 0) { + if (ret == 0) + { func_cfg_access.ois_ctrl_from_ui = (uint8_t)val & 0x1U; ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_FUNC_CFG_ACCESS, (uint8_t *)&func_cfg_access, 1); } @@ -5728,13 +6803,17 @@ int32_t lsm6dsv16x_ois_ctrl_mode_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ois_ctrl_ * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_ois_ctrl_mode_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ois_ctrl_mode_t *val) +int32_t lsm6dsv16x_ois_ctrl_mode_get(stmdev_ctx_t *ctx, + lsm6dsv16x_ois_ctrl_mode_t *val) { lsm6dsv16x_func_cfg_access_t func_cfg_access; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FUNC_CFG_ACCESS, (uint8_t *)&func_cfg_access, 1); - switch (func_cfg_access.ois_ctrl_from_ui) { + if (ret != 0) { return ret; } + + switch (func_cfg_access.ois_ctrl_from_ui) + { case LSM6DSV16X_OIS_CTRL_FROM_OIS: *val = LSM6DSV16X_OIS_CTRL_FROM_OIS; break; @@ -5747,6 +6826,7 @@ int32_t lsm6dsv16x_ois_ctrl_mode_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ois_ctrl_ *val = LSM6DSV16X_OIS_CTRL_FROM_OIS; break; } + return ret; } @@ -5758,13 +6838,14 @@ int32_t lsm6dsv16x_ois_ctrl_mode_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ois_ctrl_ * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_ois_reset_set(lsm6dsv16x_ctx_t *ctx, int8_t val) +int32_t lsm6dsv16x_ois_reset_set(stmdev_ctx_t *ctx, int8_t val) { lsm6dsv16x_func_cfg_access_t func_cfg_access; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FUNC_CFG_ACCESS, (uint8_t *)&func_cfg_access, 1); - if (ret == 0) { + if (ret == 0) + { func_cfg_access.spi2_reset = (uint8_t)val; ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_FUNC_CFG_ACCESS, (uint8_t *)&func_cfg_access, 1); } @@ -5780,7 +6861,7 @@ int32_t lsm6dsv16x_ois_reset_set(lsm6dsv16x_ctx_t *ctx, int8_t val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_ois_reset_get(lsm6dsv16x_ctx_t *ctx, int8_t *val) +int32_t lsm6dsv16x_ois_reset_get(stmdev_ctx_t *ctx, int8_t *val) { lsm6dsv16x_func_cfg_access_t func_cfg_access; int32_t ret; @@ -5799,13 +6880,14 @@ int32_t lsm6dsv16x_ois_reset_get(lsm6dsv16x_ctx_t *ctx, int8_t *val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_ois_interface_pull_up_set(lsm6dsv16x_ctx_t *ctx, uint8_t val) +int32_t lsm6dsv16x_ois_interface_pull_up_set(stmdev_ctx_t *ctx, uint8_t val) { lsm6dsv16x_pin_ctrl_t pin_ctrl; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_PIN_CTRL, (uint8_t *)&pin_ctrl, 1); - if (ret == 0) { + if (ret == 0) + { pin_ctrl.ois_pu_dis = val; ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_PIN_CTRL, (uint8_t *)&pin_ctrl, 1); } @@ -5821,7 +6903,7 @@ int32_t lsm6dsv16x_ois_interface_pull_up_set(lsm6dsv16x_ctx_t *ctx, uint8_t val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_ois_interface_pull_up_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val) +int32_t lsm6dsv16x_ois_interface_pull_up_get(stmdev_ctx_t *ctx, uint8_t *val) { lsm6dsv16x_pin_ctrl_t pin_ctrl; int32_t ret; @@ -5840,13 +6922,15 @@ int32_t lsm6dsv16x_ois_interface_pull_up_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_ois_handshake_from_ui_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ois_handshake_t val) +int32_t lsm6dsv16x_ois_handshake_from_ui_set(stmdev_ctx_t *ctx, + lsm6dsv16x_ois_handshake_t val) { lsm6dsv16x_ui_handshake_ctrl_t ui_handshake_ctrl; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_UI_HANDSHAKE_CTRL, (uint8_t *)&ui_handshake_ctrl, 1); - if (ret == 0) { + if (ret == 0) + { ui_handshake_ctrl.ui_shared_ack = val.ack; ui_handshake_ctrl.ui_shared_req = val.req; ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_UI_HANDSHAKE_CTRL, (uint8_t *)&ui_handshake_ctrl, 1); @@ -5863,12 +6947,15 @@ int32_t lsm6dsv16x_ois_handshake_from_ui_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_o * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_ois_handshake_from_ui_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ois_handshake_t *val) +int32_t lsm6dsv16x_ois_handshake_from_ui_get(stmdev_ctx_t *ctx, + lsm6dsv16x_ois_handshake_t *val) { lsm6dsv16x_ui_handshake_ctrl_t ui_handshake_ctrl; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_UI_HANDSHAKE_CTRL, (uint8_t *)&ui_handshake_ctrl, 1); + if (ret != 0) { return ret; } + val->ack = ui_handshake_ctrl.ui_shared_ack; val->req = ui_handshake_ctrl.ui_shared_req; @@ -5883,13 +6970,15 @@ int32_t lsm6dsv16x_ois_handshake_from_ui_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_o * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_ois_handshake_from_ois_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ois_handshake_t val) +int32_t lsm6dsv16x_ois_handshake_from_ois_set(stmdev_ctx_t *ctx, + lsm6dsv16x_ois_handshake_t val) { lsm6dsv16x_spi2_handshake_ctrl_t spi2_handshake_ctrl; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_SPI2_HANDSHAKE_CTRL, (uint8_t *)&spi2_handshake_ctrl, 1); - if (ret == 0) { + if (ret == 0) + { spi2_handshake_ctrl.spi2_shared_ack = val.ack; spi2_handshake_ctrl.spi2_shared_req = val.req; ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_SPI2_HANDSHAKE_CTRL, (uint8_t *)&spi2_handshake_ctrl, 1); @@ -5906,12 +6995,15 @@ int32_t lsm6dsv16x_ois_handshake_from_ois_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_ois_handshake_from_ois_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ois_handshake_t *val) +int32_t lsm6dsv16x_ois_handshake_from_ois_get(stmdev_ctx_t *ctx, + lsm6dsv16x_ois_handshake_t *val) { lsm6dsv16x_spi2_handshake_ctrl_t spi2_handshake_ctrl; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_SPI2_HANDSHAKE_CTRL, (uint8_t *)&spi2_handshake_ctrl, 1); + if (ret != 0) { return ret; } + val->ack = spi2_handshake_ctrl.spi2_shared_ack; val->req = spi2_handshake_ctrl.spi2_shared_req; @@ -5926,7 +7018,7 @@ int32_t lsm6dsv16x_ois_handshake_from_ois_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_ois_shared_set(lsm6dsv16x_ctx_t *ctx, uint8_t val[6]) +int32_t lsm6dsv16x_ois_shared_set(stmdev_ctx_t *ctx, uint8_t val[6]) { int32_t ret; @@ -5943,7 +7035,7 @@ int32_t lsm6dsv16x_ois_shared_set(lsm6dsv16x_ctx_t *ctx, uint8_t val[6]) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_ois_shared_get(lsm6dsv16x_ctx_t *ctx, uint8_t val[6]) +int32_t lsm6dsv16x_ois_shared_get(stmdev_ctx_t *ctx, uint8_t val[6]) { int32_t ret; @@ -5960,13 +7052,14 @@ int32_t lsm6dsv16x_ois_shared_get(lsm6dsv16x_ctx_t *ctx, uint8_t val[6]) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_ois_on_spi2_set(lsm6dsv16x_ctx_t *ctx, uint8_t val) +int32_t lsm6dsv16x_ois_on_spi2_set(stmdev_ctx_t *ctx, uint8_t val) { lsm6dsv16x_ui_ctrl1_ois_t ui_ctrl1_ois; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_UI_CTRL1_OIS, (uint8_t *)&ui_ctrl1_ois, 1); - if (ret == 0) { + if (ret == 0) + { ui_ctrl1_ois.spi2_read_en = val; ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_UI_CTRL1_OIS, (uint8_t *)&ui_ctrl1_ois, 1); } @@ -5982,7 +7075,7 @@ int32_t lsm6dsv16x_ois_on_spi2_set(lsm6dsv16x_ctx_t *ctx, uint8_t val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_ois_on_spi2_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val) +int32_t lsm6dsv16x_ois_on_spi2_get(stmdev_ctx_t *ctx, uint8_t *val) { lsm6dsv16x_ui_ctrl1_ois_t ui_ctrl1_ois; int32_t ret; @@ -6001,13 +7094,14 @@ int32_t lsm6dsv16x_ois_on_spi2_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_ois_chain_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ois_chain_t val) +int32_t lsm6dsv16x_ois_chain_set(stmdev_ctx_t *ctx, lsm6dsv16x_ois_chain_t val) { lsm6dsv16x_ui_ctrl1_ois_t ui_ctrl1_ois; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_UI_CTRL1_OIS, (uint8_t *)&ui_ctrl1_ois, 1); - if (ret == 0) { + if (ret == 0) + { ui_ctrl1_ois.ois_g_en = val.gy; ui_ctrl1_ois.ois_xl_en = val.xl; ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_UI_CTRL1_OIS, (uint8_t *)&ui_ctrl1_ois, 1); @@ -6024,12 +7118,14 @@ int32_t lsm6dsv16x_ois_chain_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ois_chain_t v * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_ois_chain_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ois_chain_t *val) +int32_t lsm6dsv16x_ois_chain_get(stmdev_ctx_t *ctx, lsm6dsv16x_ois_chain_t *val) { lsm6dsv16x_ui_ctrl1_ois_t ui_ctrl1_ois; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_UI_CTRL1_OIS, (uint8_t *)&ui_ctrl1_ois, 1); + if (ret != 0) { return ret; } + val->gy = ui_ctrl1_ois.ois_g_en; val->xl = ui_ctrl1_ois.ois_xl_en; @@ -6044,13 +7140,15 @@ int32_t lsm6dsv16x_ois_chain_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ois_chain_t * * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_ois_gy_full_scale_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ois_gy_full_scale_t val) +int32_t lsm6dsv16x_ois_gy_full_scale_set(stmdev_ctx_t *ctx, + lsm6dsv16x_ois_gy_full_scale_t val) { lsm6dsv16x_ui_ctrl2_ois_t ui_ctrl2_ois; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_UI_CTRL2_OIS, (uint8_t *)&ui_ctrl2_ois, 1); - if (ret == 0) { + if (ret == 0) + { ui_ctrl2_ois.fs_g_ois = (uint8_t)val & 0x03U; ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_UI_CTRL2_OIS, (uint8_t *)&ui_ctrl2_ois, 1); } @@ -6066,13 +7164,17 @@ int32_t lsm6dsv16x_ois_gy_full_scale_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ois_g * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_ois_gy_full_scale_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ois_gy_full_scale_t *val) +int32_t lsm6dsv16x_ois_gy_full_scale_get(stmdev_ctx_t *ctx, + lsm6dsv16x_ois_gy_full_scale_t *val) { lsm6dsv16x_ui_ctrl2_ois_t ui_ctrl2_ois; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_UI_CTRL2_OIS, (uint8_t *)&ui_ctrl2_ois, 1); - switch (ui_ctrl2_ois.fs_g_ois) { + if (ret != 0) { return ret; } + + switch (ui_ctrl2_ois.fs_g_ois) + { case LSM6DSV16X_OIS_125dps: *val = LSM6DSV16X_OIS_125dps; break; @@ -6097,6 +7199,7 @@ int32_t lsm6dsv16x_ois_gy_full_scale_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ois_g *val = LSM6DSV16X_OIS_125dps; break; } + return ret; } @@ -6108,13 +7211,15 @@ int32_t lsm6dsv16x_ois_gy_full_scale_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ois_g * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_ois_xl_full_scale_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ois_xl_full_scale_t val) +int32_t lsm6dsv16x_ois_xl_full_scale_set(stmdev_ctx_t *ctx, + lsm6dsv16x_ois_xl_full_scale_t val) { lsm6dsv16x_ui_ctrl3_ois_t ui_ctrl3_ois; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_UI_CTRL3_OIS, (uint8_t *)&ui_ctrl3_ois, 1); - if (ret == 0) { + if (ret == 0) + { ui_ctrl3_ois.fs_xl_ois = (uint8_t)val & 0x3U; ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_UI_CTRL3_OIS, (uint8_t *)&ui_ctrl3_ois, 1); } @@ -6130,13 +7235,17 @@ int32_t lsm6dsv16x_ois_xl_full_scale_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ois_x * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_ois_xl_full_scale_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ois_xl_full_scale_t *val) +int32_t lsm6dsv16x_ois_xl_full_scale_get(stmdev_ctx_t *ctx, + lsm6dsv16x_ois_xl_full_scale_t *val) { lsm6dsv16x_ui_ctrl3_ois_t ui_ctrl3_ois; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_UI_CTRL3_OIS, (uint8_t *)&ui_ctrl3_ois, 1); - switch (ui_ctrl3_ois.fs_xl_ois) { + if (ret != 0) { return ret; } + + switch (ui_ctrl3_ois.fs_xl_ois) + { case LSM6DSV16X_OIS_2g: *val = LSM6DSV16X_OIS_2g; break; @@ -6157,6 +7266,7 @@ int32_t lsm6dsv16x_ois_xl_full_scale_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ois_x *val = LSM6DSV16X_OIS_2g; break; } + return ret; } @@ -6181,13 +7291,15 @@ int32_t lsm6dsv16x_ois_xl_full_scale_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ois_x * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_6d_threshold_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_6d_threshold_t val) +int32_t lsm6dsv16x_6d_threshold_set(stmdev_ctx_t *ctx, + lsm6dsv16x_6d_threshold_t val) { lsm6dsv16x_tap_ths_6d_t tap_ths_6d; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_TAP_THS_6D, (uint8_t *)&tap_ths_6d, 1); - if (ret == 0) { + if (ret == 0) + { tap_ths_6d.sixd_ths = (uint8_t)val & 0x03U; ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_TAP_THS_6D, (uint8_t *)&tap_ths_6d, 1); } @@ -6203,13 +7315,17 @@ int32_t lsm6dsv16x_6d_threshold_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_6d_thresho * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_6d_threshold_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_6d_threshold_t *val) +int32_t lsm6dsv16x_6d_threshold_get(stmdev_ctx_t *ctx, + lsm6dsv16x_6d_threshold_t *val) { lsm6dsv16x_tap_ths_6d_t tap_ths_6d; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_TAP_THS_6D, (uint8_t *)&tap_ths_6d, 1); - switch (tap_ths_6d.sixd_ths) { + if (ret != 0) { return ret; } + + switch (tap_ths_6d.sixd_ths) + { case LSM6DSV16X_DEG_80: *val = LSM6DSV16X_DEG_80; break; @@ -6230,6 +7346,7 @@ int32_t lsm6dsv16x_6d_threshold_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_6d_thresho *val = LSM6DSV16X_DEG_80; break; } + return ret; } @@ -6241,13 +7358,14 @@ int32_t lsm6dsv16x_6d_threshold_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_6d_thresho * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_4d_mode_set(lsm6dsv16x_ctx_t *ctx, uint8_t val) +int32_t lsm6dsv16x_4d_mode_set(stmdev_ctx_t *ctx, uint8_t val) { lsm6dsv16x_tap_ths_6d_t tap_ths_6d; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_TAP_THS_6D, (uint8_t *)&tap_ths_6d, 1); - if (ret == 0) { + if (ret == 0) + { tap_ths_6d.d4d_en = val; ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_TAP_THS_6D, (uint8_t *)&tap_ths_6d, 1); } @@ -6263,7 +7381,7 @@ int32_t lsm6dsv16x_4d_mode_set(lsm6dsv16x_ctx_t *ctx, uint8_t val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_4d_mode_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val) +int32_t lsm6dsv16x_4d_mode_get(stmdev_ctx_t *ctx, uint8_t *val) { lsm6dsv16x_tap_ths_6d_t tap_ths_6d; int32_t ret; @@ -6295,13 +7413,15 @@ int32_t lsm6dsv16x_4d_mode_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_ah_qvar_zin_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ah_qvar_zin_t val) +int32_t lsm6dsv16x_ah_qvar_zin_set(stmdev_ctx_t *ctx, + lsm6dsv16x_ah_qvar_zin_t val) { lsm6dsv16x_ctrl7_t ctrl7; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL7, (uint8_t *)&ctrl7, 1); - if (ret == 0) { + if (ret == 0) + { ctrl7.ah_qvar_c_zin = (uint8_t)val & 0x03U; ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_CTRL7, (uint8_t *)&ctrl7, 1); } @@ -6317,13 +7437,17 @@ int32_t lsm6dsv16x_ah_qvar_zin_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ah_qvar_zin * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_ah_qvar_zin_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ah_qvar_zin_t *val) +int32_t lsm6dsv16x_ah_qvar_zin_get(stmdev_ctx_t *ctx, + lsm6dsv16x_ah_qvar_zin_t *val) { lsm6dsv16x_ctrl7_t ctrl7; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL7, (uint8_t *)&ctrl7, 1); - switch (ctrl7.ah_qvar_c_zin) { + if (ret != 0) { return ret; } + + switch (ctrl7.ah_qvar_c_zin) + { case LSM6DSV16X_2400MOhm: *val = LSM6DSV16X_2400MOhm; break; @@ -6344,6 +7468,7 @@ int32_t lsm6dsv16x_ah_qvar_zin_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ah_qvar_zin *val = LSM6DSV16X_2400MOhm; break; } + return ret; } @@ -6355,13 +7480,15 @@ int32_t lsm6dsv16x_ah_qvar_zin_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ah_qvar_zin * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_ah_qvar_mode_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ah_qvar_mode_t val) +int32_t lsm6dsv16x_ah_qvar_mode_set(stmdev_ctx_t *ctx, + lsm6dsv16x_ah_qvar_mode_t val) { lsm6dsv16x_ctrl7_t ctrl7; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL7, (uint8_t *)&ctrl7, 1); - if (ret == 0) { + if (ret == 0) + { ctrl7.ah_qvar_en = val.ah_qvar_en; ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_CTRL7, (uint8_t *)&ctrl7, 1); } @@ -6377,7 +7504,8 @@ int32_t lsm6dsv16x_ah_qvar_mode_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ah_qvar_mo * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_ah_qvar_mode_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ah_qvar_mode_t *val) +int32_t lsm6dsv16x_ah_qvar_mode_get(stmdev_ctx_t *ctx, + lsm6dsv16x_ah_qvar_mode_t *val) { lsm6dsv16x_ctrl7_t ctrl7; int32_t ret; @@ -6409,13 +7537,15 @@ int32_t lsm6dsv16x_ah_qvar_mode_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ah_qvar_mo * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_i3c_reset_mode_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_i3c_reset_mode_t val) +int32_t lsm6dsv16x_i3c_reset_mode_set(stmdev_ctx_t *ctx, + lsm6dsv16x_i3c_reset_mode_t val) { lsm6dsv16x_pin_ctrl_t pin_ctrl; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_PIN_CTRL, (uint8_t *)&pin_ctrl, 1); - if (ret == 0) { + if (ret == 0) + { pin_ctrl.ibhr_por_en = (uint8_t)val & 0x01U; ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_PIN_CTRL, (uint8_t *)&pin_ctrl, 1); } @@ -6431,13 +7561,17 @@ int32_t lsm6dsv16x_i3c_reset_mode_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_i3c_rese * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_i3c_reset_mode_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_i3c_reset_mode_t *val) +int32_t lsm6dsv16x_i3c_reset_mode_get(stmdev_ctx_t *ctx, + lsm6dsv16x_i3c_reset_mode_t *val) { lsm6dsv16x_pin_ctrl_t pin_ctrl; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_PIN_CTRL, (uint8_t *)&pin_ctrl, 1); - switch (pin_ctrl.ibhr_por_en) { + if (ret != 0) { return ret; } + + switch (pin_ctrl.ibhr_por_en) + { case LSM6DSV16X_SW_RST_DYN_ADDRESS_RST: *val = LSM6DSV16X_SW_RST_DYN_ADDRESS_RST; break; @@ -6450,6 +7584,7 @@ int32_t lsm6dsv16x_i3c_reset_mode_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_i3c_rese *val = LSM6DSV16X_SW_RST_DYN_ADDRESS_RST; break; } + return ret; } @@ -6461,13 +7596,15 @@ int32_t lsm6dsv16x_i3c_reset_mode_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_i3c_rese * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_i3c_ibi_time_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_i3c_ibi_time_t val) +int32_t lsm6dsv16x_i3c_ibi_time_set(stmdev_ctx_t *ctx, + lsm6dsv16x_i3c_ibi_time_t val) { lsm6dsv16x_ctrl5_t ctrl5; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL5, (uint8_t *)&ctrl5, 1); - if (ret == 0) { + if (ret == 0) + { ctrl5.bus_act_sel = (uint8_t)val & 0x03U; ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_CTRL5, (uint8_t *)&ctrl5, 1); } @@ -6483,13 +7620,17 @@ int32_t lsm6dsv16x_i3c_ibi_time_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_i3c_ibi_ti * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_i3c_ibi_time_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_i3c_ibi_time_t *val) +int32_t lsm6dsv16x_i3c_ibi_time_get(stmdev_ctx_t *ctx, + lsm6dsv16x_i3c_ibi_time_t *val) { lsm6dsv16x_ctrl5_t ctrl5; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL5, (uint8_t *)&ctrl5, 1); - switch (ctrl5.bus_act_sel) { + if (ret != 0) { return ret; } + + switch (ctrl5.bus_act_sel) + { case LSM6DSV16X_IBI_2us: *val = LSM6DSV16X_IBI_2us; break; @@ -6510,6 +7651,7 @@ int32_t lsm6dsv16x_i3c_ibi_time_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_i3c_ibi_ti *val = LSM6DSV16X_IBI_2us; break; } + return ret; } @@ -6534,13 +7676,15 @@ int32_t lsm6dsv16x_i3c_ibi_time_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_i3c_ibi_ti * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_sh_master_interface_pull_up_set(lsm6dsv16x_ctx_t *ctx, uint8_t val) +int32_t lsm6dsv16x_sh_master_interface_pull_up_set(stmdev_ctx_t *ctx, + uint8_t val) { lsm6dsv16x_if_cfg_t if_cfg; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_IF_CFG, (uint8_t *)&if_cfg, 1); - if (ret == 0) { + if (ret == 0) + { if_cfg.shub_pu_en = val; ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_IF_CFG, (uint8_t *)&if_cfg, 1); } @@ -6556,7 +7700,8 @@ int32_t lsm6dsv16x_sh_master_interface_pull_up_set(lsm6dsv16x_ctx_t *ctx, uint8_ * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_sh_master_interface_pull_up_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val) +int32_t lsm6dsv16x_sh_master_interface_pull_up_get(stmdev_ctx_t *ctx, + uint8_t *val) { lsm6dsv16x_if_cfg_t if_cfg; int32_t ret; @@ -6575,21 +7720,14 @@ int32_t lsm6dsv16x_sh_master_interface_pull_up_get(lsm6dsv16x_ctx_t *ctx, uint8_ * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_sh_read_data_raw_get(lsm6dsv16x_ctx_t *ctx, - lsm6dsv16x_emb_sh_read_t *val, +int32_t lsm6dsv16x_sh_read_data_raw_get(stmdev_ctx_t *ctx, uint8_t *val, uint8_t len) { int32_t ret; ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_SENSOR_HUB_MEM_BANK); - if (ret == 0) { - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_SENSOR_HUB_1, (uint8_t *) val, - len); - } - if (ret == 0) { - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); - } - + ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_SENSOR_HUB_1, val, len); + ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); return ret; } @@ -6602,23 +7740,21 @@ int32_t lsm6dsv16x_sh_read_data_raw_get(lsm6dsv16x_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_sh_slave_connected_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_sh_slave_connected_t val) +int32_t lsm6dsv16x_sh_slave_connected_set(stmdev_ctx_t *ctx, + lsm6dsv16x_sh_slave_connected_t val) { lsm6dsv16x_master_config_t master_config; int32_t ret; ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_SENSOR_HUB_MEM_BANK); - if (ret == 0) { - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_MASTER_CONFIG, (uint8_t *)&master_config, 1); - } + ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_MASTER_CONFIG, (uint8_t *)&master_config, 1); + if (ret != 0) { goto exit; } - if (ret == 0) { - master_config.aux_sens_on = (uint8_t)val & 0x3U; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_MASTER_CONFIG, (uint8_t *)&master_config, 1); - } - if (ret == 0) { - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); - } + master_config.aux_sens_on = (uint8_t)val & 0x3U; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_MASTER_CONFIG, (uint8_t *)&master_config, 1); + +exit: + ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); return ret; } @@ -6631,20 +7767,19 @@ int32_t lsm6dsv16x_sh_slave_connected_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_sh_s * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_sh_slave_connected_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_sh_slave_connected_t *val) +int32_t lsm6dsv16x_sh_slave_connected_get(stmdev_ctx_t *ctx, + lsm6dsv16x_sh_slave_connected_t *val) { lsm6dsv16x_master_config_t master_config; int32_t ret; ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_SENSOR_HUB_MEM_BANK); - if (ret == 0) { - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_MASTER_CONFIG, (uint8_t *)&master_config, 1); - } - if (ret == 0) { - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); - } + ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_MASTER_CONFIG, (uint8_t *)&master_config, 1); + ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + if (ret != 0) { return ret; } - switch (master_config.aux_sens_on) { + switch (master_config.aux_sens_on) + { case LSM6DSV16X_SLV_0: *val = LSM6DSV16X_SLV_0; break; @@ -6665,6 +7800,7 @@ int32_t lsm6dsv16x_sh_slave_connected_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_sh_s *val = LSM6DSV16X_SLV_0; break; } + return ret; } @@ -6676,23 +7812,20 @@ int32_t lsm6dsv16x_sh_slave_connected_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_sh_s * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_sh_master_set(lsm6dsv16x_ctx_t *ctx, uint8_t val) +int32_t lsm6dsv16x_sh_master_set(stmdev_ctx_t *ctx, uint8_t val) { lsm6dsv16x_master_config_t master_config; int32_t ret; ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_SENSOR_HUB_MEM_BANK); - if (ret == 0) { - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_MASTER_CONFIG, (uint8_t *)&master_config, 1); - } + ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_MASTER_CONFIG, (uint8_t *)&master_config, 1); + if (ret != 0) { goto exit; } - if (ret == 0) { - master_config.master_on = val; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_MASTER_CONFIG, (uint8_t *)&master_config, 1); - } - if (ret == 0) { - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); - } + master_config.master_on = val; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_MASTER_CONFIG, (uint8_t *)&master_config, 1); + +exit: + ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); return ret; } @@ -6705,21 +7838,17 @@ int32_t lsm6dsv16x_sh_master_set(lsm6dsv16x_ctx_t *ctx, uint8_t val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_sh_master_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val) +int32_t lsm6dsv16x_sh_master_get(stmdev_ctx_t *ctx, uint8_t *val) { lsm6dsv16x_master_config_t master_config; int32_t ret; ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_SENSOR_HUB_MEM_BANK); - if (ret == 0) { - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_MASTER_CONFIG, (uint8_t *)&master_config, 1); - } + ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_MASTER_CONFIG, (uint8_t *)&master_config, 1); *val = master_config.master_on; - if (ret == 0) { - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); - } + ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); return ret; } @@ -6732,23 +7861,20 @@ int32_t lsm6dsv16x_sh_master_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_sh_pass_through_set(lsm6dsv16x_ctx_t *ctx, uint8_t val) +int32_t lsm6dsv16x_sh_pass_through_set(stmdev_ctx_t *ctx, uint8_t val) { lsm6dsv16x_master_config_t master_config; int32_t ret; ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_SENSOR_HUB_MEM_BANK); - if (ret == 0) { - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_MASTER_CONFIG, (uint8_t *)&master_config, 1); - } + ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_MASTER_CONFIG, (uint8_t *)&master_config, 1); + if (ret != 0) { goto exit; } - if (ret == 0) { - master_config.pass_through_mode = val; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_MASTER_CONFIG, (uint8_t *)&master_config, 1); - } - if (ret == 0) { - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); - } + master_config.pass_through_mode = val; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_MASTER_CONFIG, (uint8_t *)&master_config, 1); + +exit: + ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); return ret; } @@ -6761,21 +7887,17 @@ int32_t lsm6dsv16x_sh_pass_through_set(lsm6dsv16x_ctx_t *ctx, uint8_t val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_sh_pass_through_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val) +int32_t lsm6dsv16x_sh_pass_through_get(stmdev_ctx_t *ctx, uint8_t *val) { lsm6dsv16x_master_config_t master_config; int32_t ret; ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_SENSOR_HUB_MEM_BANK); - if (ret == 0) { - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_MASTER_CONFIG, (uint8_t *)&master_config, 1); - } + ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_MASTER_CONFIG, (uint8_t *)&master_config, 1); *val = master_config.pass_through_mode; - if (ret == 0) { - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); - } + ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); return ret; } @@ -6788,23 +7910,21 @@ int32_t lsm6dsv16x_sh_pass_through_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_sh_syncro_mode_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_sh_syncro_mode_t val) +int32_t lsm6dsv16x_sh_syncro_mode_set(stmdev_ctx_t *ctx, + lsm6dsv16x_sh_syncro_mode_t val) { lsm6dsv16x_master_config_t master_config; int32_t ret; ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_SENSOR_HUB_MEM_BANK); - if (ret == 0) { - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_MASTER_CONFIG, (uint8_t *)&master_config, 1); - } + ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_MASTER_CONFIG, (uint8_t *)&master_config, 1); + if (ret != 0) { goto exit; } - if (ret == 0) { - master_config.start_config = (uint8_t)val & 0x01U; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_MASTER_CONFIG, (uint8_t *)&master_config, 1); - } - if (ret == 0) { - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); - } + master_config.start_config = (uint8_t)val & 0x01U; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_MASTER_CONFIG, (uint8_t *)&master_config, 1); + +exit: + ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); return ret; } @@ -6817,20 +7937,19 @@ int32_t lsm6dsv16x_sh_syncro_mode_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_sh_syncr * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_sh_syncro_mode_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_sh_syncro_mode_t *val) +int32_t lsm6dsv16x_sh_syncro_mode_get(stmdev_ctx_t *ctx, + lsm6dsv16x_sh_syncro_mode_t *val) { lsm6dsv16x_master_config_t master_config; int32_t ret; ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_SENSOR_HUB_MEM_BANK); - if (ret == 0) { - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_MASTER_CONFIG, (uint8_t *)&master_config, 1); - } - if (ret == 0) { - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); - } + ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_MASTER_CONFIG, (uint8_t *)&master_config, 1); + ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + if (ret != 0) { return ret; } - switch (master_config.start_config) { + switch (master_config.start_config) + { case LSM6DSV16X_SH_TRG_XL_GY_DRDY: *val = LSM6DSV16X_SH_TRG_XL_GY_DRDY; break; @@ -6843,6 +7962,7 @@ int32_t lsm6dsv16x_sh_syncro_mode_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_sh_syncr *val = LSM6DSV16X_SH_TRG_XL_GY_DRDY; break; } + return ret; } @@ -6854,23 +7974,21 @@ int32_t lsm6dsv16x_sh_syncro_mode_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_sh_syncr * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_sh_write_mode_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_sh_write_mode_t val) +int32_t lsm6dsv16x_sh_write_mode_set(stmdev_ctx_t *ctx, + lsm6dsv16x_sh_write_mode_t val) { lsm6dsv16x_master_config_t master_config; int32_t ret; ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_SENSOR_HUB_MEM_BANK); - if (ret == 0) { - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_MASTER_CONFIG, (uint8_t *)&master_config, 1); - } + ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_MASTER_CONFIG, (uint8_t *)&master_config, 1); + if (ret != 0) { goto exit; } - if (ret == 0) { - master_config.write_once = (uint8_t)val & 0x01U; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_MASTER_CONFIG, (uint8_t *)&master_config, 1); - } - if (ret == 0) { - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); - } + master_config.write_once = (uint8_t)val & 0x01U; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_MASTER_CONFIG, (uint8_t *)&master_config, 1); + +exit: + ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); return ret; } @@ -6883,21 +8001,19 @@ int32_t lsm6dsv16x_sh_write_mode_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_sh_write_ * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_sh_write_mode_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_sh_write_mode_t *val) +int32_t lsm6dsv16x_sh_write_mode_get(stmdev_ctx_t *ctx, + lsm6dsv16x_sh_write_mode_t *val) { lsm6dsv16x_master_config_t master_config; int32_t ret; ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_SENSOR_HUB_MEM_BANK); - if (ret == 0) { - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_MASTER_CONFIG, (uint8_t *)&master_config, 1); - } - if (ret == 0) { - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); - } + ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_MASTER_CONFIG, (uint8_t *)&master_config, 1); + ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + if (ret != 0) { return ret; } - - switch (master_config.write_once) { + switch (master_config.write_once) + { case LSM6DSV16X_EACH_SH_CYCLE: *val = LSM6DSV16X_EACH_SH_CYCLE; break; @@ -6910,6 +8026,7 @@ int32_t lsm6dsv16x_sh_write_mode_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_sh_write_ *val = LSM6DSV16X_EACH_SH_CYCLE; break; } + return ret; } @@ -6921,23 +8038,20 @@ int32_t lsm6dsv16x_sh_write_mode_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_sh_write_ * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_sh_reset_set(lsm6dsv16x_ctx_t *ctx, uint8_t val) +int32_t lsm6dsv16x_sh_reset_set(stmdev_ctx_t *ctx, uint8_t val) { lsm6dsv16x_master_config_t master_config; int32_t ret; ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_SENSOR_HUB_MEM_BANK); - if (ret == 0) { - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_MASTER_CONFIG, (uint8_t *)&master_config, 1); - } + ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_MASTER_CONFIG, (uint8_t *)&master_config, 1); + if (ret != 0) { goto exit; } - if (ret == 0) { - master_config.rst_master_regs = val; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_MASTER_CONFIG, (uint8_t *)&master_config, 1); - } - if (ret == 0) { - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); - } + master_config.rst_master_regs = val; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_MASTER_CONFIG, (uint8_t *)&master_config, 1); + +exit: + ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); return ret; } @@ -6950,21 +8064,17 @@ int32_t lsm6dsv16x_sh_reset_set(lsm6dsv16x_ctx_t *ctx, uint8_t val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_sh_reset_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val) +int32_t lsm6dsv16x_sh_reset_get(stmdev_ctx_t *ctx, uint8_t *val) { lsm6dsv16x_master_config_t master_config; int32_t ret; ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_SENSOR_HUB_MEM_BANK); - if (ret == 0) { - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_MASTER_CONFIG, (uint8_t *)&master_config, 1); - } + ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_MASTER_CONFIG, (uint8_t *)&master_config, 1); *val = master_config.rst_master_regs; - if (ret == 0) { - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); - } + ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); return ret; } @@ -6980,32 +8090,29 @@ int32_t lsm6dsv16x_sh_reset_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_sh_cfg_write(lsm6dsv16x_ctx_t *ctx, +int32_t lsm6dsv16x_sh_cfg_write(stmdev_ctx_t *ctx, lsm6dsv16x_sh_cfg_write_t *val) { lsm6dsv16x_slv0_add_t reg; int32_t ret; ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_SENSOR_HUB_MEM_BANK); - if (ret == 0) { - reg.slave0_add = val->slv0_add; - reg.rw_0 = 0; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_SLV0_ADD, (uint8_t *)®, 1); - } + if (ret != 0) { return ret; } - if (ret == 0) { - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_SLV0_SUBADD, - &(val->slv0_subadd), 1); - } + reg.slave0_add = val->slv0_add; + reg.rw_0 = 0; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_SLV0_ADD, (uint8_t *)®, 1); + if (ret != 0) { goto exit; } - if (ret == 0) { - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_DATAWRITE_SLV0, - &(val->slv0_data), 1); - } + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_SLV0_SUBADD, + &(val->slv0_subadd), 1); + if (ret != 0) { goto exit; } - if (ret == 0) { - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); - } + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_DATAWRITE_SLV0, + &(val->slv0_data), 1); + +exit: + ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); return ret; } @@ -7018,23 +8125,21 @@ int32_t lsm6dsv16x_sh_cfg_write(lsm6dsv16x_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_sh_data_rate_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_sh_data_rate_t val) +int32_t lsm6dsv16x_sh_data_rate_set(stmdev_ctx_t *ctx, + lsm6dsv16x_sh_data_rate_t val) { lsm6dsv16x_slv0_config_t slv0_config; int32_t ret; ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_SENSOR_HUB_MEM_BANK); - if (ret == 0) { - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_SLV0_CONFIG, (uint8_t *)&slv0_config, 1); - } + ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_SLV0_CONFIG, (uint8_t *)&slv0_config, 1); + if (ret != 0) { goto exit; } - if (ret == 0) { - slv0_config.shub_odr = (uint8_t)val & 0x07U; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_SLV0_CONFIG, (uint8_t *)&slv0_config, 1); - } - if (ret == 0) { - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); - } + slv0_config.shub_odr = (uint8_t)val & 0x07U; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_SLV0_CONFIG, (uint8_t *)&slv0_config, 1); + +exit: + ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); return ret; } @@ -7047,20 +8152,19 @@ int32_t lsm6dsv16x_sh_data_rate_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_sh_data_ra * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_sh_data_rate_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_sh_data_rate_t *val) +int32_t lsm6dsv16x_sh_data_rate_get(stmdev_ctx_t *ctx, + lsm6dsv16x_sh_data_rate_t *val) { lsm6dsv16x_slv0_config_t slv0_config; int32_t ret; ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_SENSOR_HUB_MEM_BANK); - if (ret == 0) { - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_SLV0_CONFIG, (uint8_t *)&slv0_config, 1); - } - if (ret == 0) { - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); - } + ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_SLV0_CONFIG, (uint8_t *)&slv0_config, 1); + ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + if (ret != 0) { return ret; } - switch (slv0_config.shub_odr) { + switch (slv0_config.shub_odr) + { case LSM6DSV16X_SH_15Hz: *val = LSM6DSV16X_SH_15Hz; break; @@ -7089,402 +8193,75 @@ int32_t lsm6dsv16x_sh_data_rate_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_sh_data_ra *val = LSM6DSV16X_SH_15Hz; break; } + return ret; } /** - * @brief Configure slave 0 for perform a read.[set] + * @brief Configure slave idx for perform a read.[set] * * @param ctx read / write interface definitions * @param val Structure that contain - * - uint8_t slv1_add; 8 bit i2c device address - * - uint8_t slv1_subadd; 8 bit register device address - * - uint8_t slv1_len; num of bit to read + * - uint8_t slv_add; 8 bit i2c device address + * - uint8_t slv_subadd; 8 bit register device address + * - uint8_t slv_len; num of bit to read * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_sh_slv0_cfg_read(lsm6dsv16x_ctx_t *ctx, - lsm6dsv16x_sh_cfg_read_t *val) +int32_t lsm6dsv16x_sh_slv_cfg_read(stmdev_ctx_t *ctx, uint8_t idx, + lsm6dsv16x_sh_cfg_read_t *val) { - lsm6dsv16x_slv0_add_t slv0_add; - lsm6dsv16x_slv0_config_t slv0_config; + lsm6dsv16x_slv0_add_t slv_add; + lsm6dsv16x_slv0_config_t slv_config; int32_t ret; ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_SENSOR_HUB_MEM_BANK); + if (ret != 0) { return ret; } - if (ret == 0) { - slv0_add.slave0_add = val->slv_add; - slv0_add.rw_0 = 1; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_SLV0_ADD, (uint8_t *)&slv0_add, 1); - } + slv_add.slave0_add = val->slv_add; + slv_add.rw_0 = 1; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_SLV0_ADD + idx*3U, + (uint8_t *)&slv_add, 1); + if (ret != 0) { goto exit; } - if (ret == 0) { - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_SLV0_SUBADD, - &(val->slv_subadd), 1); - } + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_SLV0_SUBADD + idx*3U, + &(val->slv_subadd), 1); + if (ret != 0) { goto exit; } - if (ret == 0) { - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_SLV0_CONFIG, - (uint8_t *)&slv0_config, 1); - } + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_SLV0_CONFIG + idx*3U, + (uint8_t *)&slv_config, 1); + if (ret != 0) { goto exit; } - if (ret == 0) { - slv0_config.slave0_numop = val->slv_len; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_SLV0_CONFIG, - (uint8_t *)&slv0_config, 1); - } + slv_config.slave0_numop = val->slv_len; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_SLV0_CONFIG + idx*3U, + (uint8_t *)&slv_config, 1); - if (ret == 0) { - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); - } +exit: + ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); return ret; } /** - * @brief Configure slave 0 for perform a write/read.[set] + * @brief Sensor hub source register.[get] * * @param ctx read / write interface definitions - * @param val Structure that contain - * - uint8_t slv1_add; 8 bit i2c device address - * - uint8_t slv1_subadd; 8 bit register device address - * - uint8_t slv1_len; num of bit to read - * @retval interface status (MANDATORY: return 0 -> no Error) + * @param val union of registers from STATUS_MASTER to + * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_sh_slv1_cfg_read(lsm6dsv16x_ctx_t *ctx, - lsm6dsv16x_sh_cfg_read_t *val) +int32_t lsm6dsv16x_sh_status_get(stmdev_ctx_t *ctx, + lsm6dsv16x_status_master_t *val) { - lsm6dsv16x_slv1_add_t slv1_add; - lsm6dsv16x_slv1_config_t slv1_config; int32_t ret; - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_SENSOR_HUB_MEM_BANK); - - if (ret == 0) { - slv1_add.slave1_add = val->slv_add; - slv1_add.r_1 = 1; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_SLV1_ADD, (uint8_t *)&slv1_add, 1); - } - - if (ret == 0) { - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_SLV1_SUBADD, - &(val->slv_subadd), 1); - } - - if (ret == 0) { - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_SLV1_CONFIG, - (uint8_t *)&slv1_config, 1); - } - - if (ret == 0) { - slv1_config.slave1_numop = val->slv_len; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_SLV1_CONFIG, - (uint8_t *)&slv1_config, 1); - } - - if (ret == 0) { - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); - } + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_STATUS_MASTER_MAINPAGE, (uint8_t *) val, 1); return ret; } /** - * @brief Configure slave 0 for perform a write/read.[set] - * - * @param ctx read / write interface definitions - * @param val Structure that contain - * - uint8_t slv2_add; 8 bit i2c device address - * - uint8_t slv2_subadd; 8 bit register device address - * - uint8_t slv2_len; num of bit to read - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6dsv16x_sh_slv2_cfg_read(lsm6dsv16x_ctx_t *ctx, - lsm6dsv16x_sh_cfg_read_t *val) -{ - lsm6dsv16x_slv2_add_t slv2_add; - lsm6dsv16x_slv2_config_t slv2_config; - int32_t ret; - - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_SENSOR_HUB_MEM_BANK); - - if (ret == 0) { - slv2_add.slave2_add = val->slv_add; - slv2_add.r_2 = 1; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_SLV2_ADD, (uint8_t *)&slv2_add, 1); - } - - if (ret == 0) { - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_SLV2_SUBADD, - &(val->slv_subadd), 1); - } - - if (ret == 0) { - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_SLV2_CONFIG, - (uint8_t *)&slv2_config, 1); - } - - if (ret == 0) { - slv2_config.slave2_numop = val->slv_len; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_SLV2_CONFIG, - (uint8_t *)&slv2_config, 1); - } - - if (ret == 0) { - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); - } - - return ret; -} - -/** - * @brief Configure slave 0 for perform a write/read.[set] - * - * @param ctx read / write interface definitions - * @param val Structure that contain - * - uint8_t slv3_add; 8 bit i2c device address - * - uint8_t slv3_subadd; 8 bit register device address - * - uint8_t slv3_len; num of bit to read - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6dsv16x_sh_slv3_cfg_read(lsm6dsv16x_ctx_t *ctx, - lsm6dsv16x_sh_cfg_read_t *val) -{ - lsm6dsv16x_slv3_add_t slv3_add; - lsm6dsv16x_slv3_config_t slv3_config; - int32_t ret; - - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_SENSOR_HUB_MEM_BANK); - - if (ret == 0) { - slv3_add.slave3_add = val->slv_add; - slv3_add.r_3 = 1; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_SLV3_ADD, (uint8_t *)&slv3_add, 1); - } - - if (ret == 0) { - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_SLV3_SUBADD, - &(val->slv_subadd), 1); - } - - if (ret == 0) { - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_SLV3_CONFIG, - (uint8_t *)&slv3_config, 1); - } - - if (ret == 0) { - slv3_config.slave3_numop = val->slv_len; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_SLV3_CONFIG, - (uint8_t *)&slv3_config, 1); - } - - if (ret == 0) { - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); - } - - return ret; -} - -/** - * @} - * - */ - -/** - * @defgroup Sensors for Smart Mobile Devices (S4S) - * @brief This section groups all the functions that manage the - * Sensors for Smart Mobile Devices. - * @{ - * - */ - -/** - * @brief Sensor synchronization time frame register expressed in number of samples[set] - * - * @param ctx read / write interface definitions - * @param val Sensor synchronization time frame register expressed in number of samples - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6dsv16x_s4s_sync_samples_set(lsm6dsv16x_ctx_t *ctx, uint16_t val) -{ - uint8_t buff[2]; - int32_t ret; - - buff[1] = (uint8_t)(val / 256U); - buff[0] = (uint8_t)(val - (buff[1] * 256U)); - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_S4S_TPH_L, (uint8_t *)&buff[0], 2); - - return ret; -} - -/** - * @brief Sensor synchronization time frame register expressed in number of samples[get] - * - * @param ctx read / write interface definitions - * @param val Sensor synchronization time frame register expressed in number of samples - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6dsv16x_s4s_sync_samples_get(lsm6dsv16x_ctx_t *ctx, uint16_t *val) -{ - uint8_t buff[2]; - int32_t ret; - - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_S4S_TPH_L, &buff[0], 2); - *val = buff[1]; - *val = (*val * 256U) + buff[0]; - - return ret; -} - -/** - * @brief Sensor synchronization resolution ratio.[set] - * - * @param ctx read / write interface definitions - * @param val S4S_DT_RES_11, S4S_DT_RES_12, S4S_DT_RES_13, S4S_DT_RES_14, - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6dsv16x_s4s_res_ratio_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_s4s_res_ratio_t val) -{ - lsm6dsv16x_s4s_rr_t s4s_rr; - int32_t ret; - - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_S4S_RR, (uint8_t *)&s4s_rr, 1); - if (ret == 0) { - s4s_rr.rr = (uint8_t)val & 0x3U; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_S4S_RR, (uint8_t *)&s4s_rr, 1); - } - - return ret; -} - -/** - * @brief Sensor synchronization resolution ratio.[get] - * - * @param ctx read / write interface definitions - * @param val S4S_DT_RES_11, S4S_DT_RES_12, S4S_DT_RES_13, S4S_DT_RES_14, - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6dsv16x_s4s_res_ratio_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_s4s_res_ratio_t *val) -{ - lsm6dsv16x_s4s_rr_t s4s_rr; - int32_t ret; - - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_S4S_RR, (uint8_t *)&s4s_rr, 1); - switch (s4s_rr.rr) { - case LSM6DSV16X_S4S_DT_RES_11: - *val = LSM6DSV16X_S4S_DT_RES_11; - break; - - case LSM6DSV16X_S4S_DT_RES_12: - *val = LSM6DSV16X_S4S_DT_RES_12; - break; - - case LSM6DSV16X_S4S_DT_RES_13: - *val = LSM6DSV16X_S4S_DT_RES_13; - break; - - case LSM6DSV16X_S4S_DT_RES_14: - *val = LSM6DSV16X_S4S_DT_RES_14; - break; - - default: - *val = LSM6DSV16X_S4S_DT_RES_11; - break; - } - return ret; -} - -/** - * @brief Master command code used for S4S.[set] - * - * @param ctx read / write interface definitions - * @param val Master command code used for S4S. - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6dsv16x_s4s_command_set(lsm6dsv16x_ctx_t *ctx, uint8_t val) -{ - lsm6dsv16x_s4s_st_cmd_code_t s4s_st_cmd_code; - int32_t ret; - - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_S4S_ST_CMD_CODE, (uint8_t *)&s4s_st_cmd_code, 1); - if (ret == 0) { - s4s_st_cmd_code.st_cmd_code = val; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_S4S_ST_CMD_CODE, (uint8_t *)&s4s_st_cmd_code, 1); - } - - return ret; -} - -/** - * @brief Master command code used for S4S.[get] - * - * @param ctx read / write interface definitions - * @param val Master command code used for S4S. - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6dsv16x_s4s_command_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val) -{ - lsm6dsv16x_s4s_st_cmd_code_t s4s_st_cmd_code; - int32_t ret; - - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_S4S_ST_CMD_CODE, (uint8_t *)&s4s_st_cmd_code, 1); - *val = s4s_st_cmd_code.st_cmd_code; - - return ret; -} - -/** - * @brief DT used for S4S.[set] - * - * @param ctx read / write interface definitions - * @param val DT used for S4S. - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6dsv16x_s4s_dt_set(lsm6dsv16x_ctx_t *ctx, uint8_t val) -{ - lsm6dsv16x_s4s_dt_reg_t s4s_dt_reg; - int32_t ret; - - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_S4S_DT_REG, (uint8_t *)&s4s_dt_reg, 1); - if (ret == 0) { - s4s_dt_reg.dt = val; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_S4S_DT_REG, (uint8_t *)&s4s_dt_reg, 1); - } - - return ret; -} - -/** - * @brief DT used for S4S.[get] - * - * @param ctx read / write interface definitions - * @param val DT used for S4S. - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6dsv16x_s4s_dt_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val) -{ - lsm6dsv16x_s4s_dt_reg_t s4s_dt_reg; - int32_t ret; - - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_S4S_DT_REG, (uint8_t *)&s4s_dt_reg, 1); - *val = s4s_dt_reg.dt; - - return ret; -} - -/** - * @} + * @} * */ @@ -7504,13 +8281,14 @@ int32_t lsm6dsv16x_s4s_dt_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_ui_sdo_pull_up_set(lsm6dsv16x_ctx_t *ctx, uint8_t val) +int32_t lsm6dsv16x_ui_sdo_pull_up_set(stmdev_ctx_t *ctx, uint8_t val) { lsm6dsv16x_pin_ctrl_t pin_ctrl; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_PIN_CTRL, (uint8_t *)&pin_ctrl, 1); - if (ret == 0) { + if (ret == 0) + { pin_ctrl.sdo_pu_en = val; ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_PIN_CTRL, (uint8_t *)&pin_ctrl, 1); } @@ -7526,7 +8304,7 @@ int32_t lsm6dsv16x_ui_sdo_pull_up_set(lsm6dsv16x_ctx_t *ctx, uint8_t val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_ui_sdo_pull_up_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val) +int32_t lsm6dsv16x_ui_sdo_pull_up_get(stmdev_ctx_t *ctx, uint8_t *val) { lsm6dsv16x_pin_ctrl_t pin_ctrl; int32_t ret; @@ -7545,13 +8323,15 @@ int32_t lsm6dsv16x_ui_sdo_pull_up_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_ui_i2c_i3c_mode_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ui_i2c_i3c_mode_t val) +int32_t lsm6dsv16x_ui_i2c_i3c_mode_set(stmdev_ctx_t *ctx, + lsm6dsv16x_ui_i2c_i3c_mode_t val) { lsm6dsv16x_if_cfg_t if_cfg; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_IF_CFG, (uint8_t *)&if_cfg, 1); - if (ret == 0) { + if (ret == 0) + { if_cfg.i2c_i3c_disable = (uint8_t)val & 0x1U; ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_IF_CFG, (uint8_t *)&if_cfg, 1); } @@ -7567,13 +8347,17 @@ int32_t lsm6dsv16x_ui_i2c_i3c_mode_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ui_i2c_ * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_ui_i2c_i3c_mode_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ui_i2c_i3c_mode_t *val) +int32_t lsm6dsv16x_ui_i2c_i3c_mode_get(stmdev_ctx_t *ctx, + lsm6dsv16x_ui_i2c_i3c_mode_t *val) { lsm6dsv16x_if_cfg_t if_cfg; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_IF_CFG, (uint8_t *)&if_cfg, 1); - switch (if_cfg.i2c_i3c_disable) { + if (ret != 0) { return ret; } + + switch (if_cfg.i2c_i3c_disable) + { case LSM6DSV16X_I2C_I3C_ENABLE: *val = LSM6DSV16X_I2C_I3C_ENABLE; break; @@ -7586,6 +8370,7 @@ int32_t lsm6dsv16x_ui_i2c_i3c_mode_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ui_i2c_ *val = LSM6DSV16X_I2C_I3C_ENABLE; break; } + return ret; } @@ -7597,13 +8382,14 @@ int32_t lsm6dsv16x_ui_i2c_i3c_mode_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ui_i2c_ * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_spi_mode_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_spi_mode_t val) +int32_t lsm6dsv16x_spi_mode_set(stmdev_ctx_t *ctx, lsm6dsv16x_spi_mode_t val) { lsm6dsv16x_if_cfg_t if_cfg; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_IF_CFG, (uint8_t *)&if_cfg, 1); - if (ret == 0) { + if (ret == 0) + { if_cfg.sim = (uint8_t)val & 0x01U; ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_IF_CFG, (uint8_t *)&if_cfg, 1); } @@ -7619,13 +8405,16 @@ int32_t lsm6dsv16x_spi_mode_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_spi_mode_t val * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_spi_mode_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_spi_mode_t *val) +int32_t lsm6dsv16x_spi_mode_get(stmdev_ctx_t *ctx, lsm6dsv16x_spi_mode_t *val) { lsm6dsv16x_if_cfg_t if_cfg; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_IF_CFG, (uint8_t *)&if_cfg, 1); - switch (if_cfg.sim) { + if (ret != 0) { return ret; } + + switch (if_cfg.sim) + { case LSM6DSV16X_SPI_4_WIRE: *val = LSM6DSV16X_SPI_4_WIRE; break; @@ -7638,6 +8427,7 @@ int32_t lsm6dsv16x_spi_mode_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_spi_mode_t *va *val = LSM6DSV16X_SPI_4_WIRE; break; } + return ret; } @@ -7649,13 +8439,14 @@ int32_t lsm6dsv16x_spi_mode_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_spi_mode_t *va * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_ui_sda_pull_up_set(lsm6dsv16x_ctx_t *ctx, uint8_t val) +int32_t lsm6dsv16x_ui_sda_pull_up_set(stmdev_ctx_t *ctx, uint8_t val) { lsm6dsv16x_if_cfg_t if_cfg; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_IF_CFG, (uint8_t *)&if_cfg, 1); - if (ret == 0) { + if (ret == 0) + { if_cfg.sda_pu_en = val; ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_IF_CFG, (uint8_t *)&if_cfg, 1); } @@ -7671,7 +8462,7 @@ int32_t lsm6dsv16x_ui_sda_pull_up_set(lsm6dsv16x_ctx_t *ctx, uint8_t val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_ui_sda_pull_up_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val) +int32_t lsm6dsv16x_ui_sda_pull_up_get(stmdev_ctx_t *ctx, uint8_t *val) { lsm6dsv16x_if_cfg_t if_cfg; int32_t ret; @@ -7683,20 +8474,21 @@ int32_t lsm6dsv16x_ui_sda_pull_up_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val) } /** - * @brief SPI2 (OIS Interface) Serial Interface Mode selection. This function works also on OIS (UI_CTRL1_OIS = SPI2_CTRL1_OIS).[set] + * @brief SPI2 (OIS Inteface) Serial Interface Mode selection. This function works also on OIS (UI_CTRL1_OIS = SPI2_CTRL1_OIS).[set] * * @param ctx read / write interface definitions * @param val SPI2_4_WIRE, SPI2_3_WIRE, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_spi2_mode_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_spi2_mode_t val) +int32_t lsm6dsv16x_spi2_mode_set(stmdev_ctx_t *ctx, lsm6dsv16x_spi2_mode_t val) { lsm6dsv16x_ui_ctrl1_ois_t ui_ctrl1_ois; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_UI_CTRL1_OIS, (uint8_t *)&ui_ctrl1_ois, 1); - if (ret == 0) { + if (ret == 0) + { ui_ctrl1_ois.sim_ois = (uint8_t)val & 0x01U; ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_UI_CTRL1_OIS, (uint8_t *)&ui_ctrl1_ois, 1); } @@ -7705,20 +8497,23 @@ int32_t lsm6dsv16x_spi2_mode_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_spi2_mode_t v } /** - * @brief SPI2 (OIS Interface) Serial Interface Mode selection. This function works also on OIS (UI_CTRL1_OIS = SPI2_CTRL1_OIS).[get] + * @brief SPI2 (OIS Inteface) Serial Interface Mode selection. This function works also on OIS (UI_CTRL1_OIS = SPI2_CTRL1_OIS).[get] * * @param ctx read / write interface definitions * @param val SPI2_4_WIRE, SPI2_3_WIRE, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_spi2_mode_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_spi2_mode_t *val) +int32_t lsm6dsv16x_spi2_mode_get(stmdev_ctx_t *ctx, lsm6dsv16x_spi2_mode_t *val) { lsm6dsv16x_ui_ctrl1_ois_t ui_ctrl1_ois; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_UI_CTRL1_OIS, (uint8_t *)&ui_ctrl1_ois, 1); - switch (ui_ctrl1_ois.sim_ois) { + if (ret != 0) { return ret; } + + switch (ui_ctrl1_ois.sim_ois) + { case LSM6DSV16X_SPI2_4_WIRE: *val = LSM6DSV16X_SPI2_4_WIRE; break; @@ -7731,6 +8526,7 @@ int32_t lsm6dsv16x_spi2_mode_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_spi2_mode_t * *val = LSM6DSV16X_SPI2_4_WIRE; break; } + return ret; } @@ -7756,23 +8552,19 @@ int32_t lsm6dsv16x_spi2_mode_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_spi2_mode_t * * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_sigmot_mode_set(lsm6dsv16x_ctx_t *ctx, uint8_t val) +int32_t lsm6dsv16x_sigmot_mode_set(stmdev_ctx_t *ctx, uint8_t val) { lsm6dsv16x_emb_func_en_a_t emb_func_en_a; int32_t ret; ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK); - if (ret == 0) { - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_EMB_FUNC_EN_A, (uint8_t *)&emb_func_en_a, 1); - } - if (ret == 0) { - emb_func_en_a.sign_motion_en = val; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_EMB_FUNC_EN_A, (uint8_t *)&emb_func_en_a, 1); - } - if (ret == 0) { - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); - } + if (ret != 0) { return ret; } + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_EMB_FUNC_EN_A, (uint8_t *)&emb_func_en_a, 1); + emb_func_en_a.sign_motion_en = val; + ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_EMB_FUNC_EN_A, (uint8_t *)&emb_func_en_a, 1); + + ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); return ret; } @@ -7785,20 +8577,18 @@ int32_t lsm6dsv16x_sigmot_mode_set(lsm6dsv16x_ctx_t *ctx, uint8_t val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_sigmot_mode_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val) +int32_t lsm6dsv16x_sigmot_mode_get(stmdev_ctx_t *ctx, uint8_t *val) { lsm6dsv16x_emb_func_en_a_t emb_func_en_a; int32_t ret; ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK); - if (ret == 0) { - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_EMB_FUNC_EN_A, (uint8_t *)&emb_func_en_a, 1); - } + if (ret != 0) { return ret; } + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_EMB_FUNC_EN_A, (uint8_t *)&emb_func_en_a, 1); *val = emb_func_en_a.sign_motion_en; - if (ret == 0) { - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); - } + ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); return ret; } @@ -7823,7 +8613,8 @@ int32_t lsm6dsv16x_sigmot_mode_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_stpcnt_mode_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_stpcnt_mode_t val) +int32_t lsm6dsv16x_stpcnt_mode_set(stmdev_ctx_t *ctx, + lsm6dsv16x_stpcnt_mode_t val) { lsm6dsv16x_emb_func_en_a_t emb_func_en_a; lsm6dsv16x_emb_func_en_b_t emb_func_en_b; @@ -7831,29 +8622,30 @@ int32_t lsm6dsv16x_stpcnt_mode_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_stpcnt_mode int32_t ret; ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK); - if (ret == 0) { - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_EMB_FUNC_EN_A, (uint8_t *)&emb_func_en_a, 1); - } - if (ret == 0) { - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_EMB_FUNC_EN_B, (uint8_t *)&emb_func_en_b, 1); - } - if ((val.false_step_rej == PROPERTY_ENABLE) && ((emb_func_en_a.mlc_before_fsm_en & emb_func_en_b.mlc_en) == PROPERTY_DISABLE)) { + if (ret != 0) { return ret; } + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_EMB_FUNC_EN_A, (uint8_t *)&emb_func_en_a, 1); + ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_EMB_FUNC_EN_B, (uint8_t *)&emb_func_en_b, 1); + if (ret != 0) { goto exit; } + + if ((val.false_step_rej == PROPERTY_ENABLE) + && ((emb_func_en_a.mlc_before_fsm_en & emb_func_en_b.mlc_en) == + PROPERTY_DISABLE)) + { emb_func_en_a.mlc_before_fsm_en = PROPERTY_ENABLE; } - if (ret == 0) { - emb_func_en_a.pedo_en = val.step_counter_enable; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_EMB_FUNC_EN_A, (uint8_t *)&emb_func_en_a, 1); - } - if (ret == 0) { - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); - } - if (ret == 0) { + emb_func_en_a.pedo_en = val.step_counter_enable; + ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_EMB_FUNC_EN_A, (uint8_t *)&emb_func_en_a, 1); + +exit: + ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + + if (ret == 0) + { ret = lsm6dsv16x_ln_pg_read(ctx, LSM6DSV16X_EMB_ADV_PG_1 + LSM6DSV16X_PEDO_CMD_REG, (uint8_t *)&pedo_cmd_reg, 1); - } - if (ret == 0) { pedo_cmd_reg.fp_rejection_en = val.false_step_rej; - ret = lsm6dsv16x_ln_pg_write(ctx, LSM6DSV16X_EMB_ADV_PG_1 + LSM6DSV16X_PEDO_CMD_REG, (uint8_t *)&pedo_cmd_reg, 1); + ret += lsm6dsv16x_ln_pg_write(ctx, LSM6DSV16X_EMB_ADV_PG_1 + LSM6DSV16X_PEDO_CMD_REG, (uint8_t *)&pedo_cmd_reg, 1); } return ret; @@ -7867,23 +8659,21 @@ int32_t lsm6dsv16x_stpcnt_mode_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_stpcnt_mode * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_stpcnt_mode_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_stpcnt_mode_t *val) +int32_t lsm6dsv16x_stpcnt_mode_get(stmdev_ctx_t *ctx, + lsm6dsv16x_stpcnt_mode_t *val) { lsm6dsv16x_emb_func_en_a_t emb_func_en_a; lsm6dsv16x_pedo_cmd_reg_t pedo_cmd_reg; int32_t ret; ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK); - if (ret == 0) { - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_EMB_FUNC_EN_A, (uint8_t *)&emb_func_en_a, 1); - } - if (ret == 0) { - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); - } + ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_EMB_FUNC_EN_A, (uint8_t *)&emb_func_en_a, 1); + ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + if (ret != 0) { return ret; } + + ret = lsm6dsv16x_ln_pg_read(ctx, LSM6DSV16X_EMB_ADV_PG_1 + LSM6DSV16X_PEDO_CMD_REG, (uint8_t *)&pedo_cmd_reg, 1); + if (ret != 0) { return ret; } - if (ret == 0) { - ret = lsm6dsv16x_ln_pg_read(ctx, LSM6DSV16X_EMB_ADV_PG_1 + LSM6DSV16X_PEDO_CMD_REG, (uint8_t *)&pedo_cmd_reg, 1); - } val->false_step_rej = pedo_cmd_reg.fp_rejection_en; val->step_counter_enable = emb_func_en_a.pedo_en; @@ -7898,18 +8688,16 @@ int32_t lsm6dsv16x_stpcnt_mode_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_stpcnt_mode * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_stpcnt_steps_get(lsm6dsv16x_ctx_t *ctx, uint16_t *val) +int32_t lsm6dsv16x_stpcnt_steps_get(stmdev_ctx_t *ctx, uint16_t *val) { uint8_t buff[2]; int32_t ret; ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK); - if (ret == 0) { - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_STEP_COUNTER_L, &buff[0], 2); - } - if (ret == 0) { - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); - } + ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_STEP_COUNTER_L, &buff[0], 2); + ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + if (ret != 0) { return ret; } + *val = buff[1]; *val = (*val * 256U) + buff[0]; @@ -7924,23 +8712,22 @@ int32_t lsm6dsv16x_stpcnt_steps_get(lsm6dsv16x_ctx_t *ctx, uint16_t *val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_stpcnt_rst_step_set(lsm6dsv16x_ctx_t *ctx, uint8_t val) +int32_t lsm6dsv16x_stpcnt_rst_step_set(stmdev_ctx_t *ctx, uint8_t val) { lsm6dsv16x_emb_func_src_t emb_func_src; int32_t ret; ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK); - if (ret == 0) { - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_EMB_FUNC_SRC, (uint8_t *)&emb_func_src, 1); - } + if (ret != 0) { return ret; } - if (ret == 0) { - emb_func_src.pedo_rst_step = val; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_EMB_FUNC_SRC, (uint8_t *)&emb_func_src, 1); - } - if (ret == 0) { - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); - } + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_EMB_FUNC_SRC, (uint8_t *)&emb_func_src, 1); + if (ret != 0) { goto exit; } + + emb_func_src.pedo_rst_step = val; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_EMB_FUNC_SRC, (uint8_t *)&emb_func_src, 1); + +exit: + ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); return ret; } @@ -7953,21 +8740,18 @@ int32_t lsm6dsv16x_stpcnt_rst_step_set(lsm6dsv16x_ctx_t *ctx, uint8_t val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_stpcnt_rst_step_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val) +int32_t lsm6dsv16x_stpcnt_rst_step_get(stmdev_ctx_t *ctx, uint8_t *val) { lsm6dsv16x_emb_func_src_t emb_func_src; int32_t ret; ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK); - if (ret == 0) { - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_EMB_FUNC_SRC, (uint8_t *)&emb_func_src, 1); - } + if (ret != 0) { return ret; } + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_EMB_FUNC_SRC, (uint8_t *)&emb_func_src, 1); *val = emb_func_src.pedo_rst_step; - if (ret == 0) { - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); - } + ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); return ret; } @@ -7980,13 +8764,14 @@ int32_t lsm6dsv16x_stpcnt_rst_step_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_stpcnt_debounce_set(lsm6dsv16x_ctx_t *ctx, uint8_t val) +int32_t lsm6dsv16x_stpcnt_debounce_set(stmdev_ctx_t *ctx, uint8_t val) { lsm6dsv16x_pedo_deb_steps_conf_t pedo_deb_steps_conf; int32_t ret; ret = lsm6dsv16x_ln_pg_read(ctx, LSM6DSV16X_EMB_ADV_PG_1 + LSM6DSV16X_PEDO_DEB_STEPS_CONF, (uint8_t *)&pedo_deb_steps_conf, 1); - if (ret == 0) { + if (ret == 0) + { pedo_deb_steps_conf.deb_step = val; ret = lsm6dsv16x_ln_pg_write(ctx, LSM6DSV16X_EMB_ADV_PG_1 + LSM6DSV16X_PEDO_DEB_STEPS_CONF, (uint8_t *)&pedo_deb_steps_conf, 1); } @@ -8002,7 +8787,7 @@ int32_t lsm6dsv16x_stpcnt_debounce_set(lsm6dsv16x_ctx_t *ctx, uint8_t val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_stpcnt_debounce_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val) +int32_t lsm6dsv16x_stpcnt_debounce_get(stmdev_ctx_t *ctx, uint8_t *val) { lsm6dsv16x_pedo_deb_steps_conf_t pedo_deb_steps_conf; int32_t ret; @@ -8021,7 +8806,7 @@ int32_t lsm6dsv16x_stpcnt_debounce_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_stpcnt_period_set(lsm6dsv16x_ctx_t *ctx, uint16_t val) +int32_t lsm6dsv16x_stpcnt_period_set(stmdev_ctx_t *ctx, uint16_t val) { uint8_t buff[2]; int32_t ret; @@ -8041,18 +8826,167 @@ int32_t lsm6dsv16x_stpcnt_period_set(lsm6dsv16x_ctx_t *ctx, uint16_t val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_stpcnt_period_get(lsm6dsv16x_ctx_t *ctx, uint16_t *val) +int32_t lsm6dsv16x_stpcnt_period_get(stmdev_ctx_t *ctx, uint16_t *val) { uint8_t buff[2]; int32_t ret; ret = lsm6dsv16x_ln_pg_read(ctx, LSM6DSV16X_EMB_ADV_PG_1 + LSM6DSV16X_PEDO_SC_DELTAT_L, &buff[0], 2); + if (ret != 0) { return ret; } + *val = buff[1]; *val = (*val * 256U) + buff[0]; return ret; } +/** + * @} + * + */ + +/** + * @defgroup Sensor Fusion Low Power (SFLP) + * @brief This section groups all the functions that manage pedometer. + * @{ + * + */ + +/** + * @brief Enable SFLP Game Rotation Vector (6x).[set] + * + * @param ctx read / write interface definitions + * @param val Enable/Disable game rotation value (0/1). + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_sflp_game_rotation_set(stmdev_ctx_t *ctx, uint8_t val) +{ + lsm6dsv16x_emb_func_en_a_t emb_func_en_a; + int32_t ret; + + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK); + if (ret != 0) { return ret; } + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_EMB_FUNC_EN_A, (uint8_t *)&emb_func_en_a, 1); + if (ret != 0) { goto exit; } + + emb_func_en_a.sflp_game_en = val; + ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_EMB_FUNC_EN_A, + (uint8_t *)&emb_func_en_a, 1); + +exit: + ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + + return ret; +} + +/** + * @brief Enable SFLP Game Rotation Vector (6x).[get] + * + * @param ctx read / write interface definitions + * @param val Enable/Disable game rotation value (0/1). + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_sflp_game_rotation_get(stmdev_ctx_t *ctx, uint8_t *val) +{ + lsm6dsv16x_emb_func_en_a_t emb_func_en_a; + int32_t ret; + + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK); + if (ret != 0) { return ret; } + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_EMB_FUNC_EN_A, (uint8_t *)&emb_func_en_a, 1); + *val = emb_func_en_a.sflp_game_en; + + ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + + return ret; +} + +/** + * @brief SFLP Data Rate (ODR) configuration.[set] + * + * @param ctx read / write interface definitions + * @param val SFLP_15Hz, SFLP_30Hz, SFLP_60Hz, SFLP_120Hz, SFLP_240Hz, SFLP_480Hz + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_sflp_data_rate_set(stmdev_ctx_t *ctx, + lsm6dsv16x_sflp_data_rate_t val) +{ + lsm6dsv16x_sflp_odr_t sflp_odr; + int32_t ret; + + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK); + if (ret != 0) { return ret; } + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_SFLP_ODR, (uint8_t *)&sflp_odr, 1); + if (ret != 0) { goto exit; } + + sflp_odr.sflp_game_odr = (uint8_t)val & 0x07U; + ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_SFLP_ODR, (uint8_t *)&sflp_odr, 1); + +exit: + ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + + return ret; +} + +/** + * @brief SFLP Data Rate (ODR) configuration.[get] + * + * @param ctx read / write interface definitions + * @param val SFLP_15Hz, SFLP_30Hz, SFLP_60Hz, SFLP_120Hz, SFLP_240Hz, SFLP_480Hz + * @retval interface status (MANDATORY: return 0 -> no Error) + * + */ +int32_t lsm6dsv16x_sflp_data_rate_get(stmdev_ctx_t *ctx, + lsm6dsv16x_sflp_data_rate_t *val) +{ + lsm6dsv16x_sflp_odr_t sflp_odr; + int32_t ret; + + ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK); + ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_SFLP_ODR, (uint8_t *)&sflp_odr, 1); + ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + if (ret != 0) { return ret; } + + switch (sflp_odr.sflp_game_odr) + { + case LSM6DSV16X_SFLP_15Hz: + *val = LSM6DSV16X_SFLP_15Hz; + break; + + case LSM6DSV16X_SFLP_30Hz: + *val = LSM6DSV16X_SFLP_30Hz; + break; + + case LSM6DSV16X_SFLP_60Hz: + *val = LSM6DSV16X_SFLP_60Hz; + break; + + case LSM6DSV16X_SFLP_120Hz: + *val = LSM6DSV16X_SFLP_120Hz; + break; + + case LSM6DSV16X_SFLP_240Hz: + *val = LSM6DSV16X_SFLP_240Hz; + break; + + case LSM6DSV16X_SFLP_480Hz: + *val = LSM6DSV16X_SFLP_480Hz; + break; + + default: + *val = LSM6DSV16X_SFLP_15Hz; + break; + } + + return ret; +} + /** * @} * @@ -8074,16 +9008,18 @@ int32_t lsm6dsv16x_stpcnt_period_get(lsm6dsv16x_ctx_t *ctx, uint16_t *val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_tap_detection_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_tap_detection_t val) +int32_t lsm6dsv16x_tap_detection_set(stmdev_ctx_t *ctx, + lsm6dsv16x_tap_detection_t val) { lsm6dsv16x_tap_cfg0_t tap_cfg0; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_TAP_CFG0, (uint8_t *)&tap_cfg0, 1); - if (ret == 0) { - tap_cfg0.tap_z_en = val.tap_x_en; + if (ret == 0) + { + tap_cfg0.tap_x_en = val.tap_x_en; tap_cfg0.tap_y_en = val.tap_y_en; - tap_cfg0.tap_x_en = val.tap_z_en; + tap_cfg0.tap_z_en = val.tap_z_en; ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_TAP_CFG0, (uint8_t *)&tap_cfg0, 1); } @@ -8098,13 +9034,16 @@ int32_t lsm6dsv16x_tap_detection_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_tap_detec * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_tap_detection_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_tap_detection_t *val) +int32_t lsm6dsv16x_tap_detection_get(stmdev_ctx_t *ctx, + lsm6dsv16x_tap_detection_t *val) { lsm6dsv16x_tap_cfg0_t tap_cfg0; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_TAP_CFG0, (uint8_t *)&tap_cfg0, 1); - val->tap_x_en = tap_cfg0.tap_z_en; + if (ret != 0) { return ret; } + + val->tap_x_en = tap_cfg0.tap_x_en; val->tap_y_en = tap_cfg0.tap_y_en; val->tap_z_en = tap_cfg0.tap_z_en; @@ -8119,7 +9058,8 @@ int32_t lsm6dsv16x_tap_detection_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_tap_detec * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_tap_thresholds_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_tap_thresholds_t val) +int32_t lsm6dsv16x_tap_thresholds_set(stmdev_ctx_t *ctx, + lsm6dsv16x_tap_thresholds_t val) { lsm6dsv16x_tap_ths_6d_t tap_ths_6d; lsm6dsv16x_tap_cfg2_t tap_cfg2; @@ -8127,26 +9067,17 @@ int32_t lsm6dsv16x_tap_thresholds_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_tap_thre int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_TAP_CFG1, (uint8_t *)&tap_cfg1, 1); - if (ret == 0) { - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_TAP_CFG2, (uint8_t *)&tap_cfg2, 1); - } - if (ret == 0) { - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_TAP_THS_6D, (uint8_t *)&tap_ths_6d, 1); - } + ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_TAP_CFG2, (uint8_t *)&tap_cfg2, 1); + ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_TAP_THS_6D, (uint8_t *)&tap_ths_6d, 1); + if (ret != 0) { return ret; } tap_cfg1.tap_ths_x = val.x; tap_cfg2.tap_ths_y = val.y; tap_ths_6d.tap_ths_z = val.z; - if (ret == 0) { - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_TAP_THS_6D, (uint8_t *)&tap_ths_6d, 1); - } - if (ret == 0) { - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_TAP_CFG2, (uint8_t *)&tap_cfg2, 1); - } - if (ret == 0) { - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_TAP_CFG1, (uint8_t *)&tap_cfg1, 1); - } + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_TAP_THS_6D, (uint8_t *)&tap_ths_6d, 1); + ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_TAP_CFG2, (uint8_t *)&tap_cfg2, 1); + ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_TAP_CFG1, (uint8_t *)&tap_cfg1, 1); return ret; } @@ -8159,7 +9090,8 @@ int32_t lsm6dsv16x_tap_thresholds_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_tap_thre * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_tap_thresholds_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_tap_thresholds_t *val) +int32_t lsm6dsv16x_tap_thresholds_get(stmdev_ctx_t *ctx, + lsm6dsv16x_tap_thresholds_t *val) { lsm6dsv16x_tap_ths_6d_t tap_ths_6d; lsm6dsv16x_tap_cfg2_t tap_cfg2; @@ -8167,12 +9099,9 @@ int32_t lsm6dsv16x_tap_thresholds_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_tap_thre int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_TAP_CFG1, (uint8_t *)&tap_cfg1, 1); - if (ret == 0) { - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_TAP_CFG2, (uint8_t *)&tap_cfg2, 1); - } - if (ret == 0) { - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_TAP_THS_6D, (uint8_t *)&tap_ths_6d, 1); - } + ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_TAP_CFG2, (uint8_t *)&tap_cfg2, 1); + ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_TAP_THS_6D, (uint8_t *)&tap_ths_6d, 1); + if (ret != 0) { return ret; } val->x = tap_cfg1.tap_ths_x; val->y = tap_cfg2.tap_ths_y; @@ -8189,13 +9118,15 @@ int32_t lsm6dsv16x_tap_thresholds_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_tap_thre * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_tap_axis_priority_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_tap_axis_priority_t val) +int32_t lsm6dsv16x_tap_axis_priority_set(stmdev_ctx_t *ctx, + lsm6dsv16x_tap_axis_priority_t val) { lsm6dsv16x_tap_cfg1_t tap_cfg1; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_TAP_CFG1, (uint8_t *)&tap_cfg1, 1); - if (ret == 0) { + if (ret == 0) + { tap_cfg1.tap_priority = (uint8_t)val & 0x7U; ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_TAP_CFG1, (uint8_t *)&tap_cfg1, 1); } @@ -8211,13 +9142,17 @@ int32_t lsm6dsv16x_tap_axis_priority_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_tap_a * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_tap_axis_priority_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_tap_axis_priority_t *val) +int32_t lsm6dsv16x_tap_axis_priority_get(stmdev_ctx_t *ctx, + lsm6dsv16x_tap_axis_priority_t *val) { lsm6dsv16x_tap_cfg1_t tap_cfg1; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_TAP_CFG1, (uint8_t *)&tap_cfg1, 1); - switch (tap_cfg1.tap_priority) { + if (ret != 0) { return ret; } + + switch (tap_cfg1.tap_priority) + { case LSM6DSV16X_XYZ : *val = LSM6DSV16X_XYZ ; break; @@ -8246,6 +9181,7 @@ int32_t lsm6dsv16x_tap_axis_priority_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_tap_a *val = LSM6DSV16X_XYZ ; break; } + return ret; } @@ -8257,17 +9193,19 @@ int32_t lsm6dsv16x_tap_axis_priority_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_tap_a * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_tap_time_windows_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_tap_time_windows_t val) +int32_t lsm6dsv16x_tap_time_windows_set(stmdev_ctx_t *ctx, + lsm6dsv16x_tap_time_windows_t val) { - lsm6dsv16x_int_dur2_t int_dur2; + lsm6dsv16x_tap_dur_t tap_dur; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_INT_DUR2, (uint8_t *)&int_dur2, 1); - if (ret == 0) { - int_dur2.shock = val.shock; - int_dur2.quiet = val.quiet; - int_dur2.dur = val.tap_gap; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_INT_DUR2, (uint8_t *)&int_dur2, 1); + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_TAP_DUR, (uint8_t *)&tap_dur, 1); + if (ret == 0) + { + tap_dur.shock = val.shock; + tap_dur.quiet = val.quiet; + tap_dur.dur = val.tap_gap; + ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_TAP_DUR, (uint8_t *)&tap_dur, 1); } return ret; @@ -8281,15 +9219,18 @@ int32_t lsm6dsv16x_tap_time_windows_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_tap_ti * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_tap_time_windows_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_tap_time_windows_t *val) +int32_t lsm6dsv16x_tap_time_windows_get(stmdev_ctx_t *ctx, + lsm6dsv16x_tap_time_windows_t *val) { - lsm6dsv16x_int_dur2_t int_dur2; + lsm6dsv16x_tap_dur_t tap_dur; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_INT_DUR2, (uint8_t *)&int_dur2, 1); - val->shock = int_dur2.shock; - val->quiet = int_dur2.quiet; - val->tap_gap = int_dur2.dur; + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_TAP_DUR, (uint8_t *)&tap_dur, 1); + if (ret != 0) { return ret; } + + val->shock = tap_dur.shock; + val->quiet = tap_dur.quiet; + val->tap_gap = tap_dur.dur; return ret; } @@ -8302,13 +9243,14 @@ int32_t lsm6dsv16x_tap_time_windows_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_tap_ti * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_tap_mode_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_tap_mode_t val) +int32_t lsm6dsv16x_tap_mode_set(stmdev_ctx_t *ctx, lsm6dsv16x_tap_mode_t val) { lsm6dsv16x_wake_up_ths_t wake_up_ths; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_WAKE_UP_THS, (uint8_t *)&wake_up_ths, 1); - if (ret == 0) { + if (ret == 0) + { wake_up_ths.single_double_tap = (uint8_t)val & 0x01U; ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_WAKE_UP_THS, (uint8_t *)&wake_up_ths, 1); } @@ -8324,13 +9266,16 @@ int32_t lsm6dsv16x_tap_mode_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_tap_mode_t val * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_tap_mode_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_tap_mode_t *val) +int32_t lsm6dsv16x_tap_mode_get(stmdev_ctx_t *ctx, lsm6dsv16x_tap_mode_t *val) { lsm6dsv16x_wake_up_ths_t wake_up_ths; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_WAKE_UP_THS, (uint8_t *)&wake_up_ths, 1); - switch (wake_up_ths.single_double_tap) { + if (ret != 0) { return ret; } + + switch (wake_up_ths.single_double_tap) + { case LSM6DSV16X_ONLY_SINGLE: *val = LSM6DSV16X_ONLY_SINGLE; break; @@ -8343,6 +9288,7 @@ int32_t lsm6dsv16x_tap_mode_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_tap_mode_t *va *val = LSM6DSV16X_ONLY_SINGLE; break; } + return ret; } @@ -8367,22 +9313,19 @@ int32_t lsm6dsv16x_tap_mode_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_tap_mode_t *va * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_tilt_mode_set(lsm6dsv16x_ctx_t *ctx, uint8_t val) +int32_t lsm6dsv16x_tilt_mode_set(stmdev_ctx_t *ctx, uint8_t val) { lsm6dsv16x_emb_func_en_a_t emb_func_en_a; int32_t ret; ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK); - if (ret == 0) { - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_EMB_FUNC_EN_A, (uint8_t *)&emb_func_en_a, 1); - } - if (ret == 0) { - emb_func_en_a.tilt_en = val; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_EMB_FUNC_EN_A, (uint8_t *)&emb_func_en_a, 1); - } - if (ret == 0) { - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); - } + if (ret != 0) { return ret; } + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_EMB_FUNC_EN_A, (uint8_t *)&emb_func_en_a, 1); + emb_func_en_a.tilt_en = val; + ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_EMB_FUNC_EN_A, (uint8_t *)&emb_func_en_a, 1); + + ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); return ret; } @@ -8395,20 +9338,18 @@ int32_t lsm6dsv16x_tilt_mode_set(lsm6dsv16x_ctx_t *ctx, uint8_t val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_tilt_mode_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val) +int32_t lsm6dsv16x_tilt_mode_get(stmdev_ctx_t *ctx, uint8_t *val) { lsm6dsv16x_emb_func_en_a_t emb_func_en_a; int32_t ret; ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK); - if (ret == 0) { - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_EMB_FUNC_EN_A, (uint8_t *)&emb_func_en_a, 1); - } + if (ret != 0) { return ret; } + + ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_EMB_FUNC_EN_A, (uint8_t *)&emb_func_en_a, 1); *val = emb_func_en_a.tilt_en; - if (ret == 0) { - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); - } + ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); return ret; } @@ -8434,12 +9375,14 @@ int32_t lsm6dsv16x_tilt_mode_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_timestamp_raw_get(lsm6dsv16x_ctx_t *ctx, uint32_t *val) +int32_t lsm6dsv16x_timestamp_raw_get(stmdev_ctx_t *ctx, uint32_t *val) { uint8_t buff[4]; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_TIMESTAMP0, &buff[0], 4); + if (ret != 0) { return ret; } + *val = buff[3]; *val = (*val * 256U) + buff[2]; *val = (*val * 256U) + buff[1]; @@ -8456,13 +9399,14 @@ int32_t lsm6dsv16x_timestamp_raw_get(lsm6dsv16x_ctx_t *ctx, uint32_t *val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_timestamp_set(lsm6dsv16x_ctx_t *ctx, uint8_t val) +int32_t lsm6dsv16x_timestamp_set(stmdev_ctx_t *ctx, uint8_t val) { lsm6dsv16x_functions_enable_t functions_enable; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FUNCTIONS_ENABLE, (uint8_t *)&functions_enable, 1); - if (ret == 0) { + if (ret == 0) + { functions_enable.timestamp_en = val; ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_FUNCTIONS_ENABLE, (uint8_t *)&functions_enable, 1); } @@ -8478,7 +9422,7 @@ int32_t lsm6dsv16x_timestamp_set(lsm6dsv16x_ctx_t *ctx, uint8_t val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_timestamp_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val) +int32_t lsm6dsv16x_timestamp_get(stmdev_ctx_t *ctx, uint8_t *val) { lsm6dsv16x_functions_enable_t functions_enable; int32_t ret; @@ -8510,13 +9454,14 @@ int32_t lsm6dsv16x_timestamp_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_act_mode_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_act_mode_t val) +int32_t lsm6dsv16x_act_mode_set(stmdev_ctx_t *ctx, lsm6dsv16x_act_mode_t val) { lsm6dsv16x_functions_enable_t functions_enable; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FUNCTIONS_ENABLE, (uint8_t *)&functions_enable, 1); - if (ret == 0) { + if (ret == 0) + { functions_enable.inact_en = (uint8_t)val & 0x3U; ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_FUNCTIONS_ENABLE, (uint8_t *)&functions_enable, 1); } @@ -8532,13 +9477,16 @@ int32_t lsm6dsv16x_act_mode_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_act_mode_t val * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_act_mode_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_act_mode_t *val) +int32_t lsm6dsv16x_act_mode_get(stmdev_ctx_t *ctx, lsm6dsv16x_act_mode_t *val) { lsm6dsv16x_functions_enable_t functions_enable; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FUNCTIONS_ENABLE, (uint8_t *)&functions_enable, 1); - switch (functions_enable.inact_en) { + if (ret != 0) { return ret; } + + switch (functions_enable.inact_en) + { case LSM6DSV16X_XL_AND_GY_NOT_AFFECTED: *val = LSM6DSV16X_XL_AND_GY_NOT_AFFECTED; break; @@ -8559,6 +9507,7 @@ int32_t lsm6dsv16x_act_mode_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_act_mode_t *va *val = LSM6DSV16X_XL_AND_GY_NOT_AFFECTED; break; } + return ret; } @@ -8570,13 +9519,15 @@ int32_t lsm6dsv16x_act_mode_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_act_mode_t *va * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_act_from_sleep_to_act_dur_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_act_from_sleep_to_act_dur_t val) +int32_t lsm6dsv16x_act_from_sleep_to_act_dur_set(stmdev_ctx_t *ctx, + lsm6dsv16x_act_from_sleep_to_act_dur_t val) { lsm6dsv16x_inactivity_dur_t inactivity_dur; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_INACTIVITY_DUR, (uint8_t *)&inactivity_dur, 1); - if (ret == 0) { + if (ret == 0) + { inactivity_dur.inact_dur = (uint8_t)val & 0x3U; ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_INACTIVITY_DUR, (uint8_t *)&inactivity_dur, 1); } @@ -8592,13 +9543,17 @@ int32_t lsm6dsv16x_act_from_sleep_to_act_dur_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv1 * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_act_from_sleep_to_act_dur_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_act_from_sleep_to_act_dur_t *val) +int32_t lsm6dsv16x_act_from_sleep_to_act_dur_get(stmdev_ctx_t *ctx, + lsm6dsv16x_act_from_sleep_to_act_dur_t *val) { lsm6dsv16x_inactivity_dur_t inactivity_dur; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_INACTIVITY_DUR, (uint8_t *)&inactivity_dur, 1); - switch (inactivity_dur.inact_dur) { + if (ret != 0) { return ret; } + + switch (inactivity_dur.inact_dur) + { case LSM6DSV16X_SLEEP_TO_ACT_AT_1ST_SAMPLE: *val = LSM6DSV16X_SLEEP_TO_ACT_AT_1ST_SAMPLE; break; @@ -8619,6 +9574,7 @@ int32_t lsm6dsv16x_act_from_sleep_to_act_dur_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv1 *val = LSM6DSV16X_SLEEP_TO_ACT_AT_1ST_SAMPLE; break; } + return ret; } @@ -8630,13 +9586,15 @@ int32_t lsm6dsv16x_act_from_sleep_to_act_dur_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv1 * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_act_sleep_xl_odr_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_act_sleep_xl_odr_t val) +int32_t lsm6dsv16x_act_sleep_xl_odr_set(stmdev_ctx_t *ctx, + lsm6dsv16x_act_sleep_xl_odr_t val) { lsm6dsv16x_inactivity_dur_t inactivity_dur; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_INACTIVITY_DUR, (uint8_t *)&inactivity_dur, 1); - if (ret == 0) { + if (ret == 0) + { inactivity_dur.xl_inact_odr = (uint8_t)val & 0x03U; ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_INACTIVITY_DUR, (uint8_t *)&inactivity_dur, 1); } @@ -8652,13 +9610,17 @@ int32_t lsm6dsv16x_act_sleep_xl_odr_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_act_sl * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_act_sleep_xl_odr_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_act_sleep_xl_odr_t *val) +int32_t lsm6dsv16x_act_sleep_xl_odr_get(stmdev_ctx_t *ctx, + lsm6dsv16x_act_sleep_xl_odr_t *val) { lsm6dsv16x_inactivity_dur_t inactivity_dur; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_INACTIVITY_DUR, (uint8_t *)&inactivity_dur, 1); - switch (inactivity_dur.xl_inact_odr) { + if (ret != 0) { return ret; } + + switch (inactivity_dur.xl_inact_odr) + { case LSM6DSV16X_1Hz875: *val = LSM6DSV16X_1Hz875; break; @@ -8679,6 +9641,7 @@ int32_t lsm6dsv16x_act_sleep_xl_odr_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_act_sl *val = LSM6DSV16X_1Hz875; break; } + return ret; } @@ -8690,88 +9653,33 @@ int32_t lsm6dsv16x_act_sleep_xl_odr_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_act_sl * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_act_thresholds_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_act_thresholds_t val) +int32_t lsm6dsv16x_act_thresholds_set(stmdev_ctx_t *ctx, + lsm6dsv16x_act_thresholds_t *val) { lsm6dsv16x_inactivity_ths_t inactivity_ths; lsm6dsv16x_inactivity_dur_t inactivity_dur; lsm6dsv16x_wake_up_ths_t wake_up_ths; + lsm6dsv16x_wake_up_dur_t wake_up_dur; int32_t ret; - float tmp; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_INACTIVITY_DUR, (uint8_t *)&inactivity_dur, 1); - if (ret == 0) { - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_INACTIVITY_THS, (uint8_t *)&inactivity_ths, 1); - } - - if (ret == 0) { - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_WAKE_UP_THS, (uint8_t *)&wake_up_ths, 1); - } - - if ((val.wk_ths_mg < (uint32_t)(7.8125f * 63.0f)) && (val.inact_ths_mg < (uint32_t)(7.8125f * 63.0f))) { - inactivity_dur.wu_inact_ths_w = 0; - - tmp = (float)val.inact_ths_mg / 7.8125f; - inactivity_ths.inact_ths = (uint8_t)tmp; - - tmp = (float)val.wk_ths_mg / 7.8125f; - wake_up_ths.wk_ths = (uint8_t)tmp; - } else if ((val.wk_ths_mg < (uint32_t)(15.625f * 63.0f)) && (val.inact_ths_mg < (uint32_t)(15.625f * 63.0f))) { - inactivity_dur.wu_inact_ths_w = 1; - - tmp = (float)val.inact_ths_mg / 15.625f; - inactivity_ths.inact_ths = (uint8_t)tmp; - - tmp = (float)val.wk_ths_mg / 15.625f; - wake_up_ths.wk_ths = (uint8_t)tmp; - } else if ((val.wk_ths_mg < (uint32_t)(31.25f * 63.0f)) && (val.inact_ths_mg < (uint32_t)(31.25f * 63.0f))) { - inactivity_dur.wu_inact_ths_w = 2; - - tmp = (float)val.inact_ths_mg / 31.25f; - inactivity_ths.inact_ths = (uint8_t)tmp; + ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_INACTIVITY_THS, (uint8_t *)&inactivity_ths, 1); + ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_WAKE_UP_THS, (uint8_t *)&wake_up_ths, 1); + ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_WAKE_UP_DUR, (uint8_t *)&wake_up_dur, 1); + if (ret != 0) { return ret; } - tmp = (float)val.wk_ths_mg / 31.25f; - wake_up_ths.wk_ths = (uint8_t)tmp; - } else if ((val.wk_ths_mg < (uint32_t)(62.5f * 63.0f)) && (val.inact_ths_mg < (uint32_t)(62.5f * 63.0f))) { - inactivity_dur.wu_inact_ths_w = 3; + inactivity_dur.wu_inact_ths_w = val->inactivity_cfg.wu_inact_ths_w; + inactivity_dur.xl_inact_odr = val->inactivity_cfg.xl_inact_odr; + inactivity_dur.inact_dur = val->inactivity_cfg.inact_dur; - tmp = (float)val.inact_ths_mg / 62.5f; - inactivity_ths.inact_ths = (uint8_t)tmp; + inactivity_ths.inact_ths = val->inactivity_ths; + wake_up_ths.wk_ths = val->threshold; + wake_up_dur.wake_dur = val->duration; - tmp = (float)val.wk_ths_mg / 62.5f; - wake_up_ths.wk_ths = (uint8_t)tmp; - } else if ((val.wk_ths_mg < (uint32_t)(125.0f * 63.0f)) && (val.inact_ths_mg < (uint32_t)(125.0f * 63.0f))) { - inactivity_dur.wu_inact_ths_w = 4; - - tmp = (float)val.inact_ths_mg / 125.0f; - inactivity_ths.inact_ths = (uint8_t)tmp; - - tmp = (float)val.wk_ths_mg / 125.0f; - wake_up_ths.wk_ths = (uint8_t)tmp; - } else if ((val.wk_ths_mg < (uint32_t)(250.0f * 63.0f)) && (val.inact_ths_mg < (uint32_t)(250.0f * 63.0f))) { - inactivity_dur.wu_inact_ths_w = 5; - - tmp = (float)val.inact_ths_mg / 250.0f; - inactivity_ths.inact_ths = (uint8_t)tmp; - - tmp = (float)val.wk_ths_mg / 250.0f; - wake_up_ths.wk_ths = (uint8_t)tmp; - } else { // out of limit - inactivity_dur.wu_inact_ths_w = 5; - inactivity_ths.inact_ths = 0x3FU; - wake_up_ths.wk_ths = 0x3FU; - } - - if (ret == 0) { - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_INACTIVITY_DUR, (uint8_t *)&inactivity_dur, 1); - } - if (ret == 0) { - - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_INACTIVITY_THS, (uint8_t *)&inactivity_ths, 1); - } - if (ret == 0) { - - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_WAKE_UP_THS, (uint8_t *)&wake_up_ths, 1); - } + ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_INACTIVITY_DUR, (uint8_t *)&inactivity_dur, 1); + ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_INACTIVITY_THS, (uint8_t *)&inactivity_ths, 1); + ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_WAKE_UP_THS, (uint8_t *)&wake_up_ths, 1); + ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_WAKE_UP_DUR, (uint8_t *)&wake_up_dur, 1); return ret; } @@ -8784,71 +9692,28 @@ int32_t lsm6dsv16x_act_thresholds_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_act_thre * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_act_thresholds_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_act_thresholds_t *val) +int32_t lsm6dsv16x_act_thresholds_get(stmdev_ctx_t *ctx, + lsm6dsv16x_act_thresholds_t *val) { lsm6dsv16x_inactivity_dur_t inactivity_dur; lsm6dsv16x_inactivity_ths_t inactivity_ths; lsm6dsv16x_wake_up_ths_t wake_up_ths; + lsm6dsv16x_wake_up_dur_t wake_up_dur; int32_t ret; - float tmp; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_INACTIVITY_DUR, (uint8_t *)&inactivity_dur, 1); - if (ret == 0) { - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_INACTIVITY_THS, (uint8_t *)&inactivity_ths, 1); - } - if (ret == 0) { - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_WAKE_UP_THS, (uint8_t *)&wake_up_ths, 1); - } - - switch (inactivity_dur.wu_inact_ths_w) { - case 0: - tmp = (float)wake_up_ths.wk_ths * 7.8125f; - val->wk_ths_mg = (uint32_t)tmp; - - tmp = (float)inactivity_ths.inact_ths * 7.8125f; - val->inact_ths_mg = (uint32_t)tmp; - break; - - case 1: - tmp = (float)wake_up_ths.wk_ths * 15.625f; - val->wk_ths_mg = (uint32_t)tmp; - - tmp = (float)inactivity_ths.inact_ths * 15.625f; - val->inact_ths_mg = (uint32_t)tmp; - break; - - case 2: - tmp = (float)wake_up_ths.wk_ths * 31.25f; - val->wk_ths_mg = (uint32_t)tmp; - - tmp = (float)inactivity_ths.inact_ths * 31.25f; - val->inact_ths_mg = (uint32_t)tmp; - break; - - case 3: - tmp = (float)wake_up_ths.wk_ths * 62.5f; - val->wk_ths_mg = (uint32_t)tmp; - - tmp = (float)inactivity_ths.inact_ths * 62.5f; - val->inact_ths_mg = (uint32_t)tmp; - break; - - case 4: - tmp = (float)wake_up_ths.wk_ths * 125.0f; - val->wk_ths_mg = (uint32_t)tmp; - - tmp = (float)inactivity_ths.inact_ths * 125.0f; - val->inact_ths_mg = (uint32_t)tmp; - break; + ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_INACTIVITY_THS, (uint8_t *)&inactivity_ths, 1); + ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_WAKE_UP_THS, (uint8_t *)&wake_up_ths, 1); + ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_WAKE_UP_DUR, (uint8_t *)&wake_up_dur, 1); + if (ret != 0) { return ret; } - default: - tmp = (float)wake_up_ths.wk_ths * 250.0f; - val->wk_ths_mg = (uint32_t)tmp; + val->inactivity_cfg.wu_inact_ths_w = inactivity_dur.wu_inact_ths_w; + val->inactivity_cfg.xl_inact_odr = inactivity_dur.xl_inact_odr; + val->inactivity_cfg.inact_dur = inactivity_dur.inact_dur; - tmp = (float)inactivity_ths.inact_ths * 250.0f; - val->inact_ths_mg = (uint32_t)tmp; - break; - } + val->inactivity_ths = inactivity_ths.inact_ths; + val->threshold = wake_up_ths.wk_ths; + val->duration = wake_up_dur.wake_dur; return ret; } @@ -8861,13 +9726,15 @@ int32_t lsm6dsv16x_act_thresholds_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_act_thre * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_act_wkup_time_windows_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_act_wkup_time_windows_t val) +int32_t lsm6dsv16x_act_wkup_time_windows_set(stmdev_ctx_t *ctx, + lsm6dsv16x_act_wkup_time_windows_t val) { lsm6dsv16x_wake_up_dur_t wake_up_dur; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_WAKE_UP_DUR, (uint8_t *)&wake_up_dur, 1); - if (ret == 0) { + if (ret == 0) + { wake_up_dur.wake_dur = val.shock; wake_up_dur.sleep_dur = val.quiet; ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_WAKE_UP_DUR, (uint8_t *)&wake_up_dur, 1); @@ -8884,12 +9751,15 @@ int32_t lsm6dsv16x_act_wkup_time_windows_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_a * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_act_wkup_time_windows_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_act_wkup_time_windows_t *val) +int32_t lsm6dsv16x_act_wkup_time_windows_get(stmdev_ctx_t *ctx, + lsm6dsv16x_act_wkup_time_windows_t *val) { lsm6dsv16x_wake_up_dur_t wake_up_dur; int32_t ret; ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_WAKE_UP_DUR, (uint8_t *)&wake_up_dur, 1); + if (ret != 0) { return ret; } + val->shock = wake_up_dur.wake_dur; val->quiet = wake_up_dur.sleep_dur; @@ -8899,4 +9769,4 @@ int32_t lsm6dsv16x_act_wkup_time_windows_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_a /** * @} * - */ + */ \ No newline at end of file diff --git a/lib/LSM6DSV16X/lsm6dsv16x_reg.h b/lib/LSM6DSV16X/lsm6dsv16x_reg.h index 6f5077309..8371a427a 100644 --- a/lib/LSM6DSV16X/lsm6dsv16x_reg.h +++ b/lib/LSM6DSV16X/lsm6dsv16x_reg.h @@ -7,7 +7,7 @@ ****************************************************************************** * @attention * - *

© Copyright (c) 2021 STMicroelectronics. + *

© Copyright (c) 2022 STMicroelectronics. * All rights reserved.

* * This software component is licensed by ST under BSD 3-Clause license, @@ -75,7 +75,8 @@ extern "C" { #ifndef MEMS_SHARED_TYPES #define MEMS_SHARED_TYPES -typedef struct { +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN uint8_t bit0 : 1; uint8_t bit1 : 1; @@ -100,13 +101,6 @@ typedef struct { #define PROPERTY_DISABLE (0U) #define PROPERTY_ENABLE (1U) -#endif /* MEMS_SHARED_TYPES */ - -/** - * @} - * - */ - /** @addtogroup Interfaces_Functions * @brief This section provide a set of functions used to read and * write a generic register of the device. @@ -115,22 +109,28 @@ typedef struct { * */ -typedef int32_t (*lsm6dsv16x_write_ptr)(void *, uint8_t, uint8_t *, uint16_t); -typedef int32_t (*lsm6dsv16x_read_ptr)(void *, uint8_t, uint8_t *, uint16_t); +typedef int32_t (*stmdev_write_ptr)(void *, uint8_t, const uint8_t *, uint16_t); +typedef int32_t (*stmdev_read_ptr)(void *, uint8_t, uint8_t *, uint16_t); +typedef void (*stmdev_mdelay_ptr)(uint32_t millisec); -typedef struct { +typedef struct +{ /** Component mandatory fields **/ - lsm6dsv16x_write_ptr write_reg; - lsm6dsv16x_read_ptr read_reg; + stmdev_write_ptr write_reg; + stmdev_read_ptr read_reg; + /** Component optional fields **/ + stmdev_mdelay_ptr mdelay; /** Customizable optional pointer **/ void *handle; -} lsm6dsv16x_ctx_t; +} stmdev_ctx_t; /** * @} * */ +#endif /* MEMS_SHARED_TYPES */ + #ifndef MEMS_UCF_SHARED_TYPES #define MEMS_UCF_SHARED_TYPES @@ -145,7 +145,8 @@ typedef struct { * */ -typedef struct { +typedef struct +{ uint8_t address; uint8_t data; } ucf_line_t; @@ -157,6 +158,11 @@ typedef struct { #endif /* MEMS_UCF_SHARED_TYPES */ +/** + * @} + * + */ + /** @defgroup LSM6DSV16X_Infos * @{ * @@ -179,662 +185,693 @@ typedef struct { * */ -#define LSM6DSV16X_FUNC_CFG_ACCESS 0x1U -typedef struct { +#define LSM6DSV16X_FUNC_CFG_ACCESS 0x1U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t ois_ctrl_from_ui : 1; - uint8_t spi2_reset : 1; - uint8_t sw_por : 1; - uint8_t fsm_wr_ctrl_en : 1; - uint8_t not_used0 : 2; - uint8_t shub_reg_access : 1; + uint8_t ois_ctrl_from_ui : 1; + uint8_t spi2_reset : 1; + uint8_t sw_por : 1; + uint8_t fsm_wr_ctrl_en : 1; + uint8_t not_used0 : 2; + uint8_t shub_reg_access : 1; uint8_t emb_func_reg_access : 1; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t emb_func_reg_access : 1; - uint8_t shub_reg_access : 1; - uint8_t not_used0 : 2; - uint8_t fsm_wr_ctrl_en : 1; - uint8_t sw_por : 1; - uint8_t spi2_reset : 1; - uint8_t ois_ctrl_from_ui : 1; + uint8_t shub_reg_access : 1; + uint8_t not_used0 : 2; + uint8_t fsm_wr_ctrl_en : 1; + uint8_t sw_por : 1; + uint8_t spi2_reset : 1; + uint8_t ois_ctrl_from_ui : 1; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_func_cfg_access_t; -#define LSM6DSV16X_PIN_CTRL 0x2U -typedef struct { +#define LSM6DSV16X_PIN_CTRL 0x2U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t not_used0 : 5; - uint8_t ibhr_por_en : 1; - uint8_t sdo_pu_en : 1; - uint8_t ois_pu_dis : 1; + uint8_t not_used0 : 5; + uint8_t ibhr_por_en : 1; + uint8_t sdo_pu_en : 1; + uint8_t ois_pu_dis : 1; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t ois_pu_dis : 1; - uint8_t sdo_pu_en : 1; - uint8_t ibhr_por_en : 1; - uint8_t not_used0 : 5; + uint8_t ois_pu_dis : 1; + uint8_t sdo_pu_en : 1; + uint8_t ibhr_por_en : 1; + uint8_t not_used0 : 5; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_pin_ctrl_t; -#define LSM6DSV16X_IF_CFG 0x3U -typedef struct { -#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t i2c_i3c_disable : 1; - uint8_t not_used0 : 1; - uint8_t sim : 1; - uint8_t pp_od : 1; - uint8_t h_lactive : 1; - uint8_t asf_ctrl : 1; - uint8_t shub_pu_en : 1; - uint8_t sda_pu_en : 1; -#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t sda_pu_en : 1; - uint8_t shub_pu_en : 1; - uint8_t asf_ctrl : 1; - uint8_t h_lactive : 1; - uint8_t pp_od : 1; - uint8_t sim : 1; - uint8_t not_used0 : 1; - uint8_t i2c_i3c_disable : 1; +#define LSM6DSV16X_IF_CFG 0x3U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t i2c_i3c_disable : 1; + uint8_t not_used0 : 1; + uint8_t sim : 1; + uint8_t pp_od : 1; + uint8_t h_lactive : 1; + uint8_t asf_ctrl : 1; + uint8_t shub_pu_en : 1; + uint8_t sda_pu_en : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t sda_pu_en : 1; + uint8_t shub_pu_en : 1; + uint8_t asf_ctrl : 1; + uint8_t h_lactive : 1; + uint8_t pp_od : 1; + uint8_t sim : 1; + uint8_t not_used0 : 1; + uint8_t i2c_i3c_disable : 1; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_if_cfg_t; -#define LSM6DSV16X_S4S_TPH_L 0x4U -typedef struct { -#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t tph : 7; - uint8_t tph_h_sel : 1; -#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t tph_h_sel : 1; - uint8_t tph : 7; -#endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_s4s_tph_l_t; - -#define LSM6DSV16X_S4S_TPH_H 0x5U -typedef struct { -#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t tph : 8; -#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t tph : 8; -#endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_s4s_tph_h_t; - -#define LSM6DSV16X_S4S_RR 0x6U -typedef struct { +#define LSM6DSV16X_ODR_TRIG_CFG 0x6U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t rr : 2; - uint8_t not_used0 : 6; + uint8_t odr_trig_nodr : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t not_used0 : 6; - uint8_t rr : 2; + uint8_t odr_trig_nodr : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_s4s_rr_t; +} lsm6dsv16x_odr_trig_cfg_t; -#define LSM6DSV16X_FIFO_CTRL1 0x7U -typedef struct { +#define LSM6DSV16X_FIFO_CTRL1 0x7U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t wtm : 8; + uint8_t wtm : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t wtm : 8; + uint8_t wtm : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_fifo_ctrl1_t; -#define LSM6DSV16X_FIFO_CTRL2 0x8U -typedef struct { -#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t xl_dualc_batch_from_fsm : 1; - uint8_t uncompr_rate : 2; - uint8_t not_used0 : 1; - uint8_t odr_chg_en : 1; - uint8_t not_used1 : 1; - uint8_t fifo_compr_rt_en : 1; - uint8_t stop_on_wtm : 1; -#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t stop_on_wtm : 1; - uint8_t fifo_compr_rt_en : 1; - uint8_t not_used1 : 1; - uint8_t odr_chg_en : 1; - uint8_t not_used0 : 1; - uint8_t uncompr_rate : 2; - uint8_t xl_dualc_batch_from_fsm : 1; +#define LSM6DSV16X_FIFO_CTRL2 0x8U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t xl_dualc_batch_from_fsm : 1; + uint8_t uncompr_rate : 2; + uint8_t not_used0 : 1; + uint8_t odr_chg_en : 1; + uint8_t not_used1 : 1; + uint8_t fifo_compr_rt_en : 1; + uint8_t stop_on_wtm : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t stop_on_wtm : 1; + uint8_t fifo_compr_rt_en : 1; + uint8_t not_used1 : 1; + uint8_t odr_chg_en : 1; + uint8_t not_used0 : 1; + uint8_t uncompr_rate : 2; + uint8_t xl_dualc_batch_from_fsm : 1; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_fifo_ctrl2_t; -#define LSM6DSV16X_FIFO_CTRL3 0x9U -typedef struct { +#define LSM6DSV16X_FIFO_CTRL3 0x9U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t bdr_xl : 4; - uint8_t bdr_gy : 4; + uint8_t bdr_xl : 4; + uint8_t bdr_gy : 4; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t bdr_gy : 4; - uint8_t bdr_xl : 4; + uint8_t bdr_gy : 4; + uint8_t bdr_xl : 4; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_fifo_ctrl3_t; -#define LSM6DSV16X_FIFO_CTRL4 0x0AU -typedef struct { +#define LSM6DSV16X_FIFO_CTRL4 0x0AU +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t fifo_mode : 3; - uint8_t g_eis_fifo_en : 1; - uint8_t odr_t_batch : 2; - uint8_t dec_ts_batch : 2; + uint8_t fifo_mode : 3; + uint8_t g_eis_fifo_en : 1; + uint8_t odr_t_batch : 2; + uint8_t dec_ts_batch : 2; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t dec_ts_batch : 2; - uint8_t odr_t_batch : 2; - uint8_t g_eis_fifo_en : 1; - uint8_t fifo_mode : 3; + uint8_t dec_ts_batch : 2; + uint8_t odr_t_batch : 2; + uint8_t g_eis_fifo_en : 1; + uint8_t fifo_mode : 3; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_fifo_ctrl4_t; -#define LSM6DSV16X_COUNTER_BDR_REG1 0x0BU -typedef struct { +#define LSM6DSV16X_COUNTER_BDR_REG1 0x0BU +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t cnt_bdr_th : 2; - uint8_t not_used0 : 3; - uint8_t trig_counter_bdr : 2; - uint8_t not_used1 : 1; + uint8_t cnt_bdr_th : 2; + uint8_t not_used0 : 3; + uint8_t trig_counter_bdr : 2; + uint8_t not_used1 : 1; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t not_used1 : 1; - uint8_t trig_counter_bdr : 2; - uint8_t not_used0 : 3; - uint8_t cnt_bdr_th : 2; + uint8_t not_used1 : 1; + uint8_t trig_counter_bdr : 2; + uint8_t not_used0 : 3; + uint8_t cnt_bdr_th : 2; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_counter_bdr_reg1_t; -#define LSM6DSV16X_COUNTER_BDR_REG2 0x0CU -typedef struct { +#define LSM6DSV16X_COUNTER_BDR_REG2 0x0CU +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t cnt_bdr_th : 8; + uint8_t cnt_bdr_th : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t cnt_bdr_th : 8; + uint8_t cnt_bdr_th : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_counter_bdr_reg2_t; -#define LSM6DSV16X_INT1_CTRL 0x0DU -typedef struct { -#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t int1_drdy_xl : 1; - uint8_t int1_drdy_g : 1; - uint8_t not_used0 : 1; - uint8_t int1_fifo_th : 1; - uint8_t int1_fifo_ovr : 1; - uint8_t int1_fifo_full : 1; - uint8_t int1_cnt_bdr : 1; - uint8_t not_used1 : 1; -#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t not_used1 : 1; - uint8_t int1_cnt_bdr : 1; - uint8_t int1_fifo_full : 1; - uint8_t int1_fifo_ovr : 1; - uint8_t int1_fifo_th : 1; - uint8_t not_used0 : 1; - uint8_t int1_drdy_g : 1; - uint8_t int1_drdy_xl : 1; +#define LSM6DSV16X_INT1_CTRL 0x0DU +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t int1_drdy_xl : 1; + uint8_t int1_drdy_g : 1; + uint8_t not_used0 : 1; + uint8_t int1_fifo_th : 1; + uint8_t int1_fifo_ovr : 1; + uint8_t int1_fifo_full : 1; + uint8_t int1_cnt_bdr : 1; + uint8_t not_used1 : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t not_used1 : 1; + uint8_t int1_cnt_bdr : 1; + uint8_t int1_fifo_full : 1; + uint8_t int1_fifo_ovr : 1; + uint8_t int1_fifo_th : 1; + uint8_t not_used0 : 1; + uint8_t int1_drdy_g : 1; + uint8_t int1_drdy_xl : 1; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_int1_ctrl_t; -#define LSM6DSV16X_INT2_CTRL 0x0EU -typedef struct { -#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t int2_drdy_xl : 1; - uint8_t int2_drdy_g : 1; - uint8_t int2_drdy_g_eis : 1; - uint8_t int2_fifo_th : 1; - uint8_t int2_fifo_ovr : 1; - uint8_t int2_fifo_full : 1; - uint8_t int2_cnt_bdr : 1; +#define LSM6DSV16X_INT2_CTRL 0x0EU +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t int2_drdy_xl : 1; + uint8_t int2_drdy_g : 1; + uint8_t int2_drdy_g_eis : 1; + uint8_t int2_fifo_th : 1; + uint8_t int2_fifo_ovr : 1; + uint8_t int2_fifo_full : 1; + uint8_t int2_cnt_bdr : 1; uint8_t int2_emb_func_endop : 1; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t int2_emb_func_endop : 1; - uint8_t int2_cnt_bdr : 1; - uint8_t int2_fifo_full : 1; - uint8_t int2_fifo_ovr : 1; - uint8_t int2_fifo_th : 1; - uint8_t int2_drdy_g_eis : 1; - uint8_t int2_drdy_g : 1; - uint8_t int2_drdy_xl : 1; + uint8_t int2_cnt_bdr : 1; + uint8_t int2_fifo_full : 1; + uint8_t int2_fifo_ovr : 1; + uint8_t int2_fifo_th : 1; + uint8_t int2_drdy_g_eis : 1; + uint8_t int2_drdy_g : 1; + uint8_t int2_drdy_xl : 1; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_int2_ctrl_t; -#define LSM6DSV16X_WHO_AM_I 0x0FU -typedef struct { +#define LSM6DSV16X_WHO_AM_I 0x0FU +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t id : 8; + uint8_t id : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t id : 8; + uint8_t id : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_who_am_i_t; -#define LSM6DSV16X_CTRL1 0x10U -typedef struct { +#define LSM6DSV16X_CTRL1 0x10U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t odr_xl : 4; - uint8_t op_mode_xl : 3; - uint8_t not_used0 : 1; + uint8_t odr_xl : 4; + uint8_t op_mode_xl : 3; + uint8_t not_used0 : 1; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t not_used0 : 1; - uint8_t op_mode_xl : 3; - uint8_t odr_xl : 4; + uint8_t not_used0 : 1; + uint8_t op_mode_xl : 3; + uint8_t odr_xl : 4; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_ctrl1_t; -#define LSM6DSV16X_CTRL2 0x11U -typedef struct { +#define LSM6DSV16X_CTRL2 0x11U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t odr_g : 4; - uint8_t op_mode_g : 3; - uint8_t not_used0 : 1; + uint8_t odr_g : 4; + uint8_t op_mode_g : 3; + uint8_t not_used0 : 1; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t not_used0 : 1; - uint8_t op_mode_g : 3; - uint8_t odr_g : 4; + uint8_t not_used0 : 1; + uint8_t op_mode_g : 3; + uint8_t odr_g : 4; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_ctrl2_t; -#define LSM6DSV16X_CTRL3 0x12U -typedef struct { +#define LSM6DSV16X_CTRL3 0x12U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t sw_reset : 1; - uint8_t not_used0 : 1; - uint8_t if_inc : 1; - uint8_t not_used1 : 3; - uint8_t bdu : 1; - uint8_t boot : 1; + uint8_t sw_reset : 1; + uint8_t not_used0 : 1; + uint8_t if_inc : 1; + uint8_t not_used1 : 3; + uint8_t bdu : 1; + uint8_t boot : 1; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t boot : 1; - uint8_t bdu : 1; - uint8_t not_used1 : 3; - uint8_t if_inc : 1; - uint8_t not_used0 : 1; - uint8_t sw_reset : 1; + uint8_t boot : 1; + uint8_t bdu : 1; + uint8_t not_used1 : 3; + uint8_t if_inc : 1; + uint8_t not_used0 : 1; + uint8_t sw_reset : 1; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_ctrl3_t; -#define LSM6DSV16X_CTRL4 0x13U -typedef struct { +#define LSM6DSV16X_CTRL4 0x13U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t int2_in_lh : 1; - uint8_t drdy_pulsed : 1; - uint8_t int2_drdy_temp : 1; - uint8_t drdy_mask : 1; - uint8_t int2_on_int1 : 1; - uint8_t not_used0 : 3; + uint8_t int2_in_lh : 1; + uint8_t drdy_pulsed : 1; + uint8_t int2_drdy_temp : 1; + uint8_t drdy_mask : 1; + uint8_t int2_on_int1 : 1; + uint8_t not_used0 : 3; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t not_used0 : 3; - uint8_t int2_on_int1 : 1; - uint8_t drdy_mask : 1; - uint8_t int2_drdy_temp : 1; - uint8_t drdy_pulsed : 1; - uint8_t int2_in_lh : 1; + uint8_t not_used0 : 3; + uint8_t int2_on_int1 : 1; + uint8_t drdy_mask : 1; + uint8_t int2_drdy_temp : 1; + uint8_t drdy_pulsed : 1; + uint8_t int2_in_lh : 1; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_ctrl4_t; -#define LSM6DSV16X_CTRL5 0x14U -typedef struct { +#define LSM6DSV16X_CTRL5 0x14U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t int_en_i3c : 1; - uint8_t bus_act_sel : 2; - uint8_t not_used0 : 5; + uint8_t int_en_i3c : 1; + uint8_t bus_act_sel : 2; + uint8_t not_used0 : 5; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t not_used0 : 5; - uint8_t bus_act_sel : 2; - uint8_t int_en_i3c : 1; + uint8_t not_used0 : 5; + uint8_t bus_act_sel : 2; + uint8_t int_en_i3c : 1; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_ctrl5_t; -#define LSM6DSV16X_CTRL6 0x15U -typedef struct { +#define LSM6DSV16X_CTRL6 0x15U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t fs_g : 4; - uint8_t lpf1_g_bw : 3; - uint8_t not_used0 : 1; + uint8_t fs_g : 4; + uint8_t lpf1_g_bw : 3; + uint8_t not_used0 : 1; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t not_used0 : 1; - uint8_t lpf1_g_bw : 3; - uint8_t fs_g : 4; + uint8_t not_used0 : 1; + uint8_t lpf1_g_bw : 3; + uint8_t fs_g : 4; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_ctrl6_t; -#define LSM6DSV16X_CTRL7 0x16U -typedef struct { +#define LSM6DSV16X_CTRL7 0x16U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t lpf1_g_en : 1; - uint8_t not_used0 : 3; - uint8_t ah_qvar_c_zin : 2; - uint8_t int2_drdy_ah_qvar : 1; - uint8_t ah_qvar_en : 1; + uint8_t lpf1_g_en : 1; + uint8_t not_used0 : 3; + uint8_t ah_qvar_c_zin : 2; + uint8_t int2_drdy_ah_qvar : 1; + uint8_t ah_qvar_en : 1; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t ah_qvar_en : 1; - uint8_t int2_drdy_ah_qvar : 1; - uint8_t ah_qvar_c_zin : 2; - uint8_t not_used0 : 3; - uint8_t lpf1_g_en : 1; + uint8_t ah_qvar_en : 1; + uint8_t int2_drdy_ah_qvar : 1; + uint8_t ah_qvar_c_zin : 2; + uint8_t not_used0 : 3; + uint8_t lpf1_g_en : 1; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_ctrl7_t; -#define LSM6DSV16X_CTRL8 0x17U -typedef struct { +#define LSM6DSV16X_CTRL8 0x17U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t fs_xl : 2; - uint8_t not_used0 : 1; - uint8_t xl_dualc_en : 1; - uint8_t not_used1 : 1; - uint8_t hp_lpf2_xl_bw : 3; + uint8_t fs_xl : 2; + uint8_t not_used0 : 1; + uint8_t xl_dualc_en : 1; + uint8_t not_used1 : 1; + uint8_t hp_lpf2_xl_bw : 3; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t hp_lpf2_xl_bw : 3; - uint8_t not_used1 : 1; - uint8_t xl_dualc_en : 1; - uint8_t not_used0 : 1; - uint8_t fs_xl : 2; + uint8_t hp_lpf2_xl_bw : 3; + uint8_t not_used1 : 1; + uint8_t xl_dualc_en : 1; + uint8_t not_used0 : 1; + uint8_t fs_xl : 2; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_ctrl8_t; -#define LSM6DSV16X_CTRL9 0x18U -typedef struct { -#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t usr_off_on_out : 1; - uint8_t usr_off_w : 1; - uint8_t not_used0 : 1; - uint8_t lpf2_xl_en : 1; - uint8_t hp_slope_xl_en : 1; - uint8_t xl_fastsettl_mode : 1; - uint8_t hp_ref_mode_xl : 1; - uint8_t not_used1 : 1; -#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t not_used1 : 1; - uint8_t hp_ref_mode_xl : 1; - uint8_t xl_fastsettl_mode : 1; - uint8_t hp_slope_xl_en : 1; - uint8_t lpf2_xl_en : 1; - uint8_t not_used0 : 1; - uint8_t usr_off_w : 1; - uint8_t usr_off_on_out : 1; +#define LSM6DSV16X_CTRL9 0x18U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t usr_off_on_out : 1; + uint8_t usr_off_w : 1; + uint8_t not_used0 : 1; + uint8_t lpf2_xl_en : 1; + uint8_t hp_slope_xl_en : 1; + uint8_t xl_fastsettl_mode : 1; + uint8_t hp_ref_mode_xl : 1; + uint8_t not_used1 : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t not_used1 : 1; + uint8_t hp_ref_mode_xl : 1; + uint8_t xl_fastsettl_mode : 1; + uint8_t hp_slope_xl_en : 1; + uint8_t lpf2_xl_en : 1; + uint8_t not_used0 : 1; + uint8_t usr_off_w : 1; + uint8_t usr_off_on_out : 1; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_ctrl9_t; -#define LSM6DSV16X_CTRL10 0x19U -typedef struct { +#define LSM6DSV16X_CTRL10 0x19U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t st_xl : 2; - uint8_t st_g : 2; - uint8_t not_used0 : 4; + uint8_t st_xl : 2; + uint8_t st_g : 2; + uint8_t not_used0 : 2; + uint8_t emb_func_debug : 1; + uint8_t not_used1 : 1; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t not_used0 : 4; - uint8_t st_g : 2; - uint8_t st_xl : 2; + uint8_t not_used1 : 1; + uint8_t emb_func_debug : 1; + uint8_t not_used0 : 2; + uint8_t st_g : 2; + uint8_t st_xl : 2; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_ctrl10_t; -#define LSM6DSV16X_CTRL_STATUS 0x1AU -typedef struct { +#define LSM6DSV16X_CTRL_STATUS 0x1AU +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t not_used1 : 5; - uint8_t fsm_wr_ctrl_status : 1; - uint8_t not_used0 : 2; + uint8_t not_used0 : 2; + uint8_t fsm_wr_ctrl_status : 1; + uint8_t not_used1 : 5; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t not_used0 : 2; - uint8_t fsm_wr_ctrl_status : 1; - uint8_t not_used1 : 5; + uint8_t not_used1 : 5; + uint8_t fsm_wr_ctrl_status : 1; + uint8_t not_used0 : 2; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_ctrl_status_t; -#define LSM6DSV16X_FIFO_STATUS1 0x1BU -typedef struct { +#define LSM6DSV16X_FIFO_STATUS1 0x1BU +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t diff_fifo : 8; + uint8_t diff_fifo : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t diff_fifo : 8; + uint8_t diff_fifo : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_fifo_status1_t; -#define LSM6DSV16X_FIFO_STATUS2 0x1CU -typedef struct { -#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t diff_fifo : 1; - uint8_t not_used0 : 2; - uint8_t fifo_ovr_latched : 1; - uint8_t counter_bdr_ia : 1; - uint8_t fifo_full_ia : 1; - uint8_t fifo_ovr_ia : 1; - uint8_t fifo_wtm_ia : 1; -#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t fifo_wtm_ia : 1; - uint8_t fifo_ovr_ia : 1; - uint8_t fifo_full_ia : 1; - uint8_t counter_bdr_ia : 1; - uint8_t fifo_ovr_latched : 1; - uint8_t not_used0 : 2; - uint8_t diff_fifo : 1; +#define LSM6DSV16X_FIFO_STATUS2 0x1CU +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t diff_fifo : 1; + uint8_t not_used0 : 2; + uint8_t fifo_ovr_latched : 1; + uint8_t counter_bdr_ia : 1; + uint8_t fifo_full_ia : 1; + uint8_t fifo_ovr_ia : 1; + uint8_t fifo_wtm_ia : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t fifo_wtm_ia : 1; + uint8_t fifo_ovr_ia : 1; + uint8_t fifo_full_ia : 1; + uint8_t counter_bdr_ia : 1; + uint8_t fifo_ovr_latched : 1; + uint8_t not_used0 : 2; + uint8_t diff_fifo : 1; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_fifo_status2_t; -#define LSM6DSV16X_ALL_INT_SRC 0x1DU -typedef struct { -#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t ff_ia : 1; - uint8_t wu_ia : 1; - uint8_t tap_ia : 1; - uint8_t not_used0 : 1; - uint8_t d6d_ia : 1; - uint8_t sleep_change_ia : 1; - uint8_t shub_ia : 1; - uint8_t emb_func_ia : 1; -#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t emb_func_ia : 1; - uint8_t shub_ia : 1; - uint8_t sleep_change_ia : 1; - uint8_t d6d_ia : 1; - uint8_t not_used0 : 1; - uint8_t tap_ia : 1; - uint8_t wu_ia : 1; - uint8_t ff_ia : 1; +#define LSM6DSV16X_ALL_INT_SRC 0x1DU +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t ff_ia : 1; + uint8_t wu_ia : 1; + uint8_t tap_ia : 1; + uint8_t not_used0 : 1; + uint8_t d6d_ia : 1; + uint8_t sleep_change_ia : 1; + uint8_t shub_ia : 1; + uint8_t emb_func_ia : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t emb_func_ia : 1; + uint8_t shub_ia : 1; + uint8_t sleep_change_ia : 1; + uint8_t d6d_ia : 1; + uint8_t not_used0 : 1; + uint8_t tap_ia : 1; + uint8_t wu_ia : 1; + uint8_t ff_ia : 1; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_all_int_src_t; -#define LSM6DSV16X_STATUS_REG 0x1EU -typedef struct { -#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t xlda : 1; - uint8_t gda : 1; - uint8_t tda : 1; - uint8_t ah_qvarda : 1; - uint8_t gda_eis : 1; - uint8_t ois_drdy : 1; - uint8_t not_used0 : 1; - uint8_t timestamp_endcount : 1; -#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t timestamp_endcount : 1; - uint8_t not_used0 : 1; - uint8_t ois_drdy : 1; - uint8_t gda_eis : 1; - uint8_t ah_qvarda : 1; - uint8_t tda : 1; - uint8_t gda : 1; - uint8_t xlda : 1; +#define LSM6DSV16X_STATUS_REG 0x1EU +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t xlda : 1; + uint8_t gda : 1; + uint8_t tda : 1; + uint8_t ah_qvarda : 1; + uint8_t gda_eis : 1; + uint8_t ois_drdy : 1; + uint8_t not_used0 : 1; + uint8_t timestamp_endcount : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t timestamp_endcount : 1; + uint8_t not_used0 : 1; + uint8_t ois_drdy : 1; + uint8_t gda_eis : 1; + uint8_t ah_qvarda : 1; + uint8_t tda : 1; + uint8_t gda : 1; + uint8_t xlda : 1; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_status_reg_t; -#define LSM6DSV16X_OUT_TEMP_L 0x20U -typedef struct { +#define LSM6DSV16X_OUT_TEMP_L 0x20U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t temp : 8; + uint8_t temp : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t temp : 8; + uint8_t temp : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_out_temp_l_t; -#define LSM6DSV16X_OUT_TEMP_H 0x21U -typedef struct { +#define LSM6DSV16X_OUT_TEMP_H 0x21U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t temp : 8; + uint8_t temp : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t temp : 8; + uint8_t temp : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_out_temp_h_t; -#define LSM6DSV16X_OUTX_L_G 0x22U -typedef struct { +#define LSM6DSV16X_OUTX_L_G 0x22U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t outx_g : 8; + uint8_t outx_g : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t outx_g : 8; + uint8_t outx_g : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_outx_l_g_t; -#define LSM6DSV16X_OUTX_H_G 0x23U -typedef struct { +#define LSM6DSV16X_OUTX_H_G 0x23U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t outx_g : 8; + uint8_t outx_g : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t outx_g : 8; + uint8_t outx_g : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_outx_h_g_t; -#define LSM6DSV16X_OUTY_L_G 0x24U -typedef struct { +#define LSM6DSV16X_OUTY_L_G 0x24U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t outy_g : 8; + uint8_t outy_g : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t outy_g : 8; + uint8_t outy_g : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_outy_l_g_t; -#define LSM6DSV16X_OUTY_H_G 0x25U -typedef struct { +#define LSM6DSV16X_OUTY_H_G 0x25U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t outy_g : 8; + uint8_t outy_g : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t outy_g : 8; + uint8_t outy_g : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_outy_h_g_t; -#define LSM6DSV16X_OUTZ_L_G 0x26U -typedef struct { +#define LSM6DSV16X_OUTZ_L_G 0x26U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t outz_g : 8; + uint8_t outz_g : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t outz_g : 8; + uint8_t outz_g : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_outz_l_g_t; -#define LSM6DSV16X_OUTZ_H_G 0x27U -typedef struct { +#define LSM6DSV16X_OUTZ_H_G 0x27U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t outz_g : 8; + uint8_t outz_g : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t outz_g : 8; + uint8_t outz_g : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_outz_h_g_t; -#define LSM6DSV16X_OUTX_L_A 0x28U -typedef struct { +#define LSM6DSV16X_OUTX_L_A 0x28U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t outx_a : 8; + uint8_t outx_a : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t outx_a : 8; + uint8_t outx_a : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_outx_l_a_t; -#define LSM6DSV16X_OUTX_H_A 0x29U -typedef struct { +#define LSM6DSV16X_OUTX_H_A 0x29U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t outx_a : 8; + uint8_t outx_a : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t outx_a : 8; + uint8_t outx_a : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_outx_h_a_t; -#define LSM6DSV16X_OUTY_L_A 0x2AU -typedef struct { +#define LSM6DSV16X_OUTY_L_A 0x2AU +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t outy_a : 8; + uint8_t outy_a : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t outy_a : 8; + uint8_t outy_a : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_outy_l_a_t; -#define LSM6DSV16X_OUTY_H_A 0x2BU -typedef struct { +#define LSM6DSV16X_OUTY_H_A 0x2BU +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t outy_a : 8; + uint8_t outy_a : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t outy_a : 8; + uint8_t outy_a : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_outy_h_a_t; -#define LSM6DSV16X_OUTZ_L_A 0x2CU -typedef struct { +#define LSM6DSV16X_OUTZ_L_A 0x2CU +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t outz_a : 8; + uint8_t outz_a : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t outz_a : 8; + uint8_t outz_a : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_outz_l_a_t; -#define LSM6DSV16X_OUTZ_H_A 0x2DU -typedef struct { +#define LSM6DSV16X_OUTZ_H_A 0x2DU +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t outz_a : 8; + uint8_t outz_a : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t outz_a : 8; + uint8_t outz_a : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_outz_h_a_t; -#define LSM6DSV16X_UI_OUTX_L_G_OIS_EIS 0x2EU -typedef struct { +#define LSM6DSV16X_UI_OUTX_L_G_OIS_EIS 0x2EU +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t ui_outx_g_ois_eis : 8; + uint8_t ui_outx_g_ois_eis : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t ui_outx_g_ois_eis : 8; + uint8_t ui_outx_g_ois_eis : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_ui_outx_l_g_ois_eis_t; -#define LSM6DSV16X_UI_OUTX_H_G_OIS_EIS 0x2FU -typedef struct { +#define LSM6DSV16X_UI_OUTX_H_G_OIS_EIS 0x2FU +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t ui_outx_g_ois_eis : 8; + uint8_t ui_outx_g_ois_eis : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t ui_outx_g_ois_eis : 8; + uint8_t ui_outx_g_ois_eis : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_ui_outx_h_g_ois_eis_t; -#define LSM6DSV16X_UI_OUTY_L_G_OIS_EIS 0x30U -typedef struct { +#define LSM6DSV16X_UI_OUTY_L_G_OIS_EIS 0x30U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t ui_outy_g_ois_eis : 8; + uint8_t ui_outy_g_ois_eis : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t ui_outy_g_ois_eis : 8; + uint8_t ui_outy_g_ois_eis : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_ui_outy_l_g_ois_eis_t; -#define LSM6DSV16X_UI_OUTY_H_G_OIS_EIS 0x31U -typedef struct { +#define LSM6DSV16X_UI_OUTY_H_G_OIS_EIS 0x31U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t ui_outy_g_ois_eis : 8; + uint8_t ui_outy_g_ois_eis : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t ui_outy_g_ois_eis : 8; + uint8_t ui_outy_g_ois_eis : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_ui_outy_h_g_ois_eis_t; -#define LSM6DSV16X_UI_OUTZ_L_G_OIS_EIS 0x32U -typedef struct { +#define LSM6DSV16X_UI_OUTZ_L_G_OIS_EIS 0x32U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t ui_outz_g_ois_eis : 8; + uint8_t ui_outz_g_ois_eis : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t ui_outz_g_ois_eis : 8; + uint8_t ui_outz_g_ois_eis : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_ui_outz_l_g_ois_eis_t; -#define LSM6DSV16X_UI_OUTZ_H_G_OIS_EIS 0x33U -typedef struct { +#define LSM6DSV16X_UI_OUTZ_H_G_OIS_EIS 0x33U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t ui_outz_g_ois_eis : 8; + uint8_t ui_outz_g_ois_eis : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t ui_outz_g_ois_eis : 8; + uint8_t ui_outz_g_ois_eis : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_ui_outz_h_g_ois_eis_t; -#define LSM6DSV16X_UI_OUTX_L_A_OIS_DUALC 0x34U -typedef struct { +#define LSM6DSV16X_UI_OUTX_L_A_OIS_DUALC 0x34U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN uint8_t ui_outx_a_ois_dualc : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN @@ -842,8 +879,9 @@ typedef struct { #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_ui_outx_l_a_ois_dualc_t; -#define LSM6DSV16X_UI_OUTX_H_A_OIS_DUALC 0x35U -typedef struct { +#define LSM6DSV16X_UI_OUTX_H_A_OIS_DUALC 0x35U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN uint8_t ui_outx_a_ois_dualc : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN @@ -851,8 +889,9 @@ typedef struct { #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_ui_outx_h_a_ois_dualc_t; -#define LSM6DSV16X_UI_OUTY_L_A_OIS_DUALC 0x36U -typedef struct { +#define LSM6DSV16X_UI_OUTY_L_A_OIS_DUALC 0x36U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN uint8_t ui_outy_a_ois_dualc : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN @@ -860,8 +899,9 @@ typedef struct { #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_ui_outy_l_a_ois_dualc_t; -#define LSM6DSV16X_UI_OUTY_H_A_OIS_DUALC 0x37U -typedef struct { +#define LSM6DSV16X_UI_OUTY_H_A_OIS_DUALC 0x37U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN uint8_t ui_outy_a_ois_dualc : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN @@ -869,8 +909,9 @@ typedef struct { #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_ui_outy_h_a_ois_dualc_t; -#define LSM6DSV16X_UI_OUTZ_L_A_OIS_DUALC 0x38U -typedef struct { +#define LSM6DSV16X_UI_OUTZ_L_A_OIS_DUALC 0x38U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN uint8_t ui_outz_a_ois_dualc : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN @@ -878,8 +919,9 @@ typedef struct { #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_ui_outz_l_a_ois_dualc_t; -#define LSM6DSV16X_UI_OUTZ_H_A_OIS_DUALC 0x39U -typedef struct { +#define LSM6DSV16X_UI_OUTZ_H_A_OIS_DUALC 0x39U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN uint8_t ui_outz_a_ois_dualc : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN @@ -887,731 +929,777 @@ typedef struct { #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_ui_outz_h_a_ois_dualc_t; -#define LSM6DSV16X_AH_QVAR_OUT_L 0x3AU -typedef struct { +#define LSM6DSV16X_AH_QVAR_OUT_L 0x3AU +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t ah_qvar : 8; + uint8_t ah_qvar : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t ah_qvar : 8; + uint8_t ah_qvar : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_ah_qvar_out_l_t; -#define LSM6DSV16X_AH_QVAR_OUT_H 0x3BU -typedef struct { +#define LSM6DSV16X_AH_QVAR_OUT_H 0x3BU +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t ah_qvar : 8; + uint8_t ah_qvar : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t ah_qvar : 8; + uint8_t ah_qvar : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_ah_qvar_out_h_t; -#define LSM6DSV16X_TIMESTAMP0 0x40U -typedef struct { +#define LSM6DSV16X_TIMESTAMP0 0x40U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t timestamp : 8; + uint8_t timestamp : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t timestamp : 8; + uint8_t timestamp : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_timestamp0_t; -#define LSM6DSV16X_TIMESTAMP1 0x41U -typedef struct { +#define LSM6DSV16X_TIMESTAMP1 0x41U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t timestamp : 8; + uint8_t timestamp : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t timestamp : 8; + uint8_t timestamp : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_timestamp1_t; -#define LSM6DSV16X_TIMESTAMP2 0x42U -typedef struct { +#define LSM6DSV16X_TIMESTAMP2 0x42U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t timestamp : 8; + uint8_t timestamp : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t timestamp : 8; + uint8_t timestamp : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_timestamp2_t; -#define LSM6DSV16X_TIMESTAMP3 0x43U -typedef struct { +#define LSM6DSV16X_TIMESTAMP3 0x43U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t timestamp : 8; + uint8_t timestamp : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t timestamp : 8; + uint8_t timestamp : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_timestamp3_t; -#define LSM6DSV16X_UI_STATUS_REG_OIS 0x44U -typedef struct { +#define LSM6DSV16X_UI_STATUS_REG_OIS 0x44U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t xlda_ois : 1; - uint8_t gda_ois : 1; - uint8_t gyro_settling : 1; - uint8_t not_used0 : 5; + uint8_t xlda_ois : 1; + uint8_t gda_ois : 1; + uint8_t gyro_settling : 1; + uint8_t not_used0 : 5; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t not_used0 : 5; - uint8_t gyro_settling : 1; - uint8_t gda_ois : 1; - uint8_t xlda_ois : 1; + uint8_t not_used0 : 5; + uint8_t gyro_settling : 1; + uint8_t gda_ois : 1; + uint8_t xlda_ois : 1; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_ui_status_reg_ois_t; -#define LSM6DSV16X_WAKE_UP_SRC 0x45U -typedef struct { -#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t z_wu : 1; - uint8_t y_wu : 1; - uint8_t x_wu : 1; - uint8_t wu_ia : 1; - uint8_t sleep_state : 1; - uint8_t ff_ia : 1; - uint8_t sleep_change_ia : 1; - uint8_t not_used0 : 1; -#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t not_used0 : 1; - uint8_t sleep_change_ia : 1; - uint8_t ff_ia : 1; - uint8_t sleep_state : 1; - uint8_t wu_ia : 1; - uint8_t x_wu : 1; - uint8_t y_wu : 1; - uint8_t z_wu : 1; +#define LSM6DSV16X_WAKE_UP_SRC 0x45U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t z_wu : 1; + uint8_t y_wu : 1; + uint8_t x_wu : 1; + uint8_t wu_ia : 1; + uint8_t sleep_state : 1; + uint8_t ff_ia : 1; + uint8_t sleep_change_ia : 1; + uint8_t not_used0 : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t not_used0 : 1; + uint8_t sleep_change_ia : 1; + uint8_t ff_ia : 1; + uint8_t sleep_state : 1; + uint8_t wu_ia : 1; + uint8_t x_wu : 1; + uint8_t y_wu : 1; + uint8_t z_wu : 1; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_wake_up_src_t; -#define LSM6DSV16X_TAP_SRC 0x46U -typedef struct { -#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t z_tap : 1; - uint8_t y_tap : 1; - uint8_t x_tap : 1; - uint8_t tap_sign : 1; - uint8_t double_tap : 1; - uint8_t single_tap : 1; - uint8_t tap_ia : 1; - uint8_t not_used0 : 1; -#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t not_used0 : 1; - uint8_t tap_ia : 1; - uint8_t single_tap : 1; - uint8_t double_tap : 1; - uint8_t tap_sign : 1; - uint8_t x_tap : 1; - uint8_t y_tap : 1; - uint8_t z_tap : 1; +#define LSM6DSV16X_TAP_SRC 0x46U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t z_tap : 1; + uint8_t y_tap : 1; + uint8_t x_tap : 1; + uint8_t tap_sign : 1; + uint8_t double_tap : 1; + uint8_t single_tap : 1; + uint8_t tap_ia : 1; + uint8_t not_used0 : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t not_used0 : 1; + uint8_t tap_ia : 1; + uint8_t single_tap : 1; + uint8_t double_tap : 1; + uint8_t tap_sign : 1; + uint8_t x_tap : 1; + uint8_t y_tap : 1; + uint8_t z_tap : 1; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_tap_src_t; -#define LSM6DSV16X_D6D_SRC 0x47U -typedef struct { -#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t xl : 1; - uint8_t xh : 1; - uint8_t yl : 1; - uint8_t yh : 1; - uint8_t zl : 1; - uint8_t zh : 1; - uint8_t d6d_ia : 1; - uint8_t not_used0 : 1; -#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t not_used0 : 1; - uint8_t d6d_ia : 1; - uint8_t zh : 1; - uint8_t zl : 1; - uint8_t yh : 1; - uint8_t yl : 1; - uint8_t xh : 1; - uint8_t xl : 1; +#define LSM6DSV16X_D6D_SRC 0x47U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t xl : 1; + uint8_t xh : 1; + uint8_t yl : 1; + uint8_t yh : 1; + uint8_t zl : 1; + uint8_t zh : 1; + uint8_t d6d_ia : 1; + uint8_t not_used0 : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t not_used0 : 1; + uint8_t d6d_ia : 1; + uint8_t zh : 1; + uint8_t zl : 1; + uint8_t yh : 1; + uint8_t yl : 1; + uint8_t xh : 1; + uint8_t xl : 1; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_d6d_src_t; -#define LSM6DSV16X_STATUS_MASTER_MAINPAGE 0x48U -typedef struct { -#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t sens_hub_endop : 1; - uint8_t not_used0 : 2; - uint8_t slave0_nack : 1; - uint8_t slave1_nack : 1; - uint8_t slave2_nack : 1; - uint8_t slave3_nack : 1; - uint8_t wr_once_done : 1; -#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t wr_once_done : 1; - uint8_t slave3_nack : 1; - uint8_t slave2_nack : 1; - uint8_t slave1_nack : 1; - uint8_t slave0_nack : 1; - uint8_t not_used0 : 2; - uint8_t sens_hub_endop : 1; +#define LSM6DSV16X_STATUS_MASTER_MAINPAGE 0x48U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t sens_hub_endop : 1; + uint8_t not_used0 : 2; + uint8_t slave0_nack : 1; + uint8_t slave1_nack : 1; + uint8_t slave2_nack : 1; + uint8_t slave3_nack : 1; + uint8_t wr_once_done : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t wr_once_done : 1; + uint8_t slave3_nack : 1; + uint8_t slave2_nack : 1; + uint8_t slave1_nack : 1; + uint8_t slave0_nack : 1; + uint8_t not_used0 : 2; + uint8_t sens_hub_endop : 1; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_status_master_mainpage_t; #define LSM6DSV16X_EMB_FUNC_STATUS_MAINPAGE 0x49U -typedef struct { -#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t not_used0 : 3; - uint8_t is_step_det : 1; - uint8_t is_tilt : 1; - uint8_t is_sigmot : 1; - uint8_t not_used1 : 1; - uint8_t is_fsm_lc : 1; -#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t is_fsm_lc : 1; - uint8_t not_used1 : 1; - uint8_t is_sigmot : 1; - uint8_t is_tilt : 1; - uint8_t is_step_det : 1; - uint8_t not_used0 : 3; +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t not_used0 : 3; + uint8_t is_step_det : 1; + uint8_t is_tilt : 1; + uint8_t is_sigmot : 1; + uint8_t not_used1 : 1; + uint8_t is_fsm_lc : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t is_fsm_lc : 1; + uint8_t not_used1 : 1; + uint8_t is_sigmot : 1; + uint8_t is_tilt : 1; + uint8_t is_step_det : 1; + uint8_t not_used0 : 3; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_emb_func_status_mainpage_t; -#define LSM6DSV16X_FSM_STATUS_MAINPAGE 0x4AU -typedef struct { -#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t is_fsm1 : 1; - uint8_t is_fsm2 : 1; - uint8_t is_fsm3 : 1; - uint8_t is_fsm4 : 1; - uint8_t is_fsm5 : 1; - uint8_t is_fsm6 : 1; - uint8_t is_fsm7 : 1; - uint8_t is_fsm8 : 1; -#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t is_fsm8 : 1; - uint8_t is_fsm7 : 1; - uint8_t is_fsm6 : 1; - uint8_t is_fsm5 : 1; - uint8_t is_fsm4 : 1; - uint8_t is_fsm3 : 1; - uint8_t is_fsm2 : 1; - uint8_t is_fsm1 : 1; +#define LSM6DSV16X_FSM_STATUS_MAINPAGE 0x4AU +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t is_fsm1 : 1; + uint8_t is_fsm2 : 1; + uint8_t is_fsm3 : 1; + uint8_t is_fsm4 : 1; + uint8_t is_fsm5 : 1; + uint8_t is_fsm6 : 1; + uint8_t is_fsm7 : 1; + uint8_t is_fsm8 : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t is_fsm8 : 1; + uint8_t is_fsm7 : 1; + uint8_t is_fsm6 : 1; + uint8_t is_fsm5 : 1; + uint8_t is_fsm4 : 1; + uint8_t is_fsm3 : 1; + uint8_t is_fsm2 : 1; + uint8_t is_fsm1 : 1; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_fsm_status_mainpage_t; -#define LSM6DSV16X_MLC_STATUS_MAINPAGE 0x4BU -typedef struct { +#define LSM6DSV16X_MLC_STATUS_MAINPAGE 0x4BU +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t is_mlc1 : 1; - uint8_t is_mlc2 : 1; - uint8_t is_mlc3 : 1; - uint8_t is_mlc4 : 1; - uint8_t not_used0 : 4; + uint8_t is_mlc1 : 1; + uint8_t is_mlc2 : 1; + uint8_t is_mlc3 : 1; + uint8_t is_mlc4 : 1; + uint8_t not_used0 : 4; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t not_used0 : 4; - uint8_t is_mlc4 : 1; - uint8_t is_mlc3 : 1; - uint8_t is_mlc2 : 1; - uint8_t is_mlc1 : 1; + uint8_t not_used0 : 4; + uint8_t is_mlc4 : 1; + uint8_t is_mlc3 : 1; + uint8_t is_mlc2 : 1; + uint8_t is_mlc1 : 1; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_mlc_status_mainpage_t; -#define LSM6DSV16X_INTERNAL_FREQ 0x4FU -typedef struct { +#define LSM6DSV16X_INTERNAL_FREQ 0x4FU +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t freq_fine : 8; + uint8_t freq_fine : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t freq_fine : 8; + uint8_t freq_fine : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_internal_freq_t; -#define LSM6DSV16X_FUNCTIONS_ENABLE 0x50U -typedef struct { +#define LSM6DSV16X_FUNCTIONS_ENABLE 0x50U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t inact_en : 2; - uint8_t not_used0 : 1; + uint8_t inact_en : 2; + uint8_t not_used0 : 1; uint8_t dis_rst_lir_all_int : 1; - uint8_t not_used1 : 2; - uint8_t timestamp_en : 1; - uint8_t interrupts_enable : 1; + uint8_t not_used1 : 2; + uint8_t timestamp_en : 1; + uint8_t interrupts_enable : 1; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t interrupts_enable : 1; - uint8_t timestamp_en : 1; - uint8_t not_used1 : 2; + uint8_t interrupts_enable : 1; + uint8_t timestamp_en : 1; + uint8_t not_used1 : 2; uint8_t dis_rst_lir_all_int : 1; - uint8_t not_used0 : 1; - uint8_t inact_en : 2; + uint8_t not_used0 : 1; + uint8_t inact_en : 2; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_functions_enable_t; -#define LSM6DSV16X_DEN 0x51U -typedef struct { -#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t den_xl_g : 1; - uint8_t den_z : 1; - uint8_t den_y : 1; - uint8_t den_x : 1; - uint8_t den_xl_en : 1; - uint8_t lvl2_en : 1; - uint8_t lvl1_en : 1; - uint8_t not_used0 : 1; -#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t not_used0 : 1; - uint8_t lvl1_en : 1; - uint8_t lvl2_en : 1; - uint8_t den_xl_en : 1; - uint8_t den_x : 1; - uint8_t den_y : 1; - uint8_t den_z : 1; - uint8_t den_xl_g : 1; +#define LSM6DSV16X_DEN 0x51U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t den_xl_g : 1; + uint8_t den_z : 1; + uint8_t den_y : 1; + uint8_t den_x : 1; + uint8_t den_xl_en : 1; + uint8_t lvl2_en : 1; + uint8_t lvl1_en : 1; + uint8_t not_used0 : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t not_used0 : 1; + uint8_t lvl1_en : 1; + uint8_t lvl2_en : 1; + uint8_t den_xl_en : 1; + uint8_t den_x : 1; + uint8_t den_y : 1; + uint8_t den_z : 1; + uint8_t den_xl_g : 1; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_den_t; -#define LSM6DSV16X_INACTIVITY_DUR 0x54U -typedef struct { +#define LSM6DSV16X_INACTIVITY_DUR 0x54U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t inact_dur : 2; - uint8_t xl_inact_odr : 2; - uint8_t wu_inact_ths_w : 3; + uint8_t inact_dur : 2; + uint8_t xl_inact_odr : 2; + uint8_t wu_inact_ths_w : 3; uint8_t sleep_status_on_int : 1; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t sleep_status_on_int : 1; - uint8_t wu_inact_ths_w : 3; - uint8_t xl_inact_odr : 2; - uint8_t inact_dur : 2; + uint8_t wu_inact_ths_w : 3; + uint8_t xl_inact_odr : 2; + uint8_t inact_dur : 2; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_inactivity_dur_t; -#define LSM6DSV16X_INACTIVITY_THS 0x55U -typedef struct { +#define LSM6DSV16X_INACTIVITY_THS 0x55U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t inact_ths : 6; - uint8_t not_used0 : 2; + uint8_t inact_ths : 6; + uint8_t not_used0 : 2; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t not_used0 : 2; - uint8_t inact_ths : 6; + uint8_t not_used0 : 2; + uint8_t inact_ths : 6; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_inactivity_ths_t; -#define LSM6DSV16X_TAP_CFG0 0x56U -typedef struct { -#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t lir : 1; - uint8_t tap_z_en : 1; - uint8_t tap_y_en : 1; - uint8_t tap_x_en : 1; - uint8_t slope_fds : 1; - uint8_t not_used0 : 1; - uint8_t low_pass_on_6d : 1; - uint8_t not_used1 : 1; -#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t not_used1 : 1; - uint8_t low_pass_on_6d : 1; - uint8_t not_used0 : 1; - uint8_t slope_fds : 1; - uint8_t tap_x_en : 1; - uint8_t tap_y_en : 1; - uint8_t tap_z_en : 1; - uint8_t lir : 1; +#define LSM6DSV16X_TAP_CFG0 0x56U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t lir : 1; + uint8_t tap_z_en : 1; + uint8_t tap_y_en : 1; + uint8_t tap_x_en : 1; + uint8_t slope_fds : 1; + uint8_t hw_func_mask_xl_settl : 1; + uint8_t low_pass_on_6d : 1; + uint8_t not_used1 : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t not_used1 : 1; + uint8_t low_pass_on_6d : 1; + uint8_t hw_func_mask_xl_settl : 1; + uint8_t slope_fds : 1; + uint8_t tap_x_en : 1; + uint8_t tap_y_en : 1; + uint8_t tap_z_en : 1; + uint8_t lir : 1; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_tap_cfg0_t; -#define LSM6DSV16X_TAP_CFG1 0x57U -typedef struct { +#define LSM6DSV16X_TAP_CFG1 0x57U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t tap_ths_x : 5; - uint8_t tap_priority : 3; + uint8_t tap_ths_x : 5; + uint8_t tap_priority : 3; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t tap_priority : 3; - uint8_t tap_ths_x : 5; + uint8_t tap_priority : 3; + uint8_t tap_ths_x : 5; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_tap_cfg1_t; -#define LSM6DSV16X_TAP_CFG2 0x58U -typedef struct { +#define LSM6DSV16X_TAP_CFG2 0x58U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t tap_ths_y : 5; - uint8_t not_used0 : 3; + uint8_t tap_ths_y : 5; + uint8_t not_used0 : 3; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t not_used0 : 3; - uint8_t tap_ths_y : 5; + uint8_t not_used0 : 3; + uint8_t tap_ths_y : 5; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_tap_cfg2_t; -#define LSM6DSV16X_TAP_THS_6D 0x59U -typedef struct { +#define LSM6DSV16X_TAP_THS_6D 0x59U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t tap_ths_z : 5; - uint8_t sixd_ths : 2; - uint8_t d4d_en : 1; + uint8_t tap_ths_z : 5; + uint8_t sixd_ths : 2; + uint8_t d4d_en : 1; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t d4d_en : 1; - uint8_t sixd_ths : 2; - uint8_t tap_ths_z : 5; + uint8_t d4d_en : 1; + uint8_t sixd_ths : 2; + uint8_t tap_ths_z : 5; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_tap_ths_6d_t; -#define LSM6DSV16X_INT_DUR2 0x5AU -typedef struct { +#define LSM6DSV16X_TAP_DUR 0x5AU +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t shock : 2; - uint8_t quiet : 2; - uint8_t dur : 4; + uint8_t shock : 2; + uint8_t quiet : 2; + uint8_t dur : 4; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t dur : 4; - uint8_t quiet : 2; - uint8_t shock : 2; + uint8_t dur : 4; + uint8_t quiet : 2; + uint8_t shock : 2; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_int_dur2_t; +} lsm6dsv16x_tap_dur_t; -#define LSM6DSV16X_WAKE_UP_THS 0x5BU -typedef struct { +#define LSM6DSV16X_WAKE_UP_THS 0x5BU +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t wk_ths : 6; - uint8_t usr_off_on_wu : 1; - uint8_t single_double_tap : 1; + uint8_t wk_ths : 6; + uint8_t usr_off_on_wu : 1; + uint8_t single_double_tap : 1; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t single_double_tap : 1; - uint8_t usr_off_on_wu : 1; - uint8_t wk_ths : 6; + uint8_t single_double_tap : 1; + uint8_t usr_off_on_wu : 1; + uint8_t wk_ths : 6; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_wake_up_ths_t; -#define LSM6DSV16X_WAKE_UP_DUR 0x5CU -typedef struct { +#define LSM6DSV16X_WAKE_UP_DUR 0x5CU +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t sleep_dur : 4; - uint8_t not_used0 : 1; - uint8_t wake_dur : 2; - uint8_t ff_dur : 1; + uint8_t sleep_dur : 4; + uint8_t not_used0 : 1; + uint8_t wake_dur : 2; + uint8_t ff_dur : 1; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t ff_dur : 1; - uint8_t wake_dur : 2; - uint8_t not_used0 : 1; - uint8_t sleep_dur : 4; + uint8_t ff_dur : 1; + uint8_t wake_dur : 2; + uint8_t not_used0 : 1; + uint8_t sleep_dur : 4; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_wake_up_dur_t; -#define LSM6DSV16X_FREE_FALL 0x5DU -typedef struct { +#define LSM6DSV16X_FREE_FALL 0x5DU +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t ff_ths : 3; - uint8_t ff_dur : 5; + uint8_t ff_ths : 3; + uint8_t ff_dur : 5; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t ff_dur : 5; - uint8_t ff_ths : 3; + uint8_t ff_dur : 5; + uint8_t ff_ths : 3; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_free_fall_t; -#define LSM6DSV16X_MD1_CFG 0x5EU -typedef struct { -#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t int1_shub : 1; - uint8_t int1_emb_func : 1; - uint8_t int1_6d : 1; - uint8_t int1_double_tap : 1; - uint8_t int1_ff : 1; - uint8_t int1_wu : 1; - uint8_t int1_single_tap : 1; - uint8_t int1_sleep_change : 1; -#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t int1_sleep_change : 1; - uint8_t int1_single_tap : 1; - uint8_t int1_wu : 1; - uint8_t int1_ff : 1; - uint8_t int1_double_tap : 1; - uint8_t int1_6d : 1; - uint8_t int1_emb_func : 1; - uint8_t int1_shub : 1; +#define LSM6DSV16X_MD1_CFG 0x5EU +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t int1_shub : 1; + uint8_t int1_emb_func : 1; + uint8_t int1_6d : 1; + uint8_t int1_double_tap : 1; + uint8_t int1_ff : 1; + uint8_t int1_wu : 1; + uint8_t int1_single_tap : 1; + uint8_t int1_sleep_change : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t int1_sleep_change : 1; + uint8_t int1_single_tap : 1; + uint8_t int1_wu : 1; + uint8_t int1_ff : 1; + uint8_t int1_double_tap : 1; + uint8_t int1_6d : 1; + uint8_t int1_emb_func : 1; + uint8_t int1_shub : 1; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_md1_cfg_t; -#define LSM6DSV16X_MD2_CFG 0x5FU -typedef struct { -#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t int2_timestamp : 1; - uint8_t int2_emb_func : 1; - uint8_t int2_6d : 1; - uint8_t int2_double_tap : 1; - uint8_t int2_ff : 1; - uint8_t int2_wu : 1; - uint8_t int2_single_tap : 1; - uint8_t int2_sleep_change : 1; -#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t int2_sleep_change : 1; - uint8_t int2_single_tap : 1; - uint8_t int2_wu : 1; - uint8_t int2_ff : 1; - uint8_t int2_double_tap : 1; - uint8_t int2_6d : 1; - uint8_t int2_emb_func : 1; - uint8_t int2_timestamp : 1; +#define LSM6DSV16X_MD2_CFG 0x5FU +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t int2_timestamp : 1; + uint8_t int2_emb_func : 1; + uint8_t int2_6d : 1; + uint8_t int2_double_tap : 1; + uint8_t int2_ff : 1; + uint8_t int2_wu : 1; + uint8_t int2_single_tap : 1; + uint8_t int2_sleep_change : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t int2_sleep_change : 1; + uint8_t int2_single_tap : 1; + uint8_t int2_wu : 1; + uint8_t int2_ff : 1; + uint8_t int2_double_tap : 1; + uint8_t int2_6d : 1; + uint8_t int2_emb_func : 1; + uint8_t int2_timestamp : 1; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_md2_cfg_t; -#define LSM6DSV16X_S4S_ST_CMD_CODE 0x60U -typedef struct { +#define LSM6DSV16X_HAODR_CFG 0x62U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t st_cmd_code : 8; + uint8_t haodr_sel : 2; + uint8_t not_used0 : 6; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t st_cmd_code : 8; + uint8_t not_used0 : 6; + uint8_t haodr_sel : 2; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_s4s_st_cmd_code_t; +} lsm6dsv16x_haodr_cfg_t; -#define LSM6DSV16X_S4S_DT_REG 0x61U -typedef struct { +#define LSM6DSV16X_EMB_FUNC_CFG 0x63U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t dt : 8; -#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t dt : 8; -#endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_s4s_dt_reg_t; - -#define LSM6DSV16X_EMB_FUNC_CFG 0x63U -typedef struct { -#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t not_used0 : 3; - uint8_t emb_func_disable : 1; - uint8_t emb_func_irq_mask_xl_settl : 1; + uint8_t not_used0 : 3; + uint8_t emb_func_disable : 1; + uint8_t emb_func_irq_mask_xl_settl : 1; uint8_t emb_func_irq_mask_g_settl : 1; - uint8_t not_used1 : 2; + uint8_t not_used1 : 2; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t not_used1 : 2; + uint8_t not_used1 : 2; uint8_t emb_func_irq_mask_g_settl : 1; - uint8_t emb_func_irq_mask_xl_settl : 1; - uint8_t emb_func_disable : 1; - uint8_t not_used0 : 3; + uint8_t emb_func_irq_mask_xl_settl : 1; + uint8_t emb_func_disable : 1; + uint8_t not_used0 : 3; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_emb_func_cfg_t; -#define LSM6DSV16X_UI_HANDSHAKE_CTRL 0x64U -typedef struct { +#define LSM6DSV16X_UI_HANDSHAKE_CTRL 0x64U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t ui_shared_req : 1; - uint8_t ui_shared_ack : 1; - uint8_t not_used0 : 6; + uint8_t ui_shared_req : 1; + uint8_t ui_shared_ack : 1; + uint8_t not_used0 : 6; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t not_used0 : 6; - uint8_t ui_shared_ack : 1; - uint8_t ui_shared_req : 1; + uint8_t not_used0 : 6; + uint8_t ui_shared_ack : 1; + uint8_t ui_shared_req : 1; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_ui_handshake_ctrl_t; -#define LSM6DSV16X_UI_SPI2_SHARED_0 0x65U -typedef struct { +#define LSM6DSV16X_UI_SPI2_SHARED_0 0x65U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t ui_spi2_shared : 8; + uint8_t ui_spi2_shared : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t ui_spi2_shared : 8; + uint8_t ui_spi2_shared : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_ui_spi2_shared_0_t; -#define LSM6DSV16X_UI_SPI2_SHARED_1 0x66U -typedef struct { +#define LSM6DSV16X_UI_SPI2_SHARED_1 0x66U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t ui_spi2_shared : 8; + uint8_t ui_spi2_shared : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t ui_spi2_shared : 8; + uint8_t ui_spi2_shared : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_ui_spi2_shared_1_t; -#define LSM6DSV16X_UI_SPI2_SHARED_2 0x67U -typedef struct { +#define LSM6DSV16X_UI_SPI2_SHARED_2 0x67U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t ui_spi2_shared : 8; + uint8_t ui_spi2_shared : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t ui_spi2_shared : 8; + uint8_t ui_spi2_shared : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_ui_spi2_shared_2_t; -#define LSM6DSV16X_UI_SPI2_SHARED_3 0x68U -typedef struct { +#define LSM6DSV16X_UI_SPI2_SHARED_3 0x68U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t ui_spi2_shared : 8; + uint8_t ui_spi2_shared : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t ui_spi2_shared : 8; + uint8_t ui_spi2_shared : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_ui_spi2_shared_3_t; -#define LSM6DSV16X_UI_SPI2_SHARED_4 0x69U -typedef struct { +#define LSM6DSV16X_UI_SPI2_SHARED_4 0x69U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t ui_spi2_shared : 8; + uint8_t ui_spi2_shared : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t ui_spi2_shared : 8; + uint8_t ui_spi2_shared : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_ui_spi2_shared_4_t; -#define LSM6DSV16X_UI_SPI2_SHARED_5 0x6AU -typedef struct { +#define LSM6DSV16X_UI_SPI2_SHARED_5 0x6AU +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t ui_spi2_shared : 8; + uint8_t ui_spi2_shared : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t ui_spi2_shared : 8; + uint8_t ui_spi2_shared : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_ui_spi2_shared_5_t; -#define LSM6DSV16X_CTRL_EIS 0x6BU -typedef struct { +#define LSM6DSV16X_CTRL_EIS 0x6BU +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t fs_g_eis : 3; - uint8_t g_eis_on_g_ois_out_reg : 1; - uint8_t lpf_g_eis_bw : 1; - uint8_t not_used0 : 1; - uint8_t odr_g_eis : 2; + uint8_t fs_g_eis : 3; + uint8_t g_eis_on_g_ois_out_reg : 1; + uint8_t lpf_g_eis_bw : 1; + uint8_t not_used0 : 1; + uint8_t odr_g_eis : 2; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t odr_g_eis : 2; - uint8_t not_used0 : 1; - uint8_t lpf_g_eis_bw : 1; - uint8_t g_eis_on_g_ois_out_reg : 1; - uint8_t fs_g_eis : 3; + uint8_t odr_g_eis : 2; + uint8_t not_used0 : 1; + uint8_t lpf_g_eis_bw : 1; + uint8_t g_eis_on_g_ois_out_reg : 1; + uint8_t fs_g_eis : 3; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_ctrl_eis_t; -#define LSM6DSV16X_UI_INT_OIS 0x6FU -typedef struct { +#define LSM6DSV16X_UI_INT_OIS 0x6FU +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t not_used0 : 4; - uint8_t st_ois_clampdis : 1; - uint8_t not_used1 : 1; - uint8_t drdy_mask_ois : 1; - uint8_t int2_drdy_ois : 1; + uint8_t not_used0 : 4; + uint8_t st_ois_clampdis : 1; + uint8_t not_used1 : 1; + uint8_t drdy_mask_ois : 1; + uint8_t int2_drdy_ois : 1; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t int2_drdy_ois : 1; - uint8_t drdy_mask_ois : 1; - uint8_t not_used1 : 1; - uint8_t st_ois_clampdis : 1; - uint8_t not_used0 : 4; + uint8_t int2_drdy_ois : 1; + uint8_t drdy_mask_ois : 1; + uint8_t not_used1 : 1; + uint8_t st_ois_clampdis : 1; + uint8_t not_used0 : 4; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_ui_int_ois_t; -#define LSM6DSV16X_UI_CTRL1_OIS 0x70U -typedef struct { +#define LSM6DSV16X_UI_CTRL1_OIS 0x70U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t spi2_read_en : 1; - uint8_t ois_g_en : 1; - uint8_t ois_xl_en : 1; - uint8_t not_used0 : 2; - uint8_t sim_ois : 1; - uint8_t not_used1 : 2; + uint8_t spi2_read_en : 1; + uint8_t ois_g_en : 1; + uint8_t ois_xl_en : 1; + uint8_t not_used0 : 2; + uint8_t sim_ois : 1; + uint8_t not_used1 : 2; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t not_used1 : 2; - uint8_t sim_ois : 1; - uint8_t not_used0 : 2; - uint8_t ois_xl_en : 1; - uint8_t ois_g_en : 1; - uint8_t spi2_read_en : 1; + uint8_t not_used1 : 2; + uint8_t sim_ois : 1; + uint8_t not_used0 : 2; + uint8_t ois_xl_en : 1; + uint8_t ois_g_en : 1; + uint8_t spi2_read_en : 1; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_ui_ctrl1_ois_t; -#define LSM6DSV16X_UI_CTRL2_OIS 0x71U -typedef struct { +#define LSM6DSV16X_UI_CTRL2_OIS 0x71U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t fs_g_ois : 3; - uint8_t lpf1_g_ois_bw : 2; - uint8_t not_used0 : 3; + uint8_t fs_g_ois : 3; + uint8_t lpf1_g_ois_bw : 2; + uint8_t not_used0 : 3; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t not_used0 : 3; - uint8_t lpf1_g_ois_bw : 2; - uint8_t fs_g_ois : 3; + uint8_t not_used0 : 3; + uint8_t lpf1_g_ois_bw : 2; + uint8_t fs_g_ois : 3; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_ui_ctrl2_ois_t; -#define LSM6DSV16X_UI_CTRL3_OIS 0x72U -typedef struct { +#define LSM6DSV16X_UI_CTRL3_OIS 0x72U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t fs_xl_ois : 2; - uint8_t not_used0 : 1; - uint8_t lpf_xl_ois_bw : 3; - uint8_t not_used1 : 2; + uint8_t fs_xl_ois : 2; + uint8_t not_used0 : 1; + uint8_t lpf_xl_ois_bw : 3; + uint8_t not_used1 : 2; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t not_used1 : 2; - uint8_t lpf_xl_ois_bw : 3; - uint8_t not_used0 : 1; - uint8_t fs_xl_ois : 2; + uint8_t not_used1 : 2; + uint8_t lpf_xl_ois_bw : 3; + uint8_t not_used0 : 1; + uint8_t fs_xl_ois : 2; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_ui_ctrl3_ois_t; -#define LSM6DSV16X_X_OFS_USR 0x73U -typedef struct { +#define LSM6DSV16X_X_OFS_USR 0x73U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t x_ofs_usr : 8; + uint8_t x_ofs_usr : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t x_ofs_usr : 8; + uint8_t x_ofs_usr : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_x_ofs_usr_t; -#define LSM6DSV16X_Y_OFS_USR 0x74U -typedef struct { +#define LSM6DSV16X_Y_OFS_USR 0x74U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t y_ofs_usr : 8; + uint8_t y_ofs_usr : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t y_ofs_usr : 8; + uint8_t y_ofs_usr : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_y_ofs_usr_t; -#define LSM6DSV16X_Z_OFS_USR 0x75U -typedef struct { +#define LSM6DSV16X_Z_OFS_USR 0x75U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t z_ofs_usr : 8; + uint8_t z_ofs_usr : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t z_ofs_usr : 8; + uint8_t z_ofs_usr : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_z_ofs_usr_t; -#define LSM6DSV16X_FIFO_DATA_OUT_TAG 0x78U -typedef struct { +#define LSM6DSV16X_FIFO_DATA_OUT_TAG 0x78U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t not_used0 : 1; - uint8_t tag_cnt : 2; - uint8_t tag_sensor : 5; + uint8_t not_used0 : 1; + uint8_t tag_cnt : 2; + uint8_t tag_sensor : 5; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t tag_sensor : 5; - uint8_t tag_cnt : 2; - uint8_t not_used0 : 1; + uint8_t tag_sensor : 5; + uint8_t tag_cnt : 2; + uint8_t not_used0 : 1; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_fifo_data_out_tag_t; -#define LSM6DSV16X_FIFO_DATA_OUT_X_L 0x79U -typedef struct { +#define LSM6DSV16X_FIFO_DATA_OUT_X_L 0x79U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t fifo_data_out : 8; + uint8_t fifo_data_out : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t fifo_data_out : 8; + uint8_t fifo_data_out : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_fifo_data_out_x_l_t; -#define LSM6DSV16X_FIFO_DATA_OUT_X_H 0x7AU -typedef struct { +#define LSM6DSV16X_FIFO_DATA_OUT_X_H 0x7AU +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t fifo_data_out : 8; + uint8_t fifo_data_out : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t fifo_data_out : 8; + uint8_t fifo_data_out : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_fifo_data_out_x_h_t; -#define LSM6DSV16X_FIFO_DATA_OUT_Y_L 0x7BU -typedef struct { +#define LSM6DSV16X_FIFO_DATA_OUT_Y_L 0x7BU +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t fifo_data_out : 8; + uint8_t fifo_data_out : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t fifo_data_out : 8; + uint8_t fifo_data_out : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_fifo_data_out_y_l_t; -#define LSM6DSV16X_FIFO_DATA_OUT_Y_H 0x7CU -typedef struct { +#define LSM6DSV16X_FIFO_DATA_OUT_Y_H 0x7CU +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t fifo_data_out : 8; + uint8_t fifo_data_out : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t fifo_data_out : 8; + uint8_t fifo_data_out : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_fifo_data_out_y_h_t; -#define LSM6DSV16X_FIFO_DATA_OUT_Z_L 0x7DU -typedef struct { +#define LSM6DSV16X_FIFO_DATA_OUT_Z_L 0x7DU +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t fifo_data_out : 8; + uint8_t fifo_data_out : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t fifo_data_out : 8; + uint8_t fifo_data_out : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_fifo_data_out_z_l_t; -#define LSM6DSV16X_FIFO_DATA_OUT_Z_H 0x7EU -typedef struct { +#define LSM6DSV16X_FIFO_DATA_OUT_Z_H 0x7EU +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t fifo_data_out : 8; + uint8_t fifo_data_out : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t fifo_data_out : 8; + uint8_t fifo_data_out : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_fifo_data_out_z_h_t; @@ -1625,232 +1713,253 @@ typedef struct { * */ -#define LSM6DSV16X_SPI2_WHO_AM_I 0x0FU -typedef struct { +#define LSM6DSV16X_SPI2_WHO_AM_I 0x0FU +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t id : 8; + uint8_t id : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t id : 8; + uint8_t id : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_spi2_who_am_i_t; -#define LSM6DSV16X_SPI2_STATUS_REG_OIS 0x1EU -typedef struct { +#define LSM6DSV16X_SPI2_STATUS_REG_OIS 0x1EU +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t xlda : 1; - uint8_t gda : 1; - uint8_t gyro_settling : 1; - uint8_t not_used0 : 5; + uint8_t xlda : 1; + uint8_t gda : 1; + uint8_t gyro_settling : 1; + uint8_t not_used0 : 5; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t not_used0 : 5; - uint8_t gyro_settling : 1; - uint8_t gda : 1; - uint8_t xlda : 1; + uint8_t not_used0 : 5; + uint8_t gyro_settling : 1; + uint8_t gda : 1; + uint8_t xlda : 1; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_spi2_status_reg_ois_t; -#define LSM6DSV16X_SPI2_OUT_TEMP_L 0x20U -typedef struct { +#define LSM6DSV16X_SPI2_OUT_TEMP_L 0x20U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t temp : 8; + uint8_t temp : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t temp : 8; + uint8_t temp : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_spi2_out_temp_l_t; -#define LSM6DSV16X_SPI2_OUT_TEMP_H 0x21U -typedef struct { +#define LSM6DSV16X_SPI2_OUT_TEMP_H 0x21U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t temp : 8; + uint8_t temp : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t temp : 8; + uint8_t temp : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_spi2_out_temp_h_t; -#define LSM6DSV16X_SPI2_OUTX_L_G_OIS 0x22U -typedef struct { +#define LSM6DSV16X_SPI2_OUTX_L_G_OIS 0x22U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t spi2_outx_g_ois : 8; + uint8_t spi2_outx_g_ois : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t spi2_outx_g_ois : 8; + uint8_t spi2_outx_g_ois : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_spi2_outx_l_g_ois_t; -#define LSM6DSV16X_SPI2_OUTX_H_G_OIS 0x23U -typedef struct { +#define LSM6DSV16X_SPI2_OUTX_H_G_OIS 0x23U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t spi2_outx_g_ois : 8; + uint8_t spi2_outx_g_ois : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t spi2_outx_g_ois : 8; + uint8_t spi2_outx_g_ois : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_spi2_outx_h_g_ois_t; -#define LSM6DSV16X_SPI2_OUTY_L_G_OIS 0x24U -typedef struct { +#define LSM6DSV16X_SPI2_OUTY_L_G_OIS 0x24U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t spi2_outy_g_ois : 8; + uint8_t spi2_outy_g_ois : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t spi2_outy_g_ois : 8; + uint8_t spi2_outy_g_ois : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_spi2_outy_l_g_ois_t; -#define LSM6DSV16X_SPI2_OUTY_H_G_OIS 0x25U -typedef struct { +#define LSM6DSV16X_SPI2_OUTY_H_G_OIS 0x25U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t spi2_outy_g_ois : 8; + uint8_t spi2_outy_g_ois : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t spi2_outy_g_ois : 8; + uint8_t spi2_outy_g_ois : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_spi2_outy_h_g_ois_t; -#define LSM6DSV16X_SPI2_OUTZ_L_G_OIS 0x26U -typedef struct { +#define LSM6DSV16X_SPI2_OUTZ_L_G_OIS 0x26U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t spi2_outz_g_ois : 8; + uint8_t spi2_outz_g_ois : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t spi2_outz_g_ois : 8; + uint8_t spi2_outz_g_ois : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_spi2_outz_l_g_ois_t; -#define LSM6DSV16X_SPI2_OUTZ_H_G_OIS 0x27U -typedef struct { +#define LSM6DSV16X_SPI2_OUTZ_H_G_OIS 0x27U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t spi2_outz_g_ois : 8; + uint8_t spi2_outz_g_ois : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t spi2_outz_g_ois : 8; + uint8_t spi2_outz_g_ois : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_spi2_outz_h_g_ois_t; -#define LSM6DSV16X_SPI2_OUTX_L_A_OIS 0x28U -typedef struct { +#define LSM6DSV16X_SPI2_OUTX_L_A_OIS 0x28U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t spi2_outx_a_ois : 8; + uint8_t spi2_outx_a_ois : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t spi2_outx_a_ois : 8; + uint8_t spi2_outx_a_ois : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_spi2_outx_l_a_ois_t; -#define LSM6DSV16X_SPI2_OUTX_H_A_OIS 0x29U -typedef struct { +#define LSM6DSV16X_SPI2_OUTX_H_A_OIS 0x29U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t spi2_outx_a_ois : 8; + uint8_t spi2_outx_a_ois : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t spi2_outx_a_ois : 8; + uint8_t spi2_outx_a_ois : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_spi2_outx_h_a_ois_t; -#define LSM6DSV16X_SPI2_OUTY_L_A_OIS 0x2AU -typedef struct { +#define LSM6DSV16X_SPI2_OUTY_L_A_OIS 0x2AU +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t spi2_outy_a_ois : 8; + uint8_t spi2_outy_a_ois : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t spi2_outy_a_ois : 8; + uint8_t spi2_outy_a_ois : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_spi2_outy_l_a_ois_t; -#define LSM6DSV16X_SPI2_OUTY_H_A_OIS 0x2BU -typedef struct { +#define LSM6DSV16X_SPI2_OUTY_H_A_OIS 0x2BU +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t spi2_outy_a_ois : 8; + uint8_t spi2_outy_a_ois : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t spi2_outy_a_ois : 8; + uint8_t spi2_outy_a_ois : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_spi2_outy_h_a_ois_t; -#define LSM6DSV16X_SPI2_OUTZ_L_A_OIS 0x2CU -typedef struct { +#define LSM6DSV16X_SPI2_OUTZ_L_A_OIS 0x2CU +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t spi2_outz_a_ois : 8; + uint8_t spi2_outz_a_ois : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t spi2_outz_a_ois : 8; + uint8_t spi2_outz_a_ois : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_spi2_outz_l_a_ois_t; -#define LSM6DSV16X_SPI2_OUTZ_H_A_OIS 0x2DU -typedef struct { +#define LSM6DSV16X_SPI2_OUTZ_H_A_OIS 0x2DU +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t spi2_outz_a_ois : 8; + uint8_t spi2_outz_a_ois : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t spi2_outz_a_ois : 8; + uint8_t spi2_outz_a_ois : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_spi2_outz_h_a_ois_t; -#define LSM6DSV16X_SPI2_HANDSHAKE_CTRL 0x6EU -typedef struct { +#define LSM6DSV16X_SPI2_HANDSHAKE_CTRL 0x6EU +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t spi2_shared_ack : 1; - uint8_t spi2_shared_req : 1; - uint8_t not_used0 : 6; + uint8_t spi2_shared_ack : 1; + uint8_t spi2_shared_req : 1; + uint8_t not_used0 : 6; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t not_used0 : 6; - uint8_t spi2_shared_req : 1; - uint8_t spi2_shared_ack : 1; + uint8_t not_used0 : 6; + uint8_t spi2_shared_req : 1; + uint8_t spi2_shared_ack : 1; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_spi2_handshake_ctrl_t; -#define LSM6DSV16X_SPI2_INT_OIS 0x6FU -typedef struct { +#define LSM6DSV16X_SPI2_INT_OIS 0x6FU +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t st_xl_ois : 2; - uint8_t st_g_ois : 2; - uint8_t st_ois_clampdis : 1; - uint8_t not_used0 : 1; - uint8_t drdy_mask_ois : 1; - uint8_t int2_drdy_ois : 1; + uint8_t st_xl_ois : 2; + uint8_t st_g_ois : 2; + uint8_t st_ois_clampdis : 1; + uint8_t not_used0 : 1; + uint8_t drdy_mask_ois : 1; + uint8_t int2_drdy_ois : 1; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t int2_drdy_ois : 1; - uint8_t drdy_mask_ois : 1; - uint8_t not_used0 : 1; - uint8_t st_ois_clampdis : 1; - uint8_t st_g_ois : 2; - uint8_t st_xl_ois : 2; + uint8_t int2_drdy_ois : 1; + uint8_t drdy_mask_ois : 1; + uint8_t not_used0 : 1; + uint8_t st_ois_clampdis : 1; + uint8_t st_g_ois : 2; + uint8_t st_xl_ois : 2; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_spi2_int_ois_t; -#define LSM6DSV16X_SPI2_CTRL1_OIS 0x70U -typedef struct { +#define LSM6DSV16X_SPI2_CTRL1_OIS 0x70U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t spi2_read_en : 1; - uint8_t ois_g_en : 1; - uint8_t ois_xl_en : 1; - uint8_t not_used0 : 2; - uint8_t sim_ois : 1; - uint8_t not_used1 : 2; + uint8_t spi2_read_en : 1; + uint8_t ois_g_en : 1; + uint8_t ois_xl_en : 1; + uint8_t not_used0 : 2; + uint8_t sim_ois : 1; + uint8_t not_used1 : 2; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t not_used1 : 2; - uint8_t sim_ois : 1; - uint8_t not_used0 : 2; - uint8_t ois_xl_en : 1; - uint8_t ois_g_en : 1; - uint8_t spi2_read_en : 1; + uint8_t not_used1 : 2; + uint8_t sim_ois : 1; + uint8_t not_used0 : 2; + uint8_t ois_xl_en : 1; + uint8_t ois_g_en : 1; + uint8_t spi2_read_en : 1; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_spi2_ctrl1_ois_t; -#define LSM6DSV16X_SPI2_CTRL2_OIS 0x71U -typedef struct { +#define LSM6DSV16X_SPI2_CTRL2_OIS 0x71U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t fs_g_ois : 3; - uint8_t lpf1_g_ois_bw : 2; - uint8_t not_used0 : 3; + uint8_t fs_g_ois : 3; + uint8_t lpf1_g_ois_bw : 2; + uint8_t not_used0 : 3; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t not_used0 : 3; - uint8_t lpf1_g_ois_bw : 2; - uint8_t fs_g_ois : 3; + uint8_t not_used0 : 3; + uint8_t lpf1_g_ois_bw : 2; + uint8_t fs_g_ois : 3; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_spi2_ctrl2_ois_t; -#define LSM6DSV16X_SPI2_CTRL3_OIS 0x72U -typedef struct { +#define LSM6DSV16X_SPI2_CTRL3_OIS 0x72U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t fs_xl_ois : 2; - uint8_t not_used0 : 1; - uint8_t lpf_xl_ois_bw : 3; - uint8_t not_used1 : 2; + uint8_t fs_xl_ois : 2; + uint8_t not_used0 : 1; + uint8_t lpf_xl_ois_bw : 3; + uint8_t not_used1 : 2; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t not_used1 : 2; - uint8_t lpf_xl_ois_bw : 3; - uint8_t not_used0 : 1; - uint8_t fs_xl_ois : 2; + uint8_t not_used1 : 2; + uint8_t lpf_xl_ois_bw : 3; + uint8_t not_used0 : 1; + uint8_t fs_xl_ois : 2; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_spi2_ctrl3_ois_t; @@ -1864,683 +1973,741 @@ typedef struct { * */ -#define LSM6DSV16X_PAGE_SEL 0x2U -typedef struct { +#define LSM6DSV16X_PAGE_SEL 0x2U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t not_used0 : 4; - uint8_t page_sel : 4; + uint8_t not_used0 : 4; + uint8_t page_sel : 4; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t page_sel : 4; - uint8_t not_used0 : 4; + uint8_t page_sel : 4; + uint8_t not_used0 : 4; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_page_sel_t; -#define LSM6DSV16X_EMB_FUNC_EN_A 0x4U -typedef struct { -#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t not_used0 : 3; - uint8_t pedo_en : 1; - uint8_t tilt_en : 1; - uint8_t sign_motion_en : 1; - uint8_t not_used1 : 1; - uint8_t mlc_before_fsm_en : 1; -#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t mlc_before_fsm_en : 1; - uint8_t not_used1 : 1; - uint8_t sign_motion_en : 1; - uint8_t tilt_en : 1; - uint8_t pedo_en : 1; - uint8_t not_used0 : 3; +#define LSM6DSV16X_EMB_FUNC_EN_A 0x4U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t not_used0 : 1; + uint8_t sflp_game_en : 1; + uint8_t not_used2 : 1; + uint8_t pedo_en : 1; + uint8_t tilt_en : 1; + uint8_t sign_motion_en : 1; + uint8_t not_used1 : 1; + uint8_t mlc_before_fsm_en : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t mlc_before_fsm_en : 1; + uint8_t not_used1 : 1; + uint8_t sign_motion_en : 1; + uint8_t tilt_en : 1; + uint8_t pedo_en : 1; + uint8_t not_used2 : 1; + uint8_t sflp_game_en : 1; + uint8_t not_used0 : 1; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_emb_func_en_a_t; -#define LSM6DSV16X_EMB_FUNC_EN_B 0x5U -typedef struct { +#define LSM6DSV16X_EMB_FUNC_EN_B 0x5U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t fsm_en : 1; - uint8_t not_used0 : 2; - uint8_t fifo_compr_en : 1; - uint8_t mlc_en : 1; - uint8_t not_used1 : 3; + uint8_t fsm_en : 1; + uint8_t not_used0 : 2; + uint8_t fifo_compr_en : 1; + uint8_t mlc_en : 1; + uint8_t not_used1 : 3; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t not_used1 : 3; - uint8_t mlc_en : 1; - uint8_t fifo_compr_en : 1; - uint8_t not_used0 : 2; - uint8_t fsm_en : 1; + uint8_t not_used1 : 3; + uint8_t mlc_en : 1; + uint8_t fifo_compr_en : 1; + uint8_t not_used0 : 2; + uint8_t fsm_en : 1; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_emb_func_en_b_t; -#define LSM6DSV16X_EMB_FUNC_EXEC_STATUS 0x7U -typedef struct { +#define LSM6DSV16X_EMB_FUNC_EXEC_STATUS 0x7U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t emb_func_endop : 1; - uint8_t emb_func_exec_ovr : 1; - uint8_t not_used0 : 6; + uint8_t emb_func_endop : 1; + uint8_t emb_func_exec_ovr : 1; + uint8_t not_used0 : 6; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t not_used0 : 6; - uint8_t emb_func_exec_ovr : 1; - uint8_t emb_func_endop : 1; + uint8_t not_used0 : 6; + uint8_t emb_func_exec_ovr : 1; + uint8_t emb_func_endop : 1; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_emb_func_exec_status_t; -#define LSM6DSV16X_PAGE_ADDRESS 0x8U -typedef struct { +#define LSM6DSV16X_PAGE_ADDRESS 0x8U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t page_addr : 8; + uint8_t page_addr : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t page_addr : 8; + uint8_t page_addr : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_page_address_t; -#define LSM6DSV16X_PAGE_VALUE 0x9U -typedef struct { +#define LSM6DSV16X_PAGE_VALUE 0x9U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t page_value : 8; + uint8_t page_value : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t page_value : 8; + uint8_t page_value : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_page_value_t; -#define LSM6DSV16X_EMB_FUNC_INT1 0x0AU -typedef struct { +#define LSM6DSV16X_EMB_FUNC_INT1 0x0AU +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t not_used0 : 3; - uint8_t int1_step_detector : 1; - uint8_t int1_tilt : 1; - uint8_t int1_sig_mot : 1; - uint8_t not_used1 : 1; - uint8_t int1_fsm_lc : 1; + uint8_t not_used0 : 3; + uint8_t int1_step_detector : 1; + uint8_t int1_tilt : 1; + uint8_t int1_sig_mot : 1; + uint8_t not_used1 : 1; + uint8_t int1_fsm_lc : 1; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t int1_fsm_lc : 1; - uint8_t not_used1 : 1; - uint8_t int1_sig_mot : 1; - uint8_t int1_tilt : 1; - uint8_t int1_step_detector : 1; - uint8_t not_used0 : 3; + uint8_t int1_fsm_lc : 1; + uint8_t not_used1 : 1; + uint8_t int1_sig_mot : 1; + uint8_t int1_tilt : 1; + uint8_t int1_step_detector : 1; + uint8_t not_used0 : 3; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_emb_func_int1_t; -#define LSM6DSV16X_FSM_INT1 0x0BU -typedef struct { -#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t int1_fsm1 : 1; - uint8_t int1_fsm2 : 1; - uint8_t int1_fsm3 : 1; - uint8_t int1_fsm4 : 1; - uint8_t int1_fsm5 : 1; - uint8_t int1_fsm6 : 1; - uint8_t int1_fsm7 : 1; - uint8_t int1_fsm8 : 1; -#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t int1_fsm8 : 1; - uint8_t int1_fsm7 : 1; - uint8_t int1_fsm6 : 1; - uint8_t int1_fsm5 : 1; - uint8_t int1_fsm4 : 1; - uint8_t int1_fsm3 : 1; - uint8_t int1_fsm2 : 1; - uint8_t int1_fsm1 : 1; +#define LSM6DSV16X_FSM_INT1 0x0BU +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t int1_fsm1 : 1; + uint8_t int1_fsm2 : 1; + uint8_t int1_fsm3 : 1; + uint8_t int1_fsm4 : 1; + uint8_t int1_fsm5 : 1; + uint8_t int1_fsm6 : 1; + uint8_t int1_fsm7 : 1; + uint8_t int1_fsm8 : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t int1_fsm8 : 1; + uint8_t int1_fsm7 : 1; + uint8_t int1_fsm6 : 1; + uint8_t int1_fsm5 : 1; + uint8_t int1_fsm4 : 1; + uint8_t int1_fsm3 : 1; + uint8_t int1_fsm2 : 1; + uint8_t int1_fsm1 : 1; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_fsm_int1_t; -#define LSM6DSV16X_MLC_INT1 0x0DU -typedef struct { +#define LSM6DSV16X_MLC_INT1 0x0DU +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t int1_mlc1 : 1; - uint8_t int1_mlc2 : 1; - uint8_t int1_mlc3 : 1; - uint8_t int1_mlc4 : 1; - uint8_t not_used0 : 4; + uint8_t int1_mlc1 : 1; + uint8_t int1_mlc2 : 1; + uint8_t int1_mlc3 : 1; + uint8_t int1_mlc4 : 1; + uint8_t not_used0 : 4; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t not_used0 : 4; - uint8_t int1_mlc4 : 1; - uint8_t int1_mlc3 : 1; - uint8_t int1_mlc2 : 1; - uint8_t int1_mlc1 : 1; + uint8_t not_used0 : 4; + uint8_t int1_mlc4 : 1; + uint8_t int1_mlc3 : 1; + uint8_t int1_mlc2 : 1; + uint8_t int1_mlc1 : 1; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_mlc_int1_t; -#define LSM6DSV16X_EMB_FUNC_INT2 0x0EU -typedef struct { +#define LSM6DSV16X_EMB_FUNC_INT2 0x0EU +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t not_used0 : 3; - uint8_t int2_step_detector : 1; - uint8_t int2_tilt : 1; - uint8_t int2_sig_mot : 1; - uint8_t not_used1 : 1; - uint8_t int2_fsm_lc : 1; + uint8_t not_used0 : 3; + uint8_t int2_step_detector : 1; + uint8_t int2_tilt : 1; + uint8_t int2_sig_mot : 1; + uint8_t not_used1 : 1; + uint8_t int2_fsm_lc : 1; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t int2_fsm_lc : 1; - uint8_t not_used1 : 1; - uint8_t int2_sig_mot : 1; - uint8_t int2_tilt : 1; - uint8_t int2_step_detector : 1; - uint8_t not_used0 : 3; + uint8_t int2_fsm_lc : 1; + uint8_t not_used1 : 1; + uint8_t int2_sig_mot : 1; + uint8_t int2_tilt : 1; + uint8_t int2_step_detector : 1; + uint8_t not_used0 : 3; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_emb_func_int2_t; -#define LSM6DSV16X_FSM_INT2 0x0FU -typedef struct { -#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t int2_fsm1 : 1; - uint8_t int2_fsm2 : 1; - uint8_t int2_fsm3 : 1; - uint8_t int2_fsm4 : 1; - uint8_t int2_fsm5 : 1; - uint8_t int2_fsm6 : 1; - uint8_t int2_fsm7 : 1; - uint8_t int2_fsm8 : 1; -#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t int2_fsm8 : 1; - uint8_t int2_fsm7 : 1; - uint8_t int2_fsm6 : 1; - uint8_t int2_fsm5 : 1; - uint8_t int2_fsm4 : 1; - uint8_t int2_fsm3 : 1; - uint8_t int2_fsm2 : 1; - uint8_t int2_fsm1 : 1; +#define LSM6DSV16X_FSM_INT2 0x0FU +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t int2_fsm1 : 1; + uint8_t int2_fsm2 : 1; + uint8_t int2_fsm3 : 1; + uint8_t int2_fsm4 : 1; + uint8_t int2_fsm5 : 1; + uint8_t int2_fsm6 : 1; + uint8_t int2_fsm7 : 1; + uint8_t int2_fsm8 : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t int2_fsm8 : 1; + uint8_t int2_fsm7 : 1; + uint8_t int2_fsm6 : 1; + uint8_t int2_fsm5 : 1; + uint8_t int2_fsm4 : 1; + uint8_t int2_fsm3 : 1; + uint8_t int2_fsm2 : 1; + uint8_t int2_fsm1 : 1; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_fsm_int2_t; -#define LSM6DSV16X_MLC_INT2 0x11U -typedef struct { +#define LSM6DSV16X_MLC_INT2 0x11U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t int2_mlc1 : 1; - uint8_t int2_mlc2 : 1; - uint8_t int2_mlc3 : 1; - uint8_t int2_mlc4 : 1; - uint8_t not_used0 : 4; + uint8_t int2_mlc1 : 1; + uint8_t int2_mlc2 : 1; + uint8_t int2_mlc3 : 1; + uint8_t int2_mlc4 : 1; + uint8_t not_used0 : 4; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t not_used0 : 4; - uint8_t int2_mlc4 : 1; - uint8_t int2_mlc3 : 1; - uint8_t int2_mlc2 : 1; - uint8_t int2_mlc1 : 1; + uint8_t not_used0 : 4; + uint8_t int2_mlc4 : 1; + uint8_t int2_mlc3 : 1; + uint8_t int2_mlc2 : 1; + uint8_t int2_mlc1 : 1; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_mlc_int2_t; -#define LSM6DSV16X_EMB_FUNC_STATUS 0x12U -typedef struct { +#define LSM6DSV16X_EMB_FUNC_STATUS 0x12U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t not_used0 : 3; - uint8_t is_step_det : 1; - uint8_t is_tilt : 1; - uint8_t is_sigmot : 1; - uint8_t not_used1 : 1; - uint8_t is_fsm_lc : 1; + uint8_t not_used0 : 3; + uint8_t is_step_det : 1; + uint8_t is_tilt : 1; + uint8_t is_sigmot : 1; + uint8_t not_used1 : 1; + uint8_t is_fsm_lc : 1; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t is_fsm_lc : 1; - uint8_t not_used1 : 1; - uint8_t is_sigmot : 1; - uint8_t is_tilt : 1; - uint8_t is_step_det : 1; - uint8_t not_used0 : 3; + uint8_t is_fsm_lc : 1; + uint8_t not_used1 : 1; + uint8_t is_sigmot : 1; + uint8_t is_tilt : 1; + uint8_t is_step_det : 1; + uint8_t not_used0 : 3; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_emb_func_status_t; -#define LSM6DSV16X_FSM_STATUS 0x13U -typedef struct { -#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t is_fsm1 : 1; - uint8_t is_fsm2 : 1; - uint8_t is_fsm3 : 1; - uint8_t is_fsm4 : 1; - uint8_t is_fsm5 : 1; - uint8_t is_fsm6 : 1; - uint8_t is_fsm7 : 1; - uint8_t is_fsm8 : 1; -#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t is_fsm8 : 1; - uint8_t is_fsm7 : 1; - uint8_t is_fsm6 : 1; - uint8_t is_fsm5 : 1; - uint8_t is_fsm4 : 1; - uint8_t is_fsm3 : 1; - uint8_t is_fsm2 : 1; - uint8_t is_fsm1 : 1; +#define LSM6DSV16X_FSM_STATUS 0x13U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t is_fsm1 : 1; + uint8_t is_fsm2 : 1; + uint8_t is_fsm3 : 1; + uint8_t is_fsm4 : 1; + uint8_t is_fsm5 : 1; + uint8_t is_fsm6 : 1; + uint8_t is_fsm7 : 1; + uint8_t is_fsm8 : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t is_fsm8 : 1; + uint8_t is_fsm7 : 1; + uint8_t is_fsm6 : 1; + uint8_t is_fsm5 : 1; + uint8_t is_fsm4 : 1; + uint8_t is_fsm3 : 1; + uint8_t is_fsm2 : 1; + uint8_t is_fsm1 : 1; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_fsm_status_t; -#define LSM6DSV16X_MLC_STATUS 0x15U -typedef struct { +#define LSM6DSV16X_MLC_STATUS 0x15U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t is_mlc1 : 1; - uint8_t is_mlc2 : 1; - uint8_t is_mlc3 : 1; - uint8_t is_mlc4 : 1; - uint8_t not_used0 : 4; + uint8_t is_mlc1 : 1; + uint8_t is_mlc2 : 1; + uint8_t is_mlc3 : 1; + uint8_t is_mlc4 : 1; + uint8_t not_used0 : 4; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t not_used0 : 4; - uint8_t is_mlc4 : 1; - uint8_t is_mlc3 : 1; - uint8_t is_mlc2 : 1; - uint8_t is_mlc1 : 1; + uint8_t not_used0 : 4; + uint8_t is_mlc4 : 1; + uint8_t is_mlc3 : 1; + uint8_t is_mlc2 : 1; + uint8_t is_mlc1 : 1; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_mlc_status_t; -#define LSM6DSV16X_PAGE_RW 0x17U -typedef struct { +#define LSM6DSV16X_PAGE_RW 0x17U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t not_used0 : 5; - uint8_t page_read : 1; - uint8_t page_write : 1; - uint8_t emb_func_lir : 1; + uint8_t not_used0 : 5; + uint8_t page_read : 1; + uint8_t page_write : 1; + uint8_t emb_func_lir : 1; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t emb_func_lir : 1; - uint8_t page_write : 1; - uint8_t page_read : 1; - uint8_t not_used0 : 5; + uint8_t emb_func_lir : 1; + uint8_t page_write : 1; + uint8_t page_read : 1; + uint8_t not_used0 : 5; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_page_rw_t; -#define LSM6DSV16X_EMB_FUNC_FIFO_EN_A 0x44U -typedef struct { -#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t not_used0 : 6; - uint8_t step_counter_fifo_en : 1; - uint8_t mlc_fifo_en : 1; -#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t mlc_fifo_en : 1; - uint8_t step_counter_fifo_en : 1; - uint8_t not_used0 : 6; +#define LSM6DSV16X_EMB_FUNC_FIFO_EN_A 0x44U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t not_used0 : 1; + uint8_t sflp_game_fifo_en : 1; + uint8_t not_used1 : 2; + uint8_t sflp_gravity_fifo_en : 1; + uint8_t sflp_gbias_fifo_en : 1; + uint8_t step_counter_fifo_en : 1; + uint8_t mlc_fifo_en : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t mlc_fifo_en : 1; + uint8_t step_counter_fifo_en : 1; + uint8_t sflp_gbias_fifo_en : 1; + uint8_t sflp_gravity_fifo_en : 1; + uint8_t not_used1 : 2; + uint8_t sflp_game_fifo_en : 1; + uint8_t not_used0 : 1; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_emb_func_fifo_en_a_t; -#define LSM6DSV16X_EMB_FUNC_FIFO_EN_B 0x45U -typedef struct { +#define LSM6DSV16X_EMB_FUNC_FIFO_EN_B 0x45U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t not_used0 : 1; - uint8_t mlc_filter_feature_fifo_en : 1; - uint8_t not_used1 : 6; + uint8_t not_used0 : 1; + uint8_t mlc_filter_feature_fifo_en : 1; + uint8_t not_used1 : 6; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t not_used1 : 6; - uint8_t mlc_filter_feature_fifo_en : 1; - uint8_t not_used0 : 1; + uint8_t not_used1 : 6; + uint8_t mlc_filter_feature_fifo_en : 1; + uint8_t not_used0 : 1; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_emb_func_fifo_en_b_t; -#define LSM6DSV16X_FSM_ENABLE 0x46U -typedef struct { -#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t fsm1_en : 1; - uint8_t fsm2_en : 1; - uint8_t fsm3_en : 1; - uint8_t fsm4_en : 1; - uint8_t fsm5_en : 1; - uint8_t fsm6_en : 1; - uint8_t fsm7_en : 1; - uint8_t fsm8_en : 1; -#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t fsm8_en : 1; - uint8_t fsm7_en : 1; - uint8_t fsm6_en : 1; - uint8_t fsm5_en : 1; - uint8_t fsm4_en : 1; - uint8_t fsm3_en : 1; - uint8_t fsm2_en : 1; - uint8_t fsm1_en : 1; +#define LSM6DSV16X_FSM_ENABLE 0x46U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t fsm1_en : 1; + uint8_t fsm2_en : 1; + uint8_t fsm3_en : 1; + uint8_t fsm4_en : 1; + uint8_t fsm5_en : 1; + uint8_t fsm6_en : 1; + uint8_t fsm7_en : 1; + uint8_t fsm8_en : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t fsm8_en : 1; + uint8_t fsm7_en : 1; + uint8_t fsm6_en : 1; + uint8_t fsm5_en : 1; + uint8_t fsm4_en : 1; + uint8_t fsm3_en : 1; + uint8_t fsm2_en : 1; + uint8_t fsm1_en : 1; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_fsm_enable_t; -#define LSM6DSV16X_FSM_LONG_COUNTER_L 0x48U -typedef struct { +#define LSM6DSV16X_FSM_LONG_COUNTER_L 0x48U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t fsm_lc : 8; + uint8_t fsm_lc : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t fsm_lc : 8; + uint8_t fsm_lc : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_fsm_long_counter_l_t; -#define LSM6DSV16X_FSM_LONG_COUNTER_H 0x49U -typedef struct { +#define LSM6DSV16X_FSM_LONG_COUNTER_H 0x49U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t fsm_lc : 8; + uint8_t fsm_lc : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t fsm_lc : 8; + uint8_t fsm_lc : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_fsm_long_counter_h_t; -#define LSM6DSV16X_INT_ACK_MASK 0x4BU -typedef struct { +#define LSM6DSV16X_INT_ACK_MASK 0x4BU +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t iack_mask : 8; + uint8_t iack_mask : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t iack_mask : 8; + uint8_t iack_mask : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_int_ack_mask_t; -#define LSM6DSV16X_FSM_OUTS1 0x4CU -typedef struct { -#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t fsm1_n_v : 1; - uint8_t fsm1_p_v : 1; - uint8_t fsm1_n_z : 1; - uint8_t fsm1_p_z : 1; - uint8_t fsm1_n_y : 1; - uint8_t fsm1_p_y : 1; - uint8_t fsm1_n_x : 1; - uint8_t fsm1_p_x : 1; -#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t fsm1_p_x : 1; - uint8_t fsm1_n_x : 1; - uint8_t fsm1_p_y : 1; - uint8_t fsm1_n_y : 1; - uint8_t fsm1_p_z : 1; - uint8_t fsm1_n_z : 1; - uint8_t fsm1_p_v : 1; - uint8_t fsm1_n_v : 1; +#define LSM6DSV16X_FSM_OUTS1 0x4CU +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t fsm1_n_v : 1; + uint8_t fsm1_p_v : 1; + uint8_t fsm1_n_z : 1; + uint8_t fsm1_p_z : 1; + uint8_t fsm1_n_y : 1; + uint8_t fsm1_p_y : 1; + uint8_t fsm1_n_x : 1; + uint8_t fsm1_p_x : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t fsm1_p_x : 1; + uint8_t fsm1_n_x : 1; + uint8_t fsm1_p_y : 1; + uint8_t fsm1_n_y : 1; + uint8_t fsm1_p_z : 1; + uint8_t fsm1_n_z : 1; + uint8_t fsm1_p_v : 1; + uint8_t fsm1_n_v : 1; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_fsm_outs1_t; -#define LSM6DSV16X_FSM_OUTS2 0x4DU -typedef struct { -#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t fsm2_n_v : 1; - uint8_t fsm2_p_v : 1; - uint8_t fsm2_n_z : 1; - uint8_t fsm2_p_z : 1; - uint8_t fsm2_n_y : 1; - uint8_t fsm2_p_y : 1; - uint8_t fsm2_n_x : 1; - uint8_t fsm2_p_x : 1; -#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t fsm2_p_x : 1; - uint8_t fsm2_n_x : 1; - uint8_t fsm2_p_y : 1; - uint8_t fsm2_n_y : 1; - uint8_t fsm2_p_z : 1; - uint8_t fsm2_n_z : 1; - uint8_t fsm2_p_v : 1; - uint8_t fsm2_n_v : 1; +#define LSM6DSV16X_FSM_OUTS2 0x4DU +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t fsm2_n_v : 1; + uint8_t fsm2_p_v : 1; + uint8_t fsm2_n_z : 1; + uint8_t fsm2_p_z : 1; + uint8_t fsm2_n_y : 1; + uint8_t fsm2_p_y : 1; + uint8_t fsm2_n_x : 1; + uint8_t fsm2_p_x : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t fsm2_p_x : 1; + uint8_t fsm2_n_x : 1; + uint8_t fsm2_p_y : 1; + uint8_t fsm2_n_y : 1; + uint8_t fsm2_p_z : 1; + uint8_t fsm2_n_z : 1; + uint8_t fsm2_p_v : 1; + uint8_t fsm2_n_v : 1; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_fsm_outs2_t; -#define LSM6DSV16X_FSM_OUTS3 0x4EU -typedef struct { -#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t fsm3_n_v : 1; - uint8_t fsm3_p_v : 1; - uint8_t fsm3_n_z : 1; - uint8_t fsm3_p_z : 1; - uint8_t fsm3_n_y : 1; - uint8_t fsm3_p_y : 1; - uint8_t fsm3_n_x : 1; - uint8_t fsm3_p_x : 1; -#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t fsm3_p_x : 1; - uint8_t fsm3_n_x : 1; - uint8_t fsm3_p_y : 1; - uint8_t fsm3_n_y : 1; - uint8_t fsm3_p_z : 1; - uint8_t fsm3_n_z : 1; - uint8_t fsm3_p_v : 1; - uint8_t fsm3_n_v : 1; +#define LSM6DSV16X_FSM_OUTS3 0x4EU +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t fsm3_n_v : 1; + uint8_t fsm3_p_v : 1; + uint8_t fsm3_n_z : 1; + uint8_t fsm3_p_z : 1; + uint8_t fsm3_n_y : 1; + uint8_t fsm3_p_y : 1; + uint8_t fsm3_n_x : 1; + uint8_t fsm3_p_x : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t fsm3_p_x : 1; + uint8_t fsm3_n_x : 1; + uint8_t fsm3_p_y : 1; + uint8_t fsm3_n_y : 1; + uint8_t fsm3_p_z : 1; + uint8_t fsm3_n_z : 1; + uint8_t fsm3_p_v : 1; + uint8_t fsm3_n_v : 1; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_fsm_outs3_t; -#define LSM6DSV16X_FSM_OUTS4 0x4FU -typedef struct { -#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t fsm4_n_v : 1; - uint8_t fsm4_p_v : 1; - uint8_t fsm4_n_z : 1; - uint8_t fsm4_p_z : 1; - uint8_t fsm4_n_y : 1; - uint8_t fsm4_p_y : 1; - uint8_t fsm4_n_x : 1; - uint8_t fsm4_p_x : 1; -#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t fsm4_p_x : 1; - uint8_t fsm4_n_x : 1; - uint8_t fsm4_p_y : 1; - uint8_t fsm4_n_y : 1; - uint8_t fsm4_p_z : 1; - uint8_t fsm4_n_z : 1; - uint8_t fsm4_p_v : 1; - uint8_t fsm4_n_v : 1; +#define LSM6DSV16X_FSM_OUTS4 0x4FU +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t fsm4_n_v : 1; + uint8_t fsm4_p_v : 1; + uint8_t fsm4_n_z : 1; + uint8_t fsm4_p_z : 1; + uint8_t fsm4_n_y : 1; + uint8_t fsm4_p_y : 1; + uint8_t fsm4_n_x : 1; + uint8_t fsm4_p_x : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t fsm4_p_x : 1; + uint8_t fsm4_n_x : 1; + uint8_t fsm4_p_y : 1; + uint8_t fsm4_n_y : 1; + uint8_t fsm4_p_z : 1; + uint8_t fsm4_n_z : 1; + uint8_t fsm4_p_v : 1; + uint8_t fsm4_n_v : 1; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_fsm_outs4_t; -#define LSM6DSV16X_FSM_OUTS5 0x50U -typedef struct { -#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t fsm5_n_v : 1; - uint8_t fsm5_p_v : 1; - uint8_t fsm5_n_z : 1; - uint8_t fsm5_p_z : 1; - uint8_t fsm5_n_y : 1; - uint8_t fsm5_p_y : 1; - uint8_t fsm5_n_x : 1; - uint8_t fsm5_p_x : 1; -#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t fsm5_p_x : 1; - uint8_t fsm5_n_x : 1; - uint8_t fsm5_p_y : 1; - uint8_t fsm5_n_y : 1; - uint8_t fsm5_p_z : 1; - uint8_t fsm5_n_z : 1; - uint8_t fsm5_p_v : 1; - uint8_t fsm5_n_v : 1; +#define LSM6DSV16X_FSM_OUTS5 0x50U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t fsm5_n_v : 1; + uint8_t fsm5_p_v : 1; + uint8_t fsm5_n_z : 1; + uint8_t fsm5_p_z : 1; + uint8_t fsm5_n_y : 1; + uint8_t fsm5_p_y : 1; + uint8_t fsm5_n_x : 1; + uint8_t fsm5_p_x : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t fsm5_p_x : 1; + uint8_t fsm5_n_x : 1; + uint8_t fsm5_p_y : 1; + uint8_t fsm5_n_y : 1; + uint8_t fsm5_p_z : 1; + uint8_t fsm5_n_z : 1; + uint8_t fsm5_p_v : 1; + uint8_t fsm5_n_v : 1; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_fsm_outs5_t; -#define LSM6DSV16X_FSM_OUTS6 0x51U -typedef struct { -#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t fsm6_n_v : 1; - uint8_t fsm6_p_v : 1; - uint8_t fsm6_n_z : 1; - uint8_t fsm6_p_z : 1; - uint8_t fsm6_n_y : 1; - uint8_t fsm6_p_y : 1; - uint8_t fsm6_n_x : 1; - uint8_t fsm6_p_x : 1; -#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t fsm6_p_x : 1; - uint8_t fsm6_n_x : 1; - uint8_t fsm6_p_y : 1; - uint8_t fsm6_n_y : 1; - uint8_t fsm6_p_z : 1; - uint8_t fsm6_n_z : 1; - uint8_t fsm6_p_v : 1; - uint8_t fsm6_n_v : 1; +#define LSM6DSV16X_FSM_OUTS6 0x51U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t fsm6_n_v : 1; + uint8_t fsm6_p_v : 1; + uint8_t fsm6_n_z : 1; + uint8_t fsm6_p_z : 1; + uint8_t fsm6_n_y : 1; + uint8_t fsm6_p_y : 1; + uint8_t fsm6_n_x : 1; + uint8_t fsm6_p_x : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t fsm6_p_x : 1; + uint8_t fsm6_n_x : 1; + uint8_t fsm6_p_y : 1; + uint8_t fsm6_n_y : 1; + uint8_t fsm6_p_z : 1; + uint8_t fsm6_n_z : 1; + uint8_t fsm6_p_v : 1; + uint8_t fsm6_n_v : 1; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_fsm_outs6_t; -#define LSM6DSV16X_FSM_OUTS7 0x52U -typedef struct { -#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t fsm7_n_v : 1; - uint8_t fsm7_p_v : 1; - uint8_t fsm7_n_z : 1; - uint8_t fsm7_p_z : 1; - uint8_t fsm7_n_y : 1; - uint8_t fsm7_p_y : 1; - uint8_t fsm7_n_x : 1; - uint8_t fsm7_p_x : 1; -#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t fsm7_p_x : 1; - uint8_t fsm7_n_x : 1; - uint8_t fsm7_p_y : 1; - uint8_t fsm7_n_y : 1; - uint8_t fsm7_p_z : 1; - uint8_t fsm7_n_z : 1; - uint8_t fsm7_p_v : 1; - uint8_t fsm7_n_v : 1; +#define LSM6DSV16X_FSM_OUTS7 0x52U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t fsm7_n_v : 1; + uint8_t fsm7_p_v : 1; + uint8_t fsm7_n_z : 1; + uint8_t fsm7_p_z : 1; + uint8_t fsm7_n_y : 1; + uint8_t fsm7_p_y : 1; + uint8_t fsm7_n_x : 1; + uint8_t fsm7_p_x : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t fsm7_p_x : 1; + uint8_t fsm7_n_x : 1; + uint8_t fsm7_p_y : 1; + uint8_t fsm7_n_y : 1; + uint8_t fsm7_p_z : 1; + uint8_t fsm7_n_z : 1; + uint8_t fsm7_p_v : 1; + uint8_t fsm7_n_v : 1; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_fsm_outs7_t; -#define LSM6DSV16X_FSM_OUTS8 0x53U -typedef struct { -#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t fsm8_n_v : 1; - uint8_t fsm8_p_v : 1; - uint8_t fsm8_n_z : 1; - uint8_t fsm8_p_z : 1; - uint8_t fsm8_n_y : 1; - uint8_t fsm8_p_y : 1; - uint8_t fsm8_n_x : 1; - uint8_t fsm8_p_x : 1; -#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t fsm8_p_x : 1; - uint8_t fsm8_n_x : 1; - uint8_t fsm8_p_y : 1; - uint8_t fsm8_n_y : 1; - uint8_t fsm8_p_z : 1; - uint8_t fsm8_n_z : 1; - uint8_t fsm8_p_v : 1; - uint8_t fsm8_n_v : 1; +#define LSM6DSV16X_FSM_OUTS8 0x53U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t fsm8_n_v : 1; + uint8_t fsm8_p_v : 1; + uint8_t fsm8_n_z : 1; + uint8_t fsm8_p_z : 1; + uint8_t fsm8_n_y : 1; + uint8_t fsm8_p_y : 1; + uint8_t fsm8_n_x : 1; + uint8_t fsm8_p_x : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t fsm8_p_x : 1; + uint8_t fsm8_n_x : 1; + uint8_t fsm8_p_y : 1; + uint8_t fsm8_n_y : 1; + uint8_t fsm8_p_z : 1; + uint8_t fsm8_n_z : 1; + uint8_t fsm8_p_v : 1; + uint8_t fsm8_n_v : 1; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_fsm_outs8_t; -#define LSM6DSV16X_SFLP_ODR 0x5EU -typedef struct { +#define LSM6DSV16X_SFLP_ODR 0x5EU +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t not_used0 : 3; - uint8_t sflp_game_odr : 3; - uint8_t not_used1: 2; + uint8_t not_used0 : 3; + uint8_t sflp_game_odr : 3; + uint8_t not_used1 : 2; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t not_used1: 2; - uint8_t sflp_game_odr : 3; - uint8_t not_used0 : 3; + uint8_t not_used1 : 2; + uint8_t sflp_game_odr : 3; + uint8_t not_used0 : 3; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_sflp_odr_t; -#define LSM6DSV16X_FSM_ODR 0x5FU -typedef struct { +#define LSM6DSV16X_FSM_ODR 0x5FU +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t not_used0 : 3; - uint8_t fsm_odr : 3; - uint8_t not_used1 : 2; + uint8_t not_used0 : 3; + uint8_t fsm_odr : 3; + uint8_t not_used1 : 2; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t not_used1 : 2; - uint8_t fsm_odr : 3; - uint8_t not_used0 : 3; + uint8_t not_used1 : 2; + uint8_t fsm_odr : 3; + uint8_t not_used0 : 3; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_fsm_odr_t; -#define LSM6DSV16X_MLC_ODR 0x60U -typedef struct { +#define LSM6DSV16X_MLC_ODR 0x60U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t not_used0 : 4; - uint8_t mlc_odr : 3; - uint8_t not_used1 : 1; + uint8_t not_used0 : 4; + uint8_t mlc_odr : 3; + uint8_t not_used1 : 1; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t not_used1 : 1; - uint8_t mlc_odr : 3; - uint8_t not_used0 : 4; + uint8_t not_used1 : 1; + uint8_t mlc_odr : 3; + uint8_t not_used0 : 4; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_mlc_odr_t; -#define LSM6DSV16X_STEP_COUNTER_L 0x62U -typedef struct { +#define LSM6DSV16X_STEP_COUNTER_L 0x62U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t step : 8; + uint8_t step : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t step : 8; + uint8_t step : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_step_counter_l_t; -#define LSM6DSV16X_STEP_COUNTER_H 0x63U -typedef struct { +#define LSM6DSV16X_STEP_COUNTER_H 0x63U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t step : 8; + uint8_t step : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t step : 8; + uint8_t step : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_step_counter_h_t; -#define LSM6DSV16X_EMB_FUNC_SRC 0x64U -typedef struct { +#define LSM6DSV16X_EMB_FUNC_SRC 0x64U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t not_used0 : 2; + uint8_t not_used0 : 2; uint8_t stepcounter_bit_set : 1; - uint8_t step_overflow : 1; + uint8_t step_overflow : 1; uint8_t step_count_delta_ia : 1; - uint8_t step_detected : 1; - uint8_t not_used1 : 1; - uint8_t pedo_rst_step : 1; + uint8_t step_detected : 1; + uint8_t not_used1 : 1; + uint8_t pedo_rst_step : 1; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t pedo_rst_step : 1; - uint8_t not_used1 : 1; - uint8_t step_detected : 1; + uint8_t pedo_rst_step : 1; + uint8_t not_used1 : 1; + uint8_t step_detected : 1; uint8_t step_count_delta_ia : 1; - uint8_t step_overflow : 1; + uint8_t step_overflow : 1; uint8_t stepcounter_bit_set : 1; - uint8_t not_used0 : 2; + uint8_t not_used0 : 2; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_emb_func_src_t; -#define LSM6DSV16X_EMB_FUNC_INIT_A 0x66U -typedef struct { -#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t not_used0 : 3; - uint8_t step_det_init : 1; - uint8_t tilt_init : 1; - uint8_t sig_mot_init : 1; - uint8_t not_used1 : 1; +#define LSM6DSV16X_EMB_FUNC_INIT_A 0x66U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t not_used0 : 1; + uint8_t sflp_game_init : 1; + uint8_t not_used2 : 1; + uint8_t step_det_init : 1; + uint8_t tilt_init : 1; + uint8_t sig_mot_init : 1; + uint8_t not_used1 : 1; uint8_t mlc_before_fsm_init : 1; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t mlc_before_fsm_init : 1; - uint8_t not_used1 : 1; - uint8_t sig_mot_init : 1; - uint8_t tilt_init : 1; - uint8_t step_det_init : 1; - uint8_t not_used0 : 3; + uint8_t not_used1 : 1; + uint8_t sig_mot_init : 1; + uint8_t tilt_init : 1; + uint8_t step_det_init : 1; + uint8_t not_used2 : 1; + uint8_t sflp_game_init : 1; + uint8_t not_used0 : 1; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_emb_func_init_a_t; -#define LSM6DSV16X_EMB_FUNC_INIT_B 0x67U -typedef struct { +#define LSM6DSV16X_EMB_FUNC_INIT_B 0x67U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t fsm_init : 1; - uint8_t not_used0 : 2; - uint8_t fifo_compr_init : 1; - uint8_t mlc_init : 1; - uint8_t not_used1 : 3; + uint8_t fsm_init : 1; + uint8_t not_used0 : 2; + uint8_t fifo_compr_init : 1; + uint8_t mlc_init : 1; + uint8_t not_used1 : 3; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t not_used1 : 3; - uint8_t mlc_init : 1; - uint8_t fifo_compr_init : 1; - uint8_t not_used0 : 2; - uint8_t fsm_init : 1; + uint8_t not_used1 : 3; + uint8_t mlc_init : 1; + uint8_t fifo_compr_init : 1; + uint8_t not_used0 : 2; + uint8_t fsm_init : 1; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_emb_func_init_b_t; -#define LSM6DSV16X_MLC1_SRC 0x70U -typedef struct { +#define LSM6DSV16X_MLC1_SRC 0x70U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t mlc1_src : 8; + uint8_t mlc1_src : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t mlc1_src : 8; + uint8_t mlc1_src : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_mlc1_src_t; -#define LSM6DSV16X_MLC2_SRC 0x71U -typedef struct { +#define LSM6DSV16X_MLC2_SRC 0x71U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t mlc2_src : 8; + uint8_t mlc2_src : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t mlc2_src : 8; + uint8_t mlc2_src : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_mlc2_src_t; -#define LSM6DSV16X_MLC3_SRC 0x72U -typedef struct { +#define LSM6DSV16X_MLC3_SRC 0x72U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t mlc3_src : 8; + uint8_t mlc3_src : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t mlc3_src : 8; + uint8_t mlc3_src : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_mlc3_src_t; -#define LSM6DSV16X_MLC4_SRC 0x73U -typedef struct { +#define LSM6DSV16X_MLC4_SRC 0x73U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t mlc4_src : 8; + uint8_t mlc4_src : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t mlc4_src : 8; + uint8_t mlc4_src : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_mlc4_src_t; @@ -2553,211 +2720,293 @@ typedef struct { * @{ * */ -#define LSM6DSV16X_EMB_ADV_PG_0 0x000 +#define LSM6DSV16X_EMB_ADV_PG_0 0x000U + +#define LSM6DSV16X_SFLP_GAME_GBIASX_L 0x6EU +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t gbiasx : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t gbiasx : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_sflp_game_gbiasx_l_t; + +#define LSM6DSV16X_SFLP_GAME_GBIASX_H 0x6FU +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t gbiasx : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t gbiasx : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_sflp_game_gbiasx_h_t; + +#define LSM6DSV16X_SFLP_GAME_GBIASY_L 0x70U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t gbiasy : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t gbiasy : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_sflp_game_gbiasy_l_t; + +#define LSM6DSV16X_SFLP_GAME_GBIASY_H 0x71U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t gbiasy : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t gbiasy : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_sflp_game_gbiasy_h_t; + +#define LSM6DSV16X_SFLP_GAME_GBIASZ_L 0x72U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t gbiasz : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t gbiasz : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_sflp_game_gbiasz_l_t; + +#define LSM6DSV16X_SFLP_GAME_GBIASZ_H 0x73U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t gbiasz : 8; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t gbiasz : 8; +#endif /* DRV_BYTE_ORDER */ +} lsm6dsv16x_sflp_game_gbiasz_h_t; -#define LSM6DSV16X_FSM_EXT_SENSITIVITY_L 0xBAU -typedef struct { +#define LSM6DSV16X_FSM_EXT_SENSITIVITY_L 0xBAU +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t fsm_ext_s : 8; + uint8_t fsm_ext_s : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t fsm_ext_s : 8; + uint8_t fsm_ext_s : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_fsm_ext_sensitivity_l_t; -#define LSM6DSV16X_FSM_EXT_SENSITIVITY_H 0xBBU -typedef struct { +#define LSM6DSV16X_FSM_EXT_SENSITIVITY_H 0xBBU +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t fsm_ext_s : 8; + uint8_t fsm_ext_s : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t fsm_ext_s : 8; + uint8_t fsm_ext_s : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_fsm_ext_sensitivity_h_t; -#define LSM6DSV16X_FSM_EXT_OFFX_L 0xC0U -typedef struct { +#define LSM6DSV16X_FSM_EXT_OFFX_L 0xC0U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t fsm_ext_offx : 8; + uint8_t fsm_ext_offx : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t fsm_ext_offx : 8; + uint8_t fsm_ext_offx : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_fsm_ext_offx_l_t; -#define LSM6DSV16X_FSM_EXT_OFFX_H 0xC1U -typedef struct { +#define LSM6DSV16X_FSM_EXT_OFFX_H 0xC1U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t fsm_ext_offx : 8; + uint8_t fsm_ext_offx : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t fsm_ext_offx : 8; + uint8_t fsm_ext_offx : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_fsm_ext_offx_h_t; -#define LSM6DSV16X_FSM_EXT_OFFY_L 0xC2U -typedef struct { +#define LSM6DSV16X_FSM_EXT_OFFY_L 0xC2U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t fsm_ext_offy : 8; + uint8_t fsm_ext_offy : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t fsm_ext_offy : 8; + uint8_t fsm_ext_offy : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_fsm_ext_offy_l_t; -#define LSM6DSV16X_FSM_EXT_OFFY_H 0xC3U -typedef struct { +#define LSM6DSV16X_FSM_EXT_OFFY_H 0xC3U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t fsm_ext_offy : 8; + uint8_t fsm_ext_offy : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t fsm_ext_offy : 8; + uint8_t fsm_ext_offy : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_fsm_ext_offy_h_t; -#define LSM6DSV16X_FSM_EXT_OFFZ_L 0xC4U -typedef struct { +#define LSM6DSV16X_FSM_EXT_OFFZ_L 0xC4U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t fsm_ext_offz : 8; + uint8_t fsm_ext_offz : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t fsm_ext_offz : 8; + uint8_t fsm_ext_offz : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_fsm_ext_offz_l_t; -#define LSM6DSV16X_FSM_EXT_OFFZ_H 0xC5U -typedef struct { +#define LSM6DSV16X_FSM_EXT_OFFZ_H 0xC5U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t fsm_ext_offz : 8; + uint8_t fsm_ext_offz : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t fsm_ext_offz : 8; + uint8_t fsm_ext_offz : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_fsm_ext_offz_h_t; -#define LSM6DSV16X_FSM_EXT_MATRIX_XX_L 0xC6U -typedef struct { +#define LSM6DSV16X_FSM_EXT_MATRIX_XX_L 0xC6U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t fsm_ext_mat_xx : 8; + uint8_t fsm_ext_mat_xx : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t fsm_ext_mat_xx : 8; + uint8_t fsm_ext_mat_xx : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_fsm_ext_matrix_xx_l_t; -#define LSM6DSV16X_FSM_EXT_MATRIX_XX_H 0xC7U -typedef struct { +#define LSM6DSV16X_FSM_EXT_MATRIX_XX_H 0xC7U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t fsm_ext_mat_xx : 8; + uint8_t fsm_ext_mat_xx : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t fsm_ext_mat_xx : 8; + uint8_t fsm_ext_mat_xx : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_fsm_ext_matrix_xx_h_t; -#define LSM6DSV16X_FSM_EXT_MATRIX_XY_L 0xC8U -typedef struct { +#define LSM6DSV16X_FSM_EXT_MATRIX_XY_L 0xC8U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t fsm_ext_mat_xy : 8; + uint8_t fsm_ext_mat_xy : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t fsm_ext_mat_xy : 8; + uint8_t fsm_ext_mat_xy : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_fsm_ext_matrix_xy_l_t; -#define LSM6DSV16X_FSM_EXT_MATRIX_XY_H 0xC9U -typedef struct { +#define LSM6DSV16X_FSM_EXT_MATRIX_XY_H 0xC9U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t fsm_ext_mat_xy : 8; + uint8_t fsm_ext_mat_xy : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t fsm_ext_mat_xy : 8; + uint8_t fsm_ext_mat_xy : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_fsm_ext_matrix_xy_h_t; -#define LSM6DSV16X_FSM_EXT_MATRIX_XZ_L 0xCAU -typedef struct { +#define LSM6DSV16X_FSM_EXT_MATRIX_XZ_L 0xCAU +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t fsm_ext_mat_xz : 8; + uint8_t fsm_ext_mat_xz : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t fsm_ext_mat_xz : 8; + uint8_t fsm_ext_mat_xz : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_fsm_ext_matrix_xz_l_t; -#define LSM6DSV16X_FSM_EXT_MATRIX_XZ_H 0xCBU -typedef struct { +#define LSM6DSV16X_FSM_EXT_MATRIX_XZ_H 0xCBU +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t fsm_ext_mat_xz : 8; + uint8_t fsm_ext_mat_xz : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t fsm_ext_mat_xz : 8; + uint8_t fsm_ext_mat_xz : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_fsm_ext_matrix_xz_h_t; -#define LSM6DSV16X_FSM_EXT_MATRIX_YY_L 0xCCU -typedef struct { +#define LSM6DSV16X_FSM_EXT_MATRIX_YY_L 0xCCU +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t fsm_ext_mat_yy : 8; + uint8_t fsm_ext_mat_yy : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t fsm_ext_mat_yy : 8; + uint8_t fsm_ext_mat_yy : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_fsm_ext_matrix_yy_l_t; -#define LSM6DSV16X_FSM_EXT_MATRIX_YY_H 0xCDU -typedef struct { +#define LSM6DSV16X_FSM_EXT_MATRIX_YY_H 0xCDU +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t fsm_ext_mat_yy : 8; + uint8_t fsm_ext_mat_yy : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t fsm_ext_mat_yy : 8; + uint8_t fsm_ext_mat_yy : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_fsm_ext_matrix_yy_h_t; -#define LSM6DSV16X_FSM_EXT_MATRIX_YZ_L 0xCEU -typedef struct { +#define LSM6DSV16X_FSM_EXT_MATRIX_YZ_L 0xCEU +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t fsm_ext_mat_yz : 8; + uint8_t fsm_ext_mat_yz : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t fsm_ext_mat_yz : 8; + uint8_t fsm_ext_mat_yz : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_fsm_ext_matrix_yz_l_t; -#define LSM6DSV16X_FSM_EXT_MATRIX_YZ_H 0xCFU -typedef struct { +#define LSM6DSV16X_FSM_EXT_MATRIX_YZ_H 0xCFU +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t fsm_ext_mat_yz : 8; + uint8_t fsm_ext_mat_yz : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t fsm_ext_mat_yz : 8; + uint8_t fsm_ext_mat_yz : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_fsm_ext_matrix_yz_h_t; -#define LSM6DSV16X_FSM_EXT_MATRIX_ZZ_L 0xD0U -typedef struct { +#define LSM6DSV16X_FSM_EXT_MATRIX_ZZ_L 0xD0U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t fsm_ext_mat_zz : 8; + uint8_t fsm_ext_mat_zz : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t fsm_ext_mat_zz : 8; + uint8_t fsm_ext_mat_zz : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_fsm_ext_matrix_zz_l_t; -#define LSM6DSV16X_FSM_EXT_MATRIX_ZZ_H 0xD1U -typedef struct { +#define LSM6DSV16X_FSM_EXT_MATRIX_ZZ_H 0xD1U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t fsm_ext_mat_zz : 8; + uint8_t fsm_ext_mat_zz : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t fsm_ext_mat_zz : 8; + uint8_t fsm_ext_mat_zz : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_fsm_ext_matrix_zz_h_t; -#define LSM6DSV16X_EXT_CFG_A 0xD4U -typedef struct { +#define LSM6DSV16X_EXT_CFG_A 0xD4U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t ext_z_axis : 3; - uint8_t not_used0 : 1; - uint8_t ext_y_axis : 3; - uint8_t not_used1 : 1; + uint8_t ext_z_axis : 3; + uint8_t not_used0 : 1; + uint8_t ext_y_axis : 3; + uint8_t not_used1 : 1; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t not_used1 : 1; - uint8_t ext_y_axis : 3; - uint8_t not_used0 : 1; - uint8_t ext_z_axis : 3; + uint8_t not_used1 : 1; + uint8_t ext_y_axis : 3; + uint8_t not_used0 : 1; + uint8_t ext_z_axis : 3; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_ext_cfg_a_t; -#define LSM6DSV16X_EXT_CFG_B 0xD5U -typedef struct { +#define LSM6DSV16X_EXT_CFG_B 0xD5U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t ext_x_axis : 3; - uint8_t not_used0 : 5; + uint8_t ext_x_axis : 3; + uint8_t not_used0 : 5; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t not_used0 : 5; - uint8_t ext_x_axis : 3; + uint8_t not_used0 : 5; + uint8_t ext_x_axis : 3; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_ext_cfg_b_t; @@ -2770,110 +3019,121 @@ typedef struct { * @{ * */ -#define LSM6DSV16X_EMB_ADV_PG_1 0x100 +#define LSM6DSV16X_EMB_ADV_PG_1 0x100U -#define LSM6DSV16X_FSM_LC_TIMEOUT_L 0x7AU -typedef struct { +#define LSM6DSV16X_FSM_LC_TIMEOUT_L 0x7AU +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t fsm_lc_timeout : 8; + uint8_t fsm_lc_timeout : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t fsm_lc_timeout : 8; + uint8_t fsm_lc_timeout : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_fsm_lc_timeout_l_t; -#define LSM6DSV16X_FSM_LC_TIMEOUT_H 0x7BU -typedef struct { +#define LSM6DSV16X_FSM_LC_TIMEOUT_H 0x7BU +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t fsm_lc_timeout : 8; + uint8_t fsm_lc_timeout : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t fsm_lc_timeout : 8; + uint8_t fsm_lc_timeout : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_fsm_lc_timeout_h_t; -#define LSM6DSV16X_FSM_PROGRAMS 0x7CU -typedef struct { +#define LSM6DSV16X_FSM_PROGRAMS 0x7CU +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t fsm_n_prog : 8; + uint8_t fsm_n_prog : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t fsm_n_prog : 8; + uint8_t fsm_n_prog : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_fsm_programs_t; -#define LSM6DSV16X_FSM_START_ADD_L 0x7EU -typedef struct { +#define LSM6DSV16X_FSM_START_ADD_L 0x7EU +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t fsm_start : 8; + uint8_t fsm_start : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t fsm_start : 8; + uint8_t fsm_start : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_fsm_start_add_l_t; -#define LSM6DSV16X_FSM_START_ADD_H 0x7FU -typedef struct { +#define LSM6DSV16X_FSM_START_ADD_H 0x7FU +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t fsm_start : 8; + uint8_t fsm_start : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t fsm_start : 8; + uint8_t fsm_start : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_fsm_start_add_h_t; -#define LSM6DSV16X_PEDO_CMD_REG 0x83U -typedef struct { +#define LSM6DSV16X_PEDO_CMD_REG 0x83U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t not_used0 : 2; - uint8_t fp_rejection_en : 1; - uint8_t carry_count_en : 1; - uint8_t not_used1 : 4; + uint8_t not_used0 : 2; + uint8_t fp_rejection_en : 1; + uint8_t carry_count_en : 1; + uint8_t not_used1 : 4; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t not_used1 : 4; - uint8_t carry_count_en : 1; - uint8_t fp_rejection_en : 1; - uint8_t not_used0 : 2; + uint8_t not_used1 : 4; + uint8_t carry_count_en : 1; + uint8_t fp_rejection_en : 1; + uint8_t not_used0 : 2; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_pedo_cmd_reg_t; -#define LSM6DSV16X_PEDO_DEB_STEPS_CONF 0x84U -typedef struct { +#define LSM6DSV16X_PEDO_DEB_STEPS_CONF 0x84U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t deb_step : 8; + uint8_t deb_step : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t deb_step : 8; + uint8_t deb_step : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_pedo_deb_steps_conf_t; -#define LSM6DSV16X_PEDO_SC_DELTAT_L 0xD0U -typedef struct { +#define LSM6DSV16X_PEDO_SC_DELTAT_L 0xD0U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t pd_sc : 8; + uint8_t pd_sc : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t pd_sc : 8; + uint8_t pd_sc : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_pedo_sc_deltat_l_t; -#define LSM6DSV16X_PEDO_SC_DELTAT_H 0xD1U -typedef struct { +#define LSM6DSV16X_PEDO_SC_DELTAT_H 0xD1U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t pd_sc : 8; + uint8_t pd_sc : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t pd_sc : 8; + uint8_t pd_sc : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_pedo_sc_deltat_h_t; #define LSM6DSV16X_MLC_EXT_SENSITIVITY_L 0xE8U -typedef struct { +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t mlc_ext_s : 8; + uint8_t mlc_ext_s : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t mlc_ext_s : 8; + uint8_t mlc_ext_s : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_mlc_ext_sensitivity_l_t; #define LSM6DSV16X_MLC_EXT_SENSITIVITY_H 0xE9U -typedef struct { +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t mlc_ext_s : 8; + uint8_t mlc_ext_s : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t mlc_ext_s : 8; + uint8_t mlc_ext_s : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_mlc_ext_sensitivity_h_t; @@ -2881,63 +3141,69 @@ typedef struct { * @{ * */ -#define LSM6DSV16X_EMB_ADV_PG_2 0x200 +#define LSM6DSV16X_EMB_ADV_PG_2 0x200U -#define LSM6DSV16X_EXT_FORMAT 0x00 -typedef struct { +#define LSM6DSV16X_EXT_FORMAT 0x00 +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t not_used0 : 2; - uint8_t ext_format_sel : 1; - uint8_t not_used1 : 5; + uint8_t not_used0 : 2; + uint8_t ext_format_sel : 1; + uint8_t not_used1 : 5; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t not_used1 : 5; - uint8_t ext_format_sel : 1; - uint8_t not_used0 : 2; + uint8_t not_used1 : 5; + uint8_t ext_format_sel : 1; + uint8_t not_used0 : 2; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_ext_format_t; -#define LSM6DSV16X_EXT_3BYTE_SENSITIVITY_L 0x02U -typedef struct { +#define LSM6DSV16X_EXT_3BYTE_SENSITIVITY_L 0x02U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t ext_3byte_s : 8; + uint8_t ext_3byte_s : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t ext_3byte_s : 8; + uint8_t ext_3byte_s : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_ext_3byte_sensitivity_l_t; -#define LSM6DSV16X_EXT_3BYTE_SENSITIVITY_H 0x03U -typedef struct { +#define LSM6DSV16X_EXT_3BYTE_SENSITIVITY_H 0x03U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t ext_3byte_s : 8; + uint8_t ext_3byte_s : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t ext_3byte_s : 8; + uint8_t ext_3byte_s : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_ext_3byte_sensitivity_h_t; -#define LSM6DSV16X_EXT_3BYTE_OFFSET_XL 0x06U -typedef struct { +#define LSM6DSV16X_EXT_3BYTE_OFFSET_XL 0x06U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t ext_3byte_off : 8; + uint8_t ext_3byte_off : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t ext_3byte_off : 8; + uint8_t ext_3byte_off : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_ext_3byte_offset_xl_t; -#define LSM6DSV16X_EXT_3BYTE_OFFSET_L 0x07U -typedef struct { +#define LSM6DSV16X_EXT_3BYTE_OFFSET_L 0x07U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t ext_3byte_off : 8; + uint8_t ext_3byte_off : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t ext_3byte_off : 8; + uint8_t ext_3byte_off : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_ext_3byte_offset_l_t; -#define LSM6DSV16X_EXT_3BYTE_OFFSET_H 0x08U -typedef struct { +#define LSM6DSV16X_EXT_3BYTE_OFFSET_H 0x08U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t ext_3byte_off : 8; + uint8_t ext_3byte_off : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t ext_3byte_off : 8; + uint8_t ext_3byte_off : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_ext_3byte_offset_h_t; @@ -2951,350 +3217,383 @@ typedef struct { * */ -#define LSM6DSV16X_SENSOR_HUB_1 0x2U -typedef struct { +#define LSM6DSV16X_SENSOR_HUB_1 0x2U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t sensorhub1 : 8; + uint8_t sensorhub1 : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t sensorhub1 : 8; + uint8_t sensorhub1 : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_sensor_hub_1_t; -#define LSM6DSV16X_SENSOR_HUB_2 0x3U -typedef struct { +#define LSM6DSV16X_SENSOR_HUB_2 0x3U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t sensorhub2 : 8; + uint8_t sensorhub2 : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t sensorhub2 : 8; + uint8_t sensorhub2 : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_sensor_hub_2_t; -#define LSM6DSV16X_SENSOR_HUB_3 0x4U -typedef struct { +#define LSM6DSV16X_SENSOR_HUB_3 0x4U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t sensorhub3 : 8; + uint8_t sensorhub3 : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t sensorhub3 : 8; + uint8_t sensorhub3 : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_sensor_hub_3_t; -#define LSM6DSV16X_SENSOR_HUB_4 0x5U -typedef struct { +#define LSM6DSV16X_SENSOR_HUB_4 0x5U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t sensorhub4 : 8; + uint8_t sensorhub4 : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t sensorhub4 : 8; + uint8_t sensorhub4 : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_sensor_hub_4_t; -#define LSM6DSV16X_SENSOR_HUB_5 0x6U -typedef struct { +#define LSM6DSV16X_SENSOR_HUB_5 0x6U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t sensorhub5 : 8; + uint8_t sensorhub5 : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t sensorhub5 : 8; + uint8_t sensorhub5 : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_sensor_hub_5_t; -#define LSM6DSV16X_SENSOR_HUB_6 0x7U -typedef struct { +#define LSM6DSV16X_SENSOR_HUB_6 0x7U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t sensorhub6 : 8; + uint8_t sensorhub6 : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t sensorhub6 : 8; + uint8_t sensorhub6 : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_sensor_hub_6_t; -#define LSM6DSV16X_SENSOR_HUB_7 0x8U -typedef struct { +#define LSM6DSV16X_SENSOR_HUB_7 0x8U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t sensorhub7 : 8; + uint8_t sensorhub7 : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t sensorhub7 : 8; + uint8_t sensorhub7 : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_sensor_hub_7_t; -#define LSM6DSV16X_SENSOR_HUB_8 0x9U -typedef struct { +#define LSM6DSV16X_SENSOR_HUB_8 0x9U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t sensorhub8 : 8; + uint8_t sensorhub8 : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t sensorhub8 : 8; + uint8_t sensorhub8 : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_sensor_hub_8_t; -#define LSM6DSV16X_SENSOR_HUB_9 0x0AU -typedef struct { +#define LSM6DSV16X_SENSOR_HUB_9 0x0AU +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t sensorhub9 : 8; + uint8_t sensorhub9 : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t sensorhub9 : 8; + uint8_t sensorhub9 : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_sensor_hub_9_t; -#define LSM6DSV16X_SENSOR_HUB_10 0x0BU -typedef struct { +#define LSM6DSV16X_SENSOR_HUB_10 0x0BU +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t sensorhub10 : 8; + uint8_t sensorhub10 : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t sensorhub10 : 8; + uint8_t sensorhub10 : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_sensor_hub_10_t; -#define LSM6DSV16X_SENSOR_HUB_11 0x0CU -typedef struct { +#define LSM6DSV16X_SENSOR_HUB_11 0x0CU +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t sensorhub11 : 8; + uint8_t sensorhub11 : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t sensorhub11 : 8; + uint8_t sensorhub11 : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_sensor_hub_11_t; -#define LSM6DSV16X_SENSOR_HUB_12 0x0DU -typedef struct { +#define LSM6DSV16X_SENSOR_HUB_12 0x0DU +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t sensorhub12 : 8; + uint8_t sensorhub12 : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t sensorhub12 : 8; + uint8_t sensorhub12 : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_sensor_hub_12_t; -#define LSM6DSV16X_SENSOR_HUB_13 0x0EU -typedef struct { +#define LSM6DSV16X_SENSOR_HUB_13 0x0EU +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t sensorhub13 : 8; + uint8_t sensorhub13 : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t sensorhub13 : 8; + uint8_t sensorhub13 : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_sensor_hub_13_t; -#define LSM6DSV16X_SENSOR_HUB_14 0x0FU -typedef struct { +#define LSM6DSV16X_SENSOR_HUB_14 0x0FU +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t sensorhub14 : 8; + uint8_t sensorhub14 : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t sensorhub14 : 8; + uint8_t sensorhub14 : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_sensor_hub_14_t; -#define LSM6DSV16X_SENSOR_HUB_15 0x10U -typedef struct { +#define LSM6DSV16X_SENSOR_HUB_15 0x10U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t sensorhub15 : 8; + uint8_t sensorhub15 : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t sensorhub15 : 8; + uint8_t sensorhub15 : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_sensor_hub_15_t; -#define LSM6DSV16X_SENSOR_HUB_16 0x11U -typedef struct { +#define LSM6DSV16X_SENSOR_HUB_16 0x11U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t sensorhub16 : 8; + uint8_t sensorhub16 : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t sensorhub16 : 8; + uint8_t sensorhub16 : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_sensor_hub_16_t; -#define LSM6DSV16X_SENSOR_HUB_17 0x12U -typedef struct { +#define LSM6DSV16X_SENSOR_HUB_17 0x12U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t sensorhub17 : 8; + uint8_t sensorhub17 : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t sensorhub17 : 8; + uint8_t sensorhub17 : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_sensor_hub_17_t; -#define LSM6DSV16X_SENSOR_HUB_18 0x13U -typedef struct { +#define LSM6DSV16X_SENSOR_HUB_18 0x13U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t sensorhub18 : 8; + uint8_t sensorhub18 : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t sensorhub18 : 8; + uint8_t sensorhub18 : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_sensor_hub_18_t; -#define LSM6DSV16X_MASTER_CONFIG 0x14U -typedef struct { -#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t aux_sens_on : 2; - uint8_t master_on : 1; - uint8_t not_used0 : 1; - uint8_t pass_through_mode : 1; - uint8_t start_config : 1; - uint8_t write_once : 1; - uint8_t rst_master_regs : 1; -#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t rst_master_regs : 1; - uint8_t write_once : 1; - uint8_t start_config : 1; - uint8_t pass_through_mode : 1; - uint8_t not_used0 : 1; - uint8_t master_on : 1; - uint8_t aux_sens_on : 2; +#define LSM6DSV16X_MASTER_CONFIG 0x14U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t aux_sens_on : 2; + uint8_t master_on : 1; + uint8_t not_used0 : 1; + uint8_t pass_through_mode : 1; + uint8_t start_config : 1; + uint8_t write_once : 1; + uint8_t rst_master_regs : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t rst_master_regs : 1; + uint8_t write_once : 1; + uint8_t start_config : 1; + uint8_t pass_through_mode : 1; + uint8_t not_used0 : 1; + uint8_t master_on : 1; + uint8_t aux_sens_on : 2; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_master_config_t; -#define LSM6DSV16X_SLV0_ADD 0x15U -typedef struct { +#define LSM6DSV16X_SLV0_ADD 0x15U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t rw_0 : 1; - uint8_t slave0_add : 7; + uint8_t rw_0 : 1; + uint8_t slave0_add : 7; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t slave0_add : 7; - uint8_t rw_0 : 1; + uint8_t slave0_add : 7; + uint8_t rw_0 : 1; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_slv0_add_t; -#define LSM6DSV16X_SLV0_SUBADD 0x16U -typedef struct { +#define LSM6DSV16X_SLV0_SUBADD 0x16U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t slave0_reg : 8; + uint8_t slave0_reg : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t slave0_reg : 8; + uint8_t slave0_reg : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_slv0_subadd_t; -#define LSM6DSV16X_SLV0_CONFIG 0x17U -typedef struct { +#define LSM6DSV16X_SLV0_CONFIG 0x17U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t slave0_numop : 3; + uint8_t slave0_numop : 3; uint8_t batch_ext_sens_0_en : 1; - uint8_t not_used0 : 1; - uint8_t shub_odr : 3; + uint8_t not_used0 : 1; + uint8_t shub_odr : 3; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t shub_odr : 3; - uint8_t not_used0 : 1; + uint8_t shub_odr : 3; + uint8_t not_used0 : 1; uint8_t batch_ext_sens_0_en : 1; - uint8_t slave0_numop : 3; + uint8_t slave0_numop : 3; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_slv0_config_t; -#define LSM6DSV16X_SLV1_ADD 0x18U -typedef struct { +#define LSM6DSV16X_SLV1_ADD 0x18U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t r_1 : 1; - uint8_t slave1_add : 7; + uint8_t r_1 : 1; + uint8_t slave1_add : 7; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t slave1_add : 7; - uint8_t r_1 : 1; + uint8_t slave1_add : 7; + uint8_t r_1 : 1; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_slv1_add_t; -#define LSM6DSV16X_SLV1_SUBADD 0x19U -typedef struct { +#define LSM6DSV16X_SLV1_SUBADD 0x19U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t slave1_reg : 8; + uint8_t slave1_reg : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t slave1_reg : 8; + uint8_t slave1_reg : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_slv1_subadd_t; -#define LSM6DSV16X_SLV1_CONFIG 0x1AU -typedef struct { +#define LSM6DSV16X_SLV1_CONFIG 0x1AU +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t slave1_numop : 3; + uint8_t slave1_numop : 3; uint8_t batch_ext_sens_1_en : 1; - uint8_t not_used0 : 4; + uint8_t not_used0 : 4; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t not_used0 : 4; + uint8_t not_used0 : 4; uint8_t batch_ext_sens_1_en : 1; - uint8_t slave1_numop : 3; + uint8_t slave1_numop : 3; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_slv1_config_t; -#define LSM6DSV16X_SLV2_ADD 0x1BU -typedef struct { +#define LSM6DSV16X_SLV2_ADD 0x1BU +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t r_2 : 1; - uint8_t slave2_add : 7; + uint8_t r_2 : 1; + uint8_t slave2_add : 7; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t slave2_add : 7; - uint8_t r_2 : 1; + uint8_t slave2_add : 7; + uint8_t r_2 : 1; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_slv2_add_t; -#define LSM6DSV16X_SLV2_SUBADD 0x1CU -typedef struct { +#define LSM6DSV16X_SLV2_SUBADD 0x1CU +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t slave2_reg : 8; + uint8_t slave2_reg : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t slave2_reg : 8; + uint8_t slave2_reg : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_slv2_subadd_t; -#define LSM6DSV16X_SLV2_CONFIG 0x1DU -typedef struct { +#define LSM6DSV16X_SLV2_CONFIG 0x1DU +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t slave2_numop : 3; + uint8_t slave2_numop : 3; uint8_t batch_ext_sens_2_en : 1; - uint8_t not_used0 : 4; + uint8_t not_used0 : 4; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t not_used0 : 4; + uint8_t not_used0 : 4; uint8_t batch_ext_sens_2_en : 1; - uint8_t slave2_numop : 3; + uint8_t slave2_numop : 3; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_slv2_config_t; -#define LSM6DSV16X_SLV3_ADD 0x1EU -typedef struct { +#define LSM6DSV16X_SLV3_ADD 0x1EU +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t r_3 : 1; - uint8_t slave3_add : 7; + uint8_t r_3 : 1; + uint8_t slave3_add : 7; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t slave3_add : 7; - uint8_t r_3 : 1; + uint8_t slave3_add : 7; + uint8_t r_3 : 1; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_slv3_add_t; -#define LSM6DSV16X_SLV3_SUBADD 0x1FU -typedef struct { +#define LSM6DSV16X_SLV3_SUBADD 0x1FU +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t slave3_reg : 8; + uint8_t slave3_reg : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t slave3_reg : 8; + uint8_t slave3_reg : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_slv3_subadd_t; -#define LSM6DSV16X_SLV3_CONFIG 0x20U -typedef struct { +#define LSM6DSV16X_SLV3_CONFIG 0x20U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t slave3_numop : 3; + uint8_t slave3_numop : 3; uint8_t batch_ext_sens_3_en : 1; - uint8_t not_used0 : 4; + uint8_t not_used0 : 4; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t not_used0 : 4; + uint8_t not_used0 : 4; uint8_t batch_ext_sens_3_en : 1; - uint8_t slave3_numop : 3; + uint8_t slave3_numop : 3; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_slv3_config_t; -#define LSM6DSV16X_DATAWRITE_SLV0 0x21U -typedef struct { +#define LSM6DSV16X_DATAWRITE_SLV0 0x21U +typedef struct +{ #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t slave0_dataw : 8; + uint8_t slave0_dataw : 8; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t slave0_dataw : 8; + uint8_t slave0_dataw : 8; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_datawrite_slv0_t; -#define LSM6DSV16X_STATUS_MASTER 0x22U -typedef struct { -#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t sens_hub_endop : 1; - uint8_t not_used0 : 2; - uint8_t slave0_nack : 1; - uint8_t slave1_nack : 1; - uint8_t slave2_nack : 1; - uint8_t slave3_nack : 1; - uint8_t wr_once_done : 1; -#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t wr_once_done : 1; - uint8_t slave3_nack : 1; - uint8_t slave2_nack : 1; - uint8_t slave1_nack : 1; - uint8_t slave0_nack : 1; - uint8_t not_used0 : 2; - uint8_t sens_hub_endop : 1; +#define LSM6DSV16X_STATUS_MASTER 0x22U +typedef struct +{ +#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN + uint8_t sens_hub_endop : 1; + uint8_t not_used0 : 2; + uint8_t slave0_nack : 1; + uint8_t slave1_nack : 1; + uint8_t slave2_nack : 1; + uint8_t slave3_nack : 1; + uint8_t wr_once_done : 1; +#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN + uint8_t wr_once_done : 1; + uint8_t slave3_nack : 1; + uint8_t slave2_nack : 1; + uint8_t slave1_nack : 1; + uint8_t slave0_nack : 1; + uint8_t not_used0 : 2; + uint8_t sens_hub_endop : 1; #endif /* DRV_BYTE_ORDER */ } lsm6dsv16x_status_master_t; @@ -3303,921 +3602,1126 @@ typedef struct { * */ -typedef union { - lsm6dsv16x_func_cfg_access_t func_cfg_access; - lsm6dsv16x_pin_ctrl_t pin_ctrl; - lsm6dsv16x_if_cfg_t if_cfg; - lsm6dsv16x_s4s_tph_l_t s4s_tph_l; - lsm6dsv16x_s4s_tph_h_t s4s_tph_h; - lsm6dsv16x_s4s_rr_t s4s_rr; - lsm6dsv16x_fifo_ctrl1_t fifo_ctrl1; - lsm6dsv16x_fifo_ctrl2_t fifo_ctrl2; - lsm6dsv16x_fifo_ctrl3_t fifo_ctrl3; - lsm6dsv16x_fifo_ctrl4_t fifo_ctrl4; - lsm6dsv16x_counter_bdr_reg1_t counter_bdr_reg1; - lsm6dsv16x_counter_bdr_reg2_t counter_bdr_reg2; - lsm6dsv16x_int1_ctrl_t int1_ctrl; - lsm6dsv16x_int2_ctrl_t int2_ctrl; - lsm6dsv16x_who_am_i_t who_am_i; - lsm6dsv16x_ctrl1_t ctrl1; - lsm6dsv16x_ctrl2_t ctrl2; - lsm6dsv16x_ctrl3_t ctrl3; - lsm6dsv16x_ctrl4_t ctrl4; - lsm6dsv16x_ctrl5_t ctrl5; - lsm6dsv16x_ctrl6_t ctrl6; - lsm6dsv16x_ctrl7_t ctrl7; - lsm6dsv16x_ctrl8_t ctrl8; - lsm6dsv16x_ctrl9_t ctrl9; - lsm6dsv16x_ctrl10_t ctrl10; - lsm6dsv16x_ctrl_status_t ctrl_status; - lsm6dsv16x_fifo_status1_t fifo_status1; - lsm6dsv16x_fifo_status2_t fifo_status2; - lsm6dsv16x_all_int_src_t all_int_src; - lsm6dsv16x_status_reg_t status_reg; - lsm6dsv16x_out_temp_l_t out_temp_l; - lsm6dsv16x_out_temp_h_t out_temp_h; - lsm6dsv16x_outx_l_g_t outx_l_g; - lsm6dsv16x_outx_h_g_t outx_h_g; - lsm6dsv16x_outy_l_g_t outy_l_g; - lsm6dsv16x_outy_h_g_t outy_h_g; - lsm6dsv16x_outz_l_g_t outz_l_g; - lsm6dsv16x_outz_h_g_t outz_h_g; - lsm6dsv16x_outx_l_a_t outx_l_a; - lsm6dsv16x_outx_h_a_t outx_h_a; - lsm6dsv16x_outy_l_a_t outy_l_a; - lsm6dsv16x_outy_h_a_t outy_h_a; - lsm6dsv16x_outz_l_a_t outz_l_a; - lsm6dsv16x_outz_h_a_t outz_h_a; - lsm6dsv16x_ui_outx_l_g_ois_eis_t ui_outx_l_g_ois_eis; - lsm6dsv16x_ui_outx_h_g_ois_eis_t ui_outx_h_g_ois_eis; - lsm6dsv16x_ui_outy_l_g_ois_eis_t ui_outy_l_g_ois_eis; - lsm6dsv16x_ui_outy_h_g_ois_eis_t ui_outy_h_g_ois_eis; - lsm6dsv16x_ui_outz_l_g_ois_eis_t ui_outz_l_g_ois_eis; - lsm6dsv16x_ui_outz_h_g_ois_eis_t ui_outz_h_g_ois_eis; +/** + * @defgroup LSM6DSO_Register_Union + * @brief This union group all the registers having a bit-field + * description. + * This union is useful but it's not needed by the driver. + * + * REMOVING this union you are compliant with: + * MISRA-C 2012 [Rule 19.2] -> " Union are not allowed " + * + * @{ + * + */ +typedef union +{ + lsm6dsv16x_func_cfg_access_t func_cfg_access; + lsm6dsv16x_pin_ctrl_t pin_ctrl; + lsm6dsv16x_if_cfg_t if_cfg; + lsm6dsv16x_odr_trig_cfg_t odr_trig_cfg; + lsm6dsv16x_fifo_ctrl1_t fifo_ctrl1; + lsm6dsv16x_fifo_ctrl2_t fifo_ctrl2; + lsm6dsv16x_fifo_ctrl3_t fifo_ctrl3; + lsm6dsv16x_fifo_ctrl4_t fifo_ctrl4; + lsm6dsv16x_counter_bdr_reg1_t counter_bdr_reg1; + lsm6dsv16x_counter_bdr_reg2_t counter_bdr_reg2; + lsm6dsv16x_int1_ctrl_t int1_ctrl; + lsm6dsv16x_int2_ctrl_t int2_ctrl; + lsm6dsv16x_who_am_i_t who_am_i; + lsm6dsv16x_ctrl1_t ctrl1; + lsm6dsv16x_ctrl2_t ctrl2; + lsm6dsv16x_ctrl3_t ctrl3; + lsm6dsv16x_ctrl4_t ctrl4; + lsm6dsv16x_ctrl5_t ctrl5; + lsm6dsv16x_ctrl6_t ctrl6; + lsm6dsv16x_ctrl7_t ctrl7; + lsm6dsv16x_ctrl8_t ctrl8; + lsm6dsv16x_ctrl9_t ctrl9; + lsm6dsv16x_ctrl10_t ctrl10; + lsm6dsv16x_ctrl_status_t ctrl_status; + lsm6dsv16x_fifo_status1_t fifo_status1; + lsm6dsv16x_fifo_status2_t fifo_status2; + lsm6dsv16x_all_int_src_t all_int_src; + lsm6dsv16x_status_reg_t status_reg; + lsm6dsv16x_out_temp_l_t out_temp_l; + lsm6dsv16x_out_temp_h_t out_temp_h; + lsm6dsv16x_outx_l_g_t outx_l_g; + lsm6dsv16x_outx_h_g_t outx_h_g; + lsm6dsv16x_outy_l_g_t outy_l_g; + lsm6dsv16x_outy_h_g_t outy_h_g; + lsm6dsv16x_outz_l_g_t outz_l_g; + lsm6dsv16x_outz_h_g_t outz_h_g; + lsm6dsv16x_outx_l_a_t outx_l_a; + lsm6dsv16x_outx_h_a_t outx_h_a; + lsm6dsv16x_outy_l_a_t outy_l_a; + lsm6dsv16x_outy_h_a_t outy_h_a; + lsm6dsv16x_outz_l_a_t outz_l_a; + lsm6dsv16x_outz_h_a_t outz_h_a; + lsm6dsv16x_ui_outx_l_g_ois_eis_t ui_outx_l_g_ois_eis; + lsm6dsv16x_ui_outx_h_g_ois_eis_t ui_outx_h_g_ois_eis; + lsm6dsv16x_ui_outy_l_g_ois_eis_t ui_outy_l_g_ois_eis; + lsm6dsv16x_ui_outy_h_g_ois_eis_t ui_outy_h_g_ois_eis; + lsm6dsv16x_ui_outz_l_g_ois_eis_t ui_outz_l_g_ois_eis; + lsm6dsv16x_ui_outz_h_g_ois_eis_t ui_outz_h_g_ois_eis; lsm6dsv16x_ui_outx_l_a_ois_dualc_t ui_outx_l_a_ois_dualc; lsm6dsv16x_ui_outx_h_a_ois_dualc_t ui_outx_h_a_ois_dualc; lsm6dsv16x_ui_outy_l_a_ois_dualc_t ui_outy_l_a_ois_dualc; lsm6dsv16x_ui_outy_h_a_ois_dualc_t ui_outy_h_a_ois_dualc; lsm6dsv16x_ui_outz_l_a_ois_dualc_t ui_outz_l_a_ois_dualc; lsm6dsv16x_ui_outz_h_a_ois_dualc_t ui_outz_h_a_ois_dualc; - lsm6dsv16x_ah_qvar_out_l_t ah_qvar_out_l; - lsm6dsv16x_ah_qvar_out_h_t ah_qvar_out_h; - lsm6dsv16x_timestamp0_t timestamp0; - lsm6dsv16x_timestamp1_t timestamp1; - lsm6dsv16x_timestamp2_t timestamp2; - lsm6dsv16x_timestamp3_t timestamp3; - lsm6dsv16x_ui_status_reg_ois_t ui_status_reg_ois; - lsm6dsv16x_wake_up_src_t wake_up_src; - lsm6dsv16x_tap_src_t tap_src; - lsm6dsv16x_d6d_src_t d6d_src; - lsm6dsv16x_status_master_mainpage_t status_master_mainpage; - lsm6dsv16x_emb_func_status_mainpage_t emb_func_status_mainpage; - lsm6dsv16x_fsm_status_mainpage_t fsm_status_mainpage; - lsm6dsv16x_mlc_status_mainpage_t mlc_status_mainpage; - lsm6dsv16x_internal_freq_t internal_freq; - lsm6dsv16x_functions_enable_t functions_enable; - lsm6dsv16x_den_t den; - lsm6dsv16x_inactivity_dur_t inactivity_dur; - lsm6dsv16x_inactivity_ths_t inactivity_ths; - lsm6dsv16x_tap_cfg0_t tap_cfg0; - lsm6dsv16x_tap_cfg1_t tap_cfg1; - lsm6dsv16x_tap_cfg2_t tap_cfg2; - lsm6dsv16x_tap_ths_6d_t tap_ths_6d; - lsm6dsv16x_int_dur2_t int_dur2; - lsm6dsv16x_wake_up_ths_t wake_up_ths; - lsm6dsv16x_wake_up_dur_t wake_up_dur; - lsm6dsv16x_free_fall_t free_fall; - lsm6dsv16x_md1_cfg_t md1_cfg; - lsm6dsv16x_md2_cfg_t md2_cfg; - lsm6dsv16x_s4s_st_cmd_code_t s4s_st_cmd_code; - lsm6dsv16x_s4s_dt_reg_t s4s_dt_reg; - lsm6dsv16x_emb_func_cfg_t emb_func_cfg; - lsm6dsv16x_ui_handshake_ctrl_t ui_handshake_ctrl; - lsm6dsv16x_ui_spi2_shared_0_t ui_spi2_shared_0; - lsm6dsv16x_ui_spi2_shared_1_t ui_spi2_shared_1; - lsm6dsv16x_ui_spi2_shared_2_t ui_spi2_shared_2; - lsm6dsv16x_ui_spi2_shared_3_t ui_spi2_shared_3; - lsm6dsv16x_ui_spi2_shared_4_t ui_spi2_shared_4; - lsm6dsv16x_ui_spi2_shared_5_t ui_spi2_shared_5; - lsm6dsv16x_ctrl_eis_t ctrl_eis; - lsm6dsv16x_ui_int_ois_t ui_int_ois; - lsm6dsv16x_ui_ctrl1_ois_t ui_ctrl1_ois; - lsm6dsv16x_ui_ctrl2_ois_t ui_ctrl2_ois; - lsm6dsv16x_ui_ctrl3_ois_t ui_ctrl3_ois; - lsm6dsv16x_x_ofs_usr_t x_ofs_usr; - lsm6dsv16x_y_ofs_usr_t y_ofs_usr; - lsm6dsv16x_z_ofs_usr_t z_ofs_usr; - lsm6dsv16x_fifo_data_out_tag_t fifo_data_out_tag; - lsm6dsv16x_fifo_data_out_x_l_t fifo_data_out_x_l; - lsm6dsv16x_fifo_data_out_x_h_t fifo_data_out_x_h; - lsm6dsv16x_fifo_data_out_y_l_t fifo_data_out_y_l; - lsm6dsv16x_fifo_data_out_y_h_t fifo_data_out_y_h; - lsm6dsv16x_fifo_data_out_z_l_t fifo_data_out_z_l; - lsm6dsv16x_fifo_data_out_z_h_t fifo_data_out_z_h; - bitwise_t bitwise; - uint8_t byte; -} prefix_lowmain_t; - -typedef union { - lsm6dsv16x_spi2_who_am_i_t spi2_who_am_i; - lsm6dsv16x_spi2_status_reg_ois_t spi2_status_reg_ois; - lsm6dsv16x_spi2_out_temp_l_t spi2_out_temp_l; - lsm6dsv16x_spi2_out_temp_h_t spi2_out_temp_h; - lsm6dsv16x_spi2_outx_l_g_ois_t spi2_outx_l_g_ois; - lsm6dsv16x_spi2_outx_h_g_ois_t spi2_outx_h_g_ois; - lsm6dsv16x_spi2_outy_l_g_ois_t spi2_outy_l_g_ois; - lsm6dsv16x_spi2_outy_h_g_ois_t spi2_outy_h_g_ois; - lsm6dsv16x_spi2_outz_l_g_ois_t spi2_outz_l_g_ois; - lsm6dsv16x_spi2_outz_h_g_ois_t spi2_outz_h_g_ois; - lsm6dsv16x_spi2_outx_l_a_ois_t spi2_outx_l_a_ois; - lsm6dsv16x_spi2_outx_h_a_ois_t spi2_outx_h_a_ois; - lsm6dsv16x_spi2_outy_l_a_ois_t spi2_outy_l_a_ois; - lsm6dsv16x_spi2_outy_h_a_ois_t spi2_outy_h_a_ois; - lsm6dsv16x_spi2_outz_l_a_ois_t spi2_outz_l_a_ois; - lsm6dsv16x_spi2_outz_h_a_ois_t spi2_outz_h_a_ois; - lsm6dsv16x_spi2_handshake_ctrl_t spi2_handshake_ctrl; - lsm6dsv16x_spi2_int_ois_t spi2_int_ois; - lsm6dsv16x_spi2_ctrl1_ois_t spi2_ctrl1_ois; - lsm6dsv16x_spi2_ctrl2_ois_t spi2_ctrl2_ois; - lsm6dsv16x_spi2_ctrl3_ois_t spi2_ctrl3_ois; - bitwise_t bitwise; - uint8_t byte; -} prefix_lowspi2_t; - -typedef union { - lsm6dsv16x_page_sel_t page_sel; - lsm6dsv16x_emb_func_en_a_t emb_func_en_a; - lsm6dsv16x_emb_func_en_b_t emb_func_en_b; - lsm6dsv16x_emb_func_exec_status_t emb_func_exec_status; - lsm6dsv16x_page_address_t page_address; - lsm6dsv16x_page_value_t page_value; - lsm6dsv16x_emb_func_int1_t emb_func_int1; - lsm6dsv16x_fsm_int1_t fsm_int1; - lsm6dsv16x_mlc_int1_t mlc_int1; - lsm6dsv16x_emb_func_int2_t emb_func_int2; - lsm6dsv16x_fsm_int2_t fsm_int2; - lsm6dsv16x_mlc_int2_t mlc_int2; - lsm6dsv16x_emb_func_status_t emb_func_status; - lsm6dsv16x_fsm_status_t fsm_status; - lsm6dsv16x_mlc_status_t mlc_status; - lsm6dsv16x_page_rw_t page_rw; - lsm6dsv16x_emb_func_fifo_en_a_t emb_func_fifo_en_a; - lsm6dsv16x_emb_func_fifo_en_b_t emb_func_fifo_en_b; - lsm6dsv16x_fsm_enable_t fsm_enable; - lsm6dsv16x_fsm_long_counter_l_t fsm_long_counter_l; - lsm6dsv16x_fsm_long_counter_h_t fsm_long_counter_h; - lsm6dsv16x_int_ack_mask_t int_ack_mask; - lsm6dsv16x_fsm_outs1_t fsm_outs1; - lsm6dsv16x_fsm_outs2_t fsm_outs2; - lsm6dsv16x_fsm_outs3_t fsm_outs3; - lsm6dsv16x_fsm_outs4_t fsm_outs4; - lsm6dsv16x_fsm_outs5_t fsm_outs5; - lsm6dsv16x_fsm_outs6_t fsm_outs6; - lsm6dsv16x_fsm_outs7_t fsm_outs7; - lsm6dsv16x_fsm_outs8_t fsm_outs8; - lsm6dsv16x_fsm_odr_t fsm_odr; - lsm6dsv16x_mlc_odr_t mlc_odr; - lsm6dsv16x_step_counter_l_t step_counter_l; - lsm6dsv16x_step_counter_h_t step_counter_h; - lsm6dsv16x_emb_func_src_t emb_func_src; - lsm6dsv16x_emb_func_init_a_t emb_func_init_a; - lsm6dsv16x_emb_func_init_b_t emb_func_init_b; - lsm6dsv16x_mlc1_src_t mlc1_src; - lsm6dsv16x_mlc2_src_t mlc2_src; - lsm6dsv16x_mlc3_src_t mlc3_src; - lsm6dsv16x_mlc4_src_t mlc4_src; - bitwise_t bitwise; - uint8_t byte; -} prefix_lowembedded_t; - -typedef union { + lsm6dsv16x_ah_qvar_out_l_t ah_qvar_out_l; + lsm6dsv16x_ah_qvar_out_h_t ah_qvar_out_h; + lsm6dsv16x_timestamp0_t timestamp0; + lsm6dsv16x_timestamp1_t timestamp1; + lsm6dsv16x_timestamp2_t timestamp2; + lsm6dsv16x_timestamp3_t timestamp3; + lsm6dsv16x_ui_status_reg_ois_t ui_status_reg_ois; + lsm6dsv16x_wake_up_src_t wake_up_src; + lsm6dsv16x_tap_src_t tap_src; + lsm6dsv16x_d6d_src_t d6d_src; + lsm6dsv16x_status_master_mainpage_t status_master_mainpage; + lsm6dsv16x_emb_func_status_mainpage_t emb_func_status_mainpage; + lsm6dsv16x_fsm_status_mainpage_t fsm_status_mainpage; + lsm6dsv16x_mlc_status_mainpage_t mlc_status_mainpage; + lsm6dsv16x_internal_freq_t internal_freq; + lsm6dsv16x_functions_enable_t functions_enable; + lsm6dsv16x_den_t den; + lsm6dsv16x_inactivity_dur_t inactivity_dur; + lsm6dsv16x_inactivity_ths_t inactivity_ths; + lsm6dsv16x_tap_cfg0_t tap_cfg0; + lsm6dsv16x_tap_cfg1_t tap_cfg1; + lsm6dsv16x_tap_cfg2_t tap_cfg2; + lsm6dsv16x_tap_ths_6d_t tap_ths_6d; + lsm6dsv16x_tap_dur_t tap_dur; + lsm6dsv16x_wake_up_ths_t wake_up_ths; + lsm6dsv16x_wake_up_dur_t wake_up_dur; + lsm6dsv16x_free_fall_t free_fall; + lsm6dsv16x_md1_cfg_t md1_cfg; + lsm6dsv16x_md2_cfg_t md2_cfg; + lsm6dsv16x_emb_func_cfg_t emb_func_cfg; + lsm6dsv16x_ui_handshake_ctrl_t ui_handshake_ctrl; + lsm6dsv16x_ui_spi2_shared_0_t ui_spi2_shared_0; + lsm6dsv16x_ui_spi2_shared_1_t ui_spi2_shared_1; + lsm6dsv16x_ui_spi2_shared_2_t ui_spi2_shared_2; + lsm6dsv16x_ui_spi2_shared_3_t ui_spi2_shared_3; + lsm6dsv16x_ui_spi2_shared_4_t ui_spi2_shared_4; + lsm6dsv16x_ui_spi2_shared_5_t ui_spi2_shared_5; + lsm6dsv16x_ctrl_eis_t ctrl_eis; + lsm6dsv16x_ui_int_ois_t ui_int_ois; + lsm6dsv16x_ui_ctrl1_ois_t ui_ctrl1_ois; + lsm6dsv16x_ui_ctrl2_ois_t ui_ctrl2_ois; + lsm6dsv16x_ui_ctrl3_ois_t ui_ctrl3_ois; + lsm6dsv16x_x_ofs_usr_t x_ofs_usr; + lsm6dsv16x_y_ofs_usr_t y_ofs_usr; + lsm6dsv16x_z_ofs_usr_t z_ofs_usr; + lsm6dsv16x_fifo_data_out_tag_t fifo_data_out_tag; + lsm6dsv16x_fifo_data_out_x_l_t fifo_data_out_x_l; + lsm6dsv16x_fifo_data_out_x_h_t fifo_data_out_x_h; + lsm6dsv16x_fifo_data_out_y_l_t fifo_data_out_y_l; + lsm6dsv16x_fifo_data_out_y_h_t fifo_data_out_y_h; + lsm6dsv16x_fifo_data_out_z_l_t fifo_data_out_z_l; + lsm6dsv16x_fifo_data_out_z_h_t fifo_data_out_z_h; + lsm6dsv16x_spi2_who_am_i_t spi2_who_am_i; + lsm6dsv16x_spi2_status_reg_ois_t spi2_status_reg_ois; + lsm6dsv16x_spi2_out_temp_l_t spi2_out_temp_l; + lsm6dsv16x_spi2_out_temp_h_t spi2_out_temp_h; + lsm6dsv16x_spi2_outx_l_g_ois_t spi2_outx_l_g_ois; + lsm6dsv16x_spi2_outx_h_g_ois_t spi2_outx_h_g_ois; + lsm6dsv16x_spi2_outy_l_g_ois_t spi2_outy_l_g_ois; + lsm6dsv16x_spi2_outy_h_g_ois_t spi2_outy_h_g_ois; + lsm6dsv16x_spi2_outz_l_g_ois_t spi2_outz_l_g_ois; + lsm6dsv16x_spi2_outz_h_g_ois_t spi2_outz_h_g_ois; + lsm6dsv16x_spi2_outx_l_a_ois_t spi2_outx_l_a_ois; + lsm6dsv16x_spi2_outx_h_a_ois_t spi2_outx_h_a_ois; + lsm6dsv16x_spi2_outy_l_a_ois_t spi2_outy_l_a_ois; + lsm6dsv16x_spi2_outy_h_a_ois_t spi2_outy_h_a_ois; + lsm6dsv16x_spi2_outz_l_a_ois_t spi2_outz_l_a_ois; + lsm6dsv16x_spi2_outz_h_a_ois_t spi2_outz_h_a_ois; + lsm6dsv16x_spi2_handshake_ctrl_t spi2_handshake_ctrl; + lsm6dsv16x_spi2_int_ois_t spi2_int_ois; + lsm6dsv16x_spi2_ctrl1_ois_t spi2_ctrl1_ois; + lsm6dsv16x_spi2_ctrl2_ois_t spi2_ctrl2_ois; + lsm6dsv16x_spi2_ctrl3_ois_t spi2_ctrl3_ois; + lsm6dsv16x_page_sel_t page_sel; + lsm6dsv16x_emb_func_en_a_t emb_func_en_a; + lsm6dsv16x_emb_func_en_b_t emb_func_en_b; + lsm6dsv16x_emb_func_exec_status_t emb_func_exec_status; + lsm6dsv16x_page_address_t page_address; + lsm6dsv16x_page_value_t page_value; + lsm6dsv16x_emb_func_int1_t emb_func_int1; + lsm6dsv16x_fsm_int1_t fsm_int1; + lsm6dsv16x_mlc_int1_t mlc_int1; + lsm6dsv16x_emb_func_int2_t emb_func_int2; + lsm6dsv16x_fsm_int2_t fsm_int2; + lsm6dsv16x_mlc_int2_t mlc_int2; + lsm6dsv16x_emb_func_status_t emb_func_status; + lsm6dsv16x_fsm_status_t fsm_status; + lsm6dsv16x_mlc_status_t mlc_status; + lsm6dsv16x_page_rw_t page_rw; + lsm6dsv16x_emb_func_fifo_en_a_t emb_func_fifo_en_a; + lsm6dsv16x_emb_func_fifo_en_b_t emb_func_fifo_en_b; + lsm6dsv16x_fsm_enable_t fsm_enable; + lsm6dsv16x_fsm_long_counter_l_t fsm_long_counter_l; + lsm6dsv16x_fsm_long_counter_h_t fsm_long_counter_h; + lsm6dsv16x_int_ack_mask_t int_ack_mask; + lsm6dsv16x_fsm_outs1_t fsm_outs1; + lsm6dsv16x_fsm_outs2_t fsm_outs2; + lsm6dsv16x_fsm_outs3_t fsm_outs3; + lsm6dsv16x_fsm_outs4_t fsm_outs4; + lsm6dsv16x_fsm_outs5_t fsm_outs5; + lsm6dsv16x_fsm_outs6_t fsm_outs6; + lsm6dsv16x_fsm_outs7_t fsm_outs7; + lsm6dsv16x_fsm_outs8_t fsm_outs8; + lsm6dsv16x_fsm_odr_t fsm_odr; + lsm6dsv16x_mlc_odr_t mlc_odr; + lsm6dsv16x_step_counter_l_t step_counter_l; + lsm6dsv16x_step_counter_h_t step_counter_h; + lsm6dsv16x_emb_func_src_t emb_func_src; + lsm6dsv16x_emb_func_init_a_t emb_func_init_a; + lsm6dsv16x_emb_func_init_b_t emb_func_init_b; + lsm6dsv16x_mlc1_src_t mlc1_src; + lsm6dsv16x_mlc2_src_t mlc2_src; + lsm6dsv16x_mlc3_src_t mlc3_src; + lsm6dsv16x_mlc4_src_t mlc4_src; lsm6dsv16x_fsm_ext_sensitivity_l_t fsm_ext_sensitivity_l; lsm6dsv16x_fsm_ext_sensitivity_h_t fsm_ext_sensitivity_h; - lsm6dsv16x_fsm_ext_offx_l_t fsm_ext_offx_l; - lsm6dsv16x_fsm_ext_offx_h_t fsm_ext_offx_h; - lsm6dsv16x_fsm_ext_offy_l_t fsm_ext_offy_l; - lsm6dsv16x_fsm_ext_offy_h_t fsm_ext_offy_h; - lsm6dsv16x_fsm_ext_offz_l_t fsm_ext_offz_l; - lsm6dsv16x_fsm_ext_offz_h_t fsm_ext_offz_h; - lsm6dsv16x_fsm_ext_matrix_xx_l_t fsm_ext_matrix_xx_l; - lsm6dsv16x_fsm_ext_matrix_xx_h_t fsm_ext_matrix_xx_h; - lsm6dsv16x_fsm_ext_matrix_xy_l_t fsm_ext_matrix_xy_l; - lsm6dsv16x_fsm_ext_matrix_xy_h_t fsm_ext_matrix_xy_h; - lsm6dsv16x_fsm_ext_matrix_xz_l_t fsm_ext_matrix_xz_l; - lsm6dsv16x_fsm_ext_matrix_xz_h_t fsm_ext_matrix_xz_h; - lsm6dsv16x_fsm_ext_matrix_yy_l_t fsm_ext_matrix_yy_l; - lsm6dsv16x_fsm_ext_matrix_yy_h_t fsm_ext_matrix_yy_h; - lsm6dsv16x_fsm_ext_matrix_yz_l_t fsm_ext_matrix_yz_l; - lsm6dsv16x_fsm_ext_matrix_yz_h_t fsm_ext_matrix_yz_h; - lsm6dsv16x_fsm_ext_matrix_zz_l_t fsm_ext_matrix_zz_l; - lsm6dsv16x_fsm_ext_matrix_zz_h_t fsm_ext_matrix_zz_h; - lsm6dsv16x_ext_cfg_a_t ext_cfg_a; - lsm6dsv16x_ext_cfg_b_t ext_cfg_b; - bitwise_t bitwise; - uint8_t byte; -} prefix_lowpg0_emb_adv_t; - -typedef union { - lsm6dsv16x_fsm_lc_timeout_l_t fsm_lc_timeout_l; - lsm6dsv16x_fsm_lc_timeout_h_t fsm_lc_timeout_h; - lsm6dsv16x_fsm_programs_t fsm_programs; - lsm6dsv16x_fsm_start_add_l_t fsm_start_add_l; - lsm6dsv16x_fsm_start_add_h_t fsm_start_add_h; - lsm6dsv16x_pedo_cmd_reg_t pedo_cmd_reg; - lsm6dsv16x_pedo_deb_steps_conf_t pedo_deb_steps_conf; - lsm6dsv16x_pedo_sc_deltat_l_t pedo_sc_deltat_l; - lsm6dsv16x_pedo_sc_deltat_h_t pedo_sc_deltat_h; + lsm6dsv16x_fsm_ext_offx_l_t fsm_ext_offx_l; + lsm6dsv16x_fsm_ext_offx_h_t fsm_ext_offx_h; + lsm6dsv16x_fsm_ext_offy_l_t fsm_ext_offy_l; + lsm6dsv16x_fsm_ext_offy_h_t fsm_ext_offy_h; + lsm6dsv16x_fsm_ext_offz_l_t fsm_ext_offz_l; + lsm6dsv16x_fsm_ext_offz_h_t fsm_ext_offz_h; + lsm6dsv16x_fsm_ext_matrix_xx_l_t fsm_ext_matrix_xx_l; + lsm6dsv16x_fsm_ext_matrix_xx_h_t fsm_ext_matrix_xx_h; + lsm6dsv16x_fsm_ext_matrix_xy_l_t fsm_ext_matrix_xy_l; + lsm6dsv16x_fsm_ext_matrix_xy_h_t fsm_ext_matrix_xy_h; + lsm6dsv16x_fsm_ext_matrix_xz_l_t fsm_ext_matrix_xz_l; + lsm6dsv16x_fsm_ext_matrix_xz_h_t fsm_ext_matrix_xz_h; + lsm6dsv16x_fsm_ext_matrix_yy_l_t fsm_ext_matrix_yy_l; + lsm6dsv16x_fsm_ext_matrix_yy_h_t fsm_ext_matrix_yy_h; + lsm6dsv16x_fsm_ext_matrix_yz_l_t fsm_ext_matrix_yz_l; + lsm6dsv16x_fsm_ext_matrix_yz_h_t fsm_ext_matrix_yz_h; + lsm6dsv16x_fsm_ext_matrix_zz_l_t fsm_ext_matrix_zz_l; + lsm6dsv16x_fsm_ext_matrix_zz_h_t fsm_ext_matrix_zz_h; + lsm6dsv16x_ext_cfg_a_t ext_cfg_a; + lsm6dsv16x_ext_cfg_b_t ext_cfg_b; + lsm6dsv16x_fsm_lc_timeout_l_t fsm_lc_timeout_l; + lsm6dsv16x_fsm_lc_timeout_h_t fsm_lc_timeout_h; + lsm6dsv16x_fsm_programs_t fsm_programs; + lsm6dsv16x_fsm_start_add_l_t fsm_start_add_l; + lsm6dsv16x_fsm_start_add_h_t fsm_start_add_h; + lsm6dsv16x_pedo_cmd_reg_t pedo_cmd_reg; + lsm6dsv16x_pedo_deb_steps_conf_t pedo_deb_steps_conf; + lsm6dsv16x_pedo_sc_deltat_l_t pedo_sc_deltat_l; + lsm6dsv16x_pedo_sc_deltat_h_t pedo_sc_deltat_h; lsm6dsv16x_mlc_ext_sensitivity_l_t mlc_ext_sensitivity_l; lsm6dsv16x_mlc_ext_sensitivity_h_t mlc_ext_sensitivity_h; - bitwise_t bitwise; - uint8_t byte; -} prefix_lowpg1_emb_adv_t; - -typedef union { - lsm6dsv16x_sensor_hub_1_t sensor_hub_1; - lsm6dsv16x_sensor_hub_2_t sensor_hub_2; - lsm6dsv16x_sensor_hub_3_t sensor_hub_3; - lsm6dsv16x_sensor_hub_4_t sensor_hub_4; - lsm6dsv16x_sensor_hub_5_t sensor_hub_5; - lsm6dsv16x_sensor_hub_6_t sensor_hub_6; - lsm6dsv16x_sensor_hub_7_t sensor_hub_7; - lsm6dsv16x_sensor_hub_8_t sensor_hub_8; - lsm6dsv16x_sensor_hub_9_t sensor_hub_9; - lsm6dsv16x_sensor_hub_10_t sensor_hub_10; - lsm6dsv16x_sensor_hub_11_t sensor_hub_11; - lsm6dsv16x_sensor_hub_12_t sensor_hub_12; - lsm6dsv16x_sensor_hub_13_t sensor_hub_13; - lsm6dsv16x_sensor_hub_14_t sensor_hub_14; - lsm6dsv16x_sensor_hub_15_t sensor_hub_15; - lsm6dsv16x_sensor_hub_16_t sensor_hub_16; - lsm6dsv16x_sensor_hub_17_t sensor_hub_17; - lsm6dsv16x_sensor_hub_18_t sensor_hub_18; - lsm6dsv16x_master_config_t master_config; - lsm6dsv16x_slv0_add_t slv0_add; - lsm6dsv16x_slv0_subadd_t slv0_subadd; - lsm6dsv16x_slv0_config_t slv0_config; - lsm6dsv16x_slv1_add_t slv1_add; - lsm6dsv16x_slv1_subadd_t slv1_subadd; - lsm6dsv16x_slv1_config_t slv1_config; - lsm6dsv16x_slv2_add_t slv2_add; - lsm6dsv16x_slv2_subadd_t slv2_subadd; - lsm6dsv16x_slv2_config_t slv2_config; - lsm6dsv16x_slv3_add_t slv3_add; - lsm6dsv16x_slv3_subadd_t slv3_subadd; - lsm6dsv16x_slv3_config_t slv3_config; - lsm6dsv16x_datawrite_slv0_t datawrite_slv0; - lsm6dsv16x_status_master_t status_master; - bitwise_t bitwise; - uint8_t byte; -} prefix_lowsensor_hub_t; + lsm6dsv16x_sensor_hub_1_t sensor_hub_1; + lsm6dsv16x_sensor_hub_2_t sensor_hub_2; + lsm6dsv16x_sensor_hub_3_t sensor_hub_3; + lsm6dsv16x_sensor_hub_4_t sensor_hub_4; + lsm6dsv16x_sensor_hub_5_t sensor_hub_5; + lsm6dsv16x_sensor_hub_6_t sensor_hub_6; + lsm6dsv16x_sensor_hub_7_t sensor_hub_7; + lsm6dsv16x_sensor_hub_8_t sensor_hub_8; + lsm6dsv16x_sensor_hub_9_t sensor_hub_9; + lsm6dsv16x_sensor_hub_10_t sensor_hub_10; + lsm6dsv16x_sensor_hub_11_t sensor_hub_11; + lsm6dsv16x_sensor_hub_12_t sensor_hub_12; + lsm6dsv16x_sensor_hub_13_t sensor_hub_13; + lsm6dsv16x_sensor_hub_14_t sensor_hub_14; + lsm6dsv16x_sensor_hub_15_t sensor_hub_15; + lsm6dsv16x_sensor_hub_16_t sensor_hub_16; + lsm6dsv16x_sensor_hub_17_t sensor_hub_17; + lsm6dsv16x_sensor_hub_18_t sensor_hub_18; + lsm6dsv16x_master_config_t master_config; + lsm6dsv16x_slv0_add_t slv0_add; + lsm6dsv16x_slv0_subadd_t slv0_subadd; + lsm6dsv16x_slv0_config_t slv0_config; + lsm6dsv16x_slv1_add_t slv1_add; + lsm6dsv16x_slv1_subadd_t slv1_subadd; + lsm6dsv16x_slv1_config_t slv1_config; + lsm6dsv16x_slv2_add_t slv2_add; + lsm6dsv16x_slv2_subadd_t slv2_subadd; + lsm6dsv16x_slv2_config_t slv2_config; + lsm6dsv16x_slv3_add_t slv3_add; + lsm6dsv16x_slv3_subadd_t slv3_subadd; + lsm6dsv16x_slv3_config_t slv3_config; + lsm6dsv16x_datawrite_slv0_t datawrite_slv0; + lsm6dsv16x_status_master_t status_master; + bitwise_t bitwise; + uint8_t byte; +} lsm6dsv16x_reg_t; /** * @} * */ -int32_t lsm6dsv16x_read_reg(lsm6dsv16x_ctx_t *ctx, uint8_t reg, +#ifndef __weak +#define __weak __attribute__((weak)) +#endif /* __weak */ + +/* + * These are the basic platform dependent I/O routines to read + * and write device registers connected on a standard bus. + * The driver keeps offering a default implementation based on function + * pointers to read/write routines for backward compatibility. + * The __weak directive allows the final application to overwrite + * them with a custom implementation. + */ + +int32_t lsm6dsv16x_read_reg(stmdev_ctx_t *ctx, uint8_t reg, uint8_t *data, uint16_t len); -int32_t lsm6dsv16x_write_reg(lsm6dsv16x_ctx_t *ctx, uint8_t reg, +int32_t lsm6dsv16x_write_reg(stmdev_ctx_t *ctx, uint8_t reg, uint8_t *data, uint16_t len); -float lsm6dsv16x_from_fs2_to_mg(int16_t lsb); -float lsm6dsv16x_from_fs4_to_mg(int16_t lsb); -float lsm6dsv16x_from_fs8_to_mg(int16_t lsb); -float lsm6dsv16x_from_fs16_to_mg(int16_t lsb); +float_t lsm6dsv16x_from_sflp_to_mg(int16_t lsb); +float_t lsm6dsv16x_from_fs2_to_mg(int16_t lsb); +float_t lsm6dsv16x_from_fs4_to_mg(int16_t lsb); +float_t lsm6dsv16x_from_fs8_to_mg(int16_t lsb); +float_t lsm6dsv16x_from_fs16_to_mg(int16_t lsb); -float lsm6dsv16x_from_fs125_to_mdps(int16_t lsb); -float lsm6dsv16x_from_fs500_to_mdps(int16_t lsb); -float lsm6dsv16x_from_fs250_to_mdps(int16_t lsb); -float lsm6dsv16x_from_fs1000_to_mdps(int16_t lsb); -float lsm6dsv16x_from_fs2000_to_mdps(int16_t lsb); -float lsm6dsv16x_from_fs4000_to_mdps(int16_t lsb); +float_t lsm6dsv16x_from_fs125_to_mdps(int16_t lsb); +float_t lsm6dsv16x_from_fs500_to_mdps(int16_t lsb); +float_t lsm6dsv16x_from_fs250_to_mdps(int16_t lsb); +float_t lsm6dsv16x_from_fs1000_to_mdps(int16_t lsb); +float_t lsm6dsv16x_from_fs2000_to_mdps(int16_t lsb); +float_t lsm6dsv16x_from_fs4000_to_mdps(int16_t lsb); -float lsm6dsv16x_from_lsb_to_celsius(int16_t lsb); +float_t lsm6dsv16x_from_lsb_to_celsius(int16_t lsb); -float lsm6dsv16x_from_lsb_to_nsec(uint32_t lsb); +float_t lsm6dsv16x_from_lsb_to_nsec(uint32_t lsb); -int32_t lsm6dsv16x_xl_offset_on_out_set(lsm6dsv16x_ctx_t *ctx, uint8_t val); -int32_t lsm6dsv16x_xl_offset_on_out_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val); +float_t lsm6dsv16x_from_lsb_to_mv(int16_t lsb); -typedef struct { - float z_mg; - float y_mg; - float x_mg; -} lsm6dsv16x_xl_offset_mg_t; -int32_t lsm6dsv16x_xl_offset_mg_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_xl_offset_mg_t val); -int32_t lsm6dsv16x_xl_offset_mg_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_xl_offset_mg_t *val); +int32_t lsm6dsv16x_xl_offset_on_out_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6dsv16x_xl_offset_on_out_get(stmdev_ctx_t *ctx, uint8_t *val); -typedef enum { - LSM6DSV16X_READY = 0x0, - LSM6DSV16X_GLOBAL_RST = 0x1, +typedef struct +{ + float_t z_mg; + float_t y_mg; + float_t x_mg; +} lsm6dsv16x_xl_offset_mg_t; +int32_t lsm6dsv16x_xl_offset_mg_set(stmdev_ctx_t *ctx, + lsm6dsv16x_xl_offset_mg_t val); +int32_t lsm6dsv16x_xl_offset_mg_get(stmdev_ctx_t *ctx, + lsm6dsv16x_xl_offset_mg_t *val); + +typedef enum +{ + LSM6DSV16X_READY = 0x0, + LSM6DSV16X_GLOBAL_RST = 0x1, LSM6DSV16X_RESTORE_CAL_PARAM = 0x2, LSM6DSV16X_RESTORE_CTRL_REGS = 0x4, } lsm6dsv16x_reset_t; -int32_t lsm6dsv16x_reset_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_reset_t val); -int32_t lsm6dsv16x_reset_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_reset_t *val); +int32_t lsm6dsv16x_reset_set(stmdev_ctx_t *ctx, lsm6dsv16x_reset_t val); +int32_t lsm6dsv16x_reset_get(stmdev_ctx_t *ctx, lsm6dsv16x_reset_t *val); -typedef enum { - LSM6DSV16X_MAIN_MEM_BANK = 0x0, +typedef enum +{ + LSM6DSV16X_MAIN_MEM_BANK = 0x0, LSM6DSV16X_EMBED_FUNC_MEM_BANK = 0x1, LSM6DSV16X_SENSOR_HUB_MEM_BANK = 0x2, } lsm6dsv16x_mem_bank_t; -int32_t lsm6dsv16x_mem_bank_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_mem_bank_t val); -int32_t lsm6dsv16x_mem_bank_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_mem_bank_t *val); - -int32_t lsm6dsv16x_device_id_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val); - -typedef enum { - LSM6DSV16X_XL_ODR_OFF = 0x0, - LSM6DSV16X_XL_ODR_AT_1Hz875 = 0x1, - LSM6DSV16X_XL_ODR_AT_7Hz5 = 0x2, - LSM6DSV16X_XL_ODR_AT_15Hz = 0x3, - LSM6DSV16X_XL_ODR_AT_30Hz = 0x4, - LSM6DSV16X_XL_ODR_AT_60Hz = 0x5, - LSM6DSV16X_XL_ODR_AT_120Hz = 0x6, - LSM6DSV16X_XL_ODR_AT_240Hz = 0x7, - LSM6DSV16X_XL_ODR_AT_480Hz = 0x8, - LSM6DSV16X_XL_ODR_AT_960Hz = 0x9, - LSM6DSV16X_XL_ODR_AT_1920Hz = 0xA, - LSM6DSV16X_XL_ODR_AT_3840Hz = 0xB, - LSM6DSV16X_XL_ODR_AT_7680Hz = 0xC, -} lsm6dsv16x_xl_data_rate_t; -int32_t lsm6dsv16x_xl_data_rate_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_xl_data_rate_t val); -int32_t lsm6dsv16x_xl_data_rate_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_xl_data_rate_t *val); - -typedef enum { - LSM6DSV16X_XL_HIGH_PERFORMANCE_MD = 0x0, - LSM6DSV16X_XL_HIGH_ACCURANCY_ODR_MD = 0x1, - LSM6DSV16X_XL_LOW_POWER_2_AVG_MD = 0x4, - LSM6DSV16X_XL_LOW_POWER_4_AVG_MD = 0x5, - LSM6DSV16X_XL_LOW_POWER_8_AVG_MD = 0x6, - LSM6DSV16X_XL_NORMAL_MD = 0x7, +int32_t lsm6dsv16x_mem_bank_set(stmdev_ctx_t *ctx, lsm6dsv16x_mem_bank_t val); +int32_t lsm6dsv16x_mem_bank_get(stmdev_ctx_t *ctx, lsm6dsv16x_mem_bank_t *val); + +int32_t lsm6dsv16x_device_id_get(stmdev_ctx_t *ctx, uint8_t *val); + +typedef enum +{ + LSM6DSV16X_ODR_OFF = 0x0, + LSM6DSV16X_ODR_AT_1Hz875 = 0x1, + LSM6DSV16X_ODR_AT_7Hz5 = 0x2, + LSM6DSV16X_ODR_AT_15Hz = 0x3, + LSM6DSV16X_ODR_AT_30Hz = 0x4, + LSM6DSV16X_ODR_AT_60Hz = 0x5, + LSM6DSV16X_ODR_AT_120Hz = 0x6, + LSM6DSV16X_ODR_AT_240Hz = 0x7, + LSM6DSV16X_ODR_AT_480Hz = 0x8, + LSM6DSV16X_ODR_AT_960Hz = 0x9, + LSM6DSV16X_ODR_AT_1920Hz = 0xA, + LSM6DSV16X_ODR_AT_3840Hz = 0xB, + LSM6DSV16X_ODR_AT_7680Hz = 0xC, + LSM6DSV16X_ODR_HA01_AT_15Hz625 = 0x13, + LSM6DSV16X_ODR_HA01_AT_31Hz25 = 0x14, + LSM6DSV16X_ODR_HA01_AT_62Hz5 = 0x15, + LSM6DSV16X_ODR_HA01_AT_125Hz = 0x16, + LSM6DSV16X_ODR_HA01_AT_250Hz = 0x17, + LSM6DSV16X_ODR_HA01_AT_500Hz = 0x18, + LSM6DSV16X_ODR_HA01_AT_1000Hz = 0x19, + LSM6DSV16X_ODR_HA01_AT_2000Hz = 0x1A, + LSM6DSV16X_ODR_HA01_AT_4000Hz = 0x1B, + LSM6DSV16X_ODR_HA01_AT_8000Hz = 0x1C, + LSM6DSV16X_ODR_HA02_AT_12Hz5 = 0x23, + LSM6DSV16X_ODR_HA02_AT_25Hz = 0x24, + LSM6DSV16X_ODR_HA02_AT_50Hz = 0x25, + LSM6DSV16X_ODR_HA02_AT_100Hz = 0x26, + LSM6DSV16X_ODR_HA02_AT_200Hz = 0x27, + LSM6DSV16X_ODR_HA02_AT_400Hz = 0x28, + LSM6DSV16X_ODR_HA02_AT_800Hz = 0x29, + LSM6DSV16X_ODR_HA02_AT_1600Hz = 0x2A, + LSM6DSV16X_ODR_HA02_AT_3200Hz = 0x2B, + LSM6DSV16X_ODR_HA02_AT_6400Hz = 0x2C, +} lsm6dsv16x_data_rate_t; +int32_t lsm6dsv16x_xl_data_rate_set(stmdev_ctx_t *ctx, + lsm6dsv16x_data_rate_t val); +int32_t lsm6dsv16x_xl_data_rate_get(stmdev_ctx_t *ctx, + lsm6dsv16x_data_rate_t *val); +int32_t lsm6dsv16x_gy_data_rate_set(stmdev_ctx_t *ctx, + lsm6dsv16x_data_rate_t val); +int32_t lsm6dsv16x_gy_data_rate_get(stmdev_ctx_t *ctx, + lsm6dsv16x_data_rate_t *val); + + +typedef enum +{ + LSM6DSV16X_XL_HIGH_PERFORMANCE_MD = 0x0, + LSM6DSV16X_XL_HIGH_ACCURACY_ODR_MD = 0x1, + LSM6DSV16X_XL_ODR_TRIGGERED_MD = 0x3, + LSM6DSV16X_XL_LOW_POWER_2_AVG_MD = 0x4, + LSM6DSV16X_XL_LOW_POWER_4_AVG_MD = 0x5, + LSM6DSV16X_XL_LOW_POWER_8_AVG_MD = 0x6, + LSM6DSV16X_XL_NORMAL_MD = 0x7, } lsm6dsv16x_xl_mode_t; -int32_t lsm6dsv16x_xl_mode_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_xl_mode_t val); -int32_t lsm6dsv16x_xl_mode_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_xl_mode_t *val); - -typedef enum { - LSM6DSV16X_GY_ODR_OFF = 0x0, - LSM6DSV16X_GY_ODR_AT_7Hz5 = 0x2, - LSM6DSV16X_GY_ODR_AT_15Hz = 0x3, - LSM6DSV16X_GY_ODR_AT_30Hz = 0x4, - LSM6DSV16X_GY_ODR_AT_60Hz = 0x5, - LSM6DSV16X_GY_ODR_AT_120Hz = 0x6, - LSM6DSV16X_GY_ODR_AT_240Hz = 0x7, - LSM6DSV16X_GY_ODR_AT_480Hz = 0x8, - LSM6DSV16X_GY_ODR_AT_960Hz = 0x9, - LSM6DSV16X_GY_ODR_AT_1920Hz = 0xa, - LSM6DSV16X_GY_ODR_AT_3840Hz = 0xb, - LSM6DSV16X_GY_ODR_AT_7680Hz = 0xc, -} lsm6dsv16x_gy_data_rate_t; -int32_t lsm6dsv16x_gy_data_rate_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_gy_data_rate_t val); -int32_t lsm6dsv16x_gy_data_rate_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_gy_data_rate_t *val); - -typedef enum { - LSM6DSV16X_GY_HIGH_PERFORMANCE_MD = 0x0, - LSM6DSV16X_GY_HIGH_ACCURANCY_ODR_MD = 0x1, - LSM6DSV16X_GY_SLEEP_MD = 0x4, - LSM6DSV16X_GY_LOW_POWER_MD = 0x5, +int32_t lsm6dsv16x_xl_mode_set(stmdev_ctx_t *ctx, lsm6dsv16x_xl_mode_t val); +int32_t lsm6dsv16x_xl_mode_get(stmdev_ctx_t *ctx, lsm6dsv16x_xl_mode_t *val); + +typedef enum +{ + LSM6DSV16X_GY_HIGH_PERFORMANCE_MD = 0x0, + LSM6DSV16X_GY_HIGH_ACCURACY_ODR_MD = 0x1, + LSM6DSV16X_GY_SLEEP_MD = 0x4, + LSM6DSV16X_GY_LOW_POWER_MD = 0x5, } lsm6dsv16x_gy_mode_t; -int32_t lsm6dsv16x_gy_mode_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_gy_mode_t val); -int32_t lsm6dsv16x_gy_mode_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_gy_mode_t *val); +int32_t lsm6dsv16x_gy_mode_set(stmdev_ctx_t *ctx, lsm6dsv16x_gy_mode_t val); +int32_t lsm6dsv16x_gy_mode_get(stmdev_ctx_t *ctx, lsm6dsv16x_gy_mode_t *val); -int32_t lsm6dsv16x_auto_increment_set(lsm6dsv16x_ctx_t *ctx, uint8_t val); -int32_t lsm6dsv16x_auto_increment_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val); +int32_t lsm6dsv16x_auto_increment_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6dsv16x_auto_increment_get(stmdev_ctx_t *ctx, uint8_t *val); -int32_t lsm6dsv16x_block_data_update_set(lsm6dsv16x_ctx_t *ctx, uint8_t val); -int32_t lsm6dsv16x_block_data_update_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val); +int32_t lsm6dsv16x_block_data_update_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6dsv16x_block_data_update_get(stmdev_ctx_t *ctx, uint8_t *val); -typedef enum { +int32_t lsm6dsv16x_odr_trig_cfg_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6dsv16x_odr_trig_cfg_get(stmdev_ctx_t *ctx, uint8_t *val); + +typedef enum +{ LSM6DSV16X_DRDY_LATCHED = 0x0, - LSM6DSV16X_DRDY_PULSED = 0x1, + LSM6DSV16X_DRDY_PULSED = 0x1, } lsm6dsv16x_data_ready_mode_t; -int32_t lsm6dsv16x_data_ready_mode_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_data_ready_mode_t val); -int32_t lsm6dsv16x_data_ready_mode_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_data_ready_mode_t *val); - -typedef enum { - LSM6DSV16X_125dps = 0x0, - LSM6DSV16X_250dps = 0x1, - LSM6DSV16X_500dps = 0x2, +int32_t lsm6dsv16x_data_ready_mode_set(stmdev_ctx_t *ctx, + lsm6dsv16x_data_ready_mode_t val); +int32_t lsm6dsv16x_data_ready_mode_get(stmdev_ctx_t *ctx, + lsm6dsv16x_data_ready_mode_t *val); + +typedef struct +{ + uint8_t enable : 1; /* interrupt enable */ + uint8_t lir : 1; /* interrupt pulsed or latched */ +} lsm6dsv16x_interrupt_mode_t; +int32_t lsm6dsv16x_interrupt_enable_set(stmdev_ctx_t *ctx, + lsm6dsv16x_interrupt_mode_t val); +int32_t lsm6dsv16x_interrupt_enable_get(stmdev_ctx_t *ctx, + lsm6dsv16x_interrupt_mode_t *val); + +typedef enum +{ + LSM6DSV16X_125dps = 0x0, + LSM6DSV16X_250dps = 0x1, + LSM6DSV16X_500dps = 0x2, LSM6DSV16X_1000dps = 0x3, LSM6DSV16X_2000dps = 0x4, - LSM6DSV16X_4000dps = 0x5, + LSM6DSV16X_4000dps = 0xc, } lsm6dsv16x_gy_full_scale_t; -int32_t lsm6dsv16x_gy_full_scale_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_gy_full_scale_t val); -int32_t lsm6dsv16x_gy_full_scale_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_gy_full_scale_t *val); - -typedef enum { - LSM6DSV16X_2g = 0x0, - LSM6DSV16X_4g = 0x1, - LSM6DSV16X_8g = 0x2, +int32_t lsm6dsv16x_gy_full_scale_set(stmdev_ctx_t *ctx, + lsm6dsv16x_gy_full_scale_t val); +int32_t lsm6dsv16x_gy_full_scale_get(stmdev_ctx_t *ctx, + lsm6dsv16x_gy_full_scale_t *val); + +typedef enum +{ + LSM6DSV16X_2g = 0x0, + LSM6DSV16X_4g = 0x1, + LSM6DSV16X_8g = 0x2, LSM6DSV16X_16g = 0x3, } lsm6dsv16x_xl_full_scale_t; -int32_t lsm6dsv16x_xl_full_scale_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_xl_full_scale_t val); -int32_t lsm6dsv16x_xl_full_scale_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_xl_full_scale_t *val); +int32_t lsm6dsv16x_xl_full_scale_set(stmdev_ctx_t *ctx, + lsm6dsv16x_xl_full_scale_t val); +int32_t lsm6dsv16x_xl_full_scale_get(stmdev_ctx_t *ctx, + lsm6dsv16x_xl_full_scale_t *val); -int32_t lsm6dsv16x_xl_dual_channel_set(lsm6dsv16x_ctx_t *ctx, uint8_t val); -int32_t lsm6dsv16x_xl_dual_channel_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val); +int32_t lsm6dsv16x_xl_dual_channel_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6dsv16x_xl_dual_channel_get(stmdev_ctx_t *ctx, uint8_t *val); -typedef enum { - LSM6DSV16X_XL_ST_DISABLE = 0x0, +typedef enum +{ + LSM6DSV16X_XL_ST_DISABLE = 0x0, LSM6DSV16X_XL_ST_POSITIVE = 0x1, LSM6DSV16X_XL_ST_NEGATIVE = 0x2, } lsm6dsv16x_xl_self_test_t; -int32_t lsm6dsv16x_xl_self_test_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_xl_self_test_t val); -int32_t lsm6dsv16x_xl_self_test_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_xl_self_test_t *val); - -typedef enum { - LSM6DSV16X_OIS_XL_ST_DISABLE = 0x0, +int32_t lsm6dsv16x_xl_self_test_set(stmdev_ctx_t *ctx, + lsm6dsv16x_xl_self_test_t val); +int32_t lsm6dsv16x_xl_self_test_get(stmdev_ctx_t *ctx, + lsm6dsv16x_xl_self_test_t *val); + +typedef enum +{ + LSM6DSV16X_OIS_XL_ST_DISABLE = 0x0, LSM6DSV16X_OIS_XL_ST_POSITIVE = 0x1, LSM6DSV16X_OIS_XL_ST_NEGATIVE = 0x2, } lsm6dsv16x_ois_xl_self_test_t; -int32_t lsm6dsv16x_ois_xl_self_test_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ois_xl_self_test_t val); -int32_t lsm6dsv16x_ois_xl_self_test_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ois_xl_self_test_t *val); - -typedef enum { - LSM6DSV16X_GY_ST_DISABLE = 0x0, +int32_t lsm6dsv16x_ois_xl_self_test_set(stmdev_ctx_t *ctx, + lsm6dsv16x_ois_xl_self_test_t val); +int32_t lsm6dsv16x_ois_xl_self_test_get(stmdev_ctx_t *ctx, + lsm6dsv16x_ois_xl_self_test_t *val); + +typedef enum +{ + LSM6DSV16X_GY_ST_DISABLE = 0x0, LSM6DSV16X_GY_ST_POSITIVE = 0x1, LSM6DSV16X_GY_ST_NEGATIVE = 0x2, } lsm6dsv16x_gy_self_test_t; -int32_t lsm6dsv16x_gy_self_test_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_gy_self_test_t val); -int32_t lsm6dsv16x_gy_self_test_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_gy_self_test_t *val); - -typedef enum { - LSM6DSV16X_OIS_GY_ST_DISABLE = 0x0, - LSM6DSV16X_OIS_GY_ST_POSITIVE = 0x1, - LSM6DSV16X_OIS_GY_ST_NEGATIVE = 0x2, +int32_t lsm6dsv16x_gy_self_test_set(stmdev_ctx_t *ctx, + lsm6dsv16x_gy_self_test_t val); +int32_t lsm6dsv16x_gy_self_test_get(stmdev_ctx_t *ctx, + lsm6dsv16x_gy_self_test_t *val); + +typedef enum +{ + LSM6DSV16X_OIS_GY_ST_DISABLE = 0x0, + LSM6DSV16X_OIS_GY_ST_POSITIVE = 0x1, + LSM6DSV16X_OIS_GY_ST_NEGATIVE = 0x2, LSM6DSV16X_OIS_GY_ST_CLAMP_POS = 0x5, LSM6DSV16X_OIS_GY_ST_CLAMP_NEG = 0x6, } lsm6dsv16x_ois_gy_self_test_t; -int32_t lsm6dsv16x_ois_gy_self_test_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ois_gy_self_test_t val); -int32_t lsm6dsv16x_ois_gy_self_test_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ois_gy_self_test_t *val); - -typedef struct { - uint8_t drdy_xl : 1; - uint8_t drdy_gy : 1; - uint8_t drdy_temp : 1; - uint8_t drdy_ah_qvar : 1; - uint8_t drdy_eis : 1; - uint8_t drdy_ois : 1; - uint8_t gy_settling : 1; - uint8_t timestamp : 1; - uint8_t free_fall : 1; - uint8_t wake_up : 1; - uint8_t wake_up_z : 1; - uint8_t wake_up_y : 1; - uint8_t wake_up_x : 1; - uint8_t single_tap : 1; - uint8_t double_tap : 1; - uint8_t tap_z : 1; - uint8_t tap_y : 1; - uint8_t tap_x : 1; - uint8_t tap_sign : 1; - uint8_t six_d : 1; - uint8_t six_d_xl : 1; - uint8_t six_d_xh : 1; - uint8_t six_d_yl : 1; - uint8_t six_d_yh : 1; - uint8_t six_d_zl : 1; - uint8_t six_d_zh : 1; - uint8_t sleep_change : 1; - uint8_t sleep_state : 1; - uint8_t step_detector : 1; - uint8_t step_count_inc : 1; - uint8_t step_count_overflow : 1; - uint8_t step_on_delta_time : 1; - uint8_t emb_func_stand_by : 1; +int32_t lsm6dsv16x_ois_gy_self_test_set(stmdev_ctx_t *ctx, + lsm6dsv16x_ois_gy_self_test_t val); +int32_t lsm6dsv16x_ois_gy_self_test_get(stmdev_ctx_t *ctx, + lsm6dsv16x_ois_gy_self_test_t *val); + +typedef struct +{ + uint8_t drdy_xl : 1; + uint8_t drdy_gy : 1; + uint8_t drdy_temp : 1; + uint8_t drdy_ah_qvar : 1; + uint8_t drdy_eis : 1; + uint8_t drdy_ois : 1; + uint8_t gy_settling : 1; + uint8_t timestamp : 1; + uint8_t free_fall : 1; + uint8_t wake_up : 1; + uint8_t wake_up_z : 1; + uint8_t wake_up_y : 1; + uint8_t wake_up_x : 1; + uint8_t single_tap : 1; + uint8_t double_tap : 1; + uint8_t tap_z : 1; + uint8_t tap_y : 1; + uint8_t tap_x : 1; + uint8_t tap_sign : 1; + uint8_t six_d : 1; + uint8_t six_d_xl : 1; + uint8_t six_d_xh : 1; + uint8_t six_d_yl : 1; + uint8_t six_d_yh : 1; + uint8_t six_d_zl : 1; + uint8_t six_d_zh : 1; + uint8_t sleep_change : 1; + uint8_t sleep_state : 1; + uint8_t step_detector : 1; + uint8_t step_count_inc : 1; + uint8_t step_count_overflow : 1; + uint8_t step_on_delta_time : 1; + uint8_t emb_func_stand_by : 1; uint8_t emb_func_time_exceed : 1; - uint8_t tilt : 1; - uint8_t sig_mot : 1; - uint8_t fsm_lc : 1; - uint8_t fsm1 : 1; - uint8_t fsm2 : 1; - uint8_t fsm3 : 1; - uint8_t fsm4 : 1; - uint8_t fsm5 : 1; - uint8_t fsm6 : 1; - uint8_t fsm7 : 1; - uint8_t fsm8 : 1; - uint8_t mlc1 : 1; - uint8_t mlc2 : 1; - uint8_t mlc3 : 1; - uint8_t mlc4 : 1; - uint8_t sh_endop : 1; - uint8_t sh_slave0_nack : 1; - uint8_t sh_slave1_nack : 1; - uint8_t sh_slave2_nack : 1; - uint8_t sh_slave3_nack : 1; - uint8_t sh_wr_once : 1; - uint8_t fifo_bdr : 1; - uint8_t fifo_full : 1; - uint8_t fifo_ovr : 1; - uint8_t fifo_th : 1; + uint8_t tilt : 1; + uint8_t sig_mot : 1; + uint8_t fsm_lc : 1; + uint8_t fsm1 : 1; + uint8_t fsm2 : 1; + uint8_t fsm3 : 1; + uint8_t fsm4 : 1; + uint8_t fsm5 : 1; + uint8_t fsm6 : 1; + uint8_t fsm7 : 1; + uint8_t fsm8 : 1; + uint8_t mlc1 : 1; + uint8_t mlc2 : 1; + uint8_t mlc3 : 1; + uint8_t mlc4 : 1; + uint8_t sh_endop : 1; + uint8_t sh_slave0_nack : 1; + uint8_t sh_slave1_nack : 1; + uint8_t sh_slave2_nack : 1; + uint8_t sh_slave3_nack : 1; + uint8_t sh_wr_once : 1; + uint8_t fifo_bdr : 1; + uint8_t fifo_full : 1; + uint8_t fifo_ovr : 1; + uint8_t fifo_th : 1; } lsm6dsv16x_all_sources_t; -int32_t lsm6dsv16x_all_sources_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_all_sources_t *val); - -int32_t lsm6dsv16x_int_ack_mask_set(lsm6dsv16x_ctx_t *ctx, uint8_t val); -int32_t lsm6dsv16x_int_ack_mask_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val); - -int32_t lsm6dsv16x_temperature_raw_get(lsm6dsv16x_ctx_t *ctx, int16_t *val); - -int32_t lsm6dsv16x_angular_rate_raw_get(lsm6dsv16x_ctx_t *ctx, int16_t *val); - -int32_t lsm6dsv16x_ois_angular_rate_raw_get(lsm6dsv16x_ctx_t *ctx, int16_t *val); - -int32_t lsm6dsv16x_ois_eis_angular_rate_raw_get(lsm6dsv16x_ctx_t *ctx, int16_t *val); - -int32_t lsm6dsv16x_acceleration_raw_get(lsm6dsv16x_ctx_t *ctx, int16_t *val); - -int32_t lsm6dsv16x_dual_acceleration_raw_get(lsm6dsv16x_ctx_t *ctx, int16_t *val); - -int32_t lsm6dsv16x_ois_dual_acceleration_raw_get(lsm6dsv16x_ctx_t *ctx, int16_t *val); - -int32_t lsm6dsv16x_ah_qvar_raw_get(lsm6dsv16x_ctx_t *ctx, int16_t *val); - -int32_t lsm6dsv16x_odr_cal_reg_get(lsm6dsv16x_ctx_t *ctx, int8_t *val); - -int32_t lsm6dsv16x_ln_pg_write(lsm6dsv16x_ctx_t *ctx, uint16_t address, uint8_t *buf, uint8_t len); -int32_t lsm6dsv16x_ln_pg_read(lsm6dsv16x_ctx_t *ctx, uint16_t address, uint8_t *buf, uint8_t len); - -typedef enum { - LSM6DSV16X_DEN_ACT_LOW = 0x0, +int32_t lsm6dsv16x_all_sources_get(stmdev_ctx_t *ctx, + lsm6dsv16x_all_sources_t *val); + +typedef struct +{ + uint8_t drdy_xl : 1; + uint8_t drdy_g : 1; + uint8_t drdy_g_eis : 1; + uint8_t fifo_th : 1; + uint8_t fifo_ovr : 1; + uint8_t fifo_full : 1; + uint8_t cnt_bdr : 1; + uint8_t emb_func_endop : 1; + uint8_t timestamp : 1; + uint8_t shub : 1; + uint8_t emb_func : 1; + uint8_t sixd : 1; + uint8_t single_tap : 1; + uint8_t double_tap : 1; + uint8_t wakeup : 1; + uint8_t freefall : 1; + uint8_t sleep_change : 1; +} lsm6dsv16x_pin_int_route_t; +int32_t lsm6dsv16x_pin_int1_route_set(stmdev_ctx_t *ctx, + lsm6dsv16x_pin_int_route_t *val); +int32_t lsm6dsv16x_pin_int1_route_get(stmdev_ctx_t *ctx, + lsm6dsv16x_pin_int_route_t *val); +int32_t lsm6dsv16x_pin_int2_route_set(stmdev_ctx_t *ctx, + lsm6dsv16x_pin_int_route_t *val); +int32_t lsm6dsv16x_pin_int2_route_get(stmdev_ctx_t *ctx, + lsm6dsv16x_pin_int_route_t *val); + +typedef struct +{ + uint8_t drdy_xl : 1; + uint8_t drdy_gy : 1; + uint8_t drdy_temp : 1; +} lsm6dsv16x_data_ready_t; +int32_t lsm6dsv16x_flag_data_ready_get(stmdev_ctx_t *ctx, + lsm6dsv16x_data_ready_t *val); + +int32_t lsm6dsv16x_int_ack_mask_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6dsv16x_int_ack_mask_get(stmdev_ctx_t *ctx, uint8_t *val); + +int32_t lsm6dsv16x_temperature_raw_get(stmdev_ctx_t *ctx, int16_t *val); + +int32_t lsm6dsv16x_angular_rate_raw_get(stmdev_ctx_t *ctx, int16_t *val); + +int32_t lsm6dsv16x_ois_angular_rate_raw_get(stmdev_ctx_t *ctx, int16_t *val); + +int32_t lsm6dsv16x_ois_eis_angular_rate_raw_get(stmdev_ctx_t *ctx, + int16_t *val); + +int32_t lsm6dsv16x_acceleration_raw_get(stmdev_ctx_t *ctx, int16_t *val); + +int32_t lsm6dsv16x_dual_acceleration_raw_get(stmdev_ctx_t *ctx, int16_t *val); + +int32_t lsm6dsv16x_ois_dual_acceleration_raw_get(stmdev_ctx_t *ctx, + int16_t *val); + +int32_t lsm6dsv16x_ah_qvar_raw_get(stmdev_ctx_t *ctx, int16_t *val); + +int32_t lsm6dsv16x_odr_cal_reg_get(stmdev_ctx_t *ctx, int8_t *val); + +int32_t lsm6dsv16x_ln_pg_write(stmdev_ctx_t *ctx, uint16_t address, + uint8_t *buf, uint8_t len); +int32_t lsm6dsv16x_ln_pg_read(stmdev_ctx_t *ctx, uint16_t address, uint8_t *buf, + uint8_t len); + +int32_t lsm6dsv16x_emb_function_dbg_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6dsv16x_emb_function_dbg_get(stmdev_ctx_t *ctx, uint8_t *val); + +typedef enum +{ + LSM6DSV16X_DEN_ACT_LOW = 0x0, LSM6DSV16X_DEN_ACT_HIGH = 0x1, } lsm6dsv16x_den_polarity_t; -int32_t lsm6dsv16x_den_polarity_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_den_polarity_t val); -int32_t lsm6dsv16x_den_polarity_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_den_polarity_t *val); - -typedef struct { - uint8_t stamp_in_gy_data : 1; - uint8_t stamp_in_xl_data : 1; - uint8_t den_x : 1; - uint8_t den_y : 1; - uint8_t den_z : 1; - enum { +int32_t lsm6dsv16x_den_polarity_set(stmdev_ctx_t *ctx, + lsm6dsv16x_den_polarity_t val); +int32_t lsm6dsv16x_den_polarity_get(stmdev_ctx_t *ctx, + lsm6dsv16x_den_polarity_t *val); + +typedef struct +{ + uint8_t stamp_in_gy_data : 1; + uint8_t stamp_in_xl_data : 1; + uint8_t den_x : 1; + uint8_t den_y : 1; + uint8_t den_z : 1; + enum + { DEN_NOT_DEFINED = 0x00, - LEVEL_TRIGGER = 0x02, - LEVEL_LETCHED = 0x03, + LEVEL_TRIGGER = 0x02, + LEVEL_LATCHED = 0x03, } mode; } lsm6dsv16x_den_conf_t; -int32_t lsm6dsv16x_den_conf_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_den_conf_t val); -int32_t lsm6dsv16x_den_conf_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_den_conf_t *val); - -typedef enum { - LSM6DSV16X_EIS_125dps = 0x0, - LSM6DSV16X_EIS_250dps = 0x1, - LSM6DSV16X_EIS_500dps = 0x2, +int32_t lsm6dsv16x_den_conf_set(stmdev_ctx_t *ctx, lsm6dsv16x_den_conf_t val); +int32_t lsm6dsv16x_den_conf_get(stmdev_ctx_t *ctx, lsm6dsv16x_den_conf_t *val); + +typedef enum +{ + LSM6DSV16X_EIS_125dps = 0x0, + LSM6DSV16X_EIS_250dps = 0x1, + LSM6DSV16X_EIS_500dps = 0x2, LSM6DSV16X_EIS_1000dps = 0x3, LSM6DSV16X_EIS_2000dps = 0x4, } lsm6dsv16x_eis_gy_full_scale_t; -int32_t lsm6dsv16x_eis_gy_full_scale_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_eis_gy_full_scale_t val); -int32_t lsm6dsv16x_eis_gy_full_scale_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_eis_gy_full_scale_t *val); +int32_t lsm6dsv16x_eis_gy_full_scale_set(stmdev_ctx_t *ctx, + lsm6dsv16x_eis_gy_full_scale_t val); +int32_t lsm6dsv16x_eis_gy_full_scale_get(stmdev_ctx_t *ctx, + lsm6dsv16x_eis_gy_full_scale_t *val); -int32_t lsm6dsv16x_eis_gy_on_spi2_set(lsm6dsv16x_ctx_t *ctx, uint8_t val); -int32_t lsm6dsv16x_eis_gy_on_spi2_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val); +int32_t lsm6dsv16x_eis_gy_on_spi2_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6dsv16x_eis_gy_on_spi2_get(stmdev_ctx_t *ctx, uint8_t *val); -typedef enum { +typedef enum +{ LSM6DSV16X_EIS_ODR_OFF = 0x0, - LSM6DSV16X_EIS_1920Hz = 0x1, - LSM6DSV16X_EIS_960Hz = 0x2, + LSM6DSV16X_EIS_1920Hz = 0x1, + LSM6DSV16X_EIS_960Hz = 0x2, } lsm6dsv16x_gy_eis_data_rate_t; -int32_t lsm6dsv16x_gy_eis_data_rate_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_gy_eis_data_rate_t val); -int32_t lsm6dsv16x_gy_eis_data_rate_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_gy_eis_data_rate_t *val); +int32_t lsm6dsv16x_gy_eis_data_rate_set(stmdev_ctx_t *ctx, + lsm6dsv16x_gy_eis_data_rate_t val); +int32_t lsm6dsv16x_gy_eis_data_rate_get(stmdev_ctx_t *ctx, + lsm6dsv16x_gy_eis_data_rate_t *val); -int32_t lsm6dsv16x_fifo_watermark_set(lsm6dsv16x_ctx_t *ctx, uint8_t val); -int32_t lsm6dsv16x_fifo_watermark_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val); +int32_t lsm6dsv16x_fifo_watermark_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6dsv16x_fifo_watermark_get(stmdev_ctx_t *ctx, uint8_t *val); -int32_t lsm6dsv16x_fifo_xl_dual_fsm_batch_set(lsm6dsv16x_ctx_t *ctx, uint8_t val); -int32_t lsm6dsv16x_fifo_xl_dual_fsm_batch_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val); +int32_t lsm6dsv16x_fifo_xl_dual_fsm_batch_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6dsv16x_fifo_xl_dual_fsm_batch_get(stmdev_ctx_t *ctx, uint8_t *val); -typedef enum { +typedef enum +{ LSM6DSV16X_CMP_DISABLE = 0x0, - LSM6DSV16X_CMP_8_TO_1 = 0x1, + LSM6DSV16X_CMP_8_TO_1 = 0x1, LSM6DSV16X_CMP_16_TO_1 = 0x2, LSM6DSV16X_CMP_32_TO_1 = 0x3, } lsm6dsv16x_fifo_compress_algo_t; -int32_t lsm6dsv16x_fifo_compress_algo_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fifo_compress_algo_t val); -int32_t lsm6dsv16x_fifo_compress_algo_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fifo_compress_algo_t *val); - -int32_t lsm6dsv16x_fifo_virtual_sens_odr_chg_set(lsm6dsv16x_ctx_t *ctx, uint8_t val); -int32_t lsm6dsv16x_fifo_virtual_sens_odr_chg_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val); - -int32_t lsm6dsv16x_fifo_compress_algo_real_time_set(lsm6dsv16x_ctx_t *ctx, uint8_t val); -int32_t lsm6dsv16x_fifo_compress_algo_real_time_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val); - -int32_t lsm6dsv16x_fifo_stop_on_wtm_set(lsm6dsv16x_ctx_t *ctx, uint8_t val); -int32_t lsm6dsv16x_fifo_stop_on_wtm_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val); - -typedef enum { - LSM6DSV16X_XL_NOT_BATCHED = 0x0, +int32_t lsm6dsv16x_fifo_compress_algo_set(stmdev_ctx_t *ctx, + lsm6dsv16x_fifo_compress_algo_t val); +int32_t lsm6dsv16x_fifo_compress_algo_get(stmdev_ctx_t *ctx, + lsm6dsv16x_fifo_compress_algo_t *val); + +int32_t lsm6dsv16x_fifo_virtual_sens_odr_chg_set(stmdev_ctx_t *ctx, + uint8_t val); +int32_t lsm6dsv16x_fifo_virtual_sens_odr_chg_get(stmdev_ctx_t *ctx, + uint8_t *val); + +int32_t lsm6dsv16x_fifo_compress_algo_real_time_set(stmdev_ctx_t *ctx, + uint8_t val); +int32_t lsm6dsv16x_fifo_compress_algo_real_time_get(stmdev_ctx_t *ctx, + uint8_t *val); + +int32_t lsm6dsv16x_fifo_stop_on_wtm_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6dsv16x_fifo_stop_on_wtm_get(stmdev_ctx_t *ctx, uint8_t *val); + +typedef enum +{ + LSM6DSV16X_XL_NOT_BATCHED = 0x0, LSM6DSV16X_XL_BATCHED_AT_1Hz875 = 0x1, - LSM6DSV16X_XL_BATCHED_AT_7Hz5 = 0x2, - LSM6DSV16X_XL_BATCHED_AT_15Hz = 0x3, - LSM6DSV16X_XL_BATCHED_AT_30Hz = 0x4, - LSM6DSV16X_XL_BATCHED_AT_60Hz = 0x5, - LSM6DSV16X_XL_BATCHED_AT_120Hz = 0x6, - LSM6DSV16X_XL_BATCHED_AT_240Hz = 0x7, - LSM6DSV16X_XL_BATCHED_AT_480Hz = 0x8, - LSM6DSV16X_XL_BATCHED_AT_960Hz = 0x9, + LSM6DSV16X_XL_BATCHED_AT_7Hz5 = 0x2, + LSM6DSV16X_XL_BATCHED_AT_15Hz = 0x3, + LSM6DSV16X_XL_BATCHED_AT_30Hz = 0x4, + LSM6DSV16X_XL_BATCHED_AT_60Hz = 0x5, + LSM6DSV16X_XL_BATCHED_AT_120Hz = 0x6, + LSM6DSV16X_XL_BATCHED_AT_240Hz = 0x7, + LSM6DSV16X_XL_BATCHED_AT_480Hz = 0x8, + LSM6DSV16X_XL_BATCHED_AT_960Hz = 0x9, LSM6DSV16X_XL_BATCHED_AT_1920Hz = 0xa, LSM6DSV16X_XL_BATCHED_AT_3840Hz = 0xb, LSM6DSV16X_XL_BATCHED_AT_7680Hz = 0xc, } lsm6dsv16x_fifo_xl_batch_t; -int32_t lsm6dsv16x_fifo_xl_batch_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fifo_xl_batch_t val); -int32_t lsm6dsv16x_fifo_xl_batch_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fifo_xl_batch_t *val); - -typedef enum { - LSM6DSV16X_GY_NOT_BATCHED = 0x0, +int32_t lsm6dsv16x_fifo_xl_batch_set(stmdev_ctx_t *ctx, + lsm6dsv16x_fifo_xl_batch_t val); +int32_t lsm6dsv16x_fifo_xl_batch_get(stmdev_ctx_t *ctx, + lsm6dsv16x_fifo_xl_batch_t *val); + +typedef enum +{ + LSM6DSV16X_GY_NOT_BATCHED = 0x0, LSM6DSV16X_GY_BATCHED_AT_1Hz875 = 0x1, - LSM6DSV16X_GY_BATCHED_AT_7Hz5 = 0x2, - LSM6DSV16X_GY_BATCHED_AT_15Hz = 0x3, - LSM6DSV16X_GY_BATCHED_AT_30Hz = 0x4, - LSM6DSV16X_GY_BATCHED_AT_60Hz = 0x5, - LSM6DSV16X_GY_BATCHED_AT_120Hz = 0x6, - LSM6DSV16X_GY_BATCHED_AT_240Hz = 0x7, - LSM6DSV16X_GY_BATCHED_AT_480Hz = 0x8, - LSM6DSV16X_GY_BATCHED_AT_960Hz = 0x9, + LSM6DSV16X_GY_BATCHED_AT_7Hz5 = 0x2, + LSM6DSV16X_GY_BATCHED_AT_15Hz = 0x3, + LSM6DSV16X_GY_BATCHED_AT_30Hz = 0x4, + LSM6DSV16X_GY_BATCHED_AT_60Hz = 0x5, + LSM6DSV16X_GY_BATCHED_AT_120Hz = 0x6, + LSM6DSV16X_GY_BATCHED_AT_240Hz = 0x7, + LSM6DSV16X_GY_BATCHED_AT_480Hz = 0x8, + LSM6DSV16X_GY_BATCHED_AT_960Hz = 0x9, LSM6DSV16X_GY_BATCHED_AT_1920Hz = 0xa, LSM6DSV16X_GY_BATCHED_AT_3840Hz = 0xb, LSM6DSV16X_GY_BATCHED_AT_7680Hz = 0xc, } lsm6dsv16x_fifo_gy_batch_t; -int32_t lsm6dsv16x_fifo_gy_batch_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fifo_gy_batch_t val); -int32_t lsm6dsv16x_fifo_gy_batch_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fifo_gy_batch_t *val); - -typedef enum { - LSM6DSV16X_BYPASS_MODE = 0x0, - LSM6DSV16X_FIFO_MODE = 0x1, +int32_t lsm6dsv16x_fifo_gy_batch_set(stmdev_ctx_t *ctx, + lsm6dsv16x_fifo_gy_batch_t val); +int32_t lsm6dsv16x_fifo_gy_batch_get(stmdev_ctx_t *ctx, + lsm6dsv16x_fifo_gy_batch_t *val); + +typedef enum +{ + LSM6DSV16X_BYPASS_MODE = 0x0, + LSM6DSV16X_FIFO_MODE = 0x1, LSM6DSV16X_STREAM_WTM_TO_FULL_MODE = 0x2, - LSM6DSV16X_STREAM_TO_FIFO_MODE = 0x3, - LSM6DSV16X_BYPASS_TO_STREAM_MODE = 0x4, - LSM6DSV16X_STREAM_MODE = 0x6, - LSM6DSV16X_BYPASS_TO_FIFO_MODE = 0x7, + LSM6DSV16X_STREAM_TO_FIFO_MODE = 0x3, + LSM6DSV16X_BYPASS_TO_STREAM_MODE = 0x4, + LSM6DSV16X_STREAM_MODE = 0x6, + LSM6DSV16X_BYPASS_TO_FIFO_MODE = 0x7, } lsm6dsv16x_fifo_mode_t; -int32_t lsm6dsv16x_fifo_mode_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fifo_mode_t val); -int32_t lsm6dsv16x_fifo_mode_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fifo_mode_t *val); +int32_t lsm6dsv16x_fifo_mode_set(stmdev_ctx_t *ctx, lsm6dsv16x_fifo_mode_t val); +int32_t lsm6dsv16x_fifo_mode_get(stmdev_ctx_t *ctx, + lsm6dsv16x_fifo_mode_t *val); -int32_t lsm6dsv16x_fifo_gy_eis_batch_set(lsm6dsv16x_ctx_t *ctx, uint8_t val); -int32_t lsm6dsv16x_fifo_gy_eis_batch_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val); +int32_t lsm6dsv16x_fifo_gy_eis_batch_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6dsv16x_fifo_gy_eis_batch_get(stmdev_ctx_t *ctx, uint8_t *val); -typedef enum { - LSM6DSV16X_TEMP_NOT_BATCHED = 0x0, +typedef enum +{ + LSM6DSV16X_TEMP_NOT_BATCHED = 0x0, LSM6DSV16X_TEMP_BATCHED_AT_1Hz875 = 0x1, - LSM6DSV16X_TEMP_BATCHED_AT_15Hz = 0x2, - LSM6DSV16X_TEMP_BATCHED_AT_60Hz = 0x3, + LSM6DSV16X_TEMP_BATCHED_AT_15Hz = 0x2, + LSM6DSV16X_TEMP_BATCHED_AT_60Hz = 0x3, } lsm6dsv16x_fifo_temp_batch_t; -int32_t lsm6dsv16x_fifo_temp_batch_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fifo_temp_batch_t val); -int32_t lsm6dsv16x_fifo_temp_batch_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fifo_temp_batch_t *val); +int32_t lsm6dsv16x_fifo_temp_batch_set(stmdev_ctx_t *ctx, + lsm6dsv16x_fifo_temp_batch_t val); +int32_t lsm6dsv16x_fifo_temp_batch_get(stmdev_ctx_t *ctx, + lsm6dsv16x_fifo_temp_batch_t *val); -typedef enum { +typedef enum +{ LSM6DSV16X_TMSTMP_NOT_BATCHED = 0x0, - LSM6DSV16X_TMSTMP_DEC_1 = 0x1, - LSM6DSV16X_TMSTMP_DEC_8 = 0x2, - LSM6DSV16X_TMSTMP_DEC_32 = 0x3, + LSM6DSV16X_TMSTMP_DEC_1 = 0x1, + LSM6DSV16X_TMSTMP_DEC_8 = 0x2, + LSM6DSV16X_TMSTMP_DEC_32 = 0x3, } lsm6dsv16x_fifo_timestamp_batch_t; -int32_t lsm6dsv16x_fifo_timestamp_batch_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fifo_timestamp_batch_t val); -int32_t lsm6dsv16x_fifo_timestamp_batch_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fifo_timestamp_batch_t *val); - -int32_t lsm6dsv16x_fifo_batch_counter_threshold_set(lsm6dsv16x_ctx_t *ctx, uint16_t val); -int32_t lsm6dsv16x_fifo_batch_counter_threshold_get(lsm6dsv16x_ctx_t *ctx, uint16_t *val); - -typedef enum { - LSM6DSV16X_XL_BATCH_EVENT = 0x0, - LSM6DSV16X_GY_BATCH_EVENT = 0x1, +int32_t lsm6dsv16x_fifo_timestamp_batch_set(stmdev_ctx_t *ctx, + lsm6dsv16x_fifo_timestamp_batch_t val); +int32_t lsm6dsv16x_fifo_timestamp_batch_get(stmdev_ctx_t *ctx, + lsm6dsv16x_fifo_timestamp_batch_t *val); + +int32_t lsm6dsv16x_fifo_batch_counter_threshold_set(stmdev_ctx_t *ctx, + uint16_t val); +int32_t lsm6dsv16x_fifo_batch_counter_threshold_get(stmdev_ctx_t *ctx, + uint16_t *val); + +typedef enum +{ + LSM6DSV16X_XL_BATCH_EVENT = 0x0, + LSM6DSV16X_GY_BATCH_EVENT = 0x1, LSM6DSV16X_GY_EIS_BATCH_EVENT = 0x2, } lsm6dsv16x_fifo_batch_cnt_event_t; -int32_t lsm6dsv16x_fifo_batch_cnt_event_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fifo_batch_cnt_event_t val); -int32_t lsm6dsv16x_fifo_batch_cnt_event_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fifo_batch_cnt_event_t *val); - -int32_t lsm6dsv16x_fifo_data_level_get(lsm6dsv16x_ctx_t *ctx, uint16_t *val); - -typedef struct { - enum { - LSM6DSV16X_FIFO_EMPTY = 0x0, - LSM6DSV16X_GY_NC_TAG = 0x1, - LSM6DSV16X_XL_NC_TAG = 0x2, - LSM6DSV16X_TEMPERATURE_TAG = 0x3, - LSM6DSV16X_TIMESTAMP_TAG = 0x4, - LSM6DSV16X_CFG_CHANGE_TAG = 0x5, - LSM6DSV16X_XL_NC_T_2_TAG = 0x6, - LSM6DSV16X_XL_NC_T_1_TAG = 0x7, - LSM6DSV16X_XL_2XC_TAG = 0x8, - LSM6DSV16X_XL_3XC_TAG = 0x9, - LSM6DSV16X_GY_NC_T_2_TAG = 0xA, - LSM6DSV16X_GY_NC_T_1_TAG = 0xB, - LSM6DSV16X_GY_2XC_TAG = 0xC, - LSM6DSV16X_GY_3XC_TAG = 0xD, - LSM6DSV16X_SENSORHUB_SLAVE0_TAG = 0xE, - LSM6DSV16X_SENSORHUB_SLAVE1_TAG = 0xF, - LSM6DSV16X_SENSORHUB_SLAVE2_TAG = 0x10, - LSM6DSV16X_SENSORHUB_SLAVE3_TAG = 0x11, - LSM6DSV16X_STEP_CPUNTER_TAG = 0x12, - LSM6DSV16X_SENSORHUB_NACK_TAG = 0x19, - LSM6DSV16X_MLC_RESULT_TAG = 0x1A, - LSM6DSV16X_MLC_FILTER = 0x1B, - LSM6DSV16X_MLC_FEATURE = 0x1C, - LSM6DSV16X_XL_DUAL_CORE = 0x1D, - LSM6DSV16X_AH_QVAR = 0x1F, +int32_t lsm6dsv16x_fifo_batch_cnt_event_set(stmdev_ctx_t *ctx, + lsm6dsv16x_fifo_batch_cnt_event_t val); +int32_t lsm6dsv16x_fifo_batch_cnt_event_get(stmdev_ctx_t *ctx, + lsm6dsv16x_fifo_batch_cnt_event_t *val); + +typedef struct +{ + uint16_t fifo_level : 9; + uint8_t fifo_bdr : 1; + uint8_t fifo_full : 1; + uint8_t fifo_ovr : 1; + uint8_t fifo_th : 1; +} lsm6dsv16x_fifo_status_t; + +int32_t lsm6dsv16x_fifo_status_get(stmdev_ctx_t *ctx, + lsm6dsv16x_fifo_status_t *val); + +typedef struct +{ + enum + { + LSM6DSV16X_FIFO_EMPTY = 0x0, + LSM6DSV16X_GY_NC_TAG = 0x1, + LSM6DSV16X_XL_NC_TAG = 0x2, + LSM6DSV16X_TEMPERATURE_TAG = 0x3, + LSM6DSV16X_TIMESTAMP_TAG = 0x4, + LSM6DSV16X_CFG_CHANGE_TAG = 0x5, + LSM6DSV16X_XL_NC_T_2_TAG = 0x6, + LSM6DSV16X_XL_NC_T_1_TAG = 0x7, + LSM6DSV16X_XL_2XC_TAG = 0x8, + LSM6DSV16X_XL_3XC_TAG = 0x9, + LSM6DSV16X_GY_NC_T_2_TAG = 0xA, + LSM6DSV16X_GY_NC_T_1_TAG = 0xB, + LSM6DSV16X_GY_2XC_TAG = 0xC, + LSM6DSV16X_GY_3XC_TAG = 0xD, + LSM6DSV16X_SENSORHUB_SLAVE0_TAG = 0xE, + LSM6DSV16X_SENSORHUB_SLAVE1_TAG = 0xF, + LSM6DSV16X_SENSORHUB_SLAVE2_TAG = 0x10, + LSM6DSV16X_SENSORHUB_SLAVE3_TAG = 0x11, + LSM6DSV16X_STEP_COUNTER_TAG = 0x12, + LSM6DSV16X_SFLP_GAME_ROTATION_VECTOR_TAG = 0x13, + LSM6DSV16X_SFLP_GYROSCOPE_BIAS_TAG = 0x16, + LSM6DSV16X_SFLP_GRAVITY_VECTOR_TAG = 0x17, + LSM6DSV16X_SENSORHUB_NACK_TAG = 0x19, + LSM6DSV16X_MLC_RESULT_TAG = 0x1A, + LSM6DSV16X_MLC_FILTER = 0x1B, + LSM6DSV16X_MLC_FEATURE = 0x1C, + LSM6DSV16X_XL_DUAL_CORE = 0x1D, + LSM6DSV16X_GY_ENHANCED_EIS = 0x1E, } tag; uint8_t cnt; uint8_t data[6]; } lsm6dsv16x_fifo_out_raw_t; -int32_t lsm6dsv16x_fifo_out_raw_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fifo_out_raw_t *val); - -int32_t lsm6dsv16x_fifo_stpcnt_batch_set(lsm6dsv16x_ctx_t *ctx, uint8_t val); -int32_t lsm6dsv16x_fifo_stpcnt_batch_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val); - -int32_t lsm6dsv16x_fifo_mlc_batch_set(lsm6dsv16x_ctx_t *ctx, uint8_t val); -int32_t lsm6dsv16x_fifo_mlc_batch_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val); - -int32_t lsm6dsv16x_fifo_mlc_filt_batch_set(lsm6dsv16x_ctx_t *ctx, uint8_t val); -int32_t lsm6dsv16x_fifo_mlc_filt_batch_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val); - -int32_t lsm6dsv16x_fifo_batch_sh_slave_0_set(lsm6dsv16x_ctx_t *ctx, uint8_t val); -int32_t lsm6dsv16x_fifo_batch_sh_slave_0_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val); - -int32_t lsm6dsv16x_fifo_batch_sh_slave_1_set(lsm6dsv16x_ctx_t *ctx, uint8_t val); -int32_t lsm6dsv16x_fifo_batch_sh_slave_1_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val); - -int32_t lsm6dsv16x_fifo_batch_sh_slave_2_set(lsm6dsv16x_ctx_t *ctx, uint8_t val); -int32_t lsm6dsv16x_fifo_batch_sh_slave_2_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val); - -int32_t lsm6dsv16x_fifo_batch_sh_slave_3_set(lsm6dsv16x_ctx_t *ctx, uint8_t val); -int32_t lsm6dsv16x_fifo_batch_sh_slave_3_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val); - -typedef enum { - LSM6DSV16X_AUTO = 0x0, +int32_t lsm6dsv16x_fifo_out_raw_get(stmdev_ctx_t *ctx, + lsm6dsv16x_fifo_out_raw_t *val); + +int32_t lsm6dsv16x_fifo_stpcnt_batch_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6dsv16x_fifo_stpcnt_batch_get(stmdev_ctx_t *ctx, uint8_t *val); + +int32_t lsm6dsv16x_fifo_mlc_batch_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6dsv16x_fifo_mlc_batch_get(stmdev_ctx_t *ctx, uint8_t *val); + +int32_t lsm6dsv16x_fifo_mlc_filt_batch_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6dsv16x_fifo_mlc_filt_batch_get(stmdev_ctx_t *ctx, uint8_t *val); + +int32_t lsm6dsv16x_fifo_sh_batch_slave_set(stmdev_ctx_t *ctx, uint8_t idx, uint8_t val); +int32_t lsm6dsv16x_fifo_sh_batch_slave_get(stmdev_ctx_t *ctx, uint8_t idx, uint8_t *val); + +typedef struct +{ + uint8_t game_rotation : 1; + uint8_t gravity : 1; + uint8_t gbias : 1; +} lsm6dsv16x_fifo_sflp_raw_t; +int32_t lsm6dsv16x_fifo_sflp_batch_set(stmdev_ctx_t *ctx, + lsm6dsv16x_fifo_sflp_raw_t val); +int32_t lsm6dsv16x_fifo_sflp_batch_get(stmdev_ctx_t *ctx, + lsm6dsv16x_fifo_sflp_raw_t *val); + +typedef enum +{ + LSM6DSV16X_AUTO = 0x0, LSM6DSV16X_ALWAYS_ACTIVE = 0x1, } lsm6dsv16x_filt_anti_spike_t; -int32_t lsm6dsv16x_filt_anti_spike_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_filt_anti_spike_t val); -int32_t lsm6dsv16x_filt_anti_spike_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_filt_anti_spike_t *val); - -typedef struct { - uint8_t drdy : 1; - uint8_t ois_drdy : 1; - uint8_t irq_xl : 1; - uint8_t irq_g : 1; +int32_t lsm6dsv16x_filt_anti_spike_set(stmdev_ctx_t *ctx, + lsm6dsv16x_filt_anti_spike_t val); +int32_t lsm6dsv16x_filt_anti_spike_get(stmdev_ctx_t *ctx, + lsm6dsv16x_filt_anti_spike_t *val); + +typedef struct +{ + uint8_t drdy : 1; + uint8_t ois_drdy : 1; + uint8_t irq_xl : 1; + uint8_t irq_g : 1; } lsm6dsv16x_filt_settling_mask_t; -int32_t lsm6dsv16x_filt_settling_mask_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_filt_settling_mask_t val); -int32_t lsm6dsv16x_filt_settling_mask_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_filt_settling_mask_t *val); - -typedef struct { - uint8_t ois_drdy : 1; +int32_t lsm6dsv16x_filt_settling_mask_set(stmdev_ctx_t *ctx, + lsm6dsv16x_filt_settling_mask_t val); +int32_t lsm6dsv16x_filt_settling_mask_get(stmdev_ctx_t *ctx, + lsm6dsv16x_filt_settling_mask_t *val); + +typedef struct +{ + uint8_t ois_drdy : 1; } lsm6dsv16x_filt_ois_settling_mask_t; -int32_t lsm6dsv16x_filt_ois_settling_mask_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_filt_ois_settling_mask_t val); -int32_t lsm6dsv16x_filt_ois_settling_mask_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_filt_ois_settling_mask_t *val); - -typedef enum { - LSM6DSV16X_GY_ULTRA_LIGHT = 0x0, - LSM6DSV16X_GY_VERY_LIGHT = 0x1, - LSM6DSV16X_GY_LIGHT = 0x2, - LSM6DSV16X_GY_MEDIUM = 0x3, - LSM6DSV16X_GY_STRONG = 0x4, - LSM6DSV16X_GY_VERY_STRONG = 0x5, - LSM6DSV16X_GY_AGGRESSIVE = 0x6, - LSM6DSV16X_GY_XTREME = 0x7, +int32_t lsm6dsv16x_filt_ois_settling_mask_set(stmdev_ctx_t *ctx, + lsm6dsv16x_filt_ois_settling_mask_t val); +int32_t lsm6dsv16x_filt_ois_settling_mask_get(stmdev_ctx_t *ctx, + lsm6dsv16x_filt_ois_settling_mask_t *val); + +typedef enum +{ + LSM6DSV16X_GY_ULTRA_LIGHT = 0x0, + LSM6DSV16X_GY_VERY_LIGHT = 0x1, + LSM6DSV16X_GY_LIGHT = 0x2, + LSM6DSV16X_GY_MEDIUM = 0x3, + LSM6DSV16X_GY_STRONG = 0x4, + LSM6DSV16X_GY_VERY_STRONG = 0x5, + LSM6DSV16X_GY_AGGRESSIVE = 0x6, + LSM6DSV16X_GY_XTREME = 0x7, } lsm6dsv16x_filt_gy_lp1_bandwidth_t; -int32_t lsm6dsv16x_filt_gy_lp1_bandwidth_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_filt_gy_lp1_bandwidth_t val); -int32_t lsm6dsv16x_filt_gy_lp1_bandwidth_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_filt_gy_lp1_bandwidth_t *val); +int32_t lsm6dsv16x_filt_gy_lp1_bandwidth_set(stmdev_ctx_t *ctx, + lsm6dsv16x_filt_gy_lp1_bandwidth_t val); +int32_t lsm6dsv16x_filt_gy_lp1_bandwidth_get(stmdev_ctx_t *ctx, + lsm6dsv16x_filt_gy_lp1_bandwidth_t *val); -int32_t lsm6dsv16x_filt_gy_lp1_set(lsm6dsv16x_ctx_t *ctx, uint8_t val); -int32_t lsm6dsv16x_filt_gy_lp1_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val); +int32_t lsm6dsv16x_filt_gy_lp1_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6dsv16x_filt_gy_lp1_get(stmdev_ctx_t *ctx, uint8_t *val); -typedef enum { +typedef enum +{ LSM6DSV16X_XL_ULTRA_LIGHT = 0x0, - LSM6DSV16X_XL_VERY_LIGHT = 0x1, - LSM6DSV16X_XL_LIGHT = 0x2, - LSM6DSV16X_XL_MEDIUM = 0x3, - LSM6DSV16X_XL_STRONG = 0x4, + LSM6DSV16X_XL_VERY_LIGHT = 0x1, + LSM6DSV16X_XL_LIGHT = 0x2, + LSM6DSV16X_XL_MEDIUM = 0x3, + LSM6DSV16X_XL_STRONG = 0x4, LSM6DSV16X_XL_VERY_STRONG = 0x5, - LSM6DSV16X_XL_AGGRESSIVE = 0x6, - LSM6DSV16X_XL_XTREME = 0x7, + LSM6DSV16X_XL_AGGRESSIVE = 0x6, + LSM6DSV16X_XL_XTREME = 0x7, } lsm6dsv16x_filt_xl_lp2_bandwidth_t; -int32_t lsm6dsv16x_filt_xl_lp2_bandwidth_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_filt_xl_lp2_bandwidth_t val); -int32_t lsm6dsv16x_filt_xl_lp2_bandwidth_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_filt_xl_lp2_bandwidth_t *val); +int32_t lsm6dsv16x_filt_xl_lp2_bandwidth_set(stmdev_ctx_t *ctx, + lsm6dsv16x_filt_xl_lp2_bandwidth_t val); +int32_t lsm6dsv16x_filt_xl_lp2_bandwidth_get(stmdev_ctx_t *ctx, + lsm6dsv16x_filt_xl_lp2_bandwidth_t *val); -int32_t lsm6dsv16x_filt_xl_lp2_set(lsm6dsv16x_ctx_t *ctx, uint8_t val); -int32_t lsm6dsv16x_filt_xl_lp2_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val); +int32_t lsm6dsv16x_filt_xl_lp2_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6dsv16x_filt_xl_lp2_get(stmdev_ctx_t *ctx, uint8_t *val); -int32_t lsm6dsv16x_filt_xl_hp_set(lsm6dsv16x_ctx_t *ctx, uint8_t val); -int32_t lsm6dsv16x_filt_xl_hp_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val); +int32_t lsm6dsv16x_filt_xl_hp_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6dsv16x_filt_xl_hp_get(stmdev_ctx_t *ctx, uint8_t *val); -int32_t lsm6dsv16x_filt_xl_fast_settling_set(lsm6dsv16x_ctx_t *ctx, uint8_t val); -int32_t lsm6dsv16x_filt_xl_fast_settling_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val); +int32_t lsm6dsv16x_filt_xl_fast_settling_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6dsv16x_filt_xl_fast_settling_get(stmdev_ctx_t *ctx, uint8_t *val); -typedef enum { - LSM6DSV16X_HP_MD_NORMAL = 0x0, +typedef enum +{ + LSM6DSV16X_HP_MD_NORMAL = 0x0, LSM6DSV16X_HP_MD_REFERENCE = 0x1, } lsm6dsv16x_filt_xl_hp_mode_t; -int32_t lsm6dsv16x_filt_xl_hp_mode_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_filt_xl_hp_mode_t val); -int32_t lsm6dsv16x_filt_xl_hp_mode_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_filt_xl_hp_mode_t *val); - -typedef enum { - LSM6DSV16X_WK_FEED_SLOPE = 0x0, - LSM6DSV16X_WK_FEED_HIGH_PASS = 0x1, +int32_t lsm6dsv16x_filt_xl_hp_mode_set(stmdev_ctx_t *ctx, + lsm6dsv16x_filt_xl_hp_mode_t val); +int32_t lsm6dsv16x_filt_xl_hp_mode_get(stmdev_ctx_t *ctx, + lsm6dsv16x_filt_xl_hp_mode_t *val); + +typedef enum +{ + LSM6DSV16X_WK_FEED_SLOPE = 0x0, + LSM6DSV16X_WK_FEED_HIGH_PASS = 0x1, LSM6DSV16X_WK_FEED_LP_WITH_OFFSET = 0x2, } lsm6dsv16x_filt_wkup_act_feed_t; -int32_t lsm6dsv16x_filt_wkup_act_feed_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_filt_wkup_act_feed_t val); -int32_t lsm6dsv16x_filt_wkup_act_feed_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_filt_wkup_act_feed_t *val); +int32_t lsm6dsv16x_filt_wkup_act_feed_set(stmdev_ctx_t *ctx, + lsm6dsv16x_filt_wkup_act_feed_t val); +int32_t lsm6dsv16x_filt_wkup_act_feed_get(stmdev_ctx_t *ctx, + lsm6dsv16x_filt_wkup_act_feed_t *val); + +int32_t lsm6dsv16x_mask_trigger_xl_settl_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6dsv16x_mask_trigger_xl_settl_get(stmdev_ctx_t *ctx, uint8_t *val); -typedef enum { +typedef enum +{ LSM6DSV16X_SIXD_FEED_ODR_DIV_2 = 0x0, - LSM6DSV16X_SIXD_FEED_LOW_PASS = 0x1, + LSM6DSV16X_SIXD_FEED_LOW_PASS = 0x1, } lsm6dsv16x_filt_sixd_feed_t; -int32_t lsm6dsv16x_filt_sixd_feed_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_filt_sixd_feed_t val); -int32_t lsm6dsv16x_filt_sixd_feed_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_filt_sixd_feed_t *val); +int32_t lsm6dsv16x_filt_sixd_feed_set(stmdev_ctx_t *ctx, + lsm6dsv16x_filt_sixd_feed_t val); +int32_t lsm6dsv16x_filt_sixd_feed_get(stmdev_ctx_t *ctx, + lsm6dsv16x_filt_sixd_feed_t *val); -typedef enum { +typedef enum +{ LSM6DSV16X_EIS_LP_NORMAL = 0x0, - LSM6DSV16X_EIS_LP_LIGHT = 0x1, + LSM6DSV16X_EIS_LP_LIGHT = 0x1, } lsm6dsv16x_filt_gy_eis_lp_bandwidth_t; -int32_t lsm6dsv16x_filt_gy_eis_lp_bandwidth_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_filt_gy_eis_lp_bandwidth_t val); -int32_t lsm6dsv16x_filt_gy_eis_lp_bandwidth_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_filt_gy_eis_lp_bandwidth_t *val); - -typedef enum { - LSM6DSV16X_OIS_GY_LP_NORMAL = 0x0, - LSM6DSV16X_OIS_GY_LP_STRONG = 0x1, +int32_t lsm6dsv16x_filt_gy_eis_lp_bandwidth_set(stmdev_ctx_t *ctx, + lsm6dsv16x_filt_gy_eis_lp_bandwidth_t val); +int32_t lsm6dsv16x_filt_gy_eis_lp_bandwidth_get(stmdev_ctx_t *ctx, + lsm6dsv16x_filt_gy_eis_lp_bandwidth_t *val); + +typedef enum +{ + LSM6DSV16X_OIS_GY_LP_NORMAL = 0x0, + LSM6DSV16X_OIS_GY_LP_STRONG = 0x1, LSM6DSV16X_OIS_GY_LP_AGGRESSIVE = 0x2, - LSM6DSV16X_OIS_GY_LP_LIGHT = 0x3, + LSM6DSV16X_OIS_GY_LP_LIGHT = 0x3, } lsm6dsv16x_filt_gy_ois_lp_bandwidth_t; -int32_t lsm6dsv16x_filt_gy_ois_lp_bandwidth_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_filt_gy_ois_lp_bandwidth_t val); -int32_t lsm6dsv16x_filt_gy_ois_lp_bandwidth_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_filt_gy_ois_lp_bandwidth_t *val); +int32_t lsm6dsv16x_filt_gy_ois_lp_bandwidth_set(stmdev_ctx_t *ctx, + lsm6dsv16x_filt_gy_ois_lp_bandwidth_t val); +int32_t lsm6dsv16x_filt_gy_ois_lp_bandwidth_get(stmdev_ctx_t *ctx, + lsm6dsv16x_filt_gy_ois_lp_bandwidth_t *val); -typedef enum { +typedef enum +{ LSM6DSV16X_OIS_XL_LP_ULTRA_LIGHT = 0x0, - LSM6DSV16X_OIS_XL_LP_VERY_LIGHT = 0x1, - LSM6DSV16X_OIS_XL_LP_LIGHT = 0x2, - LSM6DSV16X_OIS_XL_LP_NORMAL = 0x3, - LSM6DSV16X_OIS_XL_LP_STRONG = 0x4, + LSM6DSV16X_OIS_XL_LP_VERY_LIGHT = 0x1, + LSM6DSV16X_OIS_XL_LP_LIGHT = 0x2, + LSM6DSV16X_OIS_XL_LP_NORMAL = 0x3, + LSM6DSV16X_OIS_XL_LP_STRONG = 0x4, LSM6DSV16X_OIS_XL_LP_VERY_STRONG = 0x5, - LSM6DSV16X_OIS_XL_LP_AGGRESSIVE = 0x6, - LSM6DSV16X_OIS_XL_LP_XTREME = 0x7, + LSM6DSV16X_OIS_XL_LP_AGGRESSIVE = 0x6, + LSM6DSV16X_OIS_XL_LP_XTREME = 0x7, } lsm6dsv16x_filt_xl_ois_lp_bandwidth_t; -int32_t lsm6dsv16x_filt_xl_ois_lp_bandwidth_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_filt_xl_ois_lp_bandwidth_t val); -int32_t lsm6dsv16x_filt_xl_ois_lp_bandwidth_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_filt_xl_ois_lp_bandwidth_t *val); +int32_t lsm6dsv16x_filt_xl_ois_lp_bandwidth_set(stmdev_ctx_t *ctx, + lsm6dsv16x_filt_xl_ois_lp_bandwidth_t val); +int32_t lsm6dsv16x_filt_xl_ois_lp_bandwidth_get(stmdev_ctx_t *ctx, + lsm6dsv16x_filt_xl_ois_lp_bandwidth_t *val); -typedef enum { +typedef enum +{ LSM6DSV16X_PROTECT_CTRL_REGS = 0x0, - LSM6DSV16X_WRITE_CTRL_REG = 0x1, + LSM6DSV16X_WRITE_CTRL_REG = 0x1, } lsm6dsv16x_fsm_permission_t; -int32_t lsm6dsv16x_fsm_permission_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fsm_permission_t val); -int32_t lsm6dsv16x_fsm_permission_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fsm_permission_t *val); -int32_t lsm6dsv16x_fsm_permission_status(lsm6dsv16x_ctx_t *ctx, uint8_t *val); - -typedef struct { - uint8_t fsm1_en : 1; - uint8_t fsm2_en : 1; - uint8_t fsm3_en : 1; - uint8_t fsm4_en : 1; - uint8_t fsm5_en : 1; - uint8_t fsm6_en : 1; - uint8_t fsm7_en : 1; - uint8_t fsm8_en : 1; +int32_t lsm6dsv16x_fsm_permission_set(stmdev_ctx_t *ctx, + lsm6dsv16x_fsm_permission_t val); +int32_t lsm6dsv16x_fsm_permission_get(stmdev_ctx_t *ctx, + lsm6dsv16x_fsm_permission_t *val); +int32_t lsm6dsv16x_fsm_permission_status(stmdev_ctx_t *ctx, uint8_t *val); + +typedef struct +{ + uint8_t fsm1_en : 1; + uint8_t fsm2_en : 1; + uint8_t fsm3_en : 1; + uint8_t fsm4_en : 1; + uint8_t fsm5_en : 1; + uint8_t fsm6_en : 1; + uint8_t fsm7_en : 1; + uint8_t fsm8_en : 1; } lsm6dsv16x_fsm_mode_t; -int32_t lsm6dsv16x_fsm_mode_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fsm_mode_t val); -int32_t lsm6dsv16x_fsm_mode_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fsm_mode_t *val); - -int32_t lsm6dsv16x_fsm_long_cnt_set(lsm6dsv16x_ctx_t *ctx, uint16_t val); -int32_t lsm6dsv16x_fsm_long_cnt_get(lsm6dsv16x_ctx_t *ctx, uint16_t *val); - - -typedef struct { - lsm6dsv16x_fsm_outs1_t fsm_outs1; - lsm6dsv16x_fsm_outs2_t fsm_outs2; - lsm6dsv16x_fsm_outs3_t fsm_outs3; - lsm6dsv16x_fsm_outs4_t fsm_outs4; - lsm6dsv16x_fsm_outs5_t fsm_outs5; - lsm6dsv16x_fsm_outs6_t fsm_outs6; - lsm6dsv16x_fsm_outs7_t fsm_outs7; - lsm6dsv16x_fsm_outs8_t fsm_outs8; +int32_t lsm6dsv16x_fsm_mode_set(stmdev_ctx_t *ctx, lsm6dsv16x_fsm_mode_t val); +int32_t lsm6dsv16x_fsm_mode_get(stmdev_ctx_t *ctx, lsm6dsv16x_fsm_mode_t *val); + +int32_t lsm6dsv16x_fsm_long_cnt_set(stmdev_ctx_t *ctx, uint16_t val); +int32_t lsm6dsv16x_fsm_long_cnt_get(stmdev_ctx_t *ctx, uint16_t *val); + + +typedef struct +{ + uint8_t fsm_outs1; + uint8_t fsm_outs2; + uint8_t fsm_outs3; + uint8_t fsm_outs4; + uint8_t fsm_outs5; + uint8_t fsm_outs6; + uint8_t fsm_outs7; + uint8_t fsm_outs8; } lsm6dsv16x_fsm_out_t; -int32_t lsm6dsv16x_fsm_out_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fsm_out_t *val); +int32_t lsm6dsv16x_fsm_out_get(stmdev_ctx_t *ctx, lsm6dsv16x_fsm_out_t *val); -typedef enum { - LSM6DSV16X_FSM_15Hz = 0x0, - LSM6DSV16X_FSM_30Hz = 0x1, - LSM6DSV16X_FSM_60Hz = 0x2, +typedef enum +{ + LSM6DSV16X_FSM_15Hz = 0x0, + LSM6DSV16X_FSM_30Hz = 0x1, + LSM6DSV16X_FSM_60Hz = 0x2, LSM6DSV16X_FSM_120Hz = 0x3, LSM6DSV16X_FSM_240Hz = 0x4, LSM6DSV16X_FSM_480Hz = 0x5, LSM6DSV16X_FSM_960Hz = 0x6, } lsm6dsv16x_fsm_data_rate_t; -int32_t lsm6dsv16x_fsm_data_rate_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fsm_data_rate_t val); -int32_t lsm6dsv16x_fsm_data_rate_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fsm_data_rate_t *val); - -int32_t lsm6dsv16x_fsm_ext_sens_sensitivity_set(lsm6dsv16x_ctx_t *ctx, uint16_t val); -int32_t lsm6dsv16x_fsm_ext_sens_sensitivity_get(lsm6dsv16x_ctx_t *ctx, uint16_t *val); - -typedef struct { +int32_t lsm6dsv16x_fsm_data_rate_set(stmdev_ctx_t *ctx, + lsm6dsv16x_fsm_data_rate_t val); +int32_t lsm6dsv16x_fsm_data_rate_get(stmdev_ctx_t *ctx, + lsm6dsv16x_fsm_data_rate_t *val); + +int32_t lsm6dsv16x_fsm_ext_sens_sensitivity_set(stmdev_ctx_t *ctx, + uint16_t val); +int32_t lsm6dsv16x_fsm_ext_sens_sensitivity_get(stmdev_ctx_t *ctx, + uint16_t *val); + +typedef struct +{ uint16_t z; uint16_t y; uint16_t x; } lsm6dsv16x_xl_fsm_ext_sens_offset_t; -int32_t lsm6dsv16x_fsm_ext_sens_offset_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_xl_fsm_ext_sens_offset_t val); -int32_t lsm6dsv16x_fsm_ext_sens_offset_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_xl_fsm_ext_sens_offset_t *val); +int32_t lsm6dsv16x_fsm_ext_sens_offset_set(stmdev_ctx_t *ctx, + lsm6dsv16x_xl_fsm_ext_sens_offset_t val); +int32_t lsm6dsv16x_fsm_ext_sens_offset_get(stmdev_ctx_t *ctx, + lsm6dsv16x_xl_fsm_ext_sens_offset_t *val); -typedef struct { +typedef struct +{ uint16_t xx; uint16_t xy; uint16_t xz; @@ -4225,55 +4729,67 @@ typedef struct { uint16_t yz; uint16_t zz; } lsm6dsv16x_xl_fsm_ext_sens_matrix_t; -int32_t lsm6dsv16x_fsm_ext_sens_matrix_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_xl_fsm_ext_sens_matrix_t val); -int32_t lsm6dsv16x_fsm_ext_sens_matrix_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_xl_fsm_ext_sens_matrix_t *val); - -typedef enum { - LSM6DSV16X_Z_EQ_Y = 0x0, +int32_t lsm6dsv16x_fsm_ext_sens_matrix_set(stmdev_ctx_t *ctx, + lsm6dsv16x_xl_fsm_ext_sens_matrix_t val); +int32_t lsm6dsv16x_fsm_ext_sens_matrix_get(stmdev_ctx_t *ctx, + lsm6dsv16x_xl_fsm_ext_sens_matrix_t *val); + +typedef enum +{ + LSM6DSV16X_Z_EQ_Y = 0x0, LSM6DSV16X_Z_EQ_MIN_Y = 0x1, - LSM6DSV16X_Z_EQ_X = 0x2, + LSM6DSV16X_Z_EQ_X = 0x2, LSM6DSV16X_Z_EQ_MIN_X = 0x3, LSM6DSV16X_Z_EQ_MIN_Z = 0x4, - LSM6DSV16X_Z_EQ_Z = 0x5, + LSM6DSV16X_Z_EQ_Z = 0x5, } lsm6dsv16x_fsm_ext_sens_z_orient_t; -int32_t lsm6dsv16x_fsm_ext_sens_z_orient_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fsm_ext_sens_z_orient_t val); -int32_t lsm6dsv16x_fsm_ext_sens_z_orient_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fsm_ext_sens_z_orient_t *val); - -typedef enum { - LSM6DSV16X_Y_EQ_Y = 0x0, +int32_t lsm6dsv16x_fsm_ext_sens_z_orient_set(stmdev_ctx_t *ctx, + lsm6dsv16x_fsm_ext_sens_z_orient_t val); +int32_t lsm6dsv16x_fsm_ext_sens_z_orient_get(stmdev_ctx_t *ctx, + lsm6dsv16x_fsm_ext_sens_z_orient_t *val); + +typedef enum +{ + LSM6DSV16X_Y_EQ_Y = 0x0, LSM6DSV16X_Y_EQ_MIN_Y = 0x1, - LSM6DSV16X_Y_EQ_X = 0x2, + LSM6DSV16X_Y_EQ_X = 0x2, LSM6DSV16X_Y_EQ_MIN_X = 0x3, LSM6DSV16X_Y_EQ_MIN_Z = 0x4, - LSM6DSV16X_Y_EQ_Z = 0x5, + LSM6DSV16X_Y_EQ_Z = 0x5, } lsm6dsv16x_fsm_ext_sens_y_orient_t; -int32_t lsm6dsv16x_fsm_ext_sens_y_orient_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fsm_ext_sens_y_orient_t val); -int32_t lsm6dsv16x_fsm_ext_sens_y_orient_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fsm_ext_sens_y_orient_t *val); - -typedef enum { - LSM6DSV16X_X_EQ_Y = 0x0, +int32_t lsm6dsv16x_fsm_ext_sens_y_orient_set(stmdev_ctx_t *ctx, + lsm6dsv16x_fsm_ext_sens_y_orient_t val); +int32_t lsm6dsv16x_fsm_ext_sens_y_orient_get(stmdev_ctx_t *ctx, + lsm6dsv16x_fsm_ext_sens_y_orient_t *val); + +typedef enum +{ + LSM6DSV16X_X_EQ_Y = 0x0, LSM6DSV16X_X_EQ_MIN_Y = 0x1, - LSM6DSV16X_X_EQ_X = 0x2, + LSM6DSV16X_X_EQ_X = 0x2, LSM6DSV16X_X_EQ_MIN_X = 0x3, LSM6DSV16X_X_EQ_MIN_Z = 0x4, - LSM6DSV16X_X_EQ_Z = 0x5, + LSM6DSV16X_X_EQ_Z = 0x5, } lsm6dsv16x_fsm_ext_sens_x_orient_t; -int32_t lsm6dsv16x_fsm_ext_sens_x_orient_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fsm_ext_sens_x_orient_t val); -int32_t lsm6dsv16x_fsm_ext_sens_x_orient_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_fsm_ext_sens_x_orient_t *val); +int32_t lsm6dsv16x_fsm_ext_sens_x_orient_set(stmdev_ctx_t *ctx, + lsm6dsv16x_fsm_ext_sens_x_orient_t val); +int32_t lsm6dsv16x_fsm_ext_sens_x_orient_get(stmdev_ctx_t *ctx, + lsm6dsv16x_fsm_ext_sens_x_orient_t *val); -int32_t lsm6dsv16x_fsm_long_cnt_timeout_set(lsm6dsv16x_ctx_t *ctx, uint16_t val); -int32_t lsm6dsv16x_fsm_long_cnt_timeout_get(lsm6dsv16x_ctx_t *ctx, uint16_t *val); +int32_t lsm6dsv16x_fsm_long_cnt_timeout_set(stmdev_ctx_t *ctx, uint16_t val); +int32_t lsm6dsv16x_fsm_long_cnt_timeout_get(stmdev_ctx_t *ctx, uint16_t *val); -int32_t lsm6dsv16x_fsm_number_of_programs_set(lsm6dsv16x_ctx_t *ctx, uint8_t val); -int32_t lsm6dsv16x_fsm_number_of_programs_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val); +int32_t lsm6dsv16x_fsm_number_of_programs_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6dsv16x_fsm_number_of_programs_get(stmdev_ctx_t *ctx, uint8_t *val); -int32_t lsm6dsv16x_fsm_start_address_set(lsm6dsv16x_ctx_t *ctx, uint16_t val); -int32_t lsm6dsv16x_fsm_start_address_get(lsm6dsv16x_ctx_t *ctx, uint16_t *val); +int32_t lsm6dsv16x_fsm_start_address_set(stmdev_ctx_t *ctx, uint16_t val); +int32_t lsm6dsv16x_fsm_start_address_get(stmdev_ctx_t *ctx, uint16_t *val); -int32_t lsm6dsv16x_ff_time_windows_set(lsm6dsv16x_ctx_t *ctx, uint8_t val); -int32_t lsm6dsv16x_ff_time_windows_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val); +int32_t lsm6dsv16x_ff_time_windows_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6dsv16x_ff_time_windows_get(stmdev_ctx_t *ctx, uint8_t *val); -typedef enum { +typedef enum +{ LSM6DSV16X_156_mg = 0x0, LSM6DSV16X_219_mg = 0x1, LSM6DSV16X_250_mg = 0x2, @@ -4283,384 +4799,461 @@ typedef enum { LSM6DSV16X_469_mg = 0x6, LSM6DSV16X_500_mg = 0x7, } lsm6dsv16x_ff_thresholds_t; -int32_t lsm6dsv16x_ff_thresholds_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ff_thresholds_t val); -int32_t lsm6dsv16x_ff_thresholds_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ff_thresholds_t *val); - -typedef enum { - LSM6DSV16X_DISABLE = 0x0, - LSM6DSV16X_MLC_BEFORE_FSM = 0x1, - LSM6DSV16X_MLC_AFTER_FSM = 0x2, +int32_t lsm6dsv16x_ff_thresholds_set(stmdev_ctx_t *ctx, + lsm6dsv16x_ff_thresholds_t val); +int32_t lsm6dsv16x_ff_thresholds_get(stmdev_ctx_t *ctx, + lsm6dsv16x_ff_thresholds_t *val); + +typedef enum +{ + LSM6DSV16X_MLC_OFF = 0x0, + LSM6DSV16X_MLC_ON = 0x1, + LSM6DSV16X_MLC_ON_BEFORE_FSM = 0x2, } lsm6dsv16x_mlc_mode_t; -int32_t lsm6dsv16x_mlc_mode_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_mlc_mode_t val); -int32_t lsm6dsv16x_mlc_mode_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_mlc_mode_t *val); - -typedef enum { - LSM6DSV16X_MLC_15Hz = 0x0, - LSM6DSV16X_MLC_30Hz = 0x1, - LSM6DSV16X_MLC_60Hz = 0x2, +int32_t lsm6dsv16x_mlc_set(stmdev_ctx_t *ctx, lsm6dsv16x_mlc_mode_t val); +int32_t lsm6dsv16x_mlc_get(stmdev_ctx_t *ctx, lsm6dsv16x_mlc_mode_t *val); + +typedef enum +{ + LSM6DSV16X_MLC_15Hz = 0x0, + LSM6DSV16X_MLC_30Hz = 0x1, + LSM6DSV16X_MLC_60Hz = 0x2, LSM6DSV16X_MLC_120Hz = 0x3, LSM6DSV16X_MLC_240Hz = 0x4, LSM6DSV16X_MLC_480Hz = 0x5, LSM6DSV16X_MLC_960Hz = 0x6, } lsm6dsv16x_mlc_data_rate_t; -int32_t lsm6dsv16x_mlc_data_rate_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_mlc_data_rate_t val); -int32_t lsm6dsv16x_mlc_data_rate_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_mlc_data_rate_t *val); +int32_t lsm6dsv16x_mlc_data_rate_set(stmdev_ctx_t *ctx, + lsm6dsv16x_mlc_data_rate_t val); +int32_t lsm6dsv16x_mlc_data_rate_get(stmdev_ctx_t *ctx, + lsm6dsv16x_mlc_data_rate_t *val); -typedef struct { +typedef struct +{ uint8_t mlc1_src; uint8_t mlc2_src; uint8_t mlc3_src; uint8_t mlc4_src; } lsm6dsv16x_mlc_out_t; -int32_t lsm6dsv16x_mlc_out_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_mlc_out_t *val); +int32_t lsm6dsv16x_mlc_out_get(stmdev_ctx_t *ctx, lsm6dsv16x_mlc_out_t *val); -int32_t lsm6dsv16x_mlc_ext_sens_sensitivity_set(lsm6dsv16x_ctx_t *ctx, uint16_t val); -int32_t lsm6dsv16x_mlc_ext_sens_sensitivity_get(lsm6dsv16x_ctx_t *ctx, uint16_t *val); +int32_t lsm6dsv16x_mlc_ext_sens_sensitivity_set(stmdev_ctx_t *ctx, + uint16_t val); +int32_t lsm6dsv16x_mlc_ext_sens_sensitivity_get(stmdev_ctx_t *ctx, + uint16_t *val); -typedef enum { +typedef enum +{ LSM6DSV16X_OIS_CTRL_FROM_OIS = 0x0, - LSM6DSV16X_OIS_CTRL_FROM_UI = 0x1, + LSM6DSV16X_OIS_CTRL_FROM_UI = 0x1, } lsm6dsv16x_ois_ctrl_mode_t; -int32_t lsm6dsv16x_ois_ctrl_mode_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ois_ctrl_mode_t val); -int32_t lsm6dsv16x_ois_ctrl_mode_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ois_ctrl_mode_t *val); +int32_t lsm6dsv16x_ois_ctrl_mode_set(stmdev_ctx_t *ctx, + lsm6dsv16x_ois_ctrl_mode_t val); +int32_t lsm6dsv16x_ois_ctrl_mode_get(stmdev_ctx_t *ctx, + lsm6dsv16x_ois_ctrl_mode_t *val); -int32_t lsm6dsv16x_ois_reset_set(lsm6dsv16x_ctx_t *ctx, int8_t val); -int32_t lsm6dsv16x_ois_reset_get(lsm6dsv16x_ctx_t *ctx, int8_t *val); +int32_t lsm6dsv16x_ois_reset_set(stmdev_ctx_t *ctx, int8_t val); +int32_t lsm6dsv16x_ois_reset_get(stmdev_ctx_t *ctx, int8_t *val); -int32_t lsm6dsv16x_ois_interface_pull_up_set(lsm6dsv16x_ctx_t *ctx, uint8_t val); -int32_t lsm6dsv16x_ois_interface_pull_up_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val); +int32_t lsm6dsv16x_ois_interface_pull_up_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6dsv16x_ois_interface_pull_up_get(stmdev_ctx_t *ctx, uint8_t *val); -typedef struct { - uint8_t ack : 1; - uint8_t req : 1; +typedef struct +{ + uint8_t ack : 1; + uint8_t req : 1; } lsm6dsv16x_ois_handshake_t; -int32_t lsm6dsv16x_ois_handshake_from_ui_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ois_handshake_t val); -int32_t lsm6dsv16x_ois_handshake_from_ui_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ois_handshake_t *val); -int32_t lsm6dsv16x_ois_handshake_from_ois_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ois_handshake_t val); -int32_t lsm6dsv16x_ois_handshake_from_ois_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ois_handshake_t *val); - -int32_t lsm6dsv16x_ois_shared_set(lsm6dsv16x_ctx_t *ctx, uint8_t val[6]); -int32_t lsm6dsv16x_ois_shared_get(lsm6dsv16x_ctx_t *ctx, uint8_t val[6]); - -int32_t lsm6dsv16x_ois_on_spi2_set(lsm6dsv16x_ctx_t *ctx, uint8_t val); -int32_t lsm6dsv16x_ois_on_spi2_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val); - -typedef struct { - uint8_t gy : 1; - uint8_t xl : 1; +int32_t lsm6dsv16x_ois_handshake_from_ui_set(stmdev_ctx_t *ctx, + lsm6dsv16x_ois_handshake_t val); +int32_t lsm6dsv16x_ois_handshake_from_ui_get(stmdev_ctx_t *ctx, + lsm6dsv16x_ois_handshake_t *val); +int32_t lsm6dsv16x_ois_handshake_from_ois_set(stmdev_ctx_t *ctx, + lsm6dsv16x_ois_handshake_t val); +int32_t lsm6dsv16x_ois_handshake_from_ois_get(stmdev_ctx_t *ctx, + lsm6dsv16x_ois_handshake_t *val); + +int32_t lsm6dsv16x_ois_shared_set(stmdev_ctx_t *ctx, uint8_t val[6]); +int32_t lsm6dsv16x_ois_shared_get(stmdev_ctx_t *ctx, uint8_t val[6]); + +int32_t lsm6dsv16x_ois_on_spi2_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6dsv16x_ois_on_spi2_get(stmdev_ctx_t *ctx, uint8_t *val); + +typedef struct +{ + uint8_t gy : 1; + uint8_t xl : 1; } lsm6dsv16x_ois_chain_t; -int32_t lsm6dsv16x_ois_chain_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ois_chain_t val); -int32_t lsm6dsv16x_ois_chain_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ois_chain_t *val); - -typedef enum { - LSM6DSV16X_OIS_125dps = 0x0, - LSM6DSV16X_OIS_250dps = 0x1, - LSM6DSV16X_OIS_500dps = 0x2, +int32_t lsm6dsv16x_ois_chain_set(stmdev_ctx_t *ctx, lsm6dsv16x_ois_chain_t val); +int32_t lsm6dsv16x_ois_chain_get(stmdev_ctx_t *ctx, + lsm6dsv16x_ois_chain_t *val); + +typedef enum +{ + LSM6DSV16X_OIS_125dps = 0x0, + LSM6DSV16X_OIS_250dps = 0x1, + LSM6DSV16X_OIS_500dps = 0x2, LSM6DSV16X_OIS_1000dps = 0x3, LSM6DSV16X_OIS_2000dps = 0x4, } lsm6dsv16x_ois_gy_full_scale_t; -int32_t lsm6dsv16x_ois_gy_full_scale_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ois_gy_full_scale_t val); -int32_t lsm6dsv16x_ois_gy_full_scale_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ois_gy_full_scale_t *val); - -typedef enum { - LSM6DSV16X_OIS_2g = 0x0, - LSM6DSV16X_OIS_4g = 0x1, - LSM6DSV16X_OIS_8g = 0x2, +int32_t lsm6dsv16x_ois_gy_full_scale_set(stmdev_ctx_t *ctx, + lsm6dsv16x_ois_gy_full_scale_t val); +int32_t lsm6dsv16x_ois_gy_full_scale_get(stmdev_ctx_t *ctx, + lsm6dsv16x_ois_gy_full_scale_t *val); + +typedef enum +{ + LSM6DSV16X_OIS_2g = 0x0, + LSM6DSV16X_OIS_4g = 0x1, + LSM6DSV16X_OIS_8g = 0x2, LSM6DSV16X_OIS_16g = 0x3, } lsm6dsv16x_ois_xl_full_scale_t; -int32_t lsm6dsv16x_ois_xl_full_scale_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ois_xl_full_scale_t val); -int32_t lsm6dsv16x_ois_xl_full_scale_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ois_xl_full_scale_t *val); +int32_t lsm6dsv16x_ois_xl_full_scale_set(stmdev_ctx_t *ctx, + lsm6dsv16x_ois_xl_full_scale_t val); +int32_t lsm6dsv16x_ois_xl_full_scale_get(stmdev_ctx_t *ctx, + lsm6dsv16x_ois_xl_full_scale_t *val); -typedef enum { +typedef enum +{ LSM6DSV16X_DEG_80 = 0x0, LSM6DSV16X_DEG_70 = 0x1, LSM6DSV16X_DEG_60 = 0x2, LSM6DSV16X_DEG_50 = 0x3, } lsm6dsv16x_6d_threshold_t; -int32_t lsm6dsv16x_6d_threshold_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_6d_threshold_t val); -int32_t lsm6dsv16x_6d_threshold_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_6d_threshold_t *val); +int32_t lsm6dsv16x_6d_threshold_set(stmdev_ctx_t *ctx, + lsm6dsv16x_6d_threshold_t val); +int32_t lsm6dsv16x_6d_threshold_get(stmdev_ctx_t *ctx, + lsm6dsv16x_6d_threshold_t *val); -int32_t lsm6dsv16x_4d_mode_set(lsm6dsv16x_ctx_t *ctx, uint8_t val); -int32_t lsm6dsv16x_4d_mode_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val); +int32_t lsm6dsv16x_4d_mode_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6dsv16x_4d_mode_get(stmdev_ctx_t *ctx, uint8_t *val); -typedef enum { +typedef enum +{ LSM6DSV16X_2400MOhm = 0x0, LSM6DSV16X_730MOhm = 0x1, LSM6DSV16X_300MOhm = 0x2, LSM6DSV16X_255MOhm = 0x3, } lsm6dsv16x_ah_qvar_zin_t; -int32_t lsm6dsv16x_ah_qvar_zin_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ah_qvar_zin_t val); -int32_t lsm6dsv16x_ah_qvar_zin_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ah_qvar_zin_t *val); - -typedef struct { - uint8_t ah_qvar_en : 1; +int32_t lsm6dsv16x_ah_qvar_zin_set(stmdev_ctx_t *ctx, + lsm6dsv16x_ah_qvar_zin_t val); +int32_t lsm6dsv16x_ah_qvar_zin_get(stmdev_ctx_t *ctx, + lsm6dsv16x_ah_qvar_zin_t *val); + +typedef struct +{ + uint8_t ah_qvar_en : 1; } lsm6dsv16x_ah_qvar_mode_t; -int32_t lsm6dsv16x_ah_qvar_mode_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ah_qvar_mode_t val); -int32_t lsm6dsv16x_ah_qvar_mode_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ah_qvar_mode_t *val); +int32_t lsm6dsv16x_ah_qvar_mode_set(stmdev_ctx_t *ctx, + lsm6dsv16x_ah_qvar_mode_t val); +int32_t lsm6dsv16x_ah_qvar_mode_get(stmdev_ctx_t *ctx, + lsm6dsv16x_ah_qvar_mode_t *val); -typedef enum { +typedef enum +{ LSM6DSV16X_SW_RST_DYN_ADDRESS_RST = 0x0, - LSM6DSV16X_I3C_GLOBAL_RST = 0x1, + LSM6DSV16X_I3C_GLOBAL_RST = 0x1, } lsm6dsv16x_i3c_reset_mode_t; -int32_t lsm6dsv16x_i3c_reset_mode_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_i3c_reset_mode_t val); -int32_t lsm6dsv16x_i3c_reset_mode_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_i3c_reset_mode_t *val); - -typedef enum { - LSM6DSV16X_IBI_2us = 0x0, +int32_t lsm6dsv16x_i3c_reset_mode_set(stmdev_ctx_t *ctx, + lsm6dsv16x_i3c_reset_mode_t val); +int32_t lsm6dsv16x_i3c_reset_mode_get(stmdev_ctx_t *ctx, + lsm6dsv16x_i3c_reset_mode_t *val); + +typedef enum +{ + LSM6DSV16X_IBI_2us = 0x0, LSM6DSV16X_IBI_50us = 0x1, - LSM6DSV16X_IBI_1ms = 0x2, + LSM6DSV16X_IBI_1ms = 0x2, LSM6DSV16X_IBI_25ms = 0x3, } lsm6dsv16x_i3c_ibi_time_t; -int32_t lsm6dsv16x_i3c_ibi_time_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_i3c_ibi_time_t val); -int32_t lsm6dsv16x_i3c_ibi_time_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_i3c_ibi_time_t *val); - -int32_t lsm6dsv16x_sh_master_interface_pull_up_set(lsm6dsv16x_ctx_t *ctx, uint8_t val); -int32_t lsm6dsv16x_sh_master_interface_pull_up_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val); - -typedef struct { - lsm6dsv16x_sensor_hub_1_t sh_byte_1; - lsm6dsv16x_sensor_hub_2_t sh_byte_2; - lsm6dsv16x_sensor_hub_3_t sh_byte_3; - lsm6dsv16x_sensor_hub_4_t sh_byte_4; - lsm6dsv16x_sensor_hub_5_t sh_byte_5; - lsm6dsv16x_sensor_hub_6_t sh_byte_6; - lsm6dsv16x_sensor_hub_7_t sh_byte_7; - lsm6dsv16x_sensor_hub_8_t sh_byte_8; - lsm6dsv16x_sensor_hub_9_t sh_byte_9; - lsm6dsv16x_sensor_hub_10_t sh_byte_10; - lsm6dsv16x_sensor_hub_11_t sh_byte_11; - lsm6dsv16x_sensor_hub_12_t sh_byte_12; - lsm6dsv16x_sensor_hub_13_t sh_byte_13; - lsm6dsv16x_sensor_hub_14_t sh_byte_14; - lsm6dsv16x_sensor_hub_15_t sh_byte_15; - lsm6dsv16x_sensor_hub_16_t sh_byte_16; - lsm6dsv16x_sensor_hub_17_t sh_byte_17; - lsm6dsv16x_sensor_hub_18_t sh_byte_18; -} lsm6dsv16x_emb_sh_read_t; -int32_t lsm6dsv16x_sh_read_data_raw_get(lsm6dsv16x_ctx_t *ctx, - lsm6dsv16x_emb_sh_read_t *val, +int32_t lsm6dsv16x_i3c_ibi_time_set(stmdev_ctx_t *ctx, + lsm6dsv16x_i3c_ibi_time_t val); +int32_t lsm6dsv16x_i3c_ibi_time_get(stmdev_ctx_t *ctx, + lsm6dsv16x_i3c_ibi_time_t *val); + +int32_t lsm6dsv16x_sh_master_interface_pull_up_set(stmdev_ctx_t *ctx, + uint8_t val); +int32_t lsm6dsv16x_sh_master_interface_pull_up_get(stmdev_ctx_t *ctx, + uint8_t *val); + +int32_t lsm6dsv16x_sh_read_data_raw_get(stmdev_ctx_t *ctx, uint8_t *val, uint8_t len); -typedef enum { - LSM6DSV16X_SLV_0 = 0x0, - LSM6DSV16X_SLV_0_1 = 0x1, - LSM6DSV16X_SLV_0_1_2 = 0x2, +typedef enum +{ + LSM6DSV16X_SLV_0 = 0x0, + LSM6DSV16X_SLV_0_1 = 0x1, + LSM6DSV16X_SLV_0_1_2 = 0x2, LSM6DSV16X_SLV_0_1_2_3 = 0x3, } lsm6dsv16x_sh_slave_connected_t; -int32_t lsm6dsv16x_sh_slave_connected_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_sh_slave_connected_t val); -int32_t lsm6dsv16x_sh_slave_connected_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_sh_slave_connected_t *val); +int32_t lsm6dsv16x_sh_slave_connected_set(stmdev_ctx_t *ctx, + lsm6dsv16x_sh_slave_connected_t val); +int32_t lsm6dsv16x_sh_slave_connected_get(stmdev_ctx_t *ctx, + lsm6dsv16x_sh_slave_connected_t *val); -int32_t lsm6dsv16x_sh_master_set(lsm6dsv16x_ctx_t *ctx, uint8_t val); -int32_t lsm6dsv16x_sh_master_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val); +int32_t lsm6dsv16x_sh_master_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6dsv16x_sh_master_get(stmdev_ctx_t *ctx, uint8_t *val); -int32_t lsm6dsv16x_sh_pass_through_set(lsm6dsv16x_ctx_t *ctx, uint8_t val); -int32_t lsm6dsv16x_sh_pass_through_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val); +int32_t lsm6dsv16x_sh_pass_through_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6dsv16x_sh_pass_through_get(stmdev_ctx_t *ctx, uint8_t *val); -typedef enum { +typedef enum +{ LSM6DSV16X_SH_TRG_XL_GY_DRDY = 0x0, - LSM6DSV16X_SH_TRIG_INT2 = 0x1, + LSM6DSV16X_SH_TRIG_INT2 = 0x1, } lsm6dsv16x_sh_syncro_mode_t; -int32_t lsm6dsv16x_sh_syncro_mode_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_sh_syncro_mode_t val); -int32_t lsm6dsv16x_sh_syncro_mode_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_sh_syncro_mode_t *val); - -typedef enum { - LSM6DSV16X_EACH_SH_CYCLE = 0x0, +int32_t lsm6dsv16x_sh_syncro_mode_set(stmdev_ctx_t *ctx, + lsm6dsv16x_sh_syncro_mode_t val); +int32_t lsm6dsv16x_sh_syncro_mode_get(stmdev_ctx_t *ctx, + lsm6dsv16x_sh_syncro_mode_t *val); + +typedef enum +{ + LSM6DSV16X_EACH_SH_CYCLE = 0x0, LSM6DSV16X_ONLY_FIRST_CYCLE = 0x1, } lsm6dsv16x_sh_write_mode_t; -int32_t lsm6dsv16x_sh_write_mode_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_sh_write_mode_t val); -int32_t lsm6dsv16x_sh_write_mode_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_sh_write_mode_t *val); +int32_t lsm6dsv16x_sh_write_mode_set(stmdev_ctx_t *ctx, + lsm6dsv16x_sh_write_mode_t val); +int32_t lsm6dsv16x_sh_write_mode_get(stmdev_ctx_t *ctx, + lsm6dsv16x_sh_write_mode_t *val); -int32_t lsm6dsv16x_sh_reset_set(lsm6dsv16x_ctx_t *ctx, uint8_t val); -int32_t lsm6dsv16x_sh_reset_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val); +int32_t lsm6dsv16x_sh_reset_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6dsv16x_sh_reset_get(stmdev_ctx_t *ctx, uint8_t *val); -typedef struct { +typedef struct +{ uint8_t slv0_add; uint8_t slv0_subadd; uint8_t slv0_data; } lsm6dsv16x_sh_cfg_write_t; -int32_t lsm6dsv16x_sh_cfg_write(lsm6dsv16x_ctx_t *ctx, +int32_t lsm6dsv16x_sh_cfg_write(stmdev_ctx_t *ctx, lsm6dsv16x_sh_cfg_write_t *val); -typedef enum { - LSM6DSV16X_SH_15Hz = 0x1, - LSM6DSV16X_SH_30Hz = 0x2, - LSM6DSV16X_SH_60Hz = 0x3, +typedef enum +{ + LSM6DSV16X_SH_15Hz = 0x1, + LSM6DSV16X_SH_30Hz = 0x2, + LSM6DSV16X_SH_60Hz = 0x3, LSM6DSV16X_SH_120Hz = 0x4, LSM6DSV16X_SH_240Hz = 0x5, LSM6DSV16X_SH_480Hz = 0x6, } lsm6dsv16x_sh_data_rate_t; -int32_t lsm6dsv16x_sh_data_rate_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_sh_data_rate_t val); -int32_t lsm6dsv16x_sh_data_rate_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_sh_data_rate_t *val); +int32_t lsm6dsv16x_sh_data_rate_set(stmdev_ctx_t *ctx, + lsm6dsv16x_sh_data_rate_t val); +int32_t lsm6dsv16x_sh_data_rate_get(stmdev_ctx_t *ctx, + lsm6dsv16x_sh_data_rate_t *val); -typedef struct { +typedef struct +{ uint8_t slv_add; uint8_t slv_subadd; uint8_t slv_len; } lsm6dsv16x_sh_cfg_read_t; -int32_t lsm6dsv16x_sh_slv0_cfg_read(lsm6dsv16x_ctx_t *ctx, - lsm6dsv16x_sh_cfg_read_t *val); -int32_t lsm6dsv16x_sh_slv1_cfg_read(lsm6dsv16x_ctx_t *ctx, - lsm6dsv16x_sh_cfg_read_t *val); -int32_t lsm6dsv16x_sh_slv2_cfg_read(lsm6dsv16x_ctx_t *ctx, - lsm6dsv16x_sh_cfg_read_t *val); -int32_t lsm6dsv16x_sh_slv3_cfg_read(lsm6dsv16x_ctx_t *ctx, - lsm6dsv16x_sh_cfg_read_t *val); - -int32_t lsm6dsv16x_s4s_sync_samples_set(lsm6dsv16x_ctx_t *ctx, uint16_t val); -int32_t lsm6dsv16x_s4s_sync_samples_get(lsm6dsv16x_ctx_t *ctx, uint16_t *val); - -typedef enum { - LSM6DSV16X_S4S_DT_RES_11 = 0x0, - LSM6DSV16X_S4S_DT_RES_12 = 0x1, - LSM6DSV16X_S4S_DT_RES_13 = 0x2, - LSM6DSV16X_S4S_DT_RES_14 = 0x3, -} lsm6dsv16x_s4s_res_ratio_t; -int32_t lsm6dsv16x_s4s_res_ratio_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_s4s_res_ratio_t val); -int32_t lsm6dsv16x_s4s_res_ratio_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_s4s_res_ratio_t *val); - -int32_t lsm6dsv16x_s4s_command_set(lsm6dsv16x_ctx_t *ctx, uint8_t val); -int32_t lsm6dsv16x_s4s_command_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val); - -int32_t lsm6dsv16x_s4s_dt_set(lsm6dsv16x_ctx_t *ctx, uint8_t val); -int32_t lsm6dsv16x_s4s_dt_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val); - -int32_t lsm6dsv16x_ui_sdo_pull_up_set(lsm6dsv16x_ctx_t *ctx, uint8_t val); -int32_t lsm6dsv16x_ui_sdo_pull_up_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val); - -typedef enum { - LSM6DSV16X_I2C_I3C_ENABLE = 0x0, +int32_t lsm6dsv16x_sh_slv_cfg_read(stmdev_ctx_t *ctx, uint8_t idx, + lsm6dsv16x_sh_cfg_read_t *val); + +int32_t lsm6dsv16x_sh_status_get(stmdev_ctx_t *ctx, + lsm6dsv16x_status_master_t *val); + +int32_t lsm6dsv16x_ui_sdo_pull_up_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6dsv16x_ui_sdo_pull_up_get(stmdev_ctx_t *ctx, uint8_t *val); + +typedef enum +{ + LSM6DSV16X_I2C_I3C_ENABLE = 0x0, LSM6DSV16X_I2C_I3C_DISABLE = 0x1, } lsm6dsv16x_ui_i2c_i3c_mode_t; -int32_t lsm6dsv16x_ui_i2c_i3c_mode_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ui_i2c_i3c_mode_t val); -int32_t lsm6dsv16x_ui_i2c_i3c_mode_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_ui_i2c_i3c_mode_t *val); +int32_t lsm6dsv16x_ui_i2c_i3c_mode_set(stmdev_ctx_t *ctx, + lsm6dsv16x_ui_i2c_i3c_mode_t val); +int32_t lsm6dsv16x_ui_i2c_i3c_mode_get(stmdev_ctx_t *ctx, + lsm6dsv16x_ui_i2c_i3c_mode_t *val); -typedef enum { +typedef enum +{ LSM6DSV16X_SPI_4_WIRE = 0x0, LSM6DSV16X_SPI_3_WIRE = 0x1, } lsm6dsv16x_spi_mode_t; -int32_t lsm6dsv16x_spi_mode_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_spi_mode_t val); -int32_t lsm6dsv16x_spi_mode_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_spi_mode_t *val); +int32_t lsm6dsv16x_spi_mode_set(stmdev_ctx_t *ctx, lsm6dsv16x_spi_mode_t val); +int32_t lsm6dsv16x_spi_mode_get(stmdev_ctx_t *ctx, lsm6dsv16x_spi_mode_t *val); -int32_t lsm6dsv16x_ui_sda_pull_up_set(lsm6dsv16x_ctx_t *ctx, uint8_t val); -int32_t lsm6dsv16x_ui_sda_pull_up_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val); +int32_t lsm6dsv16x_ui_sda_pull_up_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6dsv16x_ui_sda_pull_up_get(stmdev_ctx_t *ctx, uint8_t *val); -typedef enum { +typedef enum +{ LSM6DSV16X_SPI2_4_WIRE = 0x0, LSM6DSV16X_SPI2_3_WIRE = 0x1, } lsm6dsv16x_spi2_mode_t; -int32_t lsm6dsv16x_spi2_mode_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_spi2_mode_t val); -int32_t lsm6dsv16x_spi2_mode_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_spi2_mode_t *val); +int32_t lsm6dsv16x_spi2_mode_set(stmdev_ctx_t *ctx, lsm6dsv16x_spi2_mode_t val); +int32_t lsm6dsv16x_spi2_mode_get(stmdev_ctx_t *ctx, + lsm6dsv16x_spi2_mode_t *val); -int32_t lsm6dsv16x_sigmot_mode_set(lsm6dsv16x_ctx_t *ctx, uint8_t val); -int32_t lsm6dsv16x_sigmot_mode_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val); +int32_t lsm6dsv16x_sigmot_mode_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6dsv16x_sigmot_mode_get(stmdev_ctx_t *ctx, uint8_t *val); -typedef struct { - uint8_t step_counter_enable : 1; - uint8_t false_step_rej : 1; +typedef struct +{ + uint8_t step_counter_enable : 1; + uint8_t false_step_rej : 1; } lsm6dsv16x_stpcnt_mode_t; -int32_t lsm6dsv16x_stpcnt_mode_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_stpcnt_mode_t val); -int32_t lsm6dsv16x_stpcnt_mode_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_stpcnt_mode_t *val); - -int32_t lsm6dsv16x_stpcnt_steps_get(lsm6dsv16x_ctx_t *ctx, uint16_t *val); - -int32_t lsm6dsv16x_stpcnt_rst_step_set(lsm6dsv16x_ctx_t *ctx, uint8_t val); -int32_t lsm6dsv16x_stpcnt_rst_step_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val); - -int32_t lsm6dsv16x_stpcnt_debounce_set(lsm6dsv16x_ctx_t *ctx, uint8_t val); -int32_t lsm6dsv16x_stpcnt_debounce_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val); - -int32_t lsm6dsv16x_stpcnt_period_set(lsm6dsv16x_ctx_t *ctx, uint16_t val); -int32_t lsm6dsv16x_stpcnt_period_get(lsm6dsv16x_ctx_t *ctx, uint16_t *val); - -typedef struct { - uint8_t tap_x_en : 1; - uint8_t tap_y_en : 1; - uint8_t tap_z_en : 1; +int32_t lsm6dsv16x_stpcnt_mode_set(stmdev_ctx_t *ctx, + lsm6dsv16x_stpcnt_mode_t val); +int32_t lsm6dsv16x_stpcnt_mode_get(stmdev_ctx_t *ctx, + lsm6dsv16x_stpcnt_mode_t *val); + +int32_t lsm6dsv16x_stpcnt_steps_get(stmdev_ctx_t *ctx, uint16_t *val); + +int32_t lsm6dsv16x_stpcnt_rst_step_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6dsv16x_stpcnt_rst_step_get(stmdev_ctx_t *ctx, uint8_t *val); + +int32_t lsm6dsv16x_stpcnt_debounce_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6dsv16x_stpcnt_debounce_get(stmdev_ctx_t *ctx, uint8_t *val); + +int32_t lsm6dsv16x_stpcnt_period_set(stmdev_ctx_t *ctx, uint16_t val); +int32_t lsm6dsv16x_stpcnt_period_get(stmdev_ctx_t *ctx, uint16_t *val); + +int32_t lsm6dsv16x_sflp_game_rotation_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6dsv16x_sflp_game_rotation_get(stmdev_ctx_t *ctx, uint8_t *val); + +typedef struct +{ + float_t gbias_x; /* dps */ + float_t gbias_y; /* dps */ + float_t gbias_z; /* dps */ +} lsm6dsv16x_sflp_gbias_t; +int32_t lsm6dsv16x_sflp_game_gbias_set(stmdev_ctx_t *ctx, + lsm6dsv16x_sflp_gbias_t *val); + +typedef enum +{ + LSM6DSV16X_SFLP_15Hz = 0x0, + LSM6DSV16X_SFLP_30Hz = 0x1, + LSM6DSV16X_SFLP_60Hz = 0x2, + LSM6DSV16X_SFLP_120Hz = 0x3, + LSM6DSV16X_SFLP_240Hz = 0x4, + LSM6DSV16X_SFLP_480Hz = 0x5, +} lsm6dsv16x_sflp_data_rate_t; +int32_t lsm6dsv16x_sflp_data_rate_set(stmdev_ctx_t *ctx, + lsm6dsv16x_sflp_data_rate_t val); +int32_t lsm6dsv16x_sflp_data_rate_get(stmdev_ctx_t *ctx, + lsm6dsv16x_sflp_data_rate_t *val); + +typedef struct +{ + uint8_t tap_x_en : 1; + uint8_t tap_y_en : 1; + uint8_t tap_z_en : 1; } lsm6dsv16x_tap_detection_t; -int32_t lsm6dsv16x_tap_detection_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_tap_detection_t val); -int32_t lsm6dsv16x_tap_detection_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_tap_detection_t *val); - -typedef struct { - uint8_t x : 1; - uint8_t y : 1; - uint8_t z : 1; +int32_t lsm6dsv16x_tap_detection_set(stmdev_ctx_t *ctx, + lsm6dsv16x_tap_detection_t val); +int32_t lsm6dsv16x_tap_detection_get(stmdev_ctx_t *ctx, + lsm6dsv16x_tap_detection_t *val); + +typedef struct +{ + uint8_t x : 5; + uint8_t y : 5; + uint8_t z : 5; } lsm6dsv16x_tap_thresholds_t; -int32_t lsm6dsv16x_tap_thresholds_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_tap_thresholds_t val); -int32_t lsm6dsv16x_tap_thresholds_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_tap_thresholds_t *val); +int32_t lsm6dsv16x_tap_thresholds_set(stmdev_ctx_t *ctx, + lsm6dsv16x_tap_thresholds_t val); +int32_t lsm6dsv16x_tap_thresholds_get(stmdev_ctx_t *ctx, + lsm6dsv16x_tap_thresholds_t *val); -typedef enum { +typedef enum +{ LSM6DSV16X_XYZ = 0x0, LSM6DSV16X_YXZ = 0x1, - LSM6DSV16X_XZY = 0x2, + LSM6DSV16X_XZY = 0x2, LSM6DSV16X_ZYX = 0x3, LSM6DSV16X_YZX = 0x5, LSM6DSV16X_ZXY = 0x6, } lsm6dsv16x_tap_axis_priority_t; -int32_t lsm6dsv16x_tap_axis_priority_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_tap_axis_priority_t val); -int32_t lsm6dsv16x_tap_axis_priority_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_tap_axis_priority_t *val); - -typedef struct { - uint8_t shock : 1; - uint8_t quiet : 1; - uint8_t tap_gap : 1; +int32_t lsm6dsv16x_tap_axis_priority_set(stmdev_ctx_t *ctx, + lsm6dsv16x_tap_axis_priority_t val); +int32_t lsm6dsv16x_tap_axis_priority_get(stmdev_ctx_t *ctx, + lsm6dsv16x_tap_axis_priority_t *val); + +typedef struct +{ + uint8_t shock : 2; + uint8_t quiet : 2; + uint8_t tap_gap : 4; } lsm6dsv16x_tap_time_windows_t; -int32_t lsm6dsv16x_tap_time_windows_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_tap_time_windows_t val); -int32_t lsm6dsv16x_tap_time_windows_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_tap_time_windows_t *val); - -typedef enum { - LSM6DSV16X_ONLY_SINGLE = 0x0, +int32_t lsm6dsv16x_tap_time_windows_set(stmdev_ctx_t *ctx, + lsm6dsv16x_tap_time_windows_t val); +int32_t lsm6dsv16x_tap_time_windows_get(stmdev_ctx_t *ctx, + lsm6dsv16x_tap_time_windows_t *val); + +typedef enum +{ + LSM6DSV16X_ONLY_SINGLE = 0x0, LSM6DSV16X_BOTH_SINGLE_DOUBLE = 0x1, } lsm6dsv16x_tap_mode_t; -int32_t lsm6dsv16x_tap_mode_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_tap_mode_t val); -int32_t lsm6dsv16x_tap_mode_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_tap_mode_t *val); +int32_t lsm6dsv16x_tap_mode_set(stmdev_ctx_t *ctx, lsm6dsv16x_tap_mode_t val); +int32_t lsm6dsv16x_tap_mode_get(stmdev_ctx_t *ctx, lsm6dsv16x_tap_mode_t *val); -int32_t lsm6dsv16x_tilt_mode_set(lsm6dsv16x_ctx_t *ctx, uint8_t val); -int32_t lsm6dsv16x_tilt_mode_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val); +int32_t lsm6dsv16x_tilt_mode_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6dsv16x_tilt_mode_get(stmdev_ctx_t *ctx, uint8_t *val); -int32_t lsm6dsv16x_timestamp_raw_get(lsm6dsv16x_ctx_t *ctx, uint32_t *val); +int32_t lsm6dsv16x_timestamp_raw_get(stmdev_ctx_t *ctx, uint32_t *val); -int32_t lsm6dsv16x_timestamp_set(lsm6dsv16x_ctx_t *ctx, uint8_t val); -int32_t lsm6dsv16x_timestamp_get(lsm6dsv16x_ctx_t *ctx, uint8_t *val); +int32_t lsm6dsv16x_timestamp_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6dsv16x_timestamp_get(stmdev_ctx_t *ctx, uint8_t *val); -typedef enum { - LSM6DSV16X_XL_AND_GY_NOT_AFFECTED = 0x0, +typedef enum +{ + LSM6DSV16X_XL_AND_GY_NOT_AFFECTED = 0x0, LSM6DSV16X_XL_LOW_POWER_GY_NOT_AFFECTED = 0x1, - LSM6DSV16X_XL_LOW_POWER_GY_SLEEP = 0x2, - LSM6DSV16X_XL_LOW_POWER_GY_POWER_DOWN = 0x3, + LSM6DSV16X_XL_LOW_POWER_GY_SLEEP = 0x2, + LSM6DSV16X_XL_LOW_POWER_GY_POWER_DOWN = 0x3, } lsm6dsv16x_act_mode_t; -int32_t lsm6dsv16x_act_mode_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_act_mode_t val); -int32_t lsm6dsv16x_act_mode_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_act_mode_t *val); +int32_t lsm6dsv16x_act_mode_set(stmdev_ctx_t *ctx, lsm6dsv16x_act_mode_t val); +int32_t lsm6dsv16x_act_mode_get(stmdev_ctx_t *ctx, lsm6dsv16x_act_mode_t *val); -typedef enum { +typedef enum +{ LSM6DSV16X_SLEEP_TO_ACT_AT_1ST_SAMPLE = 0x0, LSM6DSV16X_SLEEP_TO_ACT_AT_2ND_SAMPLE = 0x1, LSM6DSV16X_SLEEP_TO_ACT_AT_3RD_SAMPLE = 0x2, LSM6DSV16X_SLEEP_TO_ACT_AT_4th_SAMPLE = 0x3, } lsm6dsv16x_act_from_sleep_to_act_dur_t; -int32_t lsm6dsv16x_act_from_sleep_to_act_dur_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_act_from_sleep_to_act_dur_t val); -int32_t lsm6dsv16x_act_from_sleep_to_act_dur_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_act_from_sleep_to_act_dur_t *val); +int32_t lsm6dsv16x_act_from_sleep_to_act_dur_set(stmdev_ctx_t *ctx, + lsm6dsv16x_act_from_sleep_to_act_dur_t val); +int32_t lsm6dsv16x_act_from_sleep_to_act_dur_get(stmdev_ctx_t *ctx, + lsm6dsv16x_act_from_sleep_to_act_dur_t *val); -typedef enum { +typedef enum +{ LSM6DSV16X_1Hz875 = 0x0, - LSM6DSV16X_15Hz = 0x1, - LSM6DSV16X_30Hz = 0x2, - LSM6DSV16X_60Hz = 0x3, + LSM6DSV16X_15Hz = 0x1, + LSM6DSV16X_30Hz = 0x2, + LSM6DSV16X_60Hz = 0x3, } lsm6dsv16x_act_sleep_xl_odr_t; -int32_t lsm6dsv16x_act_sleep_xl_odr_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_act_sleep_xl_odr_t val); -int32_t lsm6dsv16x_act_sleep_xl_odr_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_act_sleep_xl_odr_t *val); - -typedef struct { - uint32_t wk_ths_mg; - uint32_t inact_ths_mg; +int32_t lsm6dsv16x_act_sleep_xl_odr_set(stmdev_ctx_t *ctx, + lsm6dsv16x_act_sleep_xl_odr_t val); +int32_t lsm6dsv16x_act_sleep_xl_odr_get(stmdev_ctx_t *ctx, + lsm6dsv16x_act_sleep_xl_odr_t *val); + +typedef struct +{ + lsm6dsv16x_inactivity_dur_t inactivity_cfg; + uint8_t inactivity_ths; + uint8_t threshold; + uint8_t duration; } lsm6dsv16x_act_thresholds_t; -int32_t lsm6dsv16x_act_thresholds_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_act_thresholds_t val); -int32_t lsm6dsv16x_act_thresholds_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_act_thresholds_t *val); - -typedef struct { - uint8_t shock : 2; - uint8_t quiet : 4; +int32_t lsm6dsv16x_act_thresholds_set(stmdev_ctx_t *ctx, + lsm6dsv16x_act_thresholds_t *val); +int32_t lsm6dsv16x_act_thresholds_get(stmdev_ctx_t *ctx, + lsm6dsv16x_act_thresholds_t *val); + +typedef struct +{ + uint8_t shock : 2; + uint8_t quiet : 4; } lsm6dsv16x_act_wkup_time_windows_t; -int32_t lsm6dsv16x_act_wkup_time_windows_set(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_act_wkup_time_windows_t val); -int32_t lsm6dsv16x_act_wkup_time_windows_get(lsm6dsv16x_ctx_t *ctx, lsm6dsv16x_act_wkup_time_windows_t *val); +int32_t lsm6dsv16x_act_wkup_time_windows_set(stmdev_ctx_t *ctx, + lsm6dsv16x_act_wkup_time_windows_t val); +int32_t lsm6dsv16x_act_wkup_time_windows_get(stmdev_ctx_t *ctx, + lsm6dsv16x_act_wkup_time_windows_t *val); /** * @} diff --git a/src/sensors/lsm6dsv16xsensor.cpp b/src/sensors/lsm6dsv16xsensor.cpp index 7fbcdd178..78448d860 100644 --- a/src/sensors/lsm6dsv16xsensor.cpp +++ b/src/sensors/lsm6dsv16xsensor.cpp @@ -75,18 +75,30 @@ void LSM6DSV16XSensor::motionSetup() { uint8_t status = 0; - // Enable accelerometer - status |= imu.Enable_X(); - // Enable gyro - status |= imu.Enable_G(); - // Enable only low power fusion - status |= imu.Write_Reg(LSM6DSV16X_EMB_FUNC_EN_A, 0b00000010); - status |= imu.Set_X_ODR(120); - status |= imu.Set_G_ODR(120); - // Stream (in datasheet: continuous) mode discards old data as new comes in + // Restore defaults + status |= imu.Reset_Set(LSM6DSV16X_RESET_CTRL_REGS); + + // Set maximums + status |= imu.Set_X_FS(LSM6DSV16X_ACCEL_MAX); + status |= imu.Set_G_FS(LSM6DSV16X_GYRO_MAX); + + // Set FIFO size + status |= imu.FIFO_Set_Watermark_Level(LSM6DSV16X_FIFO_MAX_ENTRIES); + + // Set FIFO SFLP Batch + // NOTE: might not need all of this + status |= imu.FIFO_Set_SFLP_Batch(true, true, true); + + // Set FIFO mode to "continuous", so old data gets thrown away status |= imu.FIFO_Set_Mode(LSM6DSV16X_STREAM_MODE); - status |= imu.Set_SFLP_ODR(120); - status |= imu.Write_Reg(LSM6DSV16X_EMB_FUNC_INIT_A, 0b00000010); + + // Set data rate + status |= imu.Set_X_ODR(LSM6DSV16X_FIFO_DATA_RATE); + status |= imu.Set_G_ODR(LSM6DSV16X_FIFO_DATA_RATE); + status |= imu.Set_SFLP_ODR(LSM6DSV16X_FIFO_DATA_RATE); + + // Enable Game Rotation Fusion + status |= imu.Enable_Game_Rotation(); #ifndef INTERRUPTFREE attachInterrupt(m_IntPin, interruptHandler, RISING); diff --git a/src/sensors/lsm6dsv16xsensor.h b/src/sensors/lsm6dsv16xsensor.h index e8b24bce4..1b5f934eb 100644 --- a/src/sensors/lsm6dsv16xsensor.h +++ b/src/sensors/lsm6dsv16xsensor.h @@ -28,6 +28,22 @@ #include "sensor.h" +#ifndef LSM6DSV16X_ACCEL_MAX +#define LSM6DSV16X_ACCEL_MAX 4 +#endif + +#ifndef LSM6DSV16X_GYRO_MAX +#define LSM6DSV16X_GYRO_MAX 2000 +#endif + +#ifndef LSM6DSV16X_FIFO_MAX_ENTRIES +#define LSM6DSV16X_FIFO_MAX_ENTRIES 32 +#endif + +#ifndef LSM6DSV16X_FIFO_DATA_RATE +#define LSM6DSV16X_FIFO_DATA_RATE 120 +#endif + class LSM6DSV16XSensor : public Sensor { public: LSM6DSV16XSensor( From 177e8c023cfc19411b0ee3a3a97e7a3b354faf81 Mon Sep 17 00:00:00 2001 From: Gorbit99 Date: Thu, 3 Aug 2023 01:12:45 +0200 Subject: [PATCH 11/60] Fixed mistake in 'Set SFLP Batch' and cleaned up code --- lib/LSM6DSV16X/LSM6DSV16X.cpp | 33 ++++++++++----------------------- 1 file changed, 10 insertions(+), 23 deletions(-) diff --git a/lib/LSM6DSV16X/LSM6DSV16X.cpp b/lib/LSM6DSV16X/LSM6DSV16X.cpp index cc4f0b23a..ff02139cf 100644 --- a/lib/LSM6DSV16X/LSM6DSV16X.cpp +++ b/lib/LSM6DSV16X/LSM6DSV16X.cpp @@ -2601,13 +2601,10 @@ LSM6DSV16XStatusTypeDef LSM6DSV16X::FIFO_Set_G_BDR(float Bdr) LSM6DSV16XStatusTypeDef LSM6DSV16X::FIFO_Set_SFLP_Batch(bool GameRotation, bool Gravity, bool gBias) { lsm6dsv16x_fifo_sflp_raw_t fifo_sflp; - fifo_sflp.game_rotation = 1; - fifo_sflp.gravity = 1; - fifo_sflp.gbias = 1; - if (lsm6dsv16x_fifo_sflp_batch_set(®_ctx, fifo_sflp) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - return LSM6DSV16X_OK; + fifo_sflp.game_rotation = GameRotation; + fifo_sflp.gravity = Gravity; + fifo_sflp.gbias = gBias; + return (LSM6DSV16XStatusTypeDef)lsm6dsv16x_fifo_sflp_batch_set(®_ctx, fifo_sflp); } /** @@ -2854,12 +2851,7 @@ LSM6DSV16XStatusTypeDef LSM6DSV16X::Set_G_ODR_When_Enabled(float Odr) : (Odr <= 3840.0f) ? LSM6DSV16X_ODR_AT_3840Hz : LSM6DSV16X_ODR_AT_7680Hz; - /* Output data rate selection. */ - if (lsm6dsv16x_gy_data_rate_set(®_ctx, new_odr) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - return LSM6DSV16X_OK; + return (LSM6DSV16XStatusTypeDef) lsm6dsv16x_gy_data_rate_set(®_ctx, new_odr); } /** @@ -2948,11 +2940,7 @@ LSM6DSV16XStatusTypeDef LSM6DSV16X::Set_G_FS(int32_t FullScale) : (FullScale <= 2000) ? LSM6DSV16X_2000dps : LSM6DSV16X_4000dps; - if (lsm6dsv16x_gy_full_scale_set(®_ctx, new_fs) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - return LSM6DSV16X_OK; + return (LSM6DSV16XStatusTypeDef) lsm6dsv16x_gy_full_scale_set(®_ctx, new_fs); } /** @@ -3029,11 +3017,10 @@ LSM6DSV16XStatusTypeDef LSM6DSV16X::Get_G_DRDY_Status(uint8_t *Status) */ LSM6DSV16XStatusTypeDef LSM6DSV16X::Set_G_Power_Mode(uint8_t PowerMode) { - if (lsm6dsv16x_gy_mode_set(®_ctx, (lsm6dsv16x_gy_mode_t)PowerMode) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - return LSM6DSV16X_OK; + return (LSM6DSV16XStatusTypeDef)lsm6dsv16x_gy_mode_set( + ®_ctx, + (lsm6dsv16x_gy_mode_t)PowerMode + ); } /** From 20b74844391aea06dbf79ee7d366369e2aac27d6 Mon Sep 17 00:00:00 2001 From: Gorbit99 Date: Thu, 3 Aug 2023 02:25:05 +0200 Subject: [PATCH 12/60] Everything should be the same but nothing --- lib/LSM6DSV16X/LSM6DSV16X.cpp | 67 ++++++++++++++++++++++---------- lib/LSM6DSV16X/LSM6DSV16X.h | 5 +++ lib/LSM6DSV16X/lsm6dsv16x_reg.c | 2 +- src/sensors/lsm6dsv16xsensor.cpp | 35 ++++++++++++++--- src/sensors/lsm6dsv16xsensor.h | 6 +-- 5 files changed, 85 insertions(+), 30 deletions(-) diff --git a/lib/LSM6DSV16X/LSM6DSV16X.cpp b/lib/LSM6DSV16X/LSM6DSV16X.cpp index ff02139cf..cff46798a 100644 --- a/lib/LSM6DSV16X/LSM6DSV16X.cpp +++ b/lib/LSM6DSV16X/LSM6DSV16X.cpp @@ -51,6 +51,7 @@ LSM6DSV16X::LSM6DSV16X(TwoWire *i2c, uint8_t address) : dev_i2c(i2c), address(ad { reg_ctx.write_reg = LSM6DSV16X_io_write; reg_ctx.read_reg = LSM6DSV16X_io_read; + reg_ctx.mdelay = LSM6DSV16X_sleep; reg_ctx.handle = (void *)this; dev_spi = NULL; acc_is_enabled = 0L; @@ -91,7 +92,7 @@ LSM6DSV16XStatusTypeDef LSM6DSV16X::begin() } /* Enable BDU */ - if (lsm6dsv16x_block_data_update_set(®_ctx, PROPERTY_ENABLE) != LSM6DSV16X_OK) { + if (Enable_Block_Data_Update() != LSM6DSV16X_OK) { return LSM6DSV16X_ERROR; } @@ -2607,6 +2608,11 @@ LSM6DSV16XStatusTypeDef LSM6DSV16X::FIFO_Set_SFLP_Batch(bool GameRotation, bool return (LSM6DSV16XStatusTypeDef)lsm6dsv16x_fifo_sflp_batch_set(®_ctx, fifo_sflp); } +LSM6DSV16XStatusTypeDef LSM6DSV16X::FIFO_Get_Status(lsm6dsv16x_fifo_status_t * Status) +{ + return (LSM6DSV16XStatusTypeDef) lsm6dsv16x_fifo_status_get(®_ctx, Status); +} + /** * @brief Enable the LSM6DSV16X gyroscope sensor * @retval 0 in case of success, an error code otherwise @@ -3053,23 +3059,30 @@ LSM6DSV16XStatusTypeDef LSM6DSV16X::Set_G_Filter_Mode(uint8_t LowHighPassFlag, u return LSM6DSV16X_OK; } -LSM6DSV16XStatusTypeDef LSM6DSV16X::Set_SFLP_ODR(float odr) { - uint8_t bits = odr <= 15 ? 0b000 - : odr <= 30 ? 0b001 - : odr <= 60 ? 0b010 - : odr <= 120 ? 0b011 - : odr <= 240 ? 0b100 - : 0b101; - - lsm6dsv16x_sflp_odr_t value; - value.not_used0 = 0b011; - value.sflp_game_odr = bits; - value.not_used1 = 0b01; +LSM6DSV16XStatusTypeDef LSM6DSV16X::Set_G_Bias(float x, float y, float z) +{ + lsm6dsv16x_sflp_gbias_t gbias; + gbias.gbias_x = x; + gbias.gbias_y = y; + gbias.gbias_z = z; + return (LSM6DSV16XStatusTypeDef)lsm6dsv16x_sflp_game_gbias_set( + ®_ctx, + &gbias + ); +} - if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_SFLP_ODR, (uint8_t *)&value, sizeof(value)) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - return LSM6DSV16X_OK; +LSM6DSV16XStatusTypeDef LSM6DSV16X::Set_SFLP_ODR(float odr) { + lsm6dsv16x_sflp_data_rate_t rate = odr <= 15 ? LSM6DSV16X_SFLP_15Hz + : odr <= 30 ? LSM6DSV16X_SFLP_30Hz + : odr <= 60 ? LSM6DSV16X_SFLP_60Hz + : odr <= 120 ? LSM6DSV16X_SFLP_120Hz + : odr <= 240 ? LSM6DSV16X_SFLP_240Hz + : LSM6DSV16X_SFLP_480Hz; + + return (LSM6DSV16XStatusTypeDef)lsm6dsv16x_sflp_data_rate_set( + ®_ctx, + rate + ); } @@ -3236,6 +3249,10 @@ int32_t LSM6DSV16X_io_read(void *handle, uint8_t ReadAddr, uint8_t *pBuffer, uin return ((LSM6DSV16X *)handle)->IO_Read(pBuffer, ReadAddr, nBytesToRead); } +void LSM6DSV16X_sleep(uint32_t ms) { + delay(ms); +} + LSM6DSV16XStatusTypeDef LSM6DSV16X::Reset_Set(uint8_t flags) { if (lsm6dsv16x_reset_set(®_ctx, (lsm6dsv16x_reset_t)flags) != LSM6DSV16X_OK) { @@ -3253,8 +3270,16 @@ LSM6DSV16XStatusTypeDef LSM6DSV16X::Reset_Set(uint8_t flags) LSM6DSV16XStatusTypeDef LSM6DSV16X::Enable_Game_Rotation(bool enable) { - if (lsm6dsv16x_sflp_game_rotation_set(®_ctx, enable ? PROPERTY_ENABLE : PROPERTY_DISABLE) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - return LSM6DSV16X_OK; + return (LSM6DSV16XStatusTypeDef)lsm6dsv16x_sflp_game_rotation_set( + ®_ctx, + enable ? PROPERTY_ENABLE : PROPERTY_DISABLE + ); +} + +LSM6DSV16XStatusTypeDef LSM6DSV16X::Enable_Block_Data_Update(bool enable) +{ + return (LSM6DSV16XStatusTypeDef)lsm6dsv16x_block_data_update_set( + ®_ctx, + enable ? PROPERTY_ENABLE : PROPERTY_DISABLE + ); } diff --git a/lib/LSM6DSV16X/LSM6DSV16X.h b/lib/LSM6DSV16X/LSM6DSV16X.h index a5e6639ca..a1a6f25a6 100644 --- a/lib/LSM6DSV16X/LSM6DSV16X.h +++ b/lib/LSM6DSV16X/LSM6DSV16X.h @@ -161,6 +161,7 @@ class LSM6DSV16X { LSM6DSV16XStatusTypeDef Get_G_DRDY_Status(uint8_t *Status); LSM6DSV16XStatusTypeDef Set_G_Power_Mode(uint8_t PowerMode); LSM6DSV16XStatusTypeDef Set_G_Filter_Mode(uint8_t LowHighPassFlag, uint8_t FilterMode); + LSM6DSV16XStatusTypeDef Set_G_Bias(float x, float y, float z); LSM6DSV16XStatusTypeDef Set_SFLP_ODR(float Odr); @@ -215,6 +216,7 @@ class LSM6DSV16X { LSM6DSV16XStatusTypeDef FIFO_Get_G_Axes(int32_t *AngularVelocity); LSM6DSV16XStatusTypeDef FIFO_Set_G_BDR(float Bdr); LSM6DSV16XStatusTypeDef FIFO_Set_SFLP_Batch(bool GameRotation, bool Gravity, bool gBias); + LSM6DSV16XStatusTypeDef FIFO_Get_Status(lsm6dsv16x_fifo_status_t *Status); LSM6DSV16XStatusTypeDef QVAR_Enable(); LSM6DSV16XStatusTypeDef QVAR_GetStatus(uint8_t *val); @@ -231,6 +233,8 @@ class LSM6DSV16X { LSM6DSV16XStatusTypeDef Enable_Game_Rotation(bool enable = true); + LSM6DSV16XStatusTypeDef Enable_Block_Data_Update(bool enable = true); + /** * @brief Utility function to read data. * @param pBuffer: pointer to data to be read. @@ -350,6 +354,7 @@ extern "C" { #endif int32_t LSM6DSV16X_io_write(void *handle, uint8_t WriteAddr, const uint8_t *pBuffer, uint16_t nBytesToWrite); int32_t LSM6DSV16X_io_read(void *handle, uint8_t ReadAddr, uint8_t *pBuffer, uint16_t nBytesToRead); +void LSM6DSV16X_sleep(uint32_t ms); #ifdef __cplusplus } #endif diff --git a/lib/LSM6DSV16X/lsm6dsv16x_reg.c b/lib/LSM6DSV16X/lsm6dsv16x_reg.c index 030f80909..36c00ff84 100644 --- a/lib/LSM6DSV16X/lsm6dsv16x_reg.c +++ b/lib/LSM6DSV16X/lsm6dsv16x_reg.c @@ -5849,7 +5849,7 @@ int32_t lsm6dsv16x_sflp_game_gbias_set(stmdev_ctx_t *ctx, // disable gbias setting ctrl10.emb_func_debug = 0; ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_CTRL10, (uint8_t *)&ctrl10, 1); - + /* reload previous master configuration */ ret += lsm6dsv16x_sh_master_set(ctx, master_config); diff --git a/src/sensors/lsm6dsv16xsensor.cpp b/src/sensors/lsm6dsv16xsensor.cpp index 78448d860..a208e2413 100644 --- a/src/sensors/lsm6dsv16xsensor.cpp +++ b/src/sensors/lsm6dsv16xsensor.cpp @@ -30,8 +30,6 @@ #define INTERRUPTFREE // TODO: change based on int pin number (255 = interruptFree) #define DEBUG_SENSOR -constexpr uint8_t SFLP_GAME_EN_BIT = 1 << 1; - volatile bool imuEvent = false; void IRAM_ATTR interruptHandler() { imuEvent = true; } @@ -76,14 +74,17 @@ void LSM6DSV16XSensor::motionSetup() { uint8_t status = 0; // Restore defaults - status |= imu.Reset_Set(LSM6DSV16X_RESET_CTRL_REGS); + // status |= imu.Reset_Set(LSM6DSV16X_RESET_CTRL_REGS); + + // Enable Block Data Update + status |= imu.Enable_Block_Data_Update(); // Set maximums status |= imu.Set_X_FS(LSM6DSV16X_ACCEL_MAX); status |= imu.Set_G_FS(LSM6DSV16X_GYRO_MAX); // Set FIFO size - status |= imu.FIFO_Set_Watermark_Level(LSM6DSV16X_FIFO_MAX_ENTRIES); + status |= imu.FIFO_Set_Watermark_Level(LSM6DSV16X_FIFO_MAX_SIZE); // Set FIFO SFLP Batch // NOTE: might not need all of this @@ -100,6 +101,9 @@ void LSM6DSV16XSensor::motionSetup() { // Enable Game Rotation Fusion status |= imu.Enable_Game_Rotation(); + // Set GBias + status |= imu.Set_G_Bias(0, 0, 0); + #ifndef INTERRUPTFREE attachInterrupt(m_IntPin, interruptHandler, RISING); @@ -125,7 +129,24 @@ void LSM6DSV16XSensor::motionSetup() { } void LSM6DSV16XSensor::motionLoop() { - uint16_t fifo_samples; + lsm6dsv16x_fifo_status_t fifo_status; + if (imu.FIFO_Get_Status(&fifo_status) != LSM6DSV16X_OK) { + m_Logger.error( + "Error getting FIFO status on %s at address 0x%02x", + getIMUNameByType(sensorType), + addr + ); + errorCounter++; + return; + } + + m_Logger.info("FIFO status: %d", fifo_status.fifo_level); + + if (fifo_status.fifo_th != 1) { + return; + } + + uint16_t fifo_samples = fifo_status.fifo_level; if (imu.FIFO_Get_Num_Samples(&fifo_samples) == LSM6DSV16X_ERROR) { m_Logger.error( "Error getting number of samples in FIFO on %s at address 0x%02x", @@ -136,6 +157,10 @@ void LSM6DSV16XSensor::motionLoop() { return; } + if (fifo_samples != 0) { + m_Logger.info("Got %d samples", fifo_samples); + } + for (uint16_t i = 0; i < fifo_samples; i++) { uint8_t tag; if (imu.FIFO_Get_Tag(&tag) != LSM6DSV16X_OK) { diff --git a/src/sensors/lsm6dsv16xsensor.h b/src/sensors/lsm6dsv16xsensor.h index 1b5f934eb..8f572f582 100644 --- a/src/sensors/lsm6dsv16xsensor.h +++ b/src/sensors/lsm6dsv16xsensor.h @@ -36,12 +36,12 @@ #define LSM6DSV16X_GYRO_MAX 2000 #endif -#ifndef LSM6DSV16X_FIFO_MAX_ENTRIES -#define LSM6DSV16X_FIFO_MAX_ENTRIES 32 +#ifndef LSM6DSV16X_FIFO_MAX_SIZE +#define LSM6DSV16X_FIFO_MAX_SIZE 32 #endif #ifndef LSM6DSV16X_FIFO_DATA_RATE -#define LSM6DSV16X_FIFO_DATA_RATE 120 +#define LSM6DSV16X_FIFO_DATA_RATE 30 #endif class LSM6DSV16XSensor : public Sensor { From 57f22ae4d2d5849333aa2fe9b660b6b0b7d61fd3 Mon Sep 17 00:00:00 2001 From: Carter Meyers Date: Wed, 2 Aug 2023 19:31:44 -0700 Subject: [PATCH 13/60] Add IMU Self Test --- lib/LSM6DSV16X/LSM6DSV16X.cpp | 280 +++++++++++++++++++++++++++++++ lib/LSM6DSV16X/LSM6DSV16X.h | 11 ++ src/sensors/lsm6dsv16xsensor.cpp | 9 + 3 files changed, 300 insertions(+) diff --git a/lib/LSM6DSV16X/LSM6DSV16X.cpp b/lib/LSM6DSV16X/LSM6DSV16X.cpp index cff46798a..b6a2f30ac 100644 --- a/lib/LSM6DSV16X/LSM6DSV16X.cpp +++ b/lib/LSM6DSV16X/LSM6DSV16X.cpp @@ -588,6 +588,25 @@ LSM6DSV16XStatusTypeDef LSM6DSV16X::Get_X_AxesRaw(int16_t *Value) return LSM6DSV16X_OK; } +/** + * @brief Get the LSM6DSV16X accelerometer sensor raw axes when avaiable (Blocking) + * @param Value pointer where the raw values of the axes are written + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSV16XStatusTypeDef LSM6DSV16X::Get_X_AxesRaw_When_Aval(int16_t *Value) { + lsm6dsv16x_data_ready_t drdy; + do { + if (lsm6dsv16x_flag_data_ready_get(®_ctx, &drdy) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + } while (!drdy.drdy_xl); + + if (Get_X_AxesRaw(Value) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + return LSM6DSV16X_OK; +} + /** * @brief Get the LSM6DSV16X ACC data ready bit value * @param Status the status of data ready bit @@ -759,6 +778,248 @@ LSM6DSV16XStatusTypeDef LSM6DSV16X::Set_X_Filter_Mode(uint8_t LowHighPassFlag, u return LSM6DSV16X_OK; } +/** + * @brief Runs the ST specified accelerometer and gyroscope self test + * @param XTestType LSM6DSV16X_XL_ST_DISABLE = 0x0, LSM6DSV16X_XL_ST_POSITIVE = 0x1, LSM6DSV16X_XL_ST_NEGATIVE = 0x2 + * @param GTestType LSM6DSV16X_GY_ST_DISABLE = 0x0, LSM6DSV16X_GY_ST_POSITIVE = 0x1, LSM6DSV16X_GY_ST_NEGATIVE = 0x2 + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSV16XStatusTypeDef LSM6DSV16X::Test_IMU(uint8_t XTestType, uint8_t GTestType) +{ + uint8_t whoamI; + + + if (ReadID(&whoamI) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + if (whoamI != LSM6DSV16X_ID) { + return LSM6DSV16X_ERROR; + } + + + if (Test_X_IMU(XTestType) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + if (Test_G_IMU(GTestType) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + return LSM6DSV16X_OK; +} + +/** + * @brief Runs the ST specified accelerometer self test + * @param TestType LSM6DSV16X_XL_ST_DISABLE = 0x0, LSM6DSV16X_XL_ST_POSITIVE = 0x1, LSM6DSV16X_XL_ST_NEGATIVE = 0x2 + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSV16XStatusTypeDef LSM6DSV16X::Test_X_IMU(uint8_t TestType) +{ + int16_t data_raw[3]; + float val_st_off[3]; + float val_st_on[3]; + float test_val[3]; + + + if (Reset_Set(LSM6DSV16X_RESTORE_CTRL_REGS) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + if (lsm6dsv16x_block_data_update_set(®_ctx, PROPERTY_ENABLE) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + /* + * Accelerometer Self Test + */ + if (lsm6dsv16x_xl_data_rate_set(®_ctx, LSM6DSV16X_ODR_AT_60Hz) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + if (lsm6dsv16x_xl_full_scale_set(®_ctx, LSM6DSV16X_4g) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + delayMicroseconds(100); //Wait for Accelerometer to stabalize; + memset(val_st_off, 0x00, 3 * sizeof(float)); + memset(val_st_on, 0x00, 3 * sizeof(float)); + + + /*Ignore First Data*/ + if (Get_X_AxesRaw_When_Aval(data_raw) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + for (uint8_t i = 0; i < 5; i++) { + if (Get_X_AxesRaw_When_Aval(data_raw) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + /*Average the data in each axis*/ + for (uint8_t j = 0; j < 3; j++) { + val_st_off[j] += lsm6dsv16x_from_fs4_to_mg(data_raw[j]); + } + } + + /* Calculate the mg average values */ + for (uint8_t i = 0; i < 3; i++) { + val_st_off[i] /= 5.0f; + } + + if (lsm6dsv16x_xl_self_test_set(®_ctx, (lsm6dsv16x_xl_self_test_t)TestType) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + delayMicroseconds(100); + + + /*Ignore First Data*/ + if (Get_X_AxesRaw_When_Aval(data_raw) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + for (uint8_t i = 0; i < 5; i++) { + if (Get_X_AxesRaw_When_Aval(data_raw) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + /*Average the data in each axis*/ + for (uint8_t j = 0; j < 3; j++) { + val_st_on[j] += lsm6dsv16x_from_fs4_to_mg(data_raw[j]); + } + } + + /* Calculate the mg average values */ + for (uint8_t i = 0; i < 3; i++) { + val_st_on[i] /= 5.0f; + } + + /* Calculate the mg values for self test */ + for (uint8_t i = 0; i < 3; i++) { + test_val[i] = fabs((val_st_on[i] - val_st_off[i])); + } + + for (uint8_t i = 0; i < 3; i++) { + if (( LSM6DSV16X_MIN_ST_LIMIT_mg > test_val[i] ) || ( test_val[i] > LSM6DSV16X_MAX_ST_LIMIT_mg)) + return LSM6DSV16X_ERROR; + } + + if (lsm6dsv16x_xl_self_test_set(®_ctx, LSM6DSV16X_XL_ST_DISABLE) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + if (lsm6dsv16x_xl_data_rate_set(®_ctx, LSM6DSV16X_ODR_OFF) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + return LSM6DSV16X_OK; +} + +/** + * @brief Runs the ST specified self test on the acceleration axis of the IMU + * @param TestType LSM6DSV16X_GY_ST_DISABLE = 0x0, LSM6DSV16X_GY_ST_POSITIVE = 0x1, LSM6DSV16X_GY_ST_NEGATIVE = 0x2 + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSV16XStatusTypeDef LSM6DSV16X::Test_G_IMU(uint8_t TestType = LSM6DSV16X_GY_ST_POSITIVE) +{ + int16_t data_raw[3]; + float val_st_off[3]; + float val_st_on[3]; + float test_val[3]; + + if (Reset_Set(LSM6DSV16X_RESTORE_CTRL_REGS) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + if (lsm6dsv16x_block_data_update_set(®_ctx, PROPERTY_ENABLE) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + /* + * Gyroscope Self Test + */ + + if (lsm6dsv16x_gy_data_rate_set(®_ctx, LSM6DSV16X_ODR_AT_240Hz) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + if (lsm6dsv16x_gy_full_scale_set(®_ctx, LSM6DSV16X_2000dps) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + delayMicroseconds(100); + memset(val_st_off, 0x00, 3 * sizeof(float)); + memset(val_st_on, 0x00, 3 * sizeof(float)); + + + /*Ignore First Data*/ + if (Get_G_AxesRaw_When_Aval(data_raw) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + for (uint8_t i = 0; i < 5; i++) { + if (Get_G_AxesRaw_When_Aval(data_raw) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + /*Average the data in each axis*/ + for (uint8_t j = 0; j < 3; j++) { + val_st_off[j] += lsm6dsv16x_from_fs2000_to_mdps(data_raw[j]); + } + } + + /* Calculate the mg average values */ + for (uint8_t i = 0; i < 3; i++) { + val_st_off[i] /= 5.0f; + } + + if (lsm6dsv16x_gy_self_test_set(®_ctx, (lsm6dsv16x_gy_self_test_t)TestType) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + delayMicroseconds(100); + + + /*Ignore First Data*/ + if (Get_G_AxesRaw_When_Aval(data_raw) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + for (uint8_t i = 0; i < 5; i++) { + if (Get_G_AxesRaw_When_Aval(data_raw) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + /*Average the data in each axis*/ + for (uint8_t j = 0; j < 3; j++) { + val_st_on[j] += lsm6dsv16x_from_fs2000_to_mdps(data_raw[j]); + } + } + + /* Calculate the mg average values */ + for (uint8_t i = 0; i < 3; i++) { + val_st_on[i] /= 5.0f; + } + + /* Calculate the mg values for self test */ + for (uint8_t i = 0; i < 3; i++) { + test_val[i] = fabs((val_st_on[i] - val_st_off[i])); + } + + /* Check self test limit */ + for (uint8_t i = 0; i < 3; i++) { + if (( LSM6DSV16X_MIN_ST_LIMIT_mdps > test_val[i] ) || + ( test_val[i] > LSM6DSV16X_MAX_ST_LIMIT_mdps)) { + return LSM6DSV16X_ERROR; + } + } + + + if (lsm6dsv16x_gy_self_test_set(®_ctx, LSM6DSV16X_GY_ST_DISABLE) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + if (lsm6dsv16x_xl_data_rate_set(®_ctx, LSM6DSV16X_ODR_OFF) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + return LSM6DSV16X_OK; +} + /** * @brief Enable 6D orientation detection * @param IntPin interrupt pin line to be used @@ -2971,6 +3232,25 @@ LSM6DSV16XStatusTypeDef LSM6DSV16X::Get_G_AxesRaw(int16_t *Value) return LSM6DSV16X_OK; } +/** + * @brief Get the LSM6DSV16X gyroscope sensor raw axes when data available (Blocking) + * @param Value pointer where the raw values of the axes are written + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSV16XStatusTypeDef LSM6DSV16X::Get_G_AxesRaw_When_Aval(int16_t *Value) { + lsm6dsv16x_data_ready_t drdy; + do { + if (lsm6dsv16x_flag_data_ready_get(®_ctx, &drdy) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + } while (!drdy.drdy_xl); + + if (Get_G_AxesRaw(Value) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + return LSM6DSV16X_OK; +} + /** * @brief Get the LSM6DSV16X gyroscope sensor axes * @param AngularRate pointer where the values of the axes are written diff --git a/lib/LSM6DSV16X/LSM6DSV16X.h b/lib/LSM6DSV16X/LSM6DSV16X.h index a1a6f25a6..618684a48 100644 --- a/lib/LSM6DSV16X/LSM6DSV16X.h +++ b/lib/LSM6DSV16X/LSM6DSV16X.h @@ -63,6 +63,11 @@ #define LSM6DSV16X_GYRO_SENSITIVITY_FS_2000DPS 70.000f #define LSM6DSV16X_GYRO_SENSITIVITY_FS_4000DPS 140.000f +#define LSM6DSV16X_MIN_ST_LIMIT_mg 50.0f +#define LSM6DSV16X_MAX_ST_LIMIT_mg 1700.0f +#define LSM6DSV16X_MIN_ST_LIMIT_mdps 150000.0f +#define LSM6DSV16X_MAX_ST_LIMIT_mdps 700000.0f + #define LSM6DSV16X_QVAR_GAIN 78.000f /* Typedefs ------------------------------------------------------------------*/ @@ -143,6 +148,7 @@ class LSM6DSV16X { LSM6DSV16XStatusTypeDef Get_X_FS(int32_t *FullScale); LSM6DSV16XStatusTypeDef Set_X_FS(int32_t FullScale); LSM6DSV16XStatusTypeDef Get_X_AxesRaw(int16_t *Value); + LSM6DSV16XStatusTypeDef Get_X_AxesRaw_When_Aval(int16_t *Value); LSM6DSV16XStatusTypeDef Get_X_Axes(int32_t *Acceleration); LSM6DSV16XStatusTypeDef Get_X_DRDY_Status(uint8_t *Status); LSM6DSV16XStatusTypeDef Get_X_Event_Status(LSM6DSV16X_Event_Status_t *Status); @@ -157,12 +163,17 @@ class LSM6DSV16X { LSM6DSV16XStatusTypeDef Get_G_FS(int32_t *FullScale); LSM6DSV16XStatusTypeDef Set_G_FS(int32_t FullScale); LSM6DSV16XStatusTypeDef Get_G_AxesRaw(int16_t *Value); + LSM6DSV16XStatusTypeDef Get_G_AxesRaw_When_Aval(int16_t *Value); LSM6DSV16XStatusTypeDef Get_G_Axes(int32_t *AngularRate); LSM6DSV16XStatusTypeDef Get_G_DRDY_Status(uint8_t *Status); LSM6DSV16XStatusTypeDef Set_G_Power_Mode(uint8_t PowerMode); LSM6DSV16XStatusTypeDef Set_G_Filter_Mode(uint8_t LowHighPassFlag, uint8_t FilterMode); LSM6DSV16XStatusTypeDef Set_G_Bias(float x, float y, float z); + LSM6DSV16XStatusTypeDef Test_IMU(uint8_t XTestType, uint8_t GTestType); + LSM6DSV16XStatusTypeDef Test_X_IMU(uint8_t TestType); + LSM6DSV16XStatusTypeDef Test_G_IMU(uint8_t TestType); + LSM6DSV16XStatusTypeDef Set_SFLP_ODR(float Odr); LSM6DSV16XStatusTypeDef Enable_6D_Orientation(LSM6DSV16X_SensorIntPin_t IntPin); diff --git a/src/sensors/lsm6dsv16xsensor.cpp b/src/sensors/lsm6dsv16xsensor.cpp index a208e2413..97a90d3c5 100644 --- a/src/sensors/lsm6dsv16xsensor.cpp +++ b/src/sensors/lsm6dsv16xsensor.cpp @@ -73,6 +73,15 @@ void LSM6DSV16XSensor::motionSetup() { uint8_t status = 0; + //Test + if (imu.Test_IMU(LSM6DSV16X_XL_ST_NEGATIVE, LSM6DSV16X_GY_ST_NEGATIVE) == LSM6DSV16X_ERROR) { + m_Logger.fatal( + "The IMU returned an error during the self test" + ); + ledManager.pattern(50, 50, 200); + return; + } + // Restore defaults // status |= imu.Reset_Set(LSM6DSV16X_RESET_CTRL_REGS); From 3f4a5b4283f56a01afa4026a16b80545f6c61acd Mon Sep 17 00:00:00 2001 From: Carter Meyers Date: Thu, 3 Aug 2023 16:48:11 -0700 Subject: [PATCH 14/60] Slimevr gets rotation --- lib/LSM6DSV16X/LSM6DSV16X.cpp | 65 +++++++++++++++- lib/LSM6DSV16X/LSM6DSV16X.h | 31 +++++++- src/sensors/lsm6dsv16xsensor.cpp | 128 +++++++++++++++++++++++-------- src/sensors/lsm6dsv16xsensor.h | 2 +- 4 files changed, 189 insertions(+), 37 deletions(-) diff --git a/lib/LSM6DSV16X/LSM6DSV16X.cpp b/lib/LSM6DSV16X/LSM6DSV16X.cpp index b6a2f30ac..3eb1fdf94 100644 --- a/lib/LSM6DSV16X/LSM6DSV16X.cpp +++ b/lib/LSM6DSV16X/LSM6DSV16X.cpp @@ -801,10 +801,10 @@ LSM6DSV16XStatusTypeDef LSM6DSV16X::Test_IMU(uint8_t XTestType, uint8_t GTestTyp if (Test_X_IMU(XTestType) != LSM6DSV16X_OK) { return LSM6DSV16X_ERROR; } - + if (Test_G_IMU(GTestType) != LSM6DSV16X_OK) { return LSM6DSV16X_ERROR; - } + } return LSM6DSV16X_OK; } @@ -864,7 +864,7 @@ LSM6DSV16XStatusTypeDef LSM6DSV16X::Test_X_IMU(uint8_t TestType) for (uint8_t i = 0; i < 3; i++) { val_st_off[i] /= 5.0f; } - + if (lsm6dsv16x_xl_self_test_set(®_ctx, (lsm6dsv16x_xl_self_test_t)TestType) != LSM6DSV16X_OK) { return LSM6DSV16X_ERROR; } @@ -1020,6 +1020,52 @@ LSM6DSV16XStatusTypeDef LSM6DSV16X::Test_G_IMU(uint8_t TestType = LSM6DSV16X_GY_ return LSM6DSV16X_OK; } + +LSM6DSV16XStatusTypeDef LSM6DSV16X::Get_T_ODR(float *Odr) { + LSM6DSV16XStatusTypeDef ret; + lsm6dsv16x_fifo_ctrl4_t ctrl4; + + if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_FIFO_CTRL4, (uint8_t *)&ctrl4, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + if (ctrl4.odr_t_batch == 0x00) + *Odr = 0.0F; + if (ctrl4.odr_t_batch <= 0x01) + *Odr = 1.875F; + if (ctrl4.odr_t_batch <= 0x02) + *Odr = 15.0F; + else + *Odr = 60.0F; + + return LSM6DSV16X_OK; +} + + + +LSM6DSV16XStatusTypeDef LSM6DSV16X::Set_T_ODR(float Odr) { + LSM6DSV16XStatusTypeDef ret; + lsm6dsv16x_fifo_ctrl4_t ctrl4; + + if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_FIFO_CTRL4, (uint8_t *)&ctrl4, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + if (Odr == 0.0F) + ctrl4.odr_t_batch = 0x00; + if (Odr <= 1.875F) + ctrl4.odr_t_batch = 0x01; + if (Odr <= 15.0F) + ctrl4.odr_t_batch = 0x02; + else + ctrl4.odr_t_batch = 0x03; + + if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_FIFO_CTRL4, (uint8_t *)&ctrl4, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + return LSM6DSV16X_OK; +} + /** * @brief Enable 6D orientation detection * @param IntPin interrupt pin line to be used @@ -3563,3 +3609,16 @@ LSM6DSV16XStatusTypeDef LSM6DSV16X::Enable_Block_Data_Update(bool enable) enable ? PROPERTY_ENABLE : PROPERTY_DISABLE ); } + +/** + * @brief Enable register address automatically incremented during a multiple byte + access with a serial interface. + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSV16XStatusTypeDef LSM6DSV16X::Set_Auto_Increment(bool enable) +{ + return (LSM6DSV16XStatusTypeDef)lsm6dsv16x_auto_increment_set( + ®_ctx, + enable ? PROPERTY_ENABLE : PROPERTY_DISABLE + ); +} diff --git a/lib/LSM6DSV16X/LSM6DSV16X.h b/lib/LSM6DSV16X/LSM6DSV16X.h index 618684a48..7bda015b8 100644 --- a/lib/LSM6DSV16X/LSM6DSV16X.h +++ b/lib/LSM6DSV16X/LSM6DSV16X.h @@ -70,6 +70,11 @@ #define LSM6DSV16X_QVAR_GAIN 78.000f + + +//#define esp32 +//#define I2C_LIB_DEBUG + /* Typedefs ------------------------------------------------------------------*/ typedef enum { @@ -173,6 +178,9 @@ class LSM6DSV16X { LSM6DSV16XStatusTypeDef Test_IMU(uint8_t XTestType, uint8_t GTestType); LSM6DSV16XStatusTypeDef Test_X_IMU(uint8_t TestType); LSM6DSV16XStatusTypeDef Test_G_IMU(uint8_t TestType); + + LSM6DSV16XStatusTypeDef Get_T_ODR(float *Odr); + LSM6DSV16XStatusTypeDef Set_T_ODR(float Odr); LSM6DSV16XStatusTypeDef Set_SFLP_ODR(float Odr); @@ -245,6 +253,7 @@ class LSM6DSV16X { LSM6DSV16XStatusTypeDef Enable_Game_Rotation(bool enable = true); LSM6DSV16XStatusTypeDef Enable_Block_Data_Update(bool enable = true); + LSM6DSV16XStatusTypeDef Set_Auto_Increment(bool enable); /** * @brief Utility function to read data. @@ -256,8 +265,11 @@ class LSM6DSV16X { uint8_t IO_Read(uint8_t *pBuffer, uint8_t RegisterAddr, uint16_t NumByteToRead) { if (dev_spi) { +#ifdef esp32 + dev_spi->beginTransaction(SPISettings(spi_speed, SPI_MSBFIRST, SPI_MODE3)); +#else dev_spi->beginTransaction(SPISettings(spi_speed, MSBFIRST, SPI_MODE3)); - +#endif digitalWrite(cs_pin, LOW); /* Write Reg Address */ @@ -275,6 +287,9 @@ class LSM6DSV16X { } if (dev_i2c) { +#ifdef I2C_LIB_DEBUG + printf("\n\n[LSM LIB] Read register: 0x%02x, Byte Count: %d bytes", RegisterAddr, NumByteToRead); +#endif dev_i2c->beginTransmission(((uint8_t)(((address) >> 1) & 0x7F))); dev_i2c->write(RegisterAddr); dev_i2c->endTransmission(false); @@ -284,6 +299,9 @@ class LSM6DSV16X { int i = 0; while (dev_i2c->available()) { pBuffer[i] = dev_i2c->read(); +#ifdef I2C_LIB_DEBUG + printf("\n[LSM LIB] Register Read: 0x%02x, Data: 0x%02x", RegisterAddr + i, pBuffer[i]); +#endif i++; } @@ -303,8 +321,11 @@ class LSM6DSV16X { uint8_t IO_Write(const uint8_t *pBuffer, uint8_t RegisterAddr, uint16_t NumByteToWrite) { if (dev_spi) { +#ifdef esp32 + dev_spi->beginTransaction(SPISettings(spi_speed, SPI_MSBFIRST, SPI_MODE3)); +#else dev_spi->beginTransaction(SPISettings(spi_speed, MSBFIRST, SPI_MODE3)); - +#endif digitalWrite(cs_pin, LOW); /* Write Reg Address */ @@ -322,10 +343,16 @@ class LSM6DSV16X { } if (dev_i2c) { +#ifdef I2C_LIB_DEBUG + printf("\n\n[LSM LIB] Write register: 0x%02x, Byte Count: %d bytes", RegisterAddr, NumByteToWrite); +#endif dev_i2c->beginTransmission(((uint8_t)(((address) >> 1) & 0x7F))); dev_i2c->write(RegisterAddr); for (uint16_t i = 0 ; i < NumByteToWrite ; i++) { +#ifdef I2C_LIB_DEBUG + printf("\n[LSM LIB] Register Wrote: 0x%02x, Data: 0x%02x", RegisterAddr + i, pBuffer[i]); +#endif dev_i2c->write(pBuffer[i]); } diff --git a/src/sensors/lsm6dsv16xsensor.cpp b/src/sensors/lsm6dsv16xsensor.cpp index 97a90d3c5..3e3a6604f 100644 --- a/src/sensors/lsm6dsv16xsensor.cpp +++ b/src/sensors/lsm6dsv16xsensor.cpp @@ -35,9 +35,6 @@ volatile bool imuEvent = false; void IRAM_ATTR interruptHandler() { imuEvent = true; } void LSM6DSV16XSensor::motionSetup() { -#ifdef DEBUG_SENSOR - // TODO: Should anything go here -#endif errorCounter = 0; // Either subtract to the error counter or handle the error if (imu.begin() == LSM6DSV16X_ERROR) { m_Logger.fatal( @@ -48,6 +45,7 @@ void LSM6DSV16XSensor::motionSetup() { ledManager.pattern(50, 50, 200); return; } + uint8_t deviceId = 0; if (imu.ReadID(&deviceId) == LSM6DSV16X_ERROR) { m_Logger.fatal( @@ -69,49 +67,65 @@ void LSM6DSV16XSensor::motionSetup() { return; } - m_Logger.info("Connected to %s on 0x%02x. ", getIMUNameByType(sensorType), addr); + + //printf("\n\n\n%s Self Test started on 0x%02x.(Disabled)", getIMUNameByType(sensorType), addr); + //Test + //if (imu.Test_IMU(LSM6DSV16X_XL_ST_NEGATIVE, LSM6DSV16X_GY_ST_NEGATIVE) == LSM6DSV16X_ERROR) { + // m_Logger.fatal( + // "The IMU returned an error during the self test" + // ); + //ledManager.pattern(50, 50, 200); + //return; + //} + printf("\nConnected to %s on 0x%02x. IMU test passed ", getIMUNameByType(sensorType), addr); uint8_t status = 0; - //Test - if (imu.Test_IMU(LSM6DSV16X_XL_ST_NEGATIVE, LSM6DSV16X_GY_ST_NEGATIVE) == LSM6DSV16X_ERROR) { - m_Logger.fatal( - "The IMU returned an error during the self test" - ); - ledManager.pattern(50, 50, 200); - return; - } + // Restore defaults - // status |= imu.Reset_Set(LSM6DSV16X_RESET_CTRL_REGS); + //status |= imu.Reset_Set(LSM6DSV16X_RESET_CTRL_REGS); // Enable Block Data Update status |= imu.Enable_Block_Data_Update(); + status |= imu.Set_Auto_Increment(true); // Set maximums status |= imu.Set_X_FS(LSM6DSV16X_ACCEL_MAX); status |= imu.Set_G_FS(LSM6DSV16X_GYRO_MAX); - // Set FIFO size - status |= imu.FIFO_Set_Watermark_Level(LSM6DSV16X_FIFO_MAX_SIZE); + // Set data rate + status |= imu.Set_X_ODR(10); + status |= imu.Set_G_ODR(10); + status |= imu.Set_SFLP_ODR(LSM6DSV16X_FIFO_DATA_RATE); - // Set FIFO SFLP Batch - // NOTE: might not need all of this - status |= imu.FIFO_Set_SFLP_Batch(true, true, true); + status |= imu.FIFO_Set_X_BDR(10); + status |= imu.FIFO_Set_G_BDR(10); + + status |= imu.Set_T_ODR(1.875); + + //Enable IMU + status |= imu.Enable_X(); + status |= imu.Enable_G(); + + // Set FIFO size + //status |= imu.FIFO_Set_Watermark_Level(LSM6DSV16X_FIFO_MAX_SIZE); // Set FIFO mode to "continuous", so old data gets thrown away status |= imu.FIFO_Set_Mode(LSM6DSV16X_STREAM_MODE); - // Set data rate - status |= imu.Set_X_ODR(LSM6DSV16X_FIFO_DATA_RATE); - status |= imu.Set_G_ODR(LSM6DSV16X_FIFO_DATA_RATE); - status |= imu.Set_SFLP_ODR(LSM6DSV16X_FIFO_DATA_RATE); - + // Set FIFO SFLP Batch + // NOTE: might not need all of this + status |= imu.FIFO_Set_SFLP_Batch(true, true, true); // Enable Game Rotation Fusion status |= imu.Enable_Game_Rotation(); + + + + // Set GBias - status |= imu.Set_G_Bias(0, 0, 0); + //status |= imu.Set_G_Bias(0, 0, 0); #ifndef INTERRUPTFREE attachInterrupt(m_IntPin, interruptHandler, RISING); @@ -138,6 +152,7 @@ void LSM6DSV16XSensor::motionSetup() { } void LSM6DSV16XSensor::motionLoop() { + delay(250); lsm6dsv16x_fifo_status_t fifo_status; if (imu.FIFO_Get_Status(&fifo_status) != LSM6DSV16X_OK) { m_Logger.error( @@ -148,14 +163,15 @@ void LSM6DSV16XSensor::motionLoop() { errorCounter++; return; } + - m_Logger.info("FIFO status: %d", fifo_status.fifo_level); + //m_Logger.info("FIFO status: %d", fifo_status.fifo_level); - if (fifo_status.fifo_th != 1) { + if (fifo_status.fifo_level < 1) { return; } - uint16_t fifo_samples = fifo_status.fifo_level; + uint16_t fifo_samples = 0; if (imu.FIFO_Get_Num_Samples(&fifo_samples) == LSM6DSV16X_ERROR) { m_Logger.error( "Error getting number of samples in FIFO on %s at address 0x%02x", @@ -166,12 +182,18 @@ void LSM6DSV16XSensor::motionLoop() { return; } - if (fifo_samples != 0) { + if (fifo_samples > 0) { m_Logger.info("Got %d samples", fifo_samples); } - + + uint16_t tagx1 = 0; + uint16_t tagx2 = 0; + uint16_t tagx3 = 0; + uint16_t tagx13 = 0; for (uint16_t i = 0; i < fifo_samples; i++) { uint8_t tag; + + //printf("\n\n\nimu.FIFO_Get_Tag(&tag)"); if (imu.FIFO_Get_Tag(&tag) != LSM6DSV16X_OK) { m_Logger.error( "Failed to get FIFO data tag on %s at address 0x%02x", @@ -181,12 +203,56 @@ void LSM6DSV16XSensor::motionLoop() { continue; } - if (tag == 1 || tag == 2) { + uint8_t data[6]; + //printf("\n\n\nimu.FIFO_Get_Data(data)"); + imu.FIFO_Get_Data(data); + if (tag == 1) { //gyro + tagx1++; + continue; + } + if (tag == 2) { //accel + tagx2++; + continue; + } + if (tag == 3) { //temp + tagx3++; + continue; + } + + if (tag == 0x13) { //SFLP game rotation vector + tagx13++; + fusedRotation.x = Conversions::convertBytesToFloat(data[0], data[1]); + fusedRotation.y = Conversions::convertBytesToFloat(data[2], data[3]); + fusedRotation.z = Conversions::convertBytesToFloat(data[4], data[5]); + + fusedRotation.w = sqrtf(1.0F - sq(fusedRotation.x) - sq(fusedRotation.y) - sq(fusedRotation.z)); + + lastReset = 0; + lastData = millis(); + + if (ENABLE_INSPECTION || !OPTIMIZE_UPDATES || !lastFusedRotationSent.equalsWithEpsilon(fusedRotation)) + { + newFusedRotation = true; + lastFusedRotationSent = fusedRotation; + } + continue; + } + + if (tag == 0x16) { //SFLP gyroscope bias + continue; + } + + if (tag == 0x17) { //SFLP gravity vector + continue; + } + + if (tag == 0x19) { //sensor hub nack continue; } - m_Logger.info("Got tag %d", tag); + m_Logger.info("Got tag 0x%02x at position %d", tag, i); } + m_Logger.info("Got %d tag 1, %d tag 2, %d tag 3, %d tag 0x13", tagx1, tagx2, tagx3, tagx13); } SensorStatus LSM6DSV16XSensor::getSensorState() { diff --git a/src/sensors/lsm6dsv16xsensor.h b/src/sensors/lsm6dsv16xsensor.h index 8f572f582..fdfc98a1d 100644 --- a/src/sensors/lsm6dsv16xsensor.h +++ b/src/sensors/lsm6dsv16xsensor.h @@ -24,7 +24,7 @@ #ifndef SENSORS_LSM6DSV16X_H #define SENSORS_LSM6DSV16X_H -#include +#include "LSM6DSV16X.h" #include "sensor.h" From 8cfc0a3c5410a7bc230308c8e6021d1932c96b36 Mon Sep 17 00:00:00 2001 From: Gorbit99 Date: Fri, 4 Aug 2023 03:24:44 +0200 Subject: [PATCH 15/60] Code cleanup --- lib/LSM6DSV16X/LSM6DSV16X.cpp | 54 +++++++++------ src/sensors/lsm6dsv16xsensor.cpp | 113 ++++++++++++++----------------- src/sensors/lsm6dsv16xsensor.h | 9 ++- 3 files changed, 91 insertions(+), 85 deletions(-) diff --git a/lib/LSM6DSV16X/LSM6DSV16X.cpp b/lib/LSM6DSV16X/LSM6DSV16X.cpp index 3eb1fdf94..2b4d8528e 100644 --- a/lib/LSM6DSV16X/LSM6DSV16X.cpp +++ b/lib/LSM6DSV16X/LSM6DSV16X.cpp @@ -1022,21 +1022,29 @@ LSM6DSV16XStatusTypeDef LSM6DSV16X::Test_G_IMU(uint8_t TestType = LSM6DSV16X_GY_ LSM6DSV16XStatusTypeDef LSM6DSV16X::Get_T_ODR(float *Odr) { - LSM6DSV16XStatusTypeDef ret; lsm6dsv16x_fifo_ctrl4_t ctrl4; if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_FIFO_CTRL4, (uint8_t *)&ctrl4, 1) != LSM6DSV16X_OK) { return LSM6DSV16X_ERROR; } - if (ctrl4.odr_t_batch == 0x00) - *Odr = 0.0F; - if (ctrl4.odr_t_batch <= 0x01) - *Odr = 1.875F; - if (ctrl4.odr_t_batch <= 0x02) - *Odr = 15.0F; - else - *Odr = 60.0F; + switch (ctrl4.odr_t_batch) + { + case LSM6DSV16X_TEMP_NOT_BATCHED: + *Odr = 0; + break; + case LSM6DSV16X_TEMP_BATCHED_AT_1Hz875: + *Odr = 1.875f; + break; + case LSM6DSV16X_TEMP_BATCHED_AT_15Hz: + *Odr = 15; + break; + case LSM6DSV16X_TEMP_BATCHED_AT_60Hz: + *Odr = 60; + break; + default: + break; + } return LSM6DSV16X_OK; } @@ -1044,26 +1052,28 @@ LSM6DSV16XStatusTypeDef LSM6DSV16X::Get_T_ODR(float *Odr) { LSM6DSV16XStatusTypeDef LSM6DSV16X::Set_T_ODR(float Odr) { - LSM6DSV16XStatusTypeDef ret; lsm6dsv16x_fifo_ctrl4_t ctrl4; if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_FIFO_CTRL4, (uint8_t *)&ctrl4, 1) != LSM6DSV16X_OK) { return LSM6DSV16X_ERROR; } - if (Odr == 0.0F) - ctrl4.odr_t_batch = 0x00; - if (Odr <= 1.875F) - ctrl4.odr_t_batch = 0x01; - if (Odr <= 15.0F) - ctrl4.odr_t_batch = 0x02; - else - ctrl4.odr_t_batch = 0x03; - - if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_FIFO_CTRL4, (uint8_t *)&ctrl4, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; + if (Odr == 0.0F) { + ctrl4.odr_t_batch = LSM6DSV16X_TEMP_NOT_BATCHED; + } else if (Odr <= 1.875F) { + ctrl4.odr_t_batch = LSM6DSV16X_TEMP_BATCHED_AT_1Hz875; + } else if (Odr <= 15.0F) { + ctrl4.odr_t_batch = LSM6DSV16X_TEMP_BATCHED_AT_15Hz; + } else { + ctrl4.odr_t_batch = LSM6DSV16X_TEMP_BATCHED_AT_60Hz; } - return LSM6DSV16X_OK; + + return (LSM6DSV16XStatusTypeDef)lsm6dsv16x_write_reg( + ®_ctx, + LSM6DSV16X_FIFO_CTRL4, + (uint8_t *)&ctrl4, + 1 + ); } /** diff --git a/src/sensors/lsm6dsv16xsensor.cpp b/src/sensors/lsm6dsv16xsensor.cpp index 3e3a6604f..2193d16df 100644 --- a/src/sensors/lsm6dsv16xsensor.cpp +++ b/src/sensors/lsm6dsv16xsensor.cpp @@ -25,6 +25,7 @@ #include "GlobalVars.h" #include "customConversions.h" +#include "lsm6dsv16xsensor.h" #include "utils.h" #define INTERRUPTFREE // TODO: change based on int pin number (255 = interruptFree) @@ -67,25 +68,23 @@ void LSM6DSV16XSensor::motionSetup() { return; } - - //printf("\n\n\n%s Self Test started on 0x%02x.(Disabled)", getIMUNameByType(sensorType), addr); - //Test - //if (imu.Test_IMU(LSM6DSV16X_XL_ST_NEGATIVE, LSM6DSV16X_GY_ST_NEGATIVE) == LSM6DSV16X_ERROR) { - // m_Logger.fatal( - // "The IMU returned an error during the self test" + // printf("\n\n\n%s Self Test started on 0x%02x.(Disabled)", + // getIMUNameByType(sensorType), addr); Test if + // (imu.Test_IMU(LSM6DSV16X_XL_ST_NEGATIVE, LSM6DSV16X_GY_ST_NEGATIVE) == + // LSM6DSV16X_ERROR) { m_Logger.fatal( "The IMU returned an error during + // the self test" // ); - //ledManager.pattern(50, 50, 200); - //return; + // ledManager.pattern(50, 50, 200); + // return; //} - printf("\nConnected to %s on 0x%02x. IMU test passed ", getIMUNameByType(sensorType), addr); + printf( + "\nConnected to %s on 0x%02x. IMU test passed ", + getIMUNameByType(sensorType), + addr + ); uint8_t status = 0; - - - // Restore defaults - //status |= imu.Reset_Set(LSM6DSV16X_RESET_CTRL_REGS); - // Enable Block Data Update status |= imu.Enable_Block_Data_Update(); status |= imu.Set_Auto_Increment(true); @@ -95,37 +94,32 @@ void LSM6DSV16XSensor::motionSetup() { status |= imu.Set_G_FS(LSM6DSV16X_GYRO_MAX); // Set data rate - status |= imu.Set_X_ODR(10); - status |= imu.Set_G_ODR(10); + status |= imu.Set_X_ODR(LSM6DSV16X_FIFO_DATA_RATE); + status |= imu.Set_G_ODR(LSM6DSV16X_FIFO_DATA_RATE); status |= imu.Set_SFLP_ODR(LSM6DSV16X_FIFO_DATA_RATE); - status |= imu.FIFO_Set_X_BDR(10); - status |= imu.FIFO_Set_G_BDR(10); + status |= imu.FIFO_Set_X_BDR(LSM6DSV16X_FIFO_DATA_RATE); - status |= imu.Set_T_ODR(1.875); + status |= imu.Set_T_ODR(LSM6DSV16X_FIFO_TEMP_DATA_RATE); - //Enable IMU + // Enable IMU status |= imu.Enable_X(); status |= imu.Enable_G(); // Set FIFO size - //status |= imu.FIFO_Set_Watermark_Level(LSM6DSV16X_FIFO_MAX_SIZE); + status |= imu.FIFO_Set_Watermark_Level(LSM6DSV16X_FIFO_MAX_SIZE); // Set FIFO mode to "continuous", so old data gets thrown away status |= imu.FIFO_Set_Mode(LSM6DSV16X_STREAM_MODE); // Set FIFO SFLP Batch // NOTE: might not need all of this - status |= imu.FIFO_Set_SFLP_Batch(true, true, true); + status |= imu.FIFO_Set_SFLP_Batch(true, false, false); // Enable Game Rotation Fusion status |= imu.Enable_Game_Rotation(); - - - - // Set GBias - //status |= imu.Set_G_Bias(0, 0, 0); + // status |= imu.Set_G_Bias(0, 0, 0); #ifndef INTERRUPTFREE attachInterrupt(m_IntPin, interruptHandler, RISING); @@ -152,7 +146,6 @@ void LSM6DSV16XSensor::motionSetup() { } void LSM6DSV16XSensor::motionLoop() { - delay(250); lsm6dsv16x_fifo_status_t fifo_status; if (imu.FIFO_Get_Status(&fifo_status) != LSM6DSV16X_OK) { m_Logger.error( @@ -163,9 +156,8 @@ void LSM6DSV16XSensor::motionLoop() { errorCounter++; return; } - - //m_Logger.info("FIFO status: %d", fifo_status.fifo_level); + // m_Logger.info("FIFO status: %d", fifo_status.fifo_level); if (fifo_status.fifo_level < 1) { return; @@ -182,18 +174,9 @@ void LSM6DSV16XSensor::motionLoop() { return; } - if (fifo_samples > 0) { - m_Logger.info("Got %d samples", fifo_samples); - } - - uint16_t tagx1 = 0; - uint16_t tagx2 = 0; - uint16_t tagx3 = 0; - uint16_t tagx13 = 0; for (uint16_t i = 0; i < fifo_samples; i++) { uint8_t tag; - - //printf("\n\n\nimu.FIFO_Get_Tag(&tag)"); + if (imu.FIFO_Get_Tag(&tag) != LSM6DSV16X_OK) { m_Logger.error( "Failed to get FIFO data tag on %s at address 0x%02x", @@ -204,55 +187,48 @@ void LSM6DSV16XSensor::motionLoop() { } uint8_t data[6]; - //printf("\n\n\nimu.FIFO_Get_Data(data)"); + // printf("\n\n\nimu.FIFO_Get_Data(data)"); imu.FIFO_Get_Data(data); - if (tag == 1) { //gyro - tagx1++; + if (tag == 1) { // gyro continue; } - if (tag == 2) { //accel - tagx2++; + if (tag == 2) { // accel continue; } - if (tag == 3) { //temp - tagx3++; + if (tag == 3) { // temp continue; } - if (tag == 0x13) { //SFLP game rotation vector - tagx13++; - fusedRotation.x = Conversions::convertBytesToFloat(data[0], data[1]); - fusedRotation.y = Conversions::convertBytesToFloat(data[2], data[3]); - fusedRotation.z = Conversions::convertBytesToFloat(data[4], data[5]); + if (tag == 0x13) { // SFLP game rotation vector + float x = Conversions::convertBytesToFloat(data[0], data[1]); + float y = Conversions::convertBytesToFloat(data[2], data[3]); + float z = Conversions::convertBytesToFloat(data[4], data[5]); - fusedRotation.w = sqrtf(1.0F - sq(fusedRotation.x) - sq(fusedRotation.y) - sq(fusedRotation.z)); + fusedRotation = fusedRotationToQuaternion(x, y, z); lastReset = 0; lastData = millis(); - if (ENABLE_INSPECTION || !OPTIMIZE_UPDATES || !lastFusedRotationSent.equalsWithEpsilon(fusedRotation)) - { + if (ENABLE_INSPECTION || !OPTIMIZE_UPDATES + || !lastFusedRotationSent.equalsWithEpsilon(fusedRotation)) { newFusedRotation = true; lastFusedRotationSent = fusedRotation; } continue; } - if (tag == 0x16) { //SFLP gyroscope bias + if (tag == 0x16) { // SFLP gyroscope bias continue; } - if (tag == 0x17) { //SFLP gravity vector + if (tag == 0x17) { // SFLP gravity vector continue; } - if (tag == 0x19) { //sensor hub nack + if (tag == 0x19) { // sensor hub nack continue; } - - m_Logger.info("Got tag 0x%02x at position %d", tag, i); } - m_Logger.info("Got %d tag 1, %d tag 2, %d tag 3, %d tag 0x13", tagx1, tagx2, tagx3, tagx13); } SensorStatus LSM6DSV16XSensor::getSensorState() { @@ -262,6 +238,21 @@ SensorStatus LSM6DSV16XSensor::getSensorState() { : SensorStatus::SENSOR_OFFLINE; } +Quat LSM6DSV16XSensor::fusedRotationToQuaternion(float x, float y, float z) { + float length2 = x * x + y * y + z * z; + + if (length2 > 1) { + float length = sqrt(length2); + x /= length; + y /= length; + z /= length; + length2 = 1; + } + + float w = sqrt(1 - length2); + return Quat(x, y, z, w); +} + void LSM6DSV16XSensor::sendData() { if (newFusedRotation) { newFusedRotation = false; diff --git a/src/sensors/lsm6dsv16xsensor.h b/src/sensors/lsm6dsv16xsensor.h index fdfc98a1d..5261a8914 100644 --- a/src/sensors/lsm6dsv16xsensor.h +++ b/src/sensors/lsm6dsv16xsensor.h @@ -25,7 +25,6 @@ #define SENSORS_LSM6DSV16X_H #include "LSM6DSV16X.h" - #include "sensor.h" #ifndef LSM6DSV16X_ACCEL_MAX @@ -41,7 +40,11 @@ #endif #ifndef LSM6DSV16X_FIFO_DATA_RATE -#define LSM6DSV16X_FIFO_DATA_RATE 30 +#define LSM6DSV16X_FIFO_DATA_RATE 120 +#endif + +#ifndef LSM6DSV16X_FIFO_TEMP_DATA_RATE +#define LSM6DSV16X_FIFO_TEMP_DATA_RATE 1.875f #endif class LSM6DSV16XSensor : public Sensor { @@ -71,6 +74,8 @@ class LSM6DSV16XSensor : public Sensor { // void interruptHandler(); // volatile bool imuEvent; //the interrupt cant be a class function + Quat fusedRotationToQuaternion(float x, float y, float z); + LSM6DSV16X imu; uint8_t m_IntPin; uint8_t errorCounter = 0; // Error is -1, OK is 0 From 93033f420d77230a4b00d1b1b7b3b75783997dc0 Mon Sep 17 00:00:00 2001 From: Carter Meyers Date: Thu, 3 Aug 2023 20:03:27 -0700 Subject: [PATCH 16/60] Cleanup and add timeout error back --- lib/LSM6DSV16X/LSM6DSV16X.cpp | 17 +++++ lib/LSM6DSV16X/LSM6DSV16X.h | 19 +++-- src/sensors/SensorManager.cpp | 3 +- src/sensors/lsm6dsv16xsensor.cpp | 124 +++++++++++++++---------------- src/sensors/lsm6dsv16xsensor.h | 22 ++++-- 5 files changed, 102 insertions(+), 83 deletions(-) diff --git a/lib/LSM6DSV16X/LSM6DSV16X.cpp b/lib/LSM6DSV16X/LSM6DSV16X.cpp index 2b4d8528e..661d7822a 100644 --- a/lib/LSM6DSV16X/LSM6DSV16X.cpp +++ b/lib/LSM6DSV16X/LSM6DSV16X.cpp @@ -132,6 +132,23 @@ LSM6DSV16XStatusTypeDef LSM6DSV16X::begin() return LSM6DSV16X_OK; } +/** + * @brief Initialize the LSM6DSV16X sensor when it has already been configured + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSV16XStatusTypeDef LSM6DSV16X::beginPreconfigured() +{ + if (dev_spi) { + // Configure CS pin + pinMode(cs_pin, OUTPUT); + digitalWrite(cs_pin, HIGH); + } + + initialized = 1U; + + return LSM6DSV16X_OK; +} + /** * @brief Deinitialize the LSM6DSV16X sensor * @retval 0 in case of success, an error code otherwise diff --git a/lib/LSM6DSV16X/LSM6DSV16X.h b/lib/LSM6DSV16X/LSM6DSV16X.h index 7bda015b8..0d1203fc6 100644 --- a/lib/LSM6DSV16X/LSM6DSV16X.h +++ b/lib/LSM6DSV16X/LSM6DSV16X.h @@ -72,7 +72,7 @@ -//#define esp32 +//#define ENABLE_SPI //#define I2C_LIB_DEBUG /* Typedefs ------------------------------------------------------------------*/ @@ -142,6 +142,7 @@ class LSM6DSV16X { LSM6DSV16X(SPIClass *spi, int cs_pin, uint32_t spi_speed = 2000000); LSM6DSV16XStatusTypeDef begin(); + LSM6DSV16XStatusTypeDef beginPreconfigured(); LSM6DSV16XStatusTypeDef end(); LSM6DSV16XStatusTypeDef ReadID(uint8_t *Id); @@ -264,12 +265,13 @@ class LSM6DSV16X { */ uint8_t IO_Read(uint8_t *pBuffer, uint8_t RegisterAddr, uint16_t NumByteToRead) { +#ifdef ENABLE_SPI if (dev_spi) { -#ifdef esp32 +//#ifdef esp32 dev_spi->beginTransaction(SPISettings(spi_speed, SPI_MSBFIRST, SPI_MODE3)); -#else +//#else dev_spi->beginTransaction(SPISettings(spi_speed, MSBFIRST, SPI_MODE3)); -#endif +//#endif digitalWrite(cs_pin, LOW); /* Write Reg Address */ @@ -285,6 +287,7 @@ class LSM6DSV16X { return 0; } +#endif if (dev_i2c) { #ifdef I2C_LIB_DEBUG @@ -320,12 +323,13 @@ class LSM6DSV16X { */ uint8_t IO_Write(const uint8_t *pBuffer, uint8_t RegisterAddr, uint16_t NumByteToWrite) { +#ifdef ENABLE_SPI if (dev_spi) { -#ifdef esp32 +//#ifdef esp32 dev_spi->beginTransaction(SPISettings(spi_speed, SPI_MSBFIRST, SPI_MODE3)); -#else +//#else dev_spi->beginTransaction(SPISettings(spi_speed, MSBFIRST, SPI_MODE3)); -#endif +//#endif digitalWrite(cs_pin, LOW); /* Write Reg Address */ @@ -341,6 +345,7 @@ class LSM6DSV16X { return 0; } +#endif if (dev_i2c) { #ifdef I2C_LIB_DEBUG diff --git a/src/sensors/SensorManager.cpp b/src/sensors/SensorManager.cpp index ebd430b9c..497fbafae 100644 --- a/src/sensors/SensorManager.cpp +++ b/src/sensors/SensorManager.cpp @@ -101,8 +101,7 @@ namespace SlimeVR case IMU_LSM6DSV16X: { uint8_t intPin = extraParam; - //We shift the address left 1 to work with the library - sensor = new LSM6DSV16XSensor(sensorID, imuType, address << 1, rotation, sclPin, sdaPin, intPin); + sensor = new LSM6DSV16XSensor(sensorID, imuType, address, rotation, sclPin, sdaPin, intPin); } break; default: diff --git a/src/sensors/lsm6dsv16xsensor.cpp b/src/sensors/lsm6dsv16xsensor.cpp index 2193d16df..8e4cba2b9 100644 --- a/src/sensors/lsm6dsv16xsensor.cpp +++ b/src/sensors/lsm6dsv16xsensor.cpp @@ -28,25 +28,7 @@ #include "lsm6dsv16xsensor.h" #include "utils.h" -#define INTERRUPTFREE // TODO: change based on int pin number (255 = interruptFree) -#define DEBUG_SENSOR - -volatile bool imuEvent = false; - -void IRAM_ATTR interruptHandler() { imuEvent = true; } - void LSM6DSV16XSensor::motionSetup() { - errorCounter = 0; // Either subtract to the error counter or handle the error - if (imu.begin() == LSM6DSV16X_ERROR) { - m_Logger.fatal( - "Can't connect to %s at address 0x%02x", - getIMUNameByType(sensorType), - addr - ); - ledManager.pattern(50, 50, 200); - return; - } - uint8_t deviceId = 0; if (imu.ReadID(&deviceId) == LSM6DSV16X_ERROR) { m_Logger.fatal( @@ -68,23 +50,29 @@ void LSM6DSV16XSensor::motionSetup() { return; } - // printf("\n\n\n%s Self Test started on 0x%02x.(Disabled)", - // getIMUNameByType(sensorType), addr); Test if - // (imu.Test_IMU(LSM6DSV16X_XL_ST_NEGATIVE, LSM6DSV16X_GY_ST_NEGATIVE) == - // LSM6DSV16X_ERROR) { m_Logger.fatal( "The IMU returned an error during - // the self test" - // ); - // ledManager.pattern(50, 50, 200); - // return; - //} - printf( - "\nConnected to %s on 0x%02x. IMU test passed ", +#ifdef SELF_TEST_ON_INIT + m_Logger.info( + "%s Self Test started on 0x%02x.(Disabled)", getIMUNameByType(sensorType), addr ); + if (imu.Test_IMU(LSM6DSV16X_XL_ST_NEGATIVE, LSM6DSV16X_GY_ST_NEGATIVE) + == LSM6DSV16X_ERROR) { + m_Logger.fatal("The IMU returned an error during the self test"); + ledManager.pattern(50, 50, 200); + return; + } + m_Logger.info("Self Test Passed"); +#endif + + m_Logger.info("Connected to %s on 0x%02x", getIMUNameByType(sensorType), addr); + uint8_t status = 0; + // Restore defaults + status |= imu.Reset_Set(LSM6DSV16X_RESET_CTRL_REGS); + // Enable Block Data Update status |= imu.Enable_Block_Data_Update(); status |= imu.Set_Auto_Increment(true); @@ -106,22 +94,18 @@ void LSM6DSV16XSensor::motionSetup() { status |= imu.Enable_X(); status |= imu.Enable_G(); - // Set FIFO size - status |= imu.FIFO_Set_Watermark_Level(LSM6DSV16X_FIFO_MAX_SIZE); - // Set FIFO mode to "continuous", so old data gets thrown away status |= imu.FIFO_Set_Mode(LSM6DSV16X_STREAM_MODE); - // Set FIFO SFLP Batch - // NOTE: might not need all of this - status |= imu.FIFO_Set_SFLP_Batch(true, false, false); // Enable Game Rotation Fusion status |= imu.Enable_Game_Rotation(); + status |= imu.begin(); + // Set GBias // status |= imu.Set_G_Bias(0, 0, 0); -#ifndef INTERRUPTFREE +#ifdef INTERRUPT_FOR_SLEEP attachInterrupt(m_IntPin, interruptHandler, RISING); errorCounter -= imu.Enable_Single_Tap_Detection(LSM6DSV16X_INT1_PIN); @@ -130,8 +114,7 @@ void LSM6DSV16XSensor::motionSetup() { if (status != LSM6DSV16X_OK) { m_Logger.fatal( - "%d Error(s) occured enabling imu features on %s at address 0x%02x", - errorCounter, + "Errors occured enabling imu features on %s at address 0x%02x", getIMUNameByType(sensorType), addr ); @@ -146,21 +129,29 @@ void LSM6DSV16XSensor::motionSetup() { } void LSM6DSV16XSensor::motionLoop() { - lsm6dsv16x_fifo_status_t fifo_status; - if (imu.FIFO_Get_Status(&fifo_status) != LSM6DSV16X_OK) { + if (lastData + 1000 < millis() && configured) { // Errors m_Logger.error( - "Error getting FIFO status on %s at address 0x%02x", + "The %s at address 0x%02x, has not responded in the last second", getIMUNameByType(sensorType), addr ); - errorCounter++; - return; - } - - // m_Logger.info("FIFO status: %d", fifo_status.fifo_level); + //statusManager.setStatus(SlimeVR::Status::IMU_ERROR, true); + //working = false; + lastData = millis(); // reset time counter for error message + +#ifdef REINIT_ON_FAILURE + if (reinitOnFailAttempts < REINIT_RETRY_MAX_ATTEMPTS) { + motionSetup(); + } else { + m_Logger.error( + "The %s at address 0x%02x, could not be revived", + getIMUNameByType(sensorType), + addr + ); + } - if (fifo_status.fifo_level < 1) { - return; + reinitOnFailAttempts++; // buf overflow will make it try again in about 4 min +#endif } uint16_t fifo_samples = 0; @@ -170,7 +161,10 @@ void LSM6DSV16XSensor::motionLoop() { getIMUNameByType(sensorType), addr ); - errorCounter++; + return; + } + + if (fifo_samples < 1) { return; } @@ -187,12 +181,25 @@ void LSM6DSV16XSensor::motionLoop() { } uint8_t data[6]; - // printf("\n\n\nimu.FIFO_Get_Data(data)"); imu.FIFO_Get_Data(data); if (tag == 1) { // gyro continue; } if (tag == 2) { // accel +#if SEND_ACCELERATION + // int32_t accelerometerInt[3]; + // errorCounter -= imu.Get_X_Axes(accelerometerInt); + // acceleration[0] = accelerometerInt[0] * 0.01F; //convert from mg to g + // acceleration[1] = accelerometerInt[1] * 0.01F; + // acceleration[2] = accelerometerInt[2] * 0.01F; + acceleration[0] + = Conversions::convertBytesToFloat(data[0], data[1]) * sensitivity; + acceleration[1] + = Conversions::convertBytesToFloat(data[2], data[3]) * sensitivity; + acceleration[2] + = Conversions::convertBytesToFloat(data[4], data[5]) * sensitivity; + newAcceleration = true; +#endif // SEND_ACCELERATION continue; } if (tag == 3) { // temp @@ -216,26 +223,11 @@ void LSM6DSV16XSensor::motionLoop() { } continue; } - - if (tag == 0x16) { // SFLP gyroscope bias - continue; - } - - if (tag == 0x17) { // SFLP gravity vector - continue; - } - - if (tag == 0x19) { // sensor hub nack - continue; - } } } SensorStatus LSM6DSV16XSensor::getSensorState() { - // TODO: this may need to be redone, errorCounter gets reset at the end of the loop - return errorCounter > 0 ? SensorStatus::SENSOR_ERROR - : isWorking() ? SensorStatus::SENSOR_OK - : SensorStatus::SENSOR_OFFLINE; + return isWorking() ? SensorStatus::SENSOR_OK : SensorStatus::SENSOR_OFFLINE; } Quat LSM6DSV16XSensor::fusedRotationToQuaternion(float x, float y, float z) { diff --git a/src/sensors/lsm6dsv16xsensor.h b/src/sensors/lsm6dsv16xsensor.h index 5261a8914..0e2f3c4db 100644 --- a/src/sensors/lsm6dsv16xsensor.h +++ b/src/sensors/lsm6dsv16xsensor.h @@ -47,6 +47,14 @@ #define LSM6DSV16X_FIFO_TEMP_DATA_RATE 1.875f #endif +//#define SELF_TEST_ON_INIT +//#define REINIT_ON_FAILURE + +#ifdef REINIT_ON_FAILURE + #define REINIT_RETRY_MAX_ATTEMPTS 5 + #undef SELF_TEST_ON_INIT +#endif + class LSM6DSV16XSensor : public Sensor { public: LSM6DSV16XSensor( @@ -59,30 +67,28 @@ class LSM6DSV16XSensor : public Sensor { uint8_t intPin ) : Sensor("LSM6DSV16XSensor", type, id, address, rotation, sclPin, sdaPin) - , imu(&Wire, addr) + , imu(&Wire, addr << 1) //We shift the address left 1 to work with the library , m_IntPin(intPin){}; ~LSM6DSV16XSensor(){}; void motionSetup() override final; - void postSetup() override { lastData = millis(); } - void motionLoop() override final; void sendData() override final; void startCalibration(int calibrationType) override final; SensorStatus getSensorState() override final; private: - // void interruptHandler(); - // volatile bool imuEvent; //the interrupt cant be a class function - Quat fusedRotationToQuaternion(float x, float y, float z); LSM6DSV16X imu; uint8_t m_IntPin; - uint8_t errorCounter = 0; // Error is -1, OK is 0 uint8_t tap = 0; unsigned long lastData = 0; - + float sensitivity = 0.0f; uint8_t lastReset = 0; + +#ifdef REINIT_ON_FAILURE + uint8_t reinitOnFailAttempts = 0; +#endif }; #endif From 79a0e540f9170e15a9c482f55e50b941727634c0 Mon Sep 17 00:00:00 2001 From: Gorbit99 Date: Fri, 4 Aug 2023 08:43:43 +0200 Subject: [PATCH 17/60] Code fixes --- src/sensors/lsm6dsv16xsensor.cpp | 104 ++++++++++++++++--------------- src/sensors/lsm6dsv16xsensor.h | 17 ++--- 2 files changed, 60 insertions(+), 61 deletions(-) diff --git a/src/sensors/lsm6dsv16xsensor.cpp b/src/sensors/lsm6dsv16xsensor.cpp index 8e4cba2b9..6e1f1d1d4 100644 --- a/src/sensors/lsm6dsv16xsensor.cpp +++ b/src/sensors/lsm6dsv16xsensor.cpp @@ -28,6 +28,19 @@ #include "lsm6dsv16xsensor.h" #include "utils.h" +LSM6DSV16XSensor::LSM6DSV16XSensor( + uint8_t id, + uint8_t type, + uint8_t address, + float rotation, + uint8_t sclPin, + uint8_t sdaPin, + uint8_t intPin +) + : Sensor("LSM6DSV16XSensor", type, id, address, rotation, sclPin, sdaPin) + , imu(&Wire, addr << 1) // We shift the address left 1 to work with the library + , m_IntPin(intPin){}; + void LSM6DSV16XSensor::motionSetup() { uint8_t deviceId = 0; if (imu.ReadID(&deviceId) == LSM6DSV16X_ERROR) { @@ -70,6 +83,8 @@ void LSM6DSV16XSensor::motionSetup() { uint8_t status = 0; + status |= imu.begin(); + // Restore defaults status |= imu.Reset_Set(LSM6DSV16X_RESET_CTRL_REGS); @@ -83,11 +98,8 @@ void LSM6DSV16XSensor::motionSetup() { // Set data rate status |= imu.Set_X_ODR(LSM6DSV16X_FIFO_DATA_RATE); - status |= imu.Set_G_ODR(LSM6DSV16X_FIFO_DATA_RATE); status |= imu.Set_SFLP_ODR(LSM6DSV16X_FIFO_DATA_RATE); - status |= imu.FIFO_Set_X_BDR(LSM6DSV16X_FIFO_DATA_RATE); - status |= imu.Set_T_ODR(LSM6DSV16X_FIFO_TEMP_DATA_RATE); // Enable IMU @@ -96,12 +108,11 @@ void LSM6DSV16XSensor::motionSetup() { // Set FIFO mode to "continuous", so old data gets thrown away status |= imu.FIFO_Set_Mode(LSM6DSV16X_STREAM_MODE); + status |= imu.FIFO_Set_SFLP_Batch(true, false, false); // Enable Game Rotation Fusion status |= imu.Enable_Game_Rotation(); - status |= imu.begin(); - // Set GBias // status |= imu.Set_G_Bias(0, 0, 0); @@ -135,8 +146,8 @@ void LSM6DSV16XSensor::motionLoop() { getIMUNameByType(sensorType), addr ); - //statusManager.setStatus(SlimeVR::Status::IMU_ERROR, true); - //working = false; + // statusManager.setStatus(SlimeVR::Status::IMU_ERROR, true); + // working = false; lastData = millis(); // reset time counter for error message #ifdef REINIT_ON_FAILURE @@ -164,13 +175,8 @@ void LSM6DSV16XSensor::motionLoop() { return; } - if (fifo_samples < 1) { - return; - } - for (uint16_t i = 0; i < fifo_samples; i++) { uint8_t tag; - if (imu.FIFO_Get_Tag(&tag) != LSM6DSV16X_OK) { m_Logger.error( "Failed to get FIFO data tag on %s at address 0x%02x", @@ -182,46 +188,46 @@ void LSM6DSV16XSensor::motionLoop() { uint8_t data[6]; imu.FIFO_Get_Data(data); - if (tag == 1) { // gyro - continue; - } - if (tag == 2) { // accel + switch (tag) { + case lsm6dsv16x_fifo_out_raw_t:: + LSM6DSV16X_XL_NC_TAG: // accel #if SEND_ACCELERATION - // int32_t accelerometerInt[3]; - // errorCounter -= imu.Get_X_Axes(accelerometerInt); - // acceleration[0] = accelerometerInt[0] * 0.01F; //convert from mg to g - // acceleration[1] = accelerometerInt[1] * 0.01F; - // acceleration[2] = accelerometerInt[2] * 0.01F; - acceleration[0] - = Conversions::convertBytesToFloat(data[0], data[1]) * sensitivity; - acceleration[1] - = Conversions::convertBytesToFloat(data[2], data[3]) * sensitivity; - acceleration[2] - = Conversions::convertBytesToFloat(data[4], data[5]) * sensitivity; - newAcceleration = true; + // int32_t accelerometerInt[3]; + // errorCounter -= imu.Get_X_Axes(accelerometerInt); + // acceleration[0] = accelerometerInt[0] * 0.01F; //convert from mg to g + // acceleration[1] = accelerometerInt[1] * 0.01F; + // acceleration[2] = accelerometerInt[2] * 0.01F; + acceleration[0] + = Conversions::convertBytesToFloat(data[0], data[1]) * sensitivity; + acceleration[1] + = Conversions::convertBytesToFloat(data[2], data[3]) * sensitivity; + acceleration[2] + = Conversions::convertBytesToFloat(data[4], data[5]) * sensitivity; + newAcceleration = true; #endif // SEND_ACCELERATION - continue; - } - if (tag == 3) { // temp - continue; - } - - if (tag == 0x13) { // SFLP game rotation vector - float x = Conversions::convertBytesToFloat(data[0], data[1]); - float y = Conversions::convertBytesToFloat(data[2], data[3]); - float z = Conversions::convertBytesToFloat(data[4], data[5]); - - fusedRotation = fusedRotationToQuaternion(x, y, z); - - lastReset = 0; - lastData = millis(); - - if (ENABLE_INSPECTION || !OPTIMIZE_UPDATES - || !lastFusedRotationSent.equalsWithEpsilon(fusedRotation)) { - newFusedRotation = true; - lastFusedRotationSent = fusedRotation; + break; + case lsm6dsv16x_fifo_out_raw_t::LSM6DSV16X_TEMPERATURE_TAG: // temp + // TODO: send temperature data to the server + break; + case lsm6dsv16x_fifo_out_raw_t:: + LSM6DSV16X_SFLP_GAME_ROTATION_VECTOR_TAG: { // SFLP game rotation + // vector + float x = Conversions::convertBytesToFloat(data[0], data[1]); + float y = Conversions::convertBytesToFloat(data[2], data[3]); + float z = Conversions::convertBytesToFloat(data[4], data[5]); + + fusedRotation = fusedRotationToQuaternion(x, y, z); + + lastReset = 0; + lastData = millis(); + + if (ENABLE_INSPECTION || !OPTIMIZE_UPDATES + || !lastFusedRotationSent.equalsWithEpsilon(fusedRotation)) { + newFusedRotation = true; + lastFusedRotationSent = fusedRotation; + } + break; } - continue; } } } diff --git a/src/sensors/lsm6dsv16xsensor.h b/src/sensors/lsm6dsv16xsensor.h index 0e2f3c4db..e7ad35fad 100644 --- a/src/sensors/lsm6dsv16xsensor.h +++ b/src/sensors/lsm6dsv16xsensor.h @@ -35,10 +35,6 @@ #define LSM6DSV16X_GYRO_MAX 2000 #endif -#ifndef LSM6DSV16X_FIFO_MAX_SIZE -#define LSM6DSV16X_FIFO_MAX_SIZE 32 -#endif - #ifndef LSM6DSV16X_FIFO_DATA_RATE #define LSM6DSV16X_FIFO_DATA_RATE 120 #endif @@ -47,12 +43,12 @@ #define LSM6DSV16X_FIFO_TEMP_DATA_RATE 1.875f #endif -//#define SELF_TEST_ON_INIT -//#define REINIT_ON_FAILURE +// #define SELF_TEST_ON_INIT +// #define REINIT_ON_FAILURE #ifdef REINIT_ON_FAILURE - #define REINIT_RETRY_MAX_ATTEMPTS 5 - #undef SELF_TEST_ON_INIT +#define REINIT_RETRY_MAX_ATTEMPTS 5 +#undef SELF_TEST_ON_INIT #endif class LSM6DSV16XSensor : public Sensor { @@ -65,10 +61,7 @@ class LSM6DSV16XSensor : public Sensor { uint8_t sclPin, uint8_t sdaPin, uint8_t intPin - ) - : Sensor("LSM6DSV16XSensor", type, id, address, rotation, sclPin, sdaPin) - , imu(&Wire, addr << 1) //We shift the address left 1 to work with the library - , m_IntPin(intPin){}; + ); ~LSM6DSV16XSensor(){}; void motionSetup() override final; void motionLoop() override final; From 93ee7494bac825cfc2746b270e3fad9cdc4beff0 Mon Sep 17 00:00:00 2001 From: Gorbit99 Date: Fri, 4 Aug 2023 09:29:30 +0200 Subject: [PATCH 18/60] Added temp monitoring --- lib/LSM6DSV16X/LSM6DSV16X.cpp | 19 ++++++++++++++++ lib/LSM6DSV16X/LSM6DSV16X.h | 3 +++ src/sensors/lsm6dsv16xsensor.cpp | 37 ++++++++++++++++++++++++++++---- src/sensors/lsm6dsv16xsensor.h | 8 +++++++ 4 files changed, 63 insertions(+), 4 deletions(-) diff --git a/lib/LSM6DSV16X/LSM6DSV16X.cpp b/lib/LSM6DSV16X/LSM6DSV16X.cpp index 661d7822a..1e90c7923 100644 --- a/lib/LSM6DSV16X/LSM6DSV16X.cpp +++ b/lib/LSM6DSV16X/LSM6DSV16X.cpp @@ -3649,3 +3649,22 @@ LSM6DSV16XStatusTypeDef LSM6DSV16X::Set_Auto_Increment(bool enable) enable ? PROPERTY_ENABLE : PROPERTY_DISABLE ); } + +LSM6DSV16XStatusTypeDef LSM6DSV16X::Get_Temperature_Raw(int16_t * value) +{ + return (LSM6DSV16XStatusTypeDef)lsm6dsv16x_temperature_raw_get( + ®_ctx, + value + ); +} + +LSM6DSV16XStatusTypeDef LSM6DSV16X::Is_New_Temperature_Data(bool * available) +{ + lsm6dsv16x_status_reg_t status; + if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_STATUS_REG, (uint8_t *)&status, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + *available = status.tda; + return LSM6DSV16X_OK; +} diff --git a/lib/LSM6DSV16X/LSM6DSV16X.h b/lib/LSM6DSV16X/LSM6DSV16X.h index 0d1203fc6..ea828a2b4 100644 --- a/lib/LSM6DSV16X/LSM6DSV16X.h +++ b/lib/LSM6DSV16X/LSM6DSV16X.h @@ -256,6 +256,9 @@ class LSM6DSV16X { LSM6DSV16XStatusTypeDef Enable_Block_Data_Update(bool enable = true); LSM6DSV16XStatusTypeDef Set_Auto_Increment(bool enable); + LSM6DSV16XStatusTypeDef Get_Temperature_Raw(int16_t *value); + LSM6DSV16XStatusTypeDef Is_New_Temperature_Data(bool *available); + /** * @brief Utility function to read data. * @param pBuffer: pointer to data to be read. diff --git a/src/sensors/lsm6dsv16xsensor.cpp b/src/sensors/lsm6dsv16xsensor.cpp index 6e1f1d1d4..fc109ddcf 100644 --- a/src/sensors/lsm6dsv16xsensor.cpp +++ b/src/sensors/lsm6dsv16xsensor.cpp @@ -100,7 +100,6 @@ void LSM6DSV16XSensor::motionSetup() { status |= imu.Set_X_ODR(LSM6DSV16X_FIFO_DATA_RATE); status |= imu.Set_SFLP_ODR(LSM6DSV16X_FIFO_DATA_RATE); status |= imu.FIFO_Set_X_BDR(LSM6DSV16X_FIFO_DATA_RATE); - status |= imu.Set_T_ODR(LSM6DSV16X_FIFO_TEMP_DATA_RATE); // Enable IMU status |= imu.Enable_X(); @@ -137,9 +136,29 @@ void LSM6DSV16XSensor::motionSetup() { lastData = millis(); working = true; configured = true; + lastTempRead = millis(); } void LSM6DSV16XSensor::motionLoop() { + if (millis() - lastTempRead > LSM6DSV16X_TEMP_READ_INTERVAL * 1000) { + lastTempRead = millis(); + + int16_t rawTemp; + if (imu.Get_Temperature_Raw(&rawTemp) != LSM6DSV16X_OK) { + m_Logger.error( + "Error getting temperature on %s at address 0x%02x", + getIMUNameByType(sensorType), + addr + ); + } else { + float actualTemp = temperatureSensorToActualTemperature(rawTemp); + if (fabsf(actualTemp - temperature) > 0.01f) { + temperature = actualTemp; + newTemperature = true; + } + } + } + if (lastData + 1000 < millis() && configured) { // Errors m_Logger.error( "The %s at address 0x%02x, has not responded in the last second", @@ -206,9 +225,6 @@ void LSM6DSV16XSensor::motionLoop() { newAcceleration = true; #endif // SEND_ACCELERATION break; - case lsm6dsv16x_fifo_out_raw_t::LSM6DSV16X_TEMPERATURE_TAG: // temp - // TODO: send temperature data to the server - break; case lsm6dsv16x_fifo_out_raw_t:: LSM6DSV16X_SFLP_GAME_ROTATION_VECTOR_TAG: { // SFLP game rotation // vector @@ -251,6 +267,14 @@ Quat LSM6DSV16XSensor::fusedRotationToQuaternion(float x, float y, float z) { return Quat(x, y, z, w); } +constexpr float TEMPERATURE_SENSITIVITY = 256; +constexpr float TEMPERATURE_OFFSET = 25; + +float LSM6DSV16XSensor::temperatureSensorToActualTemperature(int16_t temperature) { + float delta = temperature / TEMPERATURE_SENSITIVITY; + return TEMPERATURE_OFFSET + delta; +} + void LSM6DSV16XSensor::sendData() { if (newFusedRotation) { newFusedRotation = false; @@ -279,6 +303,11 @@ void LSM6DSV16XSensor::sendData() { networkConnection.sendSensorTap(sensorId, tap); tap = 0; } + + if (newTemperature) { + newTemperature = false; + networkConnection.sendTemperature(sensorId, temperature); + } } void LSM6DSV16XSensor::startCalibration(int calibrationType) { diff --git a/src/sensors/lsm6dsv16xsensor.h b/src/sensors/lsm6dsv16xsensor.h index e7ad35fad..f14354d55 100644 --- a/src/sensors/lsm6dsv16xsensor.h +++ b/src/sensors/lsm6dsv16xsensor.h @@ -43,6 +43,10 @@ #define LSM6DSV16X_FIFO_TEMP_DATA_RATE 1.875f #endif +#ifndef LSM6DSV16X_TEMP_READ_INTERVAL +#define LSM6DSV16X_TEMP_READ_INTERVAL 1 +#endif + // #define SELF_TEST_ON_INIT // #define REINIT_ON_FAILURE @@ -71,6 +75,7 @@ class LSM6DSV16XSensor : public Sensor { private: Quat fusedRotationToQuaternion(float x, float y, float z); + float temperatureSensorToActualTemperature(int16_t temperature); LSM6DSV16X imu; uint8_t m_IntPin; @@ -78,6 +83,9 @@ class LSM6DSV16XSensor : public Sensor { unsigned long lastData = 0; float sensitivity = 0.0f; uint8_t lastReset = 0; + float temperature = 0; + bool newTemperature = false; + uint32_t lastTempRead = 0; #ifdef REINIT_ON_FAILURE uint8_t reinitOnFailAttempts = 0; From e30ae303457ba899dd4b17f0e6f55d498fb2f387 Mon Sep 17 00:00:00 2001 From: Gorbit99 Date: Fri, 4 Aug 2023 09:52:01 +0200 Subject: [PATCH 19/60] Partial acceleration implementation --- src/sensors/lsm6dsv16xsensor.cpp | 31 ++++++++++++++++++------------- src/sensors/lsm6dsv16xsensor.h | 2 +- 2 files changed, 19 insertions(+), 14 deletions(-) diff --git a/src/sensors/lsm6dsv16xsensor.cpp b/src/sensors/lsm6dsv16xsensor.cpp index fc109ddcf..d1f8ed331 100644 --- a/src/sensors/lsm6dsv16xsensor.cpp +++ b/src/sensors/lsm6dsv16xsensor.cpp @@ -112,6 +112,9 @@ void LSM6DSV16XSensor::motionSetup() { // Enable Game Rotation Fusion status |= imu.Enable_Game_Rotation(); + // Get acceleration sensitivity + status |= imu.Get_X_Sensitivity(&accelSensitivity); + // Set GBias // status |= imu.Set_G_Bias(0, 0, 0); @@ -208,23 +211,25 @@ void LSM6DSV16XSensor::motionLoop() { uint8_t data[6]; imu.FIFO_Get_Data(data); switch (tag) { - case lsm6dsv16x_fifo_out_raw_t:: - LSM6DSV16X_XL_NC_TAG: // accel + case lsm6dsv16x_fifo_out_raw_t::LSM6DSV16X_XL_NC_TAG: { // accel #if SEND_ACCELERATION - // int32_t accelerometerInt[3]; - // errorCounter -= imu.Get_X_Axes(accelerometerInt); - // acceleration[0] = accelerometerInt[0] * 0.01F; //convert from mg to g - // acceleration[1] = accelerometerInt[1] * 0.01F; - // acceleration[2] = accelerometerInt[2] * 0.01F; - acceleration[0] - = Conversions::convertBytesToFloat(data[0], data[1]) * sensitivity; - acceleration[1] - = Conversions::convertBytesToFloat(data[2], data[3]) * sensitivity; - acceleration[2] - = Conversions::convertBytesToFloat(data[4], data[5]) * sensitivity; + int32_t rawAccelerations[3]; + if (imu.FIFO_Get_X_Axes(rawAccelerations) != LSM6DSV16X_OK) { + m_Logger.error( + "Failed to get accelerometer data on %s at address 0x%02x", + getIMUNameByType(sensorType), + addr + ); + continue; + } + acceleration[0] = rawAccelerations[0] / 1000.0f / 9.81; + acceleration[1] = rawAccelerations[1] / 1000.0f / 9.81; + acceleration[2] = rawAccelerations[2] / 1000.0f / 9.81; + // FIXME: this is obviously incorrect, please fix newAcceleration = true; #endif // SEND_ACCELERATION break; + } case lsm6dsv16x_fifo_out_raw_t:: LSM6DSV16X_SFLP_GAME_ROTATION_VECTOR_TAG: { // SFLP game rotation // vector diff --git a/src/sensors/lsm6dsv16xsensor.h b/src/sensors/lsm6dsv16xsensor.h index f14354d55..5214b575a 100644 --- a/src/sensors/lsm6dsv16xsensor.h +++ b/src/sensors/lsm6dsv16xsensor.h @@ -81,7 +81,7 @@ class LSM6DSV16XSensor : public Sensor { uint8_t m_IntPin; uint8_t tap = 0; unsigned long lastData = 0; - float sensitivity = 0.0f; + float accelSensitivity = 0.0f; uint8_t lastReset = 0; float temperature = 0; bool newTemperature = false; From 04cbbcb0d7daf739de957ae144f23c39397717fe Mon Sep 17 00:00:00 2001 From: Carter Meyers Date: Fri, 4 Aug 2023 05:55:26 -0700 Subject: [PATCH 20/60] Finish linear acceleration --- src/sensors/lsm6dsv16xsensor.cpp | 46 +++++++++++++++++++++++--------- src/sensors/lsm6dsv16xsensor.h | 2 +- 2 files changed, 34 insertions(+), 14 deletions(-) diff --git a/src/sensors/lsm6dsv16xsensor.cpp b/src/sensors/lsm6dsv16xsensor.cpp index d1f8ed331..83d154831 100644 --- a/src/sensors/lsm6dsv16xsensor.cpp +++ b/src/sensors/lsm6dsv16xsensor.cpp @@ -107,16 +107,14 @@ void LSM6DSV16XSensor::motionSetup() { // Set FIFO mode to "continuous", so old data gets thrown away status |= imu.FIFO_Set_Mode(LSM6DSV16X_STREAM_MODE); - status |= imu.FIFO_Set_SFLP_Batch(true, false, false); + status |= imu.FIFO_Set_SFLP_Batch(true, true, false); //TODO: Add the game specifiic SFLP to the Enable_Game_Rotation // Enable Game Rotation Fusion status |= imu.Enable_Game_Rotation(); - // Get acceleration sensitivity - status |= imu.Get_X_Sensitivity(&accelSensitivity); - // Set GBias - // status |= imu.Set_G_Bias(0, 0, 0); + //status |= imu.beginPreconfigured(); + #ifdef INTERRUPT_FOR_SLEEP attachInterrupt(m_IntPin, interruptHandler, RISING); @@ -208,8 +206,6 @@ void LSM6DSV16XSensor::motionLoop() { continue; } - uint8_t data[6]; - imu.FIFO_Get_Data(data); switch (tag) { case lsm6dsv16x_fifo_out_raw_t::LSM6DSV16X_XL_NC_TAG: { // accel #if SEND_ACCELERATION @@ -222,17 +218,22 @@ void LSM6DSV16XSensor::motionLoop() { ); continue; } - acceleration[0] = rawAccelerations[0] / 1000.0f / 9.81; - acceleration[1] = rawAccelerations[1] / 1000.0f / 9.81; - acceleration[2] = rawAccelerations[2] / 1000.0f / 9.81; - // FIXME: this is obviously incorrect, please fix + acceleration[0] = (rawAccelerations[0] / 1000.0f) - gravityX; + acceleration[1] = (rawAccelerations[1] / 1000.0f) - gravityY; + acceleration[2] = (rawAccelerations[2] / 1000.0f) - gravityZ; + + //acceleration[0] = gravityX; + //acceleration[1] = gravityY; + //acceleration[2] = gravityZ; + newAcceleration = true; #endif // SEND_ACCELERATION break; } case lsm6dsv16x_fifo_out_raw_t:: - LSM6DSV16X_SFLP_GAME_ROTATION_VECTOR_TAG: { // SFLP game rotation - // vector + LSM6DSV16X_SFLP_GAME_ROTATION_VECTOR_TAG: { + uint8_t data[6]; + imu.FIFO_Get_Data(data); float x = Conversions::convertBytesToFloat(data[0], data[1]); float y = Conversions::convertBytesToFloat(data[2], data[3]); float z = Conversions::convertBytesToFloat(data[4], data[5]); @@ -249,6 +250,25 @@ void LSM6DSV16XSensor::motionLoop() { } break; } + case lsm6dsv16x_fifo_out_raw_t::LSM6DSV16X_SFLP_GRAVITY_VECTOR_TAG: { + int32_t rawAccelerations[3]; + if (imu.FIFO_Get_X_Axes(rawAccelerations) != LSM6DSV16X_OK) { + m_Logger.error( + "Failed to get accelerometer data on %s at address 0x%02x", + getIMUNameByType(sensorType), + addr + ); + continue; + } + gravityX = rawAccelerations[0] / 2000.0F; + gravityY = rawAccelerations[1] / 2000.0F; + gravityZ = rawAccelerations[2] / 2000.0F; + break; + } + default: { //We don't use the data so remove from fifo + uint8_t data[6]; + imu.FIFO_Get_Data(data); + } } } } diff --git a/src/sensors/lsm6dsv16xsensor.h b/src/sensors/lsm6dsv16xsensor.h index 5214b575a..5d2c6e74a 100644 --- a/src/sensors/lsm6dsv16xsensor.h +++ b/src/sensors/lsm6dsv16xsensor.h @@ -81,11 +81,11 @@ class LSM6DSV16XSensor : public Sensor { uint8_t m_IntPin; uint8_t tap = 0; unsigned long lastData = 0; - float accelSensitivity = 0.0f; uint8_t lastReset = 0; float temperature = 0; bool newTemperature = false; uint32_t lastTempRead = 0; + float gravityX = 0, gravityY = 0, gravityZ = 0; #ifdef REINIT_ON_FAILURE uint8_t reinitOnFailAttempts = 0; From e53b549e05d0f664595bf39f49ba2f280cc91d6a Mon Sep 17 00:00:00 2001 From: Carter Meyers Date: Fri, 4 Aug 2023 06:28:43 -0700 Subject: [PATCH 21/60] Merge important stuff from main --- src/sensors/lsm6dsv16xsensor.cpp | 54 ++++++++++++++++---------------- src/sensors/lsm6dsv16xsensor.h | 1 + 2 files changed, 28 insertions(+), 27 deletions(-) diff --git a/src/sensors/lsm6dsv16xsensor.cpp b/src/sensors/lsm6dsv16xsensor.cpp index 83d154831..c54fb7840 100644 --- a/src/sensors/lsm6dsv16xsensor.cpp +++ b/src/sensors/lsm6dsv16xsensor.cpp @@ -28,6 +28,15 @@ #include "lsm6dsv16xsensor.h" #include "utils.h" + +#ifdef LSM6DSV16X_INTERRUPT +volatile bool imuEvent = false; + +void IRAM_ATTR interruptHandler() { + imuEvent = true; +} +#endif + LSM6DSV16XSensor::LSM6DSV16XSensor( uint8_t id, uint8_t type, @@ -43,40 +52,32 @@ LSM6DSV16XSensor::LSM6DSV16XSensor( void LSM6DSV16XSensor::motionSetup() { uint8_t deviceId = 0; - if (imu.ReadID(&deviceId) == LSM6DSV16X_ERROR) { - m_Logger.fatal( - "The IMU returned an error when reading the device ID of: 0x%02x", - deviceId - ); - ledManager.pattern(50, 50, 200); - return; - } + if(imu.ReadID(&deviceId) == LSM6DSV16X_ERROR) { + m_Logger.fatal("The %s at 0x%02x returned an error when reading the device ID of: 0x%02x", getIMUNameByType(sensorType), addr, deviceId); + ledManager.pattern(50, 50, 200); + return; + } if (deviceId != LSM6DSV16X_ID) { - m_Logger.fatal( - "The IMU returned an invalid device ID of: 0x%02x when it should have " - "been: 0x%02x", - deviceId, - LSM6DSV16X_ID - ); - ledManager.pattern(50, 50, 200); - return; - } + m_Logger.fatal("The %s at 0x%02x returned an invalid device ID of: 0x%02x when it should have been: 0x%02x", getIMUNameByType(sensorType), addr, deviceId, LSM6DSV16X_ID); + ledManager.pattern(50, 50, 200); + return; + } #ifdef SELF_TEST_ON_INIT m_Logger.info( - "%s Self Test started on 0x%02x.(Disabled)", + "%s Self Test started on address: 0x%02x", getIMUNameByType(sensorType), addr ); if (imu.Test_IMU(LSM6DSV16X_XL_ST_NEGATIVE, LSM6DSV16X_GY_ST_NEGATIVE) == LSM6DSV16X_ERROR) { - m_Logger.fatal("The IMU returned an error during the self test"); + m_Logger.fatal("The %s at 0x%02x returned an error during the self test", getIMUNameByType(sensorType), addr); ledManager.pattern(50, 50, 200); return; } - m_Logger.info("Self Test Passed"); + m_Logger.info("Self Test Passed on address: 0x%02x"); #endif m_Logger.info("Connected to %s on 0x%02x", getIMUNameByType(sensorType), addr); @@ -116,11 +117,11 @@ void LSM6DSV16XSensor::motionSetup() { //status |= imu.beginPreconfigured(); -#ifdef INTERRUPT_FOR_SLEEP +#ifdef LSM6DSV16X_INTERRUPT attachInterrupt(m_IntPin, interruptHandler, RISING); - errorCounter -= imu.Enable_Single_Tap_Detection(LSM6DSV16X_INT1_PIN); - errorCounter -= imu.Enable_Double_Tap_Detection(LSM6DSV16X_INT1_PIN); + status |= imu.Enable_Single_Tap_Detection(LSM6DSV16X_INT1_PIN); + status |= imu.Enable_Double_Tap_Detection(LSM6DSV16X_INT1_PIN); #endif if (status != LSM6DSV16X_OK) { @@ -166,11 +167,11 @@ void LSM6DSV16XSensor::motionLoop() { getIMUNameByType(sensorType), addr ); - // statusManager.setStatus(SlimeVR::Status::IMU_ERROR, true); - // working = false; + statusManager.setStatus(SlimeVR::Status::IMU_ERROR, true); + working = false; lastData = millis(); // reset time counter for error message -#ifdef REINIT_ON_FAILURE +#ifdef REINIT_ON_FAILURE //Most likely will not fix the imu (maybe remove) if (reinitOnFailAttempts < REINIT_RETRY_MAX_ATTEMPTS) { motionSetup(); } else { @@ -180,7 +181,6 @@ void LSM6DSV16XSensor::motionLoop() { addr ); } - reinitOnFailAttempts++; // buf overflow will make it try again in about 4 min #endif } diff --git a/src/sensors/lsm6dsv16xsensor.h b/src/sensors/lsm6dsv16xsensor.h index 5d2c6e74a..1b4f4c7f8 100644 --- a/src/sensors/lsm6dsv16xsensor.h +++ b/src/sensors/lsm6dsv16xsensor.h @@ -49,6 +49,7 @@ // #define SELF_TEST_ON_INIT // #define REINIT_ON_FAILURE +//#define LSM6DSV16X_INTERRUPT #ifdef REINIT_ON_FAILURE #define REINIT_RETRY_MAX_ATTEMPTS 5 From b43bd0d201edf73274989609387b2768d4a472e1 Mon Sep 17 00:00:00 2001 From: Gorbit99 Date: Fri, 4 Aug 2023 16:37:34 +0200 Subject: [PATCH 22/60] Converted Gs to m/s^2 --- src/sensors/lsm6dsv16xsensor.cpp | 67 +++++++++++++++++++------------- 1 file changed, 41 insertions(+), 26 deletions(-) diff --git a/src/sensors/lsm6dsv16xsensor.cpp b/src/sensors/lsm6dsv16xsensor.cpp index c54fb7840..ba9514233 100644 --- a/src/sensors/lsm6dsv16xsensor.cpp +++ b/src/sensors/lsm6dsv16xsensor.cpp @@ -28,13 +28,10 @@ #include "lsm6dsv16xsensor.h" #include "utils.h" - #ifdef LSM6DSV16X_INTERRUPT volatile bool imuEvent = false; -void IRAM_ATTR interruptHandler() { - imuEvent = true; -} +void IRAM_ATTR interruptHandler() { imuEvent = true; } #endif LSM6DSV16XSensor::LSM6DSV16XSensor( @@ -52,17 +49,29 @@ LSM6DSV16XSensor::LSM6DSV16XSensor( void LSM6DSV16XSensor::motionSetup() { uint8_t deviceId = 0; - if(imu.ReadID(&deviceId) == LSM6DSV16X_ERROR) { - m_Logger.fatal("The %s at 0x%02x returned an error when reading the device ID of: 0x%02x", getIMUNameByType(sensorType), addr, deviceId); - ledManager.pattern(50, 50, 200); - return; - } + if (imu.ReadID(&deviceId) == LSM6DSV16X_ERROR) { + m_Logger.fatal( + "The %s at 0x%02x returned an error when reading the device ID of: 0x%02x", + getIMUNameByType(sensorType), + addr, + deviceId + ); + ledManager.pattern(50, 50, 200); + return; + } if (deviceId != LSM6DSV16X_ID) { - m_Logger.fatal("The %s at 0x%02x returned an invalid device ID of: 0x%02x when it should have been: 0x%02x", getIMUNameByType(sensorType), addr, deviceId, LSM6DSV16X_ID); - ledManager.pattern(50, 50, 200); - return; - } + m_Logger.fatal( + "The %s at 0x%02x returned an invalid device ID of: 0x%02x when it should " + "have been: 0x%02x", + getIMUNameByType(sensorType), + addr, + deviceId, + LSM6DSV16X_ID + ); + ledManager.pattern(50, 50, 200); + return; + } #ifdef SELF_TEST_ON_INIT m_Logger.info( @@ -73,7 +82,11 @@ void LSM6DSV16XSensor::motionSetup() { if (imu.Test_IMU(LSM6DSV16X_XL_ST_NEGATIVE, LSM6DSV16X_GY_ST_NEGATIVE) == LSM6DSV16X_ERROR) { - m_Logger.fatal("The %s at 0x%02x returned an error during the self test", getIMUNameByType(sensorType), addr); + m_Logger.fatal( + "The %s at 0x%02x returned an error during the self test", + getIMUNameByType(sensorType), + addr + ); ledManager.pattern(50, 50, 200); return; } @@ -108,14 +121,16 @@ void LSM6DSV16XSensor::motionSetup() { // Set FIFO mode to "continuous", so old data gets thrown away status |= imu.FIFO_Set_Mode(LSM6DSV16X_STREAM_MODE); - status |= imu.FIFO_Set_SFLP_Batch(true, true, false); //TODO: Add the game specifiic SFLP to the Enable_Game_Rotation + status |= imu.FIFO_Set_SFLP_Batch( + true, + true, + false + ); // TODO: Add the game specifiic SFLP to the Enable_Game_Rotation // Enable Game Rotation Fusion status |= imu.Enable_Game_Rotation(); - - //status |= imu.beginPreconfigured(); - + // status |= imu.beginPreconfigured(); #ifdef LSM6DSV16X_INTERRUPT attachInterrupt(m_IntPin, interruptHandler, RISING); @@ -141,6 +156,8 @@ void LSM6DSV16XSensor::motionSetup() { lastTempRead = millis(); } +constexpr float G_TO_MS2 = 9.80665f; + void LSM6DSV16XSensor::motionLoop() { if (millis() - lastTempRead > LSM6DSV16X_TEMP_READ_INTERVAL * 1000) { lastTempRead = millis(); @@ -171,7 +188,7 @@ void LSM6DSV16XSensor::motionLoop() { working = false; lastData = millis(); // reset time counter for error message -#ifdef REINIT_ON_FAILURE //Most likely will not fix the imu (maybe remove) +#ifdef REINIT_ON_FAILURE // Most likely will not fix the imu (maybe remove) if (reinitOnFailAttempts < REINIT_RETRY_MAX_ATTEMPTS) { motionSetup(); } else { @@ -221,17 +238,15 @@ void LSM6DSV16XSensor::motionLoop() { acceleration[0] = (rawAccelerations[0] / 1000.0f) - gravityX; acceleration[1] = (rawAccelerations[1] / 1000.0f) - gravityY; acceleration[2] = (rawAccelerations[2] / 1000.0f) - gravityZ; - - //acceleration[0] = gravityX; - //acceleration[1] = gravityY; - //acceleration[2] = gravityZ; + acceleration[0] *= G_TO_MS2; + acceleration[1] *= G_TO_MS2; + acceleration[2] *= G_TO_MS2; newAcceleration = true; #endif // SEND_ACCELERATION break; } - case lsm6dsv16x_fifo_out_raw_t:: - LSM6DSV16X_SFLP_GAME_ROTATION_VECTOR_TAG: { + case lsm6dsv16x_fifo_out_raw_t::LSM6DSV16X_SFLP_GAME_ROTATION_VECTOR_TAG: { uint8_t data[6]; imu.FIFO_Get_Data(data); float x = Conversions::convertBytesToFloat(data[0], data[1]); @@ -265,7 +280,7 @@ void LSM6DSV16XSensor::motionLoop() { gravityZ = rawAccelerations[2] / 2000.0F; break; } - default: { //We don't use the data so remove from fifo + default: { // We don't use the data so remove from fifo uint8_t data[6]; imu.FIFO_Get_Data(data); } From 124f00c66359a3d09995fc48bfe4ec49240e59a2 Mon Sep 17 00:00:00 2001 From: Gorbit99 Date: Fri, 4 Aug 2023 17:52:52 +0200 Subject: [PATCH 23/60] Self-test should now work --- lib/LSM6DSV16X/LSM6DSV16X.cpp | 24 +++++++------------ src/sensors/lsm6dsv16xsensor.cpp | 41 +++++++++++++++++++++----------- src/sensors/lsm6dsv16xsensor.h | 3 ++- 3 files changed, 38 insertions(+), 30 deletions(-) diff --git a/lib/LSM6DSV16X/LSM6DSV16X.cpp b/lib/LSM6DSV16X/LSM6DSV16X.cpp index 1e90c7923..832354b26 100644 --- a/lib/LSM6DSV16X/LSM6DSV16X.cpp +++ b/lib/LSM6DSV16X/LSM6DSV16X.cpp @@ -805,7 +805,6 @@ LSM6DSV16XStatusTypeDef LSM6DSV16X::Test_IMU(uint8_t XTestType, uint8_t GTestTyp { uint8_t whoamI; - if (ReadID(&whoamI) != LSM6DSV16X_OK) { return LSM6DSV16X_ERROR; } @@ -814,7 +813,6 @@ LSM6DSV16XStatusTypeDef LSM6DSV16X::Test_IMU(uint8_t XTestType, uint8_t GTestTyp return LSM6DSV16X_ERROR; } - if (Test_X_IMU(XTestType) != LSM6DSV16X_OK) { return LSM6DSV16X_ERROR; } @@ -822,6 +820,7 @@ LSM6DSV16XStatusTypeDef LSM6DSV16X::Test_IMU(uint8_t XTestType, uint8_t GTestTyp if (Test_G_IMU(GTestType) != LSM6DSV16X_OK) { return LSM6DSV16X_ERROR; } + printf("IMU Test OK\n"); return LSM6DSV16X_OK; } @@ -837,7 +836,6 @@ LSM6DSV16XStatusTypeDef LSM6DSV16X::Test_X_IMU(uint8_t TestType) float val_st_on[3]; float test_val[3]; - if (Reset_Set(LSM6DSV16X_RESTORE_CTRL_REGS) != LSM6DSV16X_OK) { return LSM6DSV16X_ERROR; } @@ -856,11 +854,10 @@ LSM6DSV16XStatusTypeDef LSM6DSV16X::Test_X_IMU(uint8_t TestType) if (lsm6dsv16x_xl_full_scale_set(®_ctx, LSM6DSV16X_4g) != LSM6DSV16X_OK) { return LSM6DSV16X_ERROR; } - delayMicroseconds(100); //Wait for Accelerometer to stabalize; + delay(100); //Wait for Accelerometer to stabalize; memset(val_st_off, 0x00, 3 * sizeof(float)); memset(val_st_on, 0x00, 3 * sizeof(float)); - /*Ignore First Data*/ if (Get_X_AxesRaw_When_Aval(data_raw) != LSM6DSV16X_OK) { return LSM6DSV16X_ERROR; @@ -885,8 +882,7 @@ LSM6DSV16XStatusTypeDef LSM6DSV16X::Test_X_IMU(uint8_t TestType) if (lsm6dsv16x_xl_self_test_set(®_ctx, (lsm6dsv16x_xl_self_test_t)TestType) != LSM6DSV16X_OK) { return LSM6DSV16X_ERROR; } - delayMicroseconds(100); - + delay(100); /*Ignore First Data*/ if (Get_X_AxesRaw_When_Aval(data_raw) != LSM6DSV16X_OK) { @@ -937,8 +933,6 @@ LSM6DSV16XStatusTypeDef LSM6DSV16X::Test_X_IMU(uint8_t TestType) LSM6DSV16XStatusTypeDef LSM6DSV16X::Test_G_IMU(uint8_t TestType = LSM6DSV16X_GY_ST_POSITIVE) { int16_t data_raw[3]; - float val_st_off[3]; - float val_st_on[3]; float test_val[3]; if (Reset_Set(LSM6DSV16X_RESTORE_CTRL_REGS) != LSM6DSV16X_OK) { @@ -960,15 +954,16 @@ LSM6DSV16XStatusTypeDef LSM6DSV16X::Test_G_IMU(uint8_t TestType = LSM6DSV16X_GY_ if (lsm6dsv16x_gy_full_scale_set(®_ctx, LSM6DSV16X_2000dps) != LSM6DSV16X_OK) { return LSM6DSV16X_ERROR; } - delayMicroseconds(100); - memset(val_st_off, 0x00, 3 * sizeof(float)); - memset(val_st_on, 0x00, 3 * sizeof(float)); + delay(100); /*Ignore First Data*/ if (Get_G_AxesRaw_When_Aval(data_raw) != LSM6DSV16X_OK) { return LSM6DSV16X_ERROR; } + + float val_st_off[3] = {0}; + float val_st_on[3] = {0}; for (uint8_t i = 0; i < 5; i++) { if (Get_G_AxesRaw_When_Aval(data_raw) != LSM6DSV16X_OK) { @@ -989,8 +984,7 @@ LSM6DSV16XStatusTypeDef LSM6DSV16X::Test_G_IMU(uint8_t TestType = LSM6DSV16X_GY_ if (lsm6dsv16x_gy_self_test_set(®_ctx, (lsm6dsv16x_gy_self_test_t)TestType) != LSM6DSV16X_OK) { return LSM6DSV16X_ERROR; } - delayMicroseconds(100); - + delay(100); /*Ignore First Data*/ if (Get_G_AxesRaw_When_Aval(data_raw) != LSM6DSV16X_OK) { @@ -3316,7 +3310,7 @@ LSM6DSV16XStatusTypeDef LSM6DSV16X::Get_G_AxesRaw_When_Aval(int16_t *Value) { if (lsm6dsv16x_flag_data_ready_get(®_ctx, &drdy) != LSM6DSV16X_OK) { return LSM6DSV16X_ERROR; } - } while (!drdy.drdy_xl); + } while (!drdy.drdy_gy); if (Get_G_AxesRaw(Value) != LSM6DSV16X_OK) { return LSM6DSV16X_ERROR; diff --git a/src/sensors/lsm6dsv16xsensor.cpp b/src/sensors/lsm6dsv16xsensor.cpp index ba9514233..11df377e5 100644 --- a/src/sensors/lsm6dsv16xsensor.cpp +++ b/src/sensors/lsm6dsv16xsensor.cpp @@ -73,24 +73,17 @@ void LSM6DSV16XSensor::motionSetup() { return; } -#ifdef SELF_TEST_ON_INIT - m_Logger.info( - "%s Self Test started on address: 0x%02x", - getIMUNameByType(sensorType), - addr - ); - - if (imu.Test_IMU(LSM6DSV16X_XL_ST_NEGATIVE, LSM6DSV16X_GY_ST_NEGATIVE) - == LSM6DSV16X_ERROR) { - m_Logger.fatal( - "The %s at 0x%02x returned an error during the self test", +#ifndef LSM6DSV16X_NO_SELF_TEST_ON_STARTUP + if (runSelfTest() != LSM6DSV16X_OK) { + m_Logger.error( + "The %s at 0x%02x returned an error during the self test " + "(maybe it wasn't on a flat surface?)", getIMUNameByType(sensorType), addr ); - ledManager.pattern(50, 50, 200); - return; + + // TODO: if we can signal a non calibrated state, this is the time } - m_Logger.info("Self Test Passed on address: 0x%02x"); #endif m_Logger.info("Connected to %s on 0x%02x", getIMUNameByType(sensorType), addr); @@ -315,6 +308,26 @@ float LSM6DSV16XSensor::temperatureSensorToActualTemperature(int16_t temperature return TEMPERATURE_OFFSET + delta; } +LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::runSelfTest() { + m_Logger.info( + "%s Self Test started on address: 0x%02x", + getIMUNameByType(sensorType), + addr + ); + + if (imu.Test_IMU(LSM6DSV16X_XL_ST_NEGATIVE, LSM6DSV16X_GY_ST_NEGATIVE) + == LSM6DSV16X_ERROR) { + return LSM6DSV16X_ERROR; + } + m_Logger.info( + "%s Self Test Passed on address: 0x%02x", + getIMUNameByType(sensorType), + addr + ); + + return LSM6DSV16X_OK; +} + void LSM6DSV16XSensor::sendData() { if (newFusedRotation) { newFusedRotation = false; diff --git a/src/sensors/lsm6dsv16xsensor.h b/src/sensors/lsm6dsv16xsensor.h index 1b4f4c7f8..4479d8480 100644 --- a/src/sensors/lsm6dsv16xsensor.h +++ b/src/sensors/lsm6dsv16xsensor.h @@ -49,7 +49,7 @@ // #define SELF_TEST_ON_INIT // #define REINIT_ON_FAILURE -//#define LSM6DSV16X_INTERRUPT +// #define LSM6DSV16X_INTERRUPT #ifdef REINIT_ON_FAILURE #define REINIT_RETRY_MAX_ATTEMPTS 5 @@ -77,6 +77,7 @@ class LSM6DSV16XSensor : public Sensor { private: Quat fusedRotationToQuaternion(float x, float y, float z); float temperatureSensorToActualTemperature(int16_t temperature); + LSM6DSV16XStatusTypeDef runSelfTest(); LSM6DSV16X imu; uint8_t m_IntPin; From 1600a70a6f4091ec470c5d34a4cac0da1dca1113 Mon Sep 17 00:00:00 2001 From: Gorbit99 Date: Fri, 4 Aug 2023 18:49:06 +0200 Subject: [PATCH 24/60] Only do self test on facedown --- src/sensors/lsm6dsv16xsensor.cpp | 28 ++++++++++++++++++---------- 1 file changed, 18 insertions(+), 10 deletions(-) diff --git a/src/sensors/lsm6dsv16xsensor.cpp b/src/sensors/lsm6dsv16xsensor.cpp index 11df377e5..4e9b235a4 100644 --- a/src/sensors/lsm6dsv16xsensor.cpp +++ b/src/sensors/lsm6dsv16xsensor.cpp @@ -28,6 +28,8 @@ #include "lsm6dsv16xsensor.h" #include "utils.h" +void IRAM_ATTR interruptHandler() {} + #ifdef LSM6DSV16X_INTERRUPT volatile bool imuEvent = false; @@ -73,17 +75,24 @@ void LSM6DSV16XSensor::motionSetup() { return; } -#ifndef LSM6DSV16X_NO_SELF_TEST_ON_STARTUP - if (runSelfTest() != LSM6DSV16X_OK) { - m_Logger.error( - "The %s at 0x%02x returned an error during the self test " - "(maybe it wasn't on a flat surface?)", - getIMUNameByType(sensorType), - addr - ); +#ifndef LSM6DSV16X_NO_SELF_TEST_ON_FACEDOWN + imu.Enable_6D_Orientation(LSM6DSV16X_INT1_PIN); + uint8_t isFaceDown; + imu.Get_6D_Orientation_ZL(&isFaceDown); + if (isFaceDown) { + if (runSelfTest() != LSM6DSV16X_OK) { + m_Logger.fatal( + "The %s at 0x%02x returned an error during the self test " + "(maybe it wasn't on a flat surface?)", + getIMUNameByType(sensorType), + addr + ); - // TODO: if we can signal a non calibrated state, this is the time + ledManager.pattern(50, 50, 200); + return; + } } + imu.Disable_6D_Orientation(); #endif m_Logger.info("Connected to %s on 0x%02x", getIMUNameByType(sensorType), addr); @@ -127,7 +136,6 @@ void LSM6DSV16XSensor::motionSetup() { #ifdef LSM6DSV16X_INTERRUPT attachInterrupt(m_IntPin, interruptHandler, RISING); - status |= imu.Enable_Single_Tap_Detection(LSM6DSV16X_INT1_PIN); status |= imu.Enable_Double_Tap_Detection(LSM6DSV16X_INT1_PIN); #endif From f2ab2d6cf3cb07ef2dec09545e46cd6341b381ed Mon Sep 17 00:00:00 2001 From: Gorbit99 Date: Sun, 6 Aug 2023 05:12:59 +0200 Subject: [PATCH 25/60] Code cleanup --- lib/LSM6DSV16X/LSM6DSV16X.cpp | 79 +++++++++++++++++++++++++++ lib/LSM6DSV16X/LSM6DSV16X.h | 5 ++ lib/math/quat.cpp | 24 +++++++++ lib/math/quat.h | 2 +- src/sensors/lsm6dsv16xsensor.cpp | 92 +++++++++++++++++--------------- src/sensors/lsm6dsv16xsensor.h | 4 +- 6 files changed, 161 insertions(+), 45 deletions(-) diff --git a/lib/LSM6DSV16X/LSM6DSV16X.cpp b/lib/LSM6DSV16X/LSM6DSV16X.cpp index 832354b26..509aa25da 100644 --- a/lib/LSM6DSV16X/LSM6DSV16X.cpp +++ b/lib/LSM6DSV16X/LSM6DSV16X.cpp @@ -2941,6 +2941,53 @@ LSM6DSV16XStatusTypeDef LSM6DSV16X::FIFO_Get_Status(lsm6dsv16x_fifo_status_t * S return (LSM6DSV16XStatusTypeDef) lsm6dsv16x_fifo_status_get(®_ctx, Status); } +LSM6DSV16XStatusTypeDef LSM6DSV16X::FIFO_Get_Gravity_Vector(float * data) +{ + uint16_t raw_data[6]; + if (FIFO_Get_Data((uint8_t *)raw_data) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + data[0] = lsm6dsv16x_from_sflp_to_mg(raw_data[0]); + data[1] = lsm6dsv16x_from_sflp_to_mg(raw_data[1]); + data[2] = lsm6dsv16x_from_sflp_to_mg(raw_data[2]); + return LSM6DSV16X_OK; +} + +LSM6DSV16XStatusTypeDef LSM6DSV16X::FIFO_Get_Game_Vector(float * data) +{ + uint16_t raw_data[3]; + if (FIFO_Get_Data((uint8_t *)raw_data) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + union bits_to_float { + uint32_t bits; + float value; + }; + + bits_to_float float_bits[3]; + float_bits[0].bits = Half_Bits_To_Float_Bits(raw_data[0]); + float_bits[1].bits = Half_Bits_To_Float_Bits(raw_data[1]); + float_bits[2].bits = Half_Bits_To_Float_Bits(raw_data[2]); + data[0] = float_bits[0].value; + data[1] = float_bits[1].value; + data[2] = float_bits[2].value; + return LSM6DSV16X_OK; +} + +LSM6DSV16XStatusTypeDef LSM6DSV16X::FIFO_Get_GBias_Vector(float * data) +{ + uint16_t raw_data[3]; + if (FIFO_Get_Data((uint8_t *)raw_data) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + data[0] = lsm6dsv16x_from_fs125_to_mdps(raw_data[0]); + data[1] = lsm6dsv16x_from_fs125_to_mdps(raw_data[1]); + data[2] = lsm6dsv16x_from_fs125_to_mdps(raw_data[2]); + return LSM6DSV16X_OK; +} + /** * @brief Enable the LSM6DSV16X gyroscope sensor * @retval 0 in case of success, an error code otherwise @@ -3662,3 +3709,35 @@ LSM6DSV16XStatusTypeDef LSM6DSV16X::Is_New_Temperature_Data(bool * available) *available = status.tda; return LSM6DSV16X_OK; } + +uint32_t LSM6DSV16X::Half_Bits_To_Float_Bits(uint16_t h) +{ + uint16_t h_exp, h_sig; + uint32_t f_sgn, f_exp, f_sig; + + h_exp = (h&0x7c00u); + f_sgn = ((uint32_t)h&0x8000u) << 16; + switch (h_exp) { + case 0x0000u: /* 0 or subnormal */ + h_sig = (h&0x03ffu); + /* Signed zero */ + if (h_sig == 0) { + return f_sgn; + } + /* Subnormal */ + h_sig <<= 1; + while ((h_sig&0x0400u) == 0) { + h_sig <<= 1; + h_exp++; + } + f_exp = ((uint32_t)(127 - 15 - h_exp)) << 23; + f_sig = ((uint32_t)(h_sig&0x03ffu)) << 13; + return f_sgn + f_exp + f_sig; + case 0x7c00u: /* inf or NaN */ + /* All-ones exponent and a copy of the significand */ + return f_sgn + 0x7f800000u + (((uint32_t)(h&0x03ffu)) << 13); + default: /* normalized */ + /* Just need to adjust the exponent and shift */ + return f_sgn + (((uint32_t)(h&0x7fffu) + 0x1c000u) << 13); + } +} diff --git a/lib/LSM6DSV16X/LSM6DSV16X.h b/lib/LSM6DSV16X/LSM6DSV16X.h index ea828a2b4..72cfe6a6f 100644 --- a/lib/LSM6DSV16X/LSM6DSV16X.h +++ b/lib/LSM6DSV16X/LSM6DSV16X.h @@ -237,6 +237,9 @@ class LSM6DSV16X { LSM6DSV16XStatusTypeDef FIFO_Set_G_BDR(float Bdr); LSM6DSV16XStatusTypeDef FIFO_Set_SFLP_Batch(bool GameRotation, bool Gravity, bool gBias); LSM6DSV16XStatusTypeDef FIFO_Get_Status(lsm6dsv16x_fifo_status_t *Status); + LSM6DSV16XStatusTypeDef FIFO_Get_Gravity_Vector(float *data); + LSM6DSV16XStatusTypeDef FIFO_Get_Game_Vector(float *data); + LSM6DSV16XStatusTypeDef FIFO_Get_GBias_Vector(float *data); LSM6DSV16XStatusTypeDef QVAR_Enable(); LSM6DSV16XStatusTypeDef QVAR_GetStatus(uint8_t *val); @@ -258,6 +261,8 @@ class LSM6DSV16X { LSM6DSV16XStatusTypeDef Get_Temperature_Raw(int16_t *value); LSM6DSV16XStatusTypeDef Is_New_Temperature_Data(bool *available); + + uint32_t Half_Bits_To_Float_Bits(uint16_t half_bits); /** * @brief Utility function to read data. diff --git a/lib/math/quat.cpp b/lib/math/quat.cpp index af78fa3b2..72975dc2e 100644 --- a/lib/math/quat.cpp +++ b/lib/math/quat.cpp @@ -229,3 +229,27 @@ void Quat::set_axis_angle(const Vector3& axis, const float& angle) { cos_angle); } } + +Vector3 Quat::get_euler_xyz() const { +#ifdef MATH_CHECKS + ERR_FAIL_COND_V_MSG(!is_normalized(), Vector3(), "The quaternion must be normalized."); +#endif + Vector3 angles; + + // roll (x-axis rotation) + double sinr_cosp = 2 * (w * x + y * z); + double cosr_cosp = 1 - 2 * (x * x + y * y); + angles.z = std::atan2(sinr_cosp, cosr_cosp); + + // pitch (y-axis rotation) + double sinp = std::sqrt(1 + 2 * (w * y - x * z)); + double cosp = std::sqrt(1 - 2 * (w * y - x * z)); + angles.x = 2 * std::atan2(sinp, cosp) - M_PI / 2; + + // yaw (z-axis rotation) + double siny_cosp = 2 * (w * z + x * y); + double cosy_cosp = 1 - 2 * (y * y + z * z); + angles.y = std::atan2(siny_cosp, cosy_cosp); + + return angles; +} diff --git a/lib/math/quat.h b/lib/math/quat.h index 2110f43d0..9b736ba57 100644 --- a/lib/math/quat.h +++ b/lib/math/quat.h @@ -72,7 +72,7 @@ class Quat { Vector3 get_euler_yxz() const; void set_euler(const Vector3& p_euler) { set_euler_yxz(p_euler); }; - Vector3 get_euler() const { return get_euler_yxz(); }; + Vector3 get_euler() const { return get_euler_xyz(); }; Quat slerp(const Quat& q, const float& t) const; Quat slerpni(const Quat& q, const float& t) const; diff --git a/src/sensors/lsm6dsv16xsensor.cpp b/src/sensors/lsm6dsv16xsensor.cpp index 4e9b235a4..37155e4e0 100644 --- a/src/sensors/lsm6dsv16xsensor.cpp +++ b/src/sensors/lsm6dsv16xsensor.cpp @@ -93,7 +93,7 @@ void LSM6DSV16XSensor::motionSetup() { } } imu.Disable_6D_Orientation(); -#endif +#endif // LSM6DSV16X_NO_SELF_TEST_ON_FACEDOWN m_Logger.info("Connected to %s on 0x%02x", getIMUNameByType(sensorType), addr); @@ -114,6 +114,7 @@ void LSM6DSV16XSensor::motionSetup() { // Set data rate status |= imu.Set_X_ODR(LSM6DSV16X_FIFO_DATA_RATE); + status |= imu.Set_G_ODR(LSM6DSV16X_FIFO_DATA_RATE); status |= imu.Set_SFLP_ODR(LSM6DSV16X_FIFO_DATA_RATE); status |= imu.FIFO_Set_X_BDR(LSM6DSV16X_FIFO_DATA_RATE); @@ -123,13 +124,9 @@ void LSM6DSV16XSensor::motionSetup() { // Set FIFO mode to "continuous", so old data gets thrown away status |= imu.FIFO_Set_Mode(LSM6DSV16X_STREAM_MODE); - status |= imu.FIFO_Set_SFLP_Batch( - true, - true, - false - ); // TODO: Add the game specifiic SFLP to the Enable_Game_Rotation - // Enable Game Rotation Fusion + // Enable Fusion + status |= imu.FIFO_Set_SFLP_Batch(true, true, true); status |= imu.Enable_Game_Rotation(); // status |= imu.beginPreconfigured(); @@ -138,7 +135,7 @@ void LSM6DSV16XSensor::motionSetup() { attachInterrupt(m_IntPin, interruptHandler, RISING); status |= imu.Enable_Single_Tap_Detection(LSM6DSV16X_INT1_PIN); status |= imu.Enable_Double_Tap_Detection(LSM6DSV16X_INT1_PIN); -#endif +#endif // LSM6DSV16X_INTERRUPT if (status != LSM6DSV16X_OK) { m_Logger.fatal( @@ -157,7 +154,8 @@ void LSM6DSV16XSensor::motionSetup() { lastTempRead = millis(); } -constexpr float G_TO_MS2 = 9.80665f; +constexpr float mgPerG = 1000.0f; +constexpr float mdpsPerDps = 1000.0f; void LSM6DSV16XSensor::motionLoop() { if (millis() - lastTempRead > LSM6DSV16X_TEMP_READ_INTERVAL * 1000) { @@ -171,7 +169,7 @@ void LSM6DSV16XSensor::motionLoop() { addr ); } else { - float actualTemp = temperatureSensorToActualTemperature(rawTemp); + float actualTemp = lsm6dsv16x_from_lsb_to_celsius(rawTemp); if (fabsf(actualTemp - temperature) > 0.01f) { temperature = actualTemp; newTemperature = true; @@ -200,7 +198,7 @@ void LSM6DSV16XSensor::motionLoop() { ); } reinitOnFailAttempts++; // buf overflow will make it try again in about 4 min -#endif +#endif // REINIT_ON_FAILURE } uint16_t fifo_samples = 0; @@ -236,25 +234,30 @@ void LSM6DSV16XSensor::motionLoop() { ); continue; } - acceleration[0] = (rawAccelerations[0] / 1000.0f) - gravityX; - acceleration[1] = (rawAccelerations[1] / 1000.0f) - gravityY; - acceleration[2] = (rawAccelerations[2] / 1000.0f) - gravityZ; - acceleration[0] *= G_TO_MS2; - acceleration[1] *= G_TO_MS2; - acceleration[2] *= G_TO_MS2; + acceleration[0] = (rawAccelerations[0] / mgPerG) - gravity[0]; + acceleration[1] = (rawAccelerations[1] / mgPerG) - gravity[1]; + acceleration[2] = (rawAccelerations[2] / mgPerG) - gravity[2]; + acceleration[0] *= CONST_EARTH_GRAVITY; + acceleration[1] *= CONST_EARTH_GRAVITY; + acceleration[2] *= CONST_EARTH_GRAVITY; newAcceleration = true; #endif // SEND_ACCELERATION break; } case lsm6dsv16x_fifo_out_raw_t::LSM6DSV16X_SFLP_GAME_ROTATION_VECTOR_TAG: { - uint8_t data[6]; - imu.FIFO_Get_Data(data); - float x = Conversions::convertBytesToFloat(data[0], data[1]); - float y = Conversions::convertBytesToFloat(data[2], data[3]); - float z = Conversions::convertBytesToFloat(data[4], data[5]); + float data[3]; + if (imu.FIFO_Get_Game_Vector(data) != LSM6DSV16X_OK) { + m_Logger.error( + "Failed to get game rotation vector on %s at address 0x%02x", + getIMUNameByType(sensorType), + addr + ); + continue; + } - fusedRotation = fusedRotationToQuaternion(x, y, z); + fusedRotation = fusedRotationToQuaternion(data[0], data[1], data[2]) + * sensorOffset * gyroBias; lastReset = 0; lastData = millis(); @@ -267,23 +270,36 @@ void LSM6DSV16XSensor::motionLoop() { break; } case lsm6dsv16x_fifo_out_raw_t::LSM6DSV16X_SFLP_GRAVITY_VECTOR_TAG: { - int32_t rawAccelerations[3]; - if (imu.FIFO_Get_X_Axes(rawAccelerations) != LSM6DSV16X_OK) { + if (imu.FIFO_Get_Gravity_Vector(gravity) != LSM6DSV16X_OK) { m_Logger.error( - "Failed to get accelerometer data on %s at address 0x%02x", + "Failed to get gravity vector on %s at address 0x%02x", getIMUNameByType(sensorType), addr ); continue; } - gravityX = rawAccelerations[0] / 2000.0F; - gravityY = rawAccelerations[1] / 2000.0F; - gravityZ = rawAccelerations[2] / 2000.0F; + gravity[0] /= mgPerG; + gravity[1] /= mgPerG; + gravity[2] /= mgPerG; break; } - default: { // We don't use the data so remove from fifo - uint8_t data[6]; - imu.FIFO_Get_Data(data); + case lsm6dsv16x_fifo_out_raw_t::LSM6DSV16X_SFLP_GYROSCOPE_BIAS_TAG: { + float gbias[3]; + if (imu.FIFO_Get_GBias_Vector(gbias) != LSM6DSV16X_OK) { + m_Logger.error( + "Failed to get gyro bias on %s at address 0x%02x", + getIMUNameByType(sensorType), + addr + ); + continue; + } + + gbias[0] /= mdpsPerDps; + gbias[1] /= mdpsPerDps; + gbias[2] /= mdpsPerDps; + + gyroBias = fusedRotationToQuaternion(gbias[0], gbias[1], gbias[2]); + break; } } } @@ -308,14 +324,6 @@ Quat LSM6DSV16XSensor::fusedRotationToQuaternion(float x, float y, float z) { return Quat(x, y, z, w); } -constexpr float TEMPERATURE_SENSITIVITY = 256; -constexpr float TEMPERATURE_OFFSET = 25; - -float LSM6DSV16XSensor::temperatureSensorToActualTemperature(int16_t temperature) { - float delta = temperature / TEMPERATURE_SENSITIVITY; - return TEMPERATURE_OFFSET + delta; -} - LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::runSelfTest() { m_Logger.info( "%s Self Test started on address: 0x%02x", @@ -348,7 +356,7 @@ void LSM6DSV16XSensor::sendData() { #ifdef DEBUG_SENSOR m_Logger.trace("Quaternion: %f, %f, %f, %f", UNPACK_QUATERNION(fusedRotation)); -#endif +#endif // DEBUG_SENSOR } #if SEND_ACCELERATION @@ -357,7 +365,7 @@ void LSM6DSV16XSensor::sendData() { newAcceleration = false; networkConnection.sendSensorAcceleration(sensorId, acceleration); } -#endif +#endif // SEND_ACCELERATION if (tap != 0) // chip supports tap and double tap { diff --git a/src/sensors/lsm6dsv16xsensor.h b/src/sensors/lsm6dsv16xsensor.h index 4479d8480..e89a0dbbb 100644 --- a/src/sensors/lsm6dsv16xsensor.h +++ b/src/sensors/lsm6dsv16xsensor.h @@ -76,7 +76,6 @@ class LSM6DSV16XSensor : public Sensor { private: Quat fusedRotationToQuaternion(float x, float y, float z); - float temperatureSensorToActualTemperature(int16_t temperature); LSM6DSV16XStatusTypeDef runSelfTest(); LSM6DSV16X imu; @@ -87,7 +86,8 @@ class LSM6DSV16XSensor : public Sensor { float temperature = 0; bool newTemperature = false; uint32_t lastTempRead = 0; - float gravityX = 0, gravityY = 0, gravityZ = 0; + float gravity[3]; + Quat gyroBias; #ifdef REINIT_ON_FAILURE uint8_t reinitOnFailAttempts = 0; From 3c74d2a90923f06b794f25e76bb7d01723d1f859 Mon Sep 17 00:00:00 2001 From: Carter Meyers Date: Sun, 6 Aug 2023 22:35:51 -0700 Subject: [PATCH 26/60] Custom calibration works, just maybe not useful --- lib/LSM6DSV16X/LSM6DSV16X.cpp | 11 ++++ lib/LSM6DSV16X/LSM6DSV16X.h | 1 + src/sensors/lsm6dsv16xsensor.cpp | 102 ++++++++++++++++++++++++++++--- 3 files changed, 107 insertions(+), 7 deletions(-) diff --git a/lib/LSM6DSV16X/LSM6DSV16X.cpp b/lib/LSM6DSV16X/LSM6DSV16X.cpp index 509aa25da..1a75b8655 100644 --- a/lib/LSM6DSV16X/LSM6DSV16X.cpp +++ b/lib/LSM6DSV16X/LSM6DSV16X.cpp @@ -3710,6 +3710,17 @@ LSM6DSV16XStatusTypeDef LSM6DSV16X::Is_New_Temperature_Data(bool * available) return LSM6DSV16X_OK; } +LSM6DSV16XStatusTypeDef LSM6DSV16X::Set_SFLP_GBIAS(float x, float y, float z) { + lsm6dsv16x_sflp_gbias_t val = {0, 0, 0}; + val.gbias_x = x; + val.gbias_y = y; + val.gbias_z = z; + if(lsm6dsv16x_sflp_game_gbias_set(®_ctx, &val) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + return LSM6DSV16X_OK; +} + uint32_t LSM6DSV16X::Half_Bits_To_Float_Bits(uint16_t h) { uint16_t h_exp, h_sig; diff --git a/lib/LSM6DSV16X/LSM6DSV16X.h b/lib/LSM6DSV16X/LSM6DSV16X.h index 72cfe6a6f..2ae1c210b 100644 --- a/lib/LSM6DSV16X/LSM6DSV16X.h +++ b/lib/LSM6DSV16X/LSM6DSV16X.h @@ -184,6 +184,7 @@ class LSM6DSV16X { LSM6DSV16XStatusTypeDef Set_T_ODR(float Odr); LSM6DSV16XStatusTypeDef Set_SFLP_ODR(float Odr); + LSM6DSV16XStatusTypeDef Set_SFLP_GBIAS(float x, float y, float z); LSM6DSV16XStatusTypeDef Enable_6D_Orientation(LSM6DSV16X_SensorIntPin_t IntPin); LSM6DSV16XStatusTypeDef Disable_6D_Orientation(); diff --git a/src/sensors/lsm6dsv16xsensor.cpp b/src/sensors/lsm6dsv16xsensor.cpp index 37155e4e0..3c56310ab 100644 --- a/src/sensors/lsm6dsv16xsensor.cpp +++ b/src/sensors/lsm6dsv16xsensor.cpp @@ -75,10 +75,11 @@ void LSM6DSV16XSensor::motionSetup() { return; } -#ifndef LSM6DSV16X_NO_SELF_TEST_ON_FACEDOWN - imu.Enable_6D_Orientation(LSM6DSV16X_INT1_PIN); + imu.Enable_6D_Orientation(LSM6DSV16X_INT2_PIN); uint8_t isFaceDown; imu.Get_6D_Orientation_ZL(&isFaceDown); + +#ifndef LSM6DSV16X_NO_SELF_TEST_ON_FACEDOWN if (isFaceDown) { if (runSelfTest() != LSM6DSV16X_OK) { m_Logger.fatal( @@ -92,7 +93,6 @@ void LSM6DSV16XSensor::motionSetup() { return; } } - imu.Disable_6D_Orientation(); #endif // LSM6DSV16X_NO_SELF_TEST_ON_FACEDOWN m_Logger.info("Connected to %s on 0x%02x", getIMUNameByType(sensorType), addr); @@ -129,6 +129,12 @@ void LSM6DSV16XSensor::motionSetup() { status |= imu.FIFO_Set_SFLP_Batch(true, true, true); status |= imu.Enable_Game_Rotation(); + // Calibration + if (isFaceDown) { + startCalibration(0); + } + status |= imu.Disable_6D_Orientation(); + // status |= imu.beginPreconfigured(); #ifdef LSM6DSV16X_INTERRUPT @@ -298,9 +304,13 @@ void LSM6DSV16XSensor::motionLoop() { gbias[1] /= mdpsPerDps; gbias[2] /= mdpsPerDps; - gyroBias = fusedRotationToQuaternion(gbias[0], gbias[1], gbias[2]); + //printf("\nx: %f, y: %f, z: %f", gbias[0], gbias[1], gbias[2]); break; } + default: { // We don't use the data so remove from fifo + uint8_t data[6]; + imu.FIFO_Get_Data(data); + } } } } @@ -380,7 +390,85 @@ void LSM6DSV16XSensor::sendData() { } void LSM6DSV16XSensor::startCalibration(int calibrationType) { - // These IMU are factory calibrated. - // The register might be able to be changed but it could break the device - // I don't think we will need to mess with them + // Supported Calibrations: Accelerometer offset and Gyroscope Bias + // Currently only second is used + + m_Logger.info("Flip right side up in the next 5 seconds to start calibration."); + delay(5000); + uint8_t isFaceUp; + imu.Get_6D_Orientation_ZH(&isFaceUp); + + if (!isFaceUp) { + return; + } + + // give warning if temp is low ? (is this needed)? + m_Logger.info("Leave still for 30 seconds"); + delay(2000); + + double gbiasAvg[] = {0, 0, 0}; + uint16_t dataCount = 0; + + while (dataCount < 5000) { + uint16_t fifo_samples = 0; + if (imu.FIFO_Get_Num_Samples(&fifo_samples) == LSM6DSV16X_ERROR) { + m_Logger.error( + "Error getting number of samples in FIFO on %s at address 0x%02x", + getIMUNameByType(sensorType), + addr + ); + return; + } + + for (uint16_t i = 0; i < fifo_samples; i++) { + uint8_t tag; + if (imu.FIFO_Get_Tag(&tag) != LSM6DSV16X_OK) { + m_Logger.error( + "Failed to get FIFO data tag on %s at address 0x%02x", + getIMUNameByType(sensorType), + addr + ); + continue; + } + + switch (tag) { + case lsm6dsv16x_fifo_out_raw_t::LSM6DSV16X_SFLP_GYROSCOPE_BIAS_TAG: { + if (dataCount < 2000) { + dataCount++; + break; + } + float gbias[3]; + if (imu.FIFO_Get_GBias_Vector(gbias) != LSM6DSV16X_OK) { + m_Logger.error( + "Failed to get gyro bias on %s at address 0x%02x", + getIMUNameByType(sensorType), + addr + ); + continue; + } + + gbiasAvg[0] += gbias[0] / mdpsPerDps; + gbiasAvg[1] += gbias[1] / mdpsPerDps; + gbiasAvg[2] += gbias[2] / mdpsPerDps; + dataCount++; + //m_Logger.info("data: %d, x: %f, y: %f, z: %f", dataCount, gbias[0], gbias[1], gbias[2]); + break; + } + default: { // We don't use the data so remove from fifo + uint8_t data[6]; + imu.FIFO_Get_Data(data); + } + } + } + } + m_Logger.info("x: %f, y: %f, z: %f", gbiasAvg[0], gbiasAvg[1], gbiasAvg[2]); + dataCount -= 2000; + m_Logger.info( + "Calibration Data, X: %f, Y: %f, Z: %f", + gbiasAvg[0] / dataCount, + gbiasAvg[1] / dataCount, + gbiasAvg[2] / dataCount + ); + imu.Set_SFLP_GBIAS(0.00, 0.245062, 0.150119); + delay(10000); } From 5179cea954d0d20418a63fd4c732401b9a206d9f Mon Sep 17 00:00:00 2001 From: Carter Meyers Date: Mon, 7 Aug 2023 20:04:15 -0700 Subject: [PATCH 27/60] Add acceleration Offset --- lib/LSM6DSV16X/LSM6DSV16X.cpp | 97 ++++++++++++++++++++++++++++++++ lib/LSM6DSV16X/LSM6DSV16X.h | 9 +++ src/sensors/lsm6dsv16xsensor.cpp | 46 +++++++++------ 3 files changed, 134 insertions(+), 18 deletions(-) diff --git a/lib/LSM6DSV16X/LSM6DSV16X.cpp b/lib/LSM6DSV16X/LSM6DSV16X.cpp index 1a75b8655..2614253f2 100644 --- a/lib/LSM6DSV16X/LSM6DSV16X.cpp +++ b/lib/LSM6DSV16X/LSM6DSV16X.cpp @@ -3721,6 +3721,103 @@ LSM6DSV16XStatusTypeDef LSM6DSV16X::Set_SFLP_GBIAS(float x, float y, float z) { return LSM6DSV16X_OK; } +LSM6DSV16XStatusTypeDef LSM6DSV16X::Enable_X_User_Offset() { + lsm6dsv16x_ctrl9_t ctrl9; + if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_CTRL9, (uint8_t *)&ctrl9, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + ctrl9.usr_off_on_out = true; //1 On, 0 Off + + if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_CTRL9, (uint8_t *)&ctrl9, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + // lsm6dsv16x_wake_up_ths_t wake_up_ths; + // if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_CTRL9, (uint8_t *)&wake_up_ths, 1) != LSM6DSV16X_OK) { + // return LSM6DSV16X_ERROR; + //} + //wake_up_ths.usr_off_on_wu = true; //1 On, 0 Off + + //if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_CTRL9, (uint8_t *)&wake_up_ths, 1) != LSM6DSV16X_OK) { + // return LSM6DSV16X_ERROR; + //} + + return LSM6DSV16X_OK; +} + +LSM6DSV16XStatusTypeDef LSM6DSV16X::Disable_X_User_Offset() { + lsm6dsv16x_ctrl9_t ctrl9; + if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_CTRL9, (uint8_t *)&ctrl9, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + ctrl9.usr_off_on_out = false; //1 On, 0 Off + + if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_CTRL9, (uint8_t *)&ctrl9, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + // lsm6dsv16x_wake_up_ths_t wake_up_ths; + // if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_CTRL9, (uint8_t *)&wake_up_ths, 1) != LSM6DSV16X_OK) { + // return LSM6DSV16X_ERROR; + //} + //wake_up_ths.usr_off_on_wu = false; //1 On, 0 Off + + //if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_CTRL9, (uint8_t *)&wake_up_ths, 1) != LSM6DSV16X_OK) { + // return LSM6DSV16X_ERROR; + //} + + return LSM6DSV16X_OK; +} + +LSM6DSV16XStatusTypeDef LSM6DSV16X::Set_X_User_Offset(float x, float y, float z) { + lsm6dsv16x_ctrl9_t ctrl9; + if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_CTRL9, (uint8_t *)&ctrl9, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + + + + int8_t xyz[3]; + + if ( // about +- 2 G's for high and +- 0.124 G's for low + (x <= LSM6DSV16X_ACC_USR_OFF_W_LOW_MAX && x >= -LSM6DSV16X_ACC_USR_OFF_W_LOW_MAX) && + (y <= LSM6DSV16X_ACC_USR_OFF_W_LOW_MAX && y >= -LSM6DSV16X_ACC_USR_OFF_W_LOW_MAX) && + (z <= LSM6DSV16X_ACC_USR_OFF_W_LOW_MAX && z >= -LSM6DSV16X_ACC_USR_OFF_W_LOW_MAX) + ) { //Then we are under the low requirements + xyz[0] = (int8_t)(x / LSM6DSV16X_ACC_USR_OFF_W_LOW_LSB); + xyz[1] = (int8_t)(y / LSM6DSV16X_ACC_USR_OFF_W_LOW_LSB); + xyz[2] = (int8_t)(z / LSM6DSV16X_ACC_USR_OFF_W_LOW_LSB); + ctrl9.usr_off_w = false; //(0: 2^-10 g/LSB; 1: 2^-6 g/LSB) + } + + else if ( // about +- 2 G's for high and +- 0.124 G's for low + (x <= LSM6DSV16X_ACC_USR_OFF_W_HIGH_MAX && x >= -LSM6DSV16X_ACC_USR_OFF_W_HIGH_MAX) && + (y <= LSM6DSV16X_ACC_USR_OFF_W_HIGH_MAX && y >= -LSM6DSV16X_ACC_USR_OFF_W_HIGH_MAX) && + (z <= LSM6DSV16X_ACC_USR_OFF_W_HIGH_MAX && z >= -LSM6DSV16X_ACC_USR_OFF_W_HIGH_MAX) + ) { //Then we are under the high requirements + xyz[0] = (int8_t)(x / LSM6DSV16X_ACC_USR_OFF_W_HIGH_LSB); + xyz[1] = (int8_t)(y / LSM6DSV16X_ACC_USR_OFF_W_HIGH_LSB); + xyz[2] = (int8_t)(z / LSM6DSV16X_ACC_USR_OFF_W_HIGH_LSB); + ctrl9.usr_off_w = true; //(0: 2^-10 g/LSB; 1: 2^-6 g/LSB) + } else { + return LSM6DSV16X_ERROR; //Value too big + } + + + if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_CTRL9, (uint8_t *)&ctrl9, 1) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + + //convert from float in G's to what it wants. Signed byte + printf("x: %d, y: %d, z: %d", xyz[0], xyz[1], xyz[2]); + if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_X_OFS_USR, (uint8_t*)&xyz, 3) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + return LSM6DSV16X_OK; +} + uint32_t LSM6DSV16X::Half_Bits_To_Float_Bits(uint16_t h) { uint16_t h_exp, h_sig; diff --git a/lib/LSM6DSV16X/LSM6DSV16X.h b/lib/LSM6DSV16X/LSM6DSV16X.h index 2ae1c210b..8882441d0 100644 --- a/lib/LSM6DSV16X/LSM6DSV16X.h +++ b/lib/LSM6DSV16X/LSM6DSV16X.h @@ -70,6 +70,12 @@ #define LSM6DSV16X_QVAR_GAIN 78.000f +#define LSM6DSV16X_ACC_USR_OFF_W_HIGH_LSB (float)(pow(2, -6)) +#define LSM6DSV16X_ACC_USR_OFF_W_LOW_LSB (float)(pow(2, -10)) +#define LSM6DSV16X_ACC_USR_OFF_W_HIGH_MAX LSM6DSV16X_ACC_USR_OFF_W_HIGH_LSB * INT8_MAX +#define LSM6DSV16X_ACC_USR_OFF_W_LOW_MAX LSM6DSV16X_ACC_USR_OFF_W_LOW_LSB * INT8_MAX + + //#define ENABLE_SPI @@ -148,6 +154,8 @@ class LSM6DSV16X { LSM6DSV16XStatusTypeDef Enable_X(); LSM6DSV16XStatusTypeDef Disable_X(); + LSM6DSV16XStatusTypeDef Enable_X_User_Offset(); + LSM6DSV16XStatusTypeDef Disable_X_User_Offset(); LSM6DSV16XStatusTypeDef Get_X_Sensitivity(float *Sensitivity); LSM6DSV16XStatusTypeDef Get_X_ODR(float *Odr); LSM6DSV16XStatusTypeDef Set_X_ODR(float Odr, LSM6DSV16X_ACC_Operating_Mode_t Mode = LSM6DSV16X_ACC_HIGH_PERFORMANCE_MODE); @@ -160,6 +168,7 @@ class LSM6DSV16X { LSM6DSV16XStatusTypeDef Get_X_Event_Status(LSM6DSV16X_Event_Status_t *Status); LSM6DSV16XStatusTypeDef Set_X_Power_Mode(uint8_t PowerMode); LSM6DSV16XStatusTypeDef Set_X_Filter_Mode(uint8_t LowHighPassFlag, uint8_t FilterMode); + LSM6DSV16XStatusTypeDef Set_X_User_Offset(float x, float y, float z); LSM6DSV16XStatusTypeDef Enable_G(); LSM6DSV16XStatusTypeDef Disable_G(); diff --git a/src/sensors/lsm6dsv16xsensor.cpp b/src/sensors/lsm6dsv16xsensor.cpp index 3c56310ab..9dbb4babe 100644 --- a/src/sensors/lsm6dsv16xsensor.cpp +++ b/src/sensors/lsm6dsv16xsensor.cpp @@ -116,17 +116,23 @@ void LSM6DSV16XSensor::motionSetup() { status |= imu.Set_X_ODR(LSM6DSV16X_FIFO_DATA_RATE); status |= imu.Set_G_ODR(LSM6DSV16X_FIFO_DATA_RATE); status |= imu.Set_SFLP_ODR(LSM6DSV16X_FIFO_DATA_RATE); - status |= imu.FIFO_Set_X_BDR(LSM6DSV16X_FIFO_DATA_RATE); + status |= imu.FIFO_Set_X_BDR(60); // Enable IMU status |= imu.Enable_X(); status |= imu.Enable_G(); + status |= imu.Set_X_Filter_Mode(1, LSM6DSV16X_XL_LIGHT); + status |= imu.Set_G_Filter_Mode(1, LSM6DSV16X_XL_LIGHT); + + //status |= imu.Set_X_Filter_Mode(0, LSM6DSV16X_XL_ULTRA_LIGHT); + //status |= imu.Set_G_Filter_Mode(0, LSM6DSV16X_XL_ULTRA_LIGHT); + // Set FIFO mode to "continuous", so old data gets thrown away status |= imu.FIFO_Set_Mode(LSM6DSV16X_STREAM_MODE); // Enable Fusion - status |= imu.FIFO_Set_SFLP_Batch(true, true, true); + status |= imu.FIFO_Set_SFLP_Batch(true, true, false); status |= imu.Enable_Game_Rotation(); // Calibration @@ -304,7 +310,7 @@ void LSM6DSV16XSensor::motionLoop() { gbias[1] /= mdpsPerDps; gbias[2] /= mdpsPerDps; - //printf("\nx: %f, y: %f, z: %f", gbias[0], gbias[1], gbias[2]); + // printf("\nx: %f, y: %f, z: %f", gbias[0], gbias[1], gbias[2]); break; } default: { // We don't use the data so remove from fifo @@ -407,9 +413,9 @@ void LSM6DSV16XSensor::startCalibration(int calibrationType) { delay(2000); double gbiasAvg[] = {0, 0, 0}; - uint16_t dataCount = 0; + uint16_t dataCountGbias = 0; - while (dataCount < 5000) { + while (dataCountGbias < 4000) { uint16_t fifo_samples = 0; if (imu.FIFO_Get_Num_Samples(&fifo_samples) == LSM6DSV16X_ERROR) { m_Logger.error( @@ -433,8 +439,10 @@ void LSM6DSV16XSensor::startCalibration(int calibrationType) { switch (tag) { case lsm6dsv16x_fifo_out_raw_t::LSM6DSV16X_SFLP_GYROSCOPE_BIAS_TAG: { - if (dataCount < 2000) { - dataCount++; + if (dataCountGbias < 100) { + dataCountGbias++; + uint8_t data[6]; + imu.FIFO_Get_Data(data); break; } float gbias[3]; @@ -450,8 +458,7 @@ void LSM6DSV16XSensor::startCalibration(int calibrationType) { gbiasAvg[0] += gbias[0] / mdpsPerDps; gbiasAvg[1] += gbias[1] / mdpsPerDps; gbiasAvg[2] += gbias[2] / mdpsPerDps; - dataCount++; - //m_Logger.info("data: %d, x: %f, y: %f, z: %f", dataCount, gbias[0], gbias[1], gbias[2]); + dataCountGbias++; break; } default: { // We don't use the data so remove from fifo @@ -461,14 +468,17 @@ void LSM6DSV16XSensor::startCalibration(int calibrationType) { } } } - m_Logger.info("x: %f, y: %f, z: %f", gbiasAvg[0], gbiasAvg[1], gbiasAvg[2]); - dataCount -= 2000; - m_Logger.info( - "Calibration Data, X: %f, Y: %f, Z: %f", - gbiasAvg[0] / dataCount, - gbiasAvg[1] / dataCount, - gbiasAvg[2] / dataCount - ); - imu.Set_SFLP_GBIAS(0.00, 0.245062, 0.150119); + m_Logger.info("Gbias x: %f, y: %f, z: %f", gbiasAvg[0], gbiasAvg[1], gbiasAvg[2]); + dataCountGbias -= 100; + + gbiasAvg[0] = gbiasAvg[0] / dataCountGbias; + gbiasAvg[1] = gbiasAvg[1] / dataCountGbias; + gbiasAvg[2] = gbiasAvg[2] / dataCountGbias; + + m_Logger.info("Gbias Calibration Data, X: %f, Y: %f, Z: %f", gbiasAvg[0], gbiasAvg[1], gbiasAvg[2]); + + + //imu.Set_SFLP_GBIAS(0.00, 0.245062, 0.150119); + imu.Set_SFLP_GBIAS(gbiasAvg[0], gbiasAvg[1], gbiasAvg[2]); delay(10000); } From 44977ea5ce6a3a5e7e076a4ac6e1380203689163 Mon Sep 17 00:00:00 2001 From: Carter Meyers Date: Tue, 8 Aug 2023 17:00:16 -0700 Subject: [PATCH 28/60] Add fusedRotation offset calibration --- src/sensors/lsm6dsv16xsensor.cpp | 87 +++++++++++++++++++++++++++----- src/sensors/lsm6dsv16xsensor.h | 1 - 2 files changed, 75 insertions(+), 13 deletions(-) diff --git a/src/sensors/lsm6dsv16xsensor.cpp b/src/sensors/lsm6dsv16xsensor.cpp index 9dbb4babe..97e6d2327 100644 --- a/src/sensors/lsm6dsv16xsensor.cpp +++ b/src/sensors/lsm6dsv16xsensor.cpp @@ -125,14 +125,14 @@ void LSM6DSV16XSensor::motionSetup() { status |= imu.Set_X_Filter_Mode(1, LSM6DSV16X_XL_LIGHT); status |= imu.Set_G_Filter_Mode(1, LSM6DSV16X_XL_LIGHT); - //status |= imu.Set_X_Filter_Mode(0, LSM6DSV16X_XL_ULTRA_LIGHT); - //status |= imu.Set_G_Filter_Mode(0, LSM6DSV16X_XL_ULTRA_LIGHT); + // status |= imu.Set_X_Filter_Mode(0, LSM6DSV16X_XL_ULTRA_LIGHT); + // status |= imu.Set_G_Filter_Mode(0, LSM6DSV16X_XL_ULTRA_LIGHT); // Set FIFO mode to "continuous", so old data gets thrown away status |= imu.FIFO_Set_Mode(LSM6DSV16X_STREAM_MODE); // Enable Fusion - status |= imu.FIFO_Set_SFLP_Batch(true, true, false); + status |= imu.FIFO_Set_SFLP_Batch(true, true, true); status |= imu.Enable_Game_Rotation(); // Calibration @@ -269,7 +269,7 @@ void LSM6DSV16XSensor::motionLoop() { } fusedRotation = fusedRotationToQuaternion(data[0], data[1], data[2]) - * sensorOffset * gyroBias; + * sensorOffset; lastReset = 0; lastData = millis(); @@ -413,9 +413,12 @@ void LSM6DSV16XSensor::startCalibration(int calibrationType) { delay(2000); double gbiasAvg[] = {0, 0, 0}; + float startingQuat[3]; + double quatDiffAvg[] = {0, 0, 0}; uint16_t dataCountGbias = 0; + uint16_t dataCountQuatDiff = 0; - while (dataCountGbias < 4000) { + while (dataCountGbias < 4000 && dataCountQuatDiff < 4000) { uint16_t fifo_samples = 0; if (imu.FIFO_Get_Num_Samples(&fifo_samples) == LSM6DSV16X_ERROR) { m_Logger.error( @@ -461,6 +464,49 @@ void LSM6DSV16XSensor::startCalibration(int calibrationType) { dataCountGbias++; break; } + case lsm6dsv16x_fifo_out_raw_t:: + LSM6DSV16X_SFLP_GAME_ROTATION_VECTOR_TAG: { + if (dataCountQuatDiff < 99) { + dataCountQuatDiff++; + uint8_t data[6]; + imu.FIFO_Get_Data(data); + break; + } + + float data[3]; + if (imu.FIFO_Get_Game_Vector(data) != LSM6DSV16X_OK) { + m_Logger.error( + "Failed to get game rotation vector on %s at address " + "0x%02x", + getIMUNameByType(sensorType), + addr + ); + continue; + } + + if (dataCountQuatDiff == 99) { + startingQuat[0] = data[0]; + startingQuat[1] = data[1]; + startingQuat[2] = data[2]; + } + + quatDiffAvg[0] += (startingQuat[0] - data[0]); + quatDiffAvg[1] += (startingQuat[1] - data[1]); + quatDiffAvg[2] += (startingQuat[2] - data[2]); + + + printf( + "\nX: %f, Y: %f, Z: %f", + (startingQuat[0] - data[0]), + (startingQuat[1] - data[1]), + (startingQuat[2] - data[2]) + ); + startingQuat[0] = data[0]; + startingQuat[1] = data[1]; + startingQuat[2] = data[2]; + dataCountQuatDiff++; + break; + } default: { // We don't use the data so remove from fifo uint8_t data[6]; imu.FIFO_Get_Data(data); @@ -469,16 +515,33 @@ void LSM6DSV16XSensor::startCalibration(int calibrationType) { } } m_Logger.info("Gbias x: %f, y: %f, z: %f", gbiasAvg[0], gbiasAvg[1], gbiasAvg[2]); + m_Logger.info("quatAvg x: %f, y: %f, z: %f", quatDiffAvg[0], quatDiffAvg[1], quatDiffAvg[2]); dataCountGbias -= 100; + dataCountQuatDiff -= 100; + + gbiasAvg[0] /= dataCountGbias; + gbiasAvg[1] /= dataCountGbias; + gbiasAvg[2] /= dataCountGbias; - gbiasAvg[0] = gbiasAvg[0] / dataCountGbias; - gbiasAvg[1] = gbiasAvg[1] / dataCountGbias; - gbiasAvg[2] = gbiasAvg[2] / dataCountGbias; + quatDiffAvg[0] /= dataCountGbias; + quatDiffAvg[1] /= dataCountGbias; + quatDiffAvg[2] /= dataCountGbias; - m_Logger.info("Gbias Calibration Data, X: %f, Y: %f, Z: %f", gbiasAvg[0], gbiasAvg[1], gbiasAvg[2]); + m_Logger.info( + "Gbias Calibration Data, X: %f, Y: %f, Z: %f", + gbiasAvg[0], + gbiasAvg[1], + gbiasAvg[2] + ); + + m_Logger.info( + "Quat Diff Calibration Data, X: %f, Y: %f, Z: %f", + quatDiffAvg[0], + quatDiffAvg[1], + quatDiffAvg[2] + ); - - //imu.Set_SFLP_GBIAS(0.00, 0.245062, 0.150119); - imu.Set_SFLP_GBIAS(gbiasAvg[0], gbiasAvg[1], gbiasAvg[2]); + // imu.Set_SFLP_GBIAS(0.00, 0.245062, 0.150119); + // imu.Set_SFLP_GBIAS(gbiasAvg[0], gbiasAvg[1], gbiasAvg[2]); delay(10000); } diff --git a/src/sensors/lsm6dsv16xsensor.h b/src/sensors/lsm6dsv16xsensor.h index e89a0dbbb..895885da1 100644 --- a/src/sensors/lsm6dsv16xsensor.h +++ b/src/sensors/lsm6dsv16xsensor.h @@ -87,7 +87,6 @@ class LSM6DSV16XSensor : public Sensor { bool newTemperature = false; uint32_t lastTempRead = 0; float gravity[3]; - Quat gyroBias; #ifdef REINIT_ON_FAILURE uint8_t reinitOnFailAttempts = 0; From 0bba914cab8e4c40fe99bcda71fd436d8abea737 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Przemys=C5=82aw=20Romaniak?= Date: Thu, 10 Aug 2023 23:57:14 +0200 Subject: [PATCH 29/60] use the same highest ODR for gyro and accel, change accel sensitivity to 16g gyro to 1000dps --- src/sensors/lsm6dsv16xsensor.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/sensors/lsm6dsv16xsensor.cpp b/src/sensors/lsm6dsv16xsensor.cpp index 4e9b235a4..79533e45a 100644 --- a/src/sensors/lsm6dsv16xsensor.cpp +++ b/src/sensors/lsm6dsv16xsensor.cpp @@ -109,11 +109,12 @@ void LSM6DSV16XSensor::motionSetup() { status |= imu.Set_Auto_Increment(true); // Set maximums - status |= imu.Set_X_FS(LSM6DSV16X_ACCEL_MAX); - status |= imu.Set_G_FS(LSM6DSV16X_GYRO_MAX); + status |= imu.Set_X_FS(16); + status |= imu.Set_G_FS(1000); // Set data rate - status |= imu.Set_X_ODR(LSM6DSV16X_FIFO_DATA_RATE); + status |= imu.Set_X_ODR(7680.0f, LSM6DSV16X_ACC_HIGH_PERFORMANCE_MODE); + status |= imu.Set_G_ODR(7680.0f, LSM6DSV16X_GYRO_HIGH_PERFORMANCE_MODE); status |= imu.Set_SFLP_ODR(LSM6DSV16X_FIFO_DATA_RATE); status |= imu.FIFO_Set_X_BDR(LSM6DSV16X_FIFO_DATA_RATE); From 7affc34c64d24dd72c780247e19d5b241fac1fd3 Mon Sep 17 00:00:00 2001 From: Carter Meyers Date: Wed, 16 Aug 2023 18:22:10 -0700 Subject: [PATCH 30/60] Inital work on esp fusion --- lib/LSM6DSV16X/LSM6DSV16X.cpp | 22 ++ lib/LSM6DSV16X/LSM6DSV16X.h | 3 + src/sensors/lsm6dsv16xsensor.cpp | 475 ++++++++++++++++++++----------- src/sensors/lsm6dsv16xsensor.h | 32 ++- 4 files changed, 364 insertions(+), 168 deletions(-) diff --git a/lib/LSM6DSV16X/LSM6DSV16X.cpp b/lib/LSM6DSV16X/LSM6DSV16X.cpp index 2614253f2..1ca623b32 100644 --- a/lib/LSM6DSV16X/LSM6DSV16X.cpp +++ b/lib/LSM6DSV16X/LSM6DSV16X.cpp @@ -2988,6 +2988,28 @@ LSM6DSV16XStatusTypeDef LSM6DSV16X::FIFO_Get_GBias_Vector(float * data) return LSM6DSV16X_OK; } +LSM6DSV16XStatusTypeDef LSM6DSV16X::FIFO_Get_Timestamp(uint32_t *timestamp) +{ + uint32_t raw_data[2]; //first is timestamp second is half full of meta data + if (FIFO_Get_Data((uint8_t *)raw_data) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + *timestamp = raw_data[0]; + return LSM6DSV16X_OK; +} + +LSM6DSV16XStatusTypeDef LSM6DSV16X::FIFO_Set_Timestamp_Decimation(uint8_t decimation) +{ + return (LSM6DSV16XStatusTypeDef)lsm6dsv16x_fifo_timestamp_batch_set(®_ctx, (lsm6dsv16x_fifo_timestamp_batch_t)decimation); + +} + +LSM6DSV16XStatusTypeDef LSM6DSV16X::Enable_Timestamp() +{ + return (LSM6DSV16XStatusTypeDef)lsm6dsv16x_timestamp_set(®_ctx, 1); +} + + /** * @brief Enable the LSM6DSV16X gyroscope sensor * @retval 0 in case of success, an error code otherwise diff --git a/lib/LSM6DSV16X/LSM6DSV16X.h b/lib/LSM6DSV16X/LSM6DSV16X.h index 8882441d0..f0faa09b4 100644 --- a/lib/LSM6DSV16X/LSM6DSV16X.h +++ b/lib/LSM6DSV16X/LSM6DSV16X.h @@ -250,6 +250,9 @@ class LSM6DSV16X { LSM6DSV16XStatusTypeDef FIFO_Get_Gravity_Vector(float *data); LSM6DSV16XStatusTypeDef FIFO_Get_Game_Vector(float *data); LSM6DSV16XStatusTypeDef FIFO_Get_GBias_Vector(float *data); + LSM6DSV16XStatusTypeDef FIFO_Get_Timestamp(uint32_t *timestamp); + LSM6DSV16XStatusTypeDef FIFO_Set_Timestamp_Decimation(uint8_t decimation); + LSM6DSV16XStatusTypeDef Enable_Timestamp(); LSM6DSV16XStatusTypeDef QVAR_Enable(); LSM6DSV16XStatusTypeDef QVAR_GetStatus(uint8_t *val); diff --git a/src/sensors/lsm6dsv16xsensor.cpp b/src/sensors/lsm6dsv16xsensor.cpp index 97e6d2327..37872d9e8 100644 --- a/src/sensors/lsm6dsv16xsensor.cpp +++ b/src/sensors/lsm6dsv16xsensor.cpp @@ -77,7 +77,8 @@ void LSM6DSV16XSensor::motionSetup() { imu.Enable_6D_Orientation(LSM6DSV16X_INT2_PIN); uint8_t isFaceDown; - imu.Get_6D_Orientation_ZL(&isFaceDown); + imu.Get_6D_Orientation_ZL(&isFaceDown + ); // IMU rotation could be different (IMU upside down) #ifndef LSM6DSV16X_NO_SELF_TEST_ON_FACEDOWN if (isFaceDown) { @@ -97,7 +98,7 @@ void LSM6DSV16XSensor::motionSetup() { m_Logger.info("Connected to %s on 0x%02x", getIMUNameByType(sensorType), addr); - uint8_t status = 0; + int8_t status = 0; status |= imu.begin(); @@ -115,15 +116,26 @@ void LSM6DSV16XSensor::motionSetup() { // Set data rate status |= imu.Set_X_ODR(LSM6DSV16X_FIFO_DATA_RATE); status |= imu.Set_G_ODR(LSM6DSV16X_FIFO_DATA_RATE); - status |= imu.Set_SFLP_ODR(LSM6DSV16X_FIFO_DATA_RATE); - status |= imu.FIFO_Set_X_BDR(60); + + status + |= imu.Set_SFLP_ODR(LSM6DSV16X_FIFO_DATA_RATE); // Linear accel or full fusion + status |= imu.FIFO_Set_X_BDR(LSM6DSV16X_FIFO_DATA_RATE); + +#ifdef LSM6DSV16X_ESP_FUSION + status |= imu.FIFO_Set_G_BDR(LSM6DSV16X_FIFO_DATA_RATE); +#endif + + status |= imu.FIFO_Set_Timestamp_Decimation( + lsm6dsv16x_fifo_timestamp_batch_t::LSM6DSV16X_TMSTMP_DEC_1 + ); // Enable IMU + status |= imu.Enable_Timestamp(); status |= imu.Enable_X(); status |= imu.Enable_G(); - status |= imu.Set_X_Filter_Mode(1, LSM6DSV16X_XL_LIGHT); - status |= imu.Set_G_Filter_Mode(1, LSM6DSV16X_XL_LIGHT); + // status |= imu.Set_X_Filter_Mode(1, LSM6DSV16X_XL_LIGHT); + // status |= imu.Set_G_Filter_Mode(1, LSM6DSV16X_XL_LIGHT); // status |= imu.Set_X_Filter_Mode(0, LSM6DSV16X_XL_ULTRA_LIGHT); // status |= imu.Set_G_Filter_Mode(0, LSM6DSV16X_XL_ULTRA_LIGHT); @@ -131,21 +143,50 @@ void LSM6DSV16XSensor::motionSetup() { // Set FIFO mode to "continuous", so old data gets thrown away status |= imu.FIFO_Set_Mode(LSM6DSV16X_STREAM_MODE); - // Enable Fusion - status |= imu.FIFO_Set_SFLP_Batch(true, true, true); +#if defined(LSM6DSV16X_ONBOARD_FUSION) \ + && defined(LSM6DSV16X_ESP_FUSION \ + ) // Group all the data together //set the watermark level here for nrf sleep + status |= imu.FIFO_Set_SFLP_Batch( + true, + true, + false + ); // Calculate Linear acceleration works great on imu + status |= imu.Enable_Game_Rotation(); + + // Calibration + if (isFaceDown) { + startCalibration(0); // can not calibrate onboard fusion + } + loadIMUCalibration(); // accel into imu and gryo into ram +#elif defined(LSM6DSV16X_ONBOARD_FUSION) + status |= imu.FIFO_Set_SFLP_Batch( + true, + true, + false + ); // Calculate Linear acceleration works great on imu status |= imu.Enable_Game_Rotation(); +#elif defined(LSM6DSV16X_ESP_FUSION) + status |= imu.FIFO_Set_SFLP_Batch( + false, + true, + false + ); // Calculate Linear acceleration works great on imu // Calibration if (isFaceDown) { - startCalibration(0); + startCalibration(0); // can not calibrate onboard fusion } + loadIMUCalibration(); // accel into imu and gryo into ram +#endif + status |= imu.Disable_6D_Orientation(); // status |= imu.beginPreconfigured(); #ifdef LSM6DSV16X_INTERRUPT attachInterrupt(m_IntPin, interruptHandler, RISING); - status |= imu.Enable_Single_Tap_Detection(LSM6DSV16X_INT1_PIN); + status |= imu.Enable_Single_Tap_Detection(LSM6DSV16X_INT1_PIN + ); // Tap requires an interrupt status |= imu.Enable_Double_Tap_Detection(LSM6DSV16X_INT1_PIN); #endif // LSM6DSV16X_INTERRUPT @@ -159,7 +200,6 @@ void LSM6DSV16XSensor::motionSetup() { return; } - lastReset = 0; lastData = millis(); working = true; configured = true; @@ -223,6 +263,22 @@ void LSM6DSV16XSensor::motionLoop() { return; } +#if defined(LSM6DSV16X_ONBOARD_FUSION) \ + && defined(LSM6DSV16X_ESP_FUSION \ + ) // Group all the data together //set the watermark level here for nrf sleep + if (fifo_samples < 5) { // X BDR, G BDR, Game, Gravity, Timestamp + return; + } +#elif defined(LSM6DSV16X_ONBOARD_FUSION) + if (fifo_samples < 4) { // X BDR, GAME and Gravity, Timestamp + return; + } +#elif defined(LSM6DSV16X_ESP_FUSION) + if (fifo_samples < 4) { // X BDR, G BDR, Gravity, Timestamp + return; + } +#endif + for (uint16_t i = 0; i < fifo_samples; i++) { uint8_t tag; if (imu.FIFO_Get_Tag(&tag) != LSM6DSV16X_OK) { @@ -235,54 +291,52 @@ void LSM6DSV16XSensor::motionLoop() { } switch (tag) { - case lsm6dsv16x_fifo_out_raw_t::LSM6DSV16X_XL_NC_TAG: { // accel -#if SEND_ACCELERATION - int32_t rawAccelerations[3]; - if (imu.FIFO_Get_X_Axes(rawAccelerations) != LSM6DSV16X_OK) { + case lsm6dsv16x_fifo_out_raw_t::LSM6DSV16X_TIMESTAMP_TAG: { + if (i != 0) { + return; // Group data together + } + previousDataTime = currentDataTime; + if (imu.FIFO_Get_Timestamp(¤tDataTime)) { m_Logger.error( - "Failed to get accelerometer data on %s at address 0x%02x", + "Failed to get timestamp data on %s at address 0x%02x", getIMUNameByType(sensorType), addr ); continue; } - acceleration[0] = (rawAccelerations[0] / mgPerG) - gravity[0]; - acceleration[1] = (rawAccelerations[1] / mgPerG) - gravity[1]; - acceleration[2] = (rawAccelerations[2] / mgPerG) - gravity[2]; - acceleration[0] *= CONST_EARTH_GRAVITY; - acceleration[1] *= CONST_EARTH_GRAVITY; - acceleration[2] *= CONST_EARTH_GRAVITY; - - newAcceleration = true; -#endif // SEND_ACCELERATION + + // newTemperature = false; + newFusedGameRotation = false; + newRawAcceleration = false; + newRawGyro = false; + newGravityVector = false; + + m_Logger.info("timestamp: %d", currentDataTime); break; } - case lsm6dsv16x_fifo_out_raw_t::LSM6DSV16X_SFLP_GAME_ROTATION_VECTOR_TAG: { - float data[3]; - if (imu.FIFO_Get_Game_Vector(data) != LSM6DSV16X_OK) { + + case lsm6dsv16x_fifo_out_raw_t::LSM6DSV16X_XL_NC_TAG: { // accel +#if SEND_ACCELERATION || defined(LSM6DSV16X_ESP_FUSION) + int32_t acceleration[3]; + if (imu.FIFO_Get_X_Axes(acceleration) != LSM6DSV16X_OK) { m_Logger.error( - "Failed to get game rotation vector on %s at address 0x%02x", + "Failed to get accelerometer data on %s at address 0x%02x", getIMUNameByType(sensorType), addr ); continue; } + rawAcceleration[0] = (rawAcceleration[0] / mgPerG); + rawAcceleration[1] = (rawAcceleration[1] / mgPerG); + rawAcceleration[2] = (rawAcceleration[2] / mgPerG); - fusedRotation = fusedRotationToQuaternion(data[0], data[1], data[2]) - * sensorOffset; - - lastReset = 0; - lastData = millis(); - - if (ENABLE_INSPECTION || !OPTIMIZE_UPDATES - || !lastFusedRotationSent.equalsWithEpsilon(fusedRotation)) { - newFusedRotation = true; - lastFusedRotationSent = fusedRotation; - } + newRawAcceleration = true; + m_Logger.info("accel"); +#endif // SEND_ACCELERATION break; } case lsm6dsv16x_fifo_out_raw_t::LSM6DSV16X_SFLP_GRAVITY_VECTOR_TAG: { - if (imu.FIFO_Get_Gravity_Vector(gravity) != LSM6DSV16X_OK) { + if (imu.FIFO_Get_Gravity_Vector(gravityVector) != LSM6DSV16X_OK) { m_Logger.error( "Failed to get gravity vector on %s at address 0x%02x", getIMUNameByType(sensorType), @@ -290,35 +344,91 @@ void LSM6DSV16XSensor::motionLoop() { ); continue; } - gravity[0] /= mgPerG; - gravity[1] /= mgPerG; - gravity[2] /= mgPerG; + gravityVector[0] /= mgPerG; + gravityVector[1] /= mgPerG; + gravityVector[2] /= mgPerG; +#ifdef LSM6DSV16X_ESP_FUSION + gravityVector[0] + -= accelerationOffset[0]; // The SFLP block does not use the + // accelerometer calibration + gravityVector[1] -= accelerationOffset[1]; + gravityVector[2] -= accelerationOffset[2]; +#endif + newGravityVector = true; + m_Logger.info("gravity"); break; } - case lsm6dsv16x_fifo_out_raw_t::LSM6DSV16X_SFLP_GYROSCOPE_BIAS_TAG: { - float gbias[3]; - if (imu.FIFO_Get_GBias_Vector(gbias) != LSM6DSV16X_OK) { + +#ifdef LSM6DSV16X_ESP_FUSION + case lsm6dsv16x_fifo_out_raw_t::LSM6DSV16X_GY_NC_TAG: { + int32_t angularVelocity[3]; + if (imu.FIFO_Get_G_Axes(angularVelocity) != LSM6DSV16X_OK) { m_Logger.error( - "Failed to get gyro bias on %s at address 0x%02x", + "Failed to get accelerometer data on %s at address 0x%02x", getIMUNameByType(sensorType), addr ); continue; } + rawGyro[0] = angularVelocity[0] / mdpsPerDps; + rawGyro[1] = angularVelocity[1] / mdpsPerDps; + rawGyro[2] = angularVelocity[2] / mdpsPerDps; + + newRawGyro = true; + m_Logger.info("gyro"); + break; + } +#endif - gbias[0] /= mdpsPerDps; - gbias[1] /= mdpsPerDps; - gbias[2] /= mdpsPerDps; +#ifdef LSM6DSV16X_ONBOARD_FUSION + case lsm6dsv16x_fifo_out_raw_t::LSM6DSV16X_SFLP_GAME_ROTATION_VECTOR_TAG: { + if (imu.FIFO_Get_Game_Vector(fusedGameRotation) != LSM6DSV16X_OK) { + m_Logger.error( + "Failed to get game rotation vector on %s at address 0x%02x", + getIMUNameByType(sensorType), + addr + ); + continue; + } - // printf("\nx: %f, y: %f, z: %f", gbias[0], gbias[1], gbias[2]); + newFusedGameRotation = true; + m_Logger.info("game"); break; } +#endif + default: { // We don't use the data so remove from fifo uint8_t data[6]; imu.FIFO_Get_Data(data); } } } + m_Logger.info("\n\n"); + + if (newRawAcceleration && newGravityVector) { + acceleration[0] + *= (rawAcceleration[0] - gravityVector[0]) * CONST_EARTH_GRAVITY; + acceleration[1] + *= (rawAcceleration[1] - gravityVector[1]) * CONST_EARTH_GRAVITY; + acceleration[2] + *= (rawAcceleration[2] - gravityVector[2]) * CONST_EARTH_GRAVITY; + + newAcceleration = true; + } + + if (newFusedGameRotation) { + fusedRotation = fusedRotationToQuaternion( + fusedGameRotation[0], + fusedGameRotation[1], + fusedGameRotation[2] + ); + if (ENABLE_INSPECTION || !OPTIMIZE_UPDATES + || !lastFusedRotationSent.equalsWithEpsilon(fusedRotation)) { + newFusedRotation = true; + lastFusedRotationSent = fusedRotation; + } + lastData = millis(); + } } SensorStatus LSM6DSV16XSensor::getSensorState() { @@ -360,6 +470,17 @@ LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::runSelfTest() { return LSM6DSV16X_OK; } +LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::loadIMUCalibration() { + int8_t status = 0; + status |= imu.Enable_X_User_Offset(); + status |= imu.Set_X_User_Offset( + accelerationOffset[0], + accelerationOffset[1], + accelerationOffset[2] + ); + return (LSM6DSV16XStatusTypeDef)status; +} + void LSM6DSV16XSensor::sendData() { if (newFusedRotation) { newFusedRotation = false; @@ -411,137 +532,159 @@ void LSM6DSV16XSensor::startCalibration(int calibrationType) { // give warning if temp is low ? (is this needed)? m_Logger.info("Leave still for 30 seconds"); delay(2000); - - double gbiasAvg[] = {0, 0, 0}; - float startingQuat[3]; - double quatDiffAvg[] = {0, 0, 0}; - uint16_t dataCountGbias = 0; - uint16_t dataCountQuatDiff = 0; - - while (dataCountGbias < 4000 && dataCountQuatDiff < 4000) { - uint16_t fifo_samples = 0; - if (imu.FIFO_Get_Num_Samples(&fifo_samples) == LSM6DSV16X_ERROR) { - m_Logger.error( - "Error getting number of samples in FIFO on %s at address 0x%02x", - getIMUNameByType(sensorType), - addr - ); - return; - } - - for (uint16_t i = 0; i < fifo_samples; i++) { - uint8_t tag; - if (imu.FIFO_Get_Tag(&tag) != LSM6DSV16X_OK) { + /* + double gbiasAvg[] = {0, 0, 0}; + float startingQuat[3]; + double quatDiffAvg[] = {0, 0, 0}; + uint16_t dataCountGbias = 0; + uint16_t dataCountQuatDiff = 0; + + while (dataCountGbias < 4000 && dataCountQuatDiff < 4000) { + uint16_t fifo_samples = 0; + if (imu.FIFO_Get_Num_Samples(&fifo_samples) == LSM6DSV16X_ERROR) { m_Logger.error( - "Failed to get FIFO data tag on %s at address 0x%02x", + "Error getting number of samples in FIFO on %s at address 0x%02x", getIMUNameByType(sensorType), addr ); - continue; + return; } - switch (tag) { - case lsm6dsv16x_fifo_out_raw_t::LSM6DSV16X_SFLP_GYROSCOPE_BIAS_TAG: { - if (dataCountGbias < 100) { - dataCountGbias++; - uint8_t data[6]; - imu.FIFO_Get_Data(data); + for (uint16_t i = 0; i < fifo_samples; i++) { + uint8_t tag; + if (imu.FIFO_Get_Tag(&tag) != LSM6DSV16X_OK) { + m_Logger.error( + "Failed to get FIFO data tag on %s at address 0x%02x", + getIMUNameByType(sensorType), + addr + ); + continue; + } + + switch (tag) { + case lsm6dsv16x_fifo_out_raw_t::LSM6DSV16X_XL_NC_TAG: { // accel + int32_t rawAccelerations[3]; + if (imu.FIFO_Get_X_Axes(rawAccelerations) != LSM6DSV16X_OK) { + m_Logger.error( + "Failed to get accelerometer data on %s at address + 0x%02x", getIMUNameByType(sensorType), addr + ); + continue; + } + acceleration[0] = (rawAccelerations[0] / mgPerG) - gravity[0]; + acceleration[1] = (rawAccelerations[1] / mgPerG) - gravity[1]; + acceleration[2] = (rawAccelerations[2] / mgPerG) - gravity[2]; + acceleration[0] *= CONST_EARTH_GRAVITY; + acceleration[1] *= CONST_EARTH_GRAVITY; + acceleration[2] *= CONST_EARTH_GRAVITY; + + // m_Logger.info( + // "Acceleration x: %f, y: %f, z: %f", + // acceleration[0], + // acceleration[1], + // acceleration[2] + //); break; } - float gbias[3]; - if (imu.FIFO_Get_GBias_Vector(gbias) != LSM6DSV16X_OK) { - m_Logger.error( - "Failed to get gyro bias on %s at address 0x%02x", - getIMUNameByType(sensorType), - addr - ); - continue; - } - - gbiasAvg[0] += gbias[0] / mdpsPerDps; - gbiasAvg[1] += gbias[1] / mdpsPerDps; - gbiasAvg[2] += gbias[2] / mdpsPerDps; - dataCountGbias++; - break; - } - case lsm6dsv16x_fifo_out_raw_t:: - LSM6DSV16X_SFLP_GAME_ROTATION_VECTOR_TAG: { - if (dataCountQuatDiff < 99) { - dataCountQuatDiff++; - uint8_t data[6]; - imu.FIFO_Get_Data(data); + case lsm6dsv16x_fifo_out_raw_t::LSM6DSV16X_SFLP_GYROSCOPE_BIAS_TAG: + { if (dataCountGbias < 100) { dataCountGbias++; uint8_t data[6]; + imu.FIFO_Get_Data(data); + break; + } + float gbias[3]; + if (imu.FIFO_Get_GBias_Vector(gbias) != LSM6DSV16X_OK) { + m_Logger.error( + "Failed to get gyro bias on %s at address 0x%02x", + getIMUNameByType(sensorType), + addr + ); + continue; + } + + gbiasAvg[0] += gbias[0] / mdpsPerDps; + gbiasAvg[1] += gbias[1] / mdpsPerDps; + gbiasAvg[2] += gbias[2] / mdpsPerDps; + dataCountGbias++; break; } - - float data[3]; - if (imu.FIFO_Get_Game_Vector(data) != LSM6DSV16X_OK) { - m_Logger.error( - "Failed to get game rotation vector on %s at address " - "0x%02x", - getIMUNameByType(sensorType), - addr + case lsm6dsv16x_fifo_out_raw_t:: + LSM6DSV16X_SFLP_GAME_ROTATION_VECTOR_TAG: { + if (dataCountQuatDiff < 99) { + dataCountQuatDiff++; + uint8_t data[6]; + imu.FIFO_Get_Data(data); + break; + } + + float data[3]; + if (imu.FIFO_Get_Game_Vector(data) != LSM6DSV16X_OK) { + m_Logger.error( + "Failed to get game rotation vector on %s at address " + "0x%02x", + getIMUNameByType(sensorType), + addr + ); + continue; + } + + if (dataCountQuatDiff == 99) { + startingQuat[0] = data[0]; + startingQuat[1] = data[1]; + startingQuat[2] = data[2]; + } + + quatDiffAvg[0] += (startingQuat[0] - data[0]); + quatDiffAvg[1] += (startingQuat[1] - data[1]); + quatDiffAvg[2] += (startingQuat[2] - data[2]); + + m_Logger.info( + "Game Diff X: %f, Y: %f, Z: %f", + (startingQuat[0] - data[0]), + (startingQuat[1] - data[1]), + (startingQuat[2] - data[2]) ); - continue; - } - - if (dataCountQuatDiff == 99) { startingQuat[0] = data[0]; startingQuat[1] = data[1]; startingQuat[2] = data[2]; + dataCountQuatDiff++; + break; + } + default: { // We don't use the data so remove from fifo + uint8_t data[6]; + imu.FIFO_Get_Data(data); } - - quatDiffAvg[0] += (startingQuat[0] - data[0]); - quatDiffAvg[1] += (startingQuat[1] - data[1]); - quatDiffAvg[2] += (startingQuat[2] - data[2]); - - - printf( - "\nX: %f, Y: %f, Z: %f", - (startingQuat[0] - data[0]), - (startingQuat[1] - data[1]), - (startingQuat[2] - data[2]) - ); - startingQuat[0] = data[0]; - startingQuat[1] = data[1]; - startingQuat[2] = data[2]; - dataCountQuatDiff++; - break; - } - default: { // We don't use the data so remove from fifo - uint8_t data[6]; - imu.FIFO_Get_Data(data); } } } - } - m_Logger.info("Gbias x: %f, y: %f, z: %f", gbiasAvg[0], gbiasAvg[1], gbiasAvg[2]); - m_Logger.info("quatAvg x: %f, y: %f, z: %f", quatDiffAvg[0], quatDiffAvg[1], quatDiffAvg[2]); - dataCountGbias -= 100; - dataCountQuatDiff -= 100; - - gbiasAvg[0] /= dataCountGbias; - gbiasAvg[1] /= dataCountGbias; - gbiasAvg[2] /= dataCountGbias; - - quatDiffAvg[0] /= dataCountGbias; - quatDiffAvg[1] /= dataCountGbias; - quatDiffAvg[2] /= dataCountGbias; - - m_Logger.info( - "Gbias Calibration Data, X: %f, Y: %f, Z: %f", - gbiasAvg[0], - gbiasAvg[1], - gbiasAvg[2] - ); - - m_Logger.info( - "Quat Diff Calibration Data, X: %f, Y: %f, Z: %f", - quatDiffAvg[0], - quatDiffAvg[1], - quatDiffAvg[2] - ); + m_Logger.info("Gbias x: %f, y: %f, z: %f", gbiasAvg[0], gbiasAvg[1], + gbiasAvg[2]); m_Logger.info( "quatAvg x: %f, y: %f, z: %f", quatDiffAvg[0], + quatDiffAvg[1], + quatDiffAvg[2] + ); + dataCountGbias -= 100; + dataCountQuatDiff -= 100; + + gbiasAvg[0] /= dataCountGbias; + gbiasAvg[1] /= dataCountGbias; + gbiasAvg[2] /= dataCountGbias; + + quatDiffAvg[0] /= dataCountGbias; + quatDiffAvg[1] /= dataCountGbias; + quatDiffAvg[2] /= dataCountGbias; + + m_Logger.info( + "Gbias Calibration Data, X: %f, Y: %f, Z: %f", + gbiasAvg[0], + gbiasAvg[1], + gbiasAvg[2] + ); - // imu.Set_SFLP_GBIAS(0.00, 0.245062, 0.150119); - // imu.Set_SFLP_GBIAS(gbiasAvg[0], gbiasAvg[1], gbiasAvg[2]); - delay(10000); + m_Logger.info( + "Quat Diff Calibration Data, X: %f, Y: %f, Z: %f", + quatDiffAvg[0], + quatDiffAvg[1], + quatDiffAvg[2] + ); + delay(10000); + */ } diff --git a/src/sensors/lsm6dsv16xsensor.h b/src/sensors/lsm6dsv16xsensor.h index 895885da1..18ea433b5 100644 --- a/src/sensors/lsm6dsv16xsensor.h +++ b/src/sensors/lsm6dsv16xsensor.h @@ -50,12 +50,19 @@ // #define SELF_TEST_ON_INIT // #define REINIT_ON_FAILURE // #define LSM6DSV16X_INTERRUPT +// #define LSM6DSV16X_NO_SELF_TEST_ON_FACEDOWN #ifdef REINIT_ON_FAILURE #define REINIT_RETRY_MAX_ATTEMPTS 5 #undef SELF_TEST_ON_INIT #endif + + +#define LSM6DSV16X_ONBOARD_FUSION +#define LSM6DSV16X_ESP_FUSION + + class LSM6DSV16XSensor : public Sensor { public: LSM6DSV16XSensor( @@ -77,16 +84,37 @@ class LSM6DSV16XSensor : public Sensor { private: Quat fusedRotationToQuaternion(float x, float y, float z); LSM6DSV16XStatusTypeDef runSelfTest(); + LSM6DSV16XStatusTypeDef loadIMUCalibration(); LSM6DSV16X imu; uint8_t m_IntPin; uint8_t tap = 0; unsigned long lastData = 0; - uint8_t lastReset = 0; float temperature = 0; bool newTemperature = false; uint32_t lastTempRead = 0; - float gravity[3]; + float gravityVector[3]; + bool newGravityVector = false; + float rawAcceleration[3]; //not needed with only imu fused + float accelerationOffset[3] = {0, 0, 0}; //Put these in stored calibration + bool newRawAcceleration = false; + +#ifdef LSM6DSV16X_ONBOARD_FUSION + float fusedGameRotation[3]; + Quat previousGameRotation; + bool newFusedGameRotation = false; +#endif + +#ifdef LSM6DSV16X_ESP_FUSION + int32_t rawGyro[3]; + float gyroOffset[3] = {0, 0, 0}; //Put these in stored calibration + bool newRawGyro = false; + Quat previousEspRotation; + uint32_t previousDataTime = 0; + uint32_t currentDataTime = 0; + uint8_t previousTag = 0; +#endif + #ifdef REINIT_ON_FAILURE uint8_t reinitOnFailAttempts = 0; From 73f7caad2d3e99c7ab13fa40af90735d0f6f6a79 Mon Sep 17 00:00:00 2001 From: Carter Meyers Date: Wed, 16 Aug 2023 20:24:18 -0700 Subject: [PATCH 31/60] esp fusion works just not well --- src/sensors/lsm6dsv16xsensor.cpp | 134 ++++++++++++++++--------------- src/sensors/lsm6dsv16xsensor.h | 8 +- 2 files changed, 77 insertions(+), 65 deletions(-) diff --git a/src/sensors/lsm6dsv16xsensor.cpp b/src/sensors/lsm6dsv16xsensor.cpp index 37872d9e8..c51d76c43 100644 --- a/src/sensors/lsm6dsv16xsensor.cpp +++ b/src/sensors/lsm6dsv16xsensor.cpp @@ -44,10 +44,11 @@ LSM6DSV16XSensor::LSM6DSV16XSensor( uint8_t sclPin, uint8_t sdaPin, uint8_t intPin -) - : Sensor("LSM6DSV16XSensor", type, id, address, rotation, sclPin, sdaPin) - , imu(&Wire, addr << 1) // We shift the address left 1 to work with the library - , m_IntPin(intPin){}; +) : Sensor("LSM6DSV16XSensor", type, id, address, rotation, sclPin, sdaPin), + imu(&Wire, addr << 1), // We shift the address left 1 to work with the library + m_IntPin(intPin), + sfusion(1.0f, -1.0f, -1.0f) + {}; void LSM6DSV16XSensor::motionSetup() { uint8_t deviceId = 0; @@ -77,8 +78,7 @@ void LSM6DSV16XSensor::motionSetup() { imu.Enable_6D_Orientation(LSM6DSV16X_INT2_PIN); uint8_t isFaceDown; - imu.Get_6D_Orientation_ZL(&isFaceDown - ); // IMU rotation could be different (IMU upside down) + imu.Get_6D_Orientation_ZL(&isFaceDown); //TODO: IMU rotation could be different (IMU upside down then isFaceUp) #ifndef LSM6DSV16X_NO_SELF_TEST_ON_FACEDOWN if (isFaceDown) { @@ -117,17 +117,14 @@ void LSM6DSV16XSensor::motionSetup() { status |= imu.Set_X_ODR(LSM6DSV16X_FIFO_DATA_RATE); status |= imu.Set_G_ODR(LSM6DSV16X_FIFO_DATA_RATE); - status - |= imu.Set_SFLP_ODR(LSM6DSV16X_FIFO_DATA_RATE); // Linear accel or full fusion + status |= imu.Set_SFLP_ODR(LSM6DSV16X_FIFO_DATA_RATE); // Linear accel or full fusion status |= imu.FIFO_Set_X_BDR(LSM6DSV16X_FIFO_DATA_RATE); #ifdef LSM6DSV16X_ESP_FUSION status |= imu.FIFO_Set_G_BDR(LSM6DSV16X_FIFO_DATA_RATE); #endif - status |= imu.FIFO_Set_Timestamp_Decimation( - lsm6dsv16x_fifo_timestamp_batch_t::LSM6DSV16X_TMSTMP_DEC_1 - ); + status |= imu.FIFO_Set_Timestamp_Decimation(lsm6dsv16x_fifo_timestamp_batch_t::LSM6DSV16X_TMSTMP_DEC_1); // Enable IMU status |= imu.Enable_Timestamp(); @@ -143,14 +140,9 @@ void LSM6DSV16XSensor::motionSetup() { // Set FIFO mode to "continuous", so old data gets thrown away status |= imu.FIFO_Set_Mode(LSM6DSV16X_STREAM_MODE); -#if defined(LSM6DSV16X_ONBOARD_FUSION) \ - && defined(LSM6DSV16X_ESP_FUSION \ - ) // Group all the data together //set the watermark level here for nrf sleep - status |= imu.FIFO_Set_SFLP_Batch( - true, - true, - false - ); // Calculate Linear acceleration works great on imu + +#if defined(LSM6DSV16X_ONBOARD_FUSION) && defined(LSM6DSV16X_ESP_FUSION) + status |= imu.FIFO_Set_SFLP_Batch(true, true, false); status |= imu.Enable_Game_Rotation(); // Calibration @@ -159,18 +151,10 @@ void LSM6DSV16XSensor::motionSetup() { } loadIMUCalibration(); // accel into imu and gryo into ram #elif defined(LSM6DSV16X_ONBOARD_FUSION) - status |= imu.FIFO_Set_SFLP_Batch( - true, - true, - false - ); // Calculate Linear acceleration works great on imu + status |= imu.FIFO_Set_SFLP_Batch(true, true, false); status |= imu.Enable_Game_Rotation(); #elif defined(LSM6DSV16X_ESP_FUSION) - status |= imu.FIFO_Set_SFLP_Batch( - false, - true, - false - ); // Calculate Linear acceleration works great on imu + status |= imu.FIFO_Set_SFLP_Batch(false, true, false); // Calibration if (isFaceDown) { @@ -185,8 +169,7 @@ void LSM6DSV16XSensor::motionSetup() { #ifdef LSM6DSV16X_INTERRUPT attachInterrupt(m_IntPin, interruptHandler, RISING); - status |= imu.Enable_Single_Tap_Detection(LSM6DSV16X_INT1_PIN - ); // Tap requires an interrupt + status |= imu.Enable_Single_Tap_Detection(LSM6DSV16X_INT1_PIN); // Tap requires an interrupt status |= imu.Enable_Double_Tap_Detection(LSM6DSV16X_INT1_PIN); #endif // LSM6DSV16X_INTERRUPT @@ -263,9 +246,8 @@ void LSM6DSV16XSensor::motionLoop() { return; } -#if defined(LSM6DSV16X_ONBOARD_FUSION) \ - && defined(LSM6DSV16X_ESP_FUSION \ - ) // Group all the data together //set the watermark level here for nrf sleep +// Group all the data together //set the watermark level here for nrf sleep +#if defined(LSM6DSV16X_ONBOARD_FUSION) && defined(LSM6DSV16X_ESP_FUSION) if (fifo_samples < 5) { // X BDR, G BDR, Game, Gravity, Timestamp return; } @@ -311,12 +293,13 @@ void LSM6DSV16XSensor::motionLoop() { newRawGyro = false; newGravityVector = false; - m_Logger.info("timestamp: %d", currentDataTime); + // m_Logger.info("timestamp: %d", currentDataTime); break; } - case lsm6dsv16x_fifo_out_raw_t::LSM6DSV16X_XL_NC_TAG: { // accel #if SEND_ACCELERATION || defined(LSM6DSV16X_ESP_FUSION) + case lsm6dsv16x_fifo_out_raw_t::LSM6DSV16X_XL_NC_TAG: { // accel + int32_t acceleration[3]; if (imu.FIFO_Get_X_Axes(acceleration) != LSM6DSV16X_OK) { m_Logger.error( @@ -326,15 +309,16 @@ void LSM6DSV16XSensor::motionLoop() { ); continue; } - rawAcceleration[0] = (rawAcceleration[0] / mgPerG); - rawAcceleration[1] = (rawAcceleration[1] / mgPerG); - rawAcceleration[2] = (rawAcceleration[2] / mgPerG); + rawAcceleration[0] = (acceleration[0] / mgPerG); + rawAcceleration[1] = (acceleration[1] / mgPerG); + rawAcceleration[2] = (acceleration[2] / mgPerG); newRawAcceleration = true; - m_Logger.info("accel"); -#endif // SEND_ACCELERATION + // m_Logger.info("accel X: %f, Y: %f, Z: %f", rawAcceleration[0], rawAcceleration[1], rawAcceleration[2]); break; } +#endif // SEND_ACCELERATION + case lsm6dsv16x_fifo_out_raw_t::LSM6DSV16X_SFLP_GRAVITY_VECTOR_TAG: { if (imu.FIFO_Get_Gravity_Vector(gravityVector) != LSM6DSV16X_OK) { m_Logger.error( @@ -347,15 +331,15 @@ void LSM6DSV16XSensor::motionLoop() { gravityVector[0] /= mgPerG; gravityVector[1] /= mgPerG; gravityVector[2] /= mgPerG; + #ifdef LSM6DSV16X_ESP_FUSION - gravityVector[0] - -= accelerationOffset[0]; // The SFLP block does not use the - // accelerometer calibration - gravityVector[1] -= accelerationOffset[1]; - gravityVector[2] -= accelerationOffset[2]; + // The SFLP block does not use the accelerometer calibration + gravityVector[0] += accelerationOffset[0]; + gravityVector[1] += accelerationOffset[1]; + gravityVector[2] += accelerationOffset[2]; #endif + // m_Logger.info("gravity X: %f, Y: %f, Z: %f", gravityVector[0], gravityVector[1], gravityVector[2]); newGravityVector = true; - m_Logger.info("gravity"); break; } @@ -374,8 +358,12 @@ void LSM6DSV16XSensor::motionLoop() { rawGyro[1] = angularVelocity[1] / mdpsPerDps; rawGyro[2] = angularVelocity[2] / mdpsPerDps; + rawGyro[0] += gyroOffset[0]; + rawGyro[1] += gyroOffset[1]; + rawGyro[2] += gyroOffset[2]; + newRawGyro = true; - m_Logger.info("gyro"); + m_Logger.info("gyro X: %f, Y: %f, Z: %f", rawGyro[0], rawGyro[1], rawGyro[2]); break; } #endif @@ -392,7 +380,7 @@ void LSM6DSV16XSensor::motionLoop() { } newFusedGameRotation = true; - m_Logger.info("game"); + // m_Logger.info("game"); break; } #endif @@ -403,32 +391,55 @@ void LSM6DSV16XSensor::motionLoop() { } } } - m_Logger.info("\n\n"); if (newRawAcceleration && newGravityVector) { - acceleration[0] - *= (rawAcceleration[0] - gravityVector[0]) * CONST_EARTH_GRAVITY; - acceleration[1] - *= (rawAcceleration[1] - gravityVector[1]) * CONST_EARTH_GRAVITY; - acceleration[2] - *= (rawAcceleration[2] - gravityVector[2]) * CONST_EARTH_GRAVITY; + acceleration[0] = (rawAcceleration[0] - gravityVector[0]) * CONST_EARTH_GRAVITY; + acceleration[1] = (rawAcceleration[1] - gravityVector[1]) * CONST_EARTH_GRAVITY; + acceleration[2] = (rawAcceleration[2] - gravityVector[2]) * CONST_EARTH_GRAVITY; newAcceleration = true; + newGravityVector = false; } +#if defined(LSM6DSV16X_ONBOARD_FUSION) && defined(LSM6DSV16X_ESP_FUSION) + //TODO: fusion of fusion stuff + fusedRotation = sfusion.getQuaternionQuat(); + newFusedRotation = true; + lastFusedRotationSent = fusedRotation; +#elif defined(LSM6DSV16X_ONBOARD_FUSION) if (newFusedGameRotation) { - fusedRotation = fusedRotationToQuaternion( + Quat fusedRotationGame = fusedRotationToQuaternion( fusedGameRotation[0], fusedGameRotation[1], fusedGameRotation[2] ); - if (ENABLE_INSPECTION || !OPTIMIZE_UPDATES - || !lastFusedRotationSent.equalsWithEpsilon(fusedRotation)) { + if (ENABLE_INSPECTION || !OPTIMIZE_UPDATES || !lastFusedRotationSent.equalsWithEpsilon(fusedRotation)) { newFusedRotation = true; lastFusedRotationSent = fusedRotation; } lastData = millis(); + newFusedGameRotation = false; + } +#elif defined(LSM6DSV16X_ESP_FUSION) + fusedRotation = sfusion.getQuaternionQuat(); + if (ENABLE_INSPECTION || !OPTIMIZE_UPDATES || !lastFusedRotationSent.equalsWithEpsilon(fusedRotation)) { + newFusedRotation = true; + lastFusedRotationSent = fusedRotation; + } + lastData = millis(); +#endif + +#ifdef LSM6DSV16X_ESP_FUSION + if (newRawAcceleration && newRawGyro) { + sfusion.update6D( + rawAcceleration, + rawGyro, + (double)(currentDataTime - previousDataTime) * LSM6DSV16X_TIMESTAMP_LSB + ); + newRawAcceleration = false; + newRawGyro = false; } +#endif } SensorStatus LSM6DSV16XSensor::getSensorState() { @@ -457,8 +468,7 @@ LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::runSelfTest() { addr ); - if (imu.Test_IMU(LSM6DSV16X_XL_ST_NEGATIVE, LSM6DSV16X_GY_ST_NEGATIVE) - == LSM6DSV16X_ERROR) { + if (imu.Test_IMU(LSM6DSV16X_XL_ST_NEGATIVE, LSM6DSV16X_GY_ST_NEGATIVE) == LSM6DSV16X_ERROR) { return LSM6DSV16X_ERROR; } m_Logger.info( @@ -517,8 +527,6 @@ void LSM6DSV16XSensor::sendData() { } void LSM6DSV16XSensor::startCalibration(int calibrationType) { - // Supported Calibrations: Accelerometer offset and Gyroscope Bias - // Currently only second is used m_Logger.info("Flip right side up in the next 5 seconds to start calibration."); delay(5000); diff --git a/src/sensors/lsm6dsv16xsensor.h b/src/sensors/lsm6dsv16xsensor.h index 18ea433b5..88252ac6a 100644 --- a/src/sensors/lsm6dsv16xsensor.h +++ b/src/sensors/lsm6dsv16xsensor.h @@ -26,6 +26,7 @@ #include "LSM6DSV16X.h" #include "sensor.h" +#include "SensorFusion.h" #ifndef LSM6DSV16X_ACCEL_MAX #define LSM6DSV16X_ACCEL_MAX 4 @@ -47,6 +48,8 @@ #define LSM6DSV16X_TEMP_READ_INTERVAL 1 #endif +#define LSM6DSV16X_TIMESTAMP_LSB 21.75e-6 + // #define SELF_TEST_ON_INIT // #define REINIT_ON_FAILURE // #define LSM6DSV16X_INTERRUPT @@ -95,7 +98,7 @@ class LSM6DSV16XSensor : public Sensor { uint32_t lastTempRead = 0; float gravityVector[3]; bool newGravityVector = false; - float rawAcceleration[3]; //not needed with only imu fused + float rawAcceleration[3]; float accelerationOffset[3] = {0, 0, 0}; //Put these in stored calibration bool newRawAcceleration = false; @@ -106,7 +109,8 @@ class LSM6DSV16XSensor : public Sensor { #endif #ifdef LSM6DSV16X_ESP_FUSION - int32_t rawGyro[3]; + SlimeVR::Sensors::SensorFusion sfusion; + float rawGyro[3]; float gyroOffset[3] = {0, 0, 0}; //Put these in stored calibration bool newRawGyro = false; Quat previousEspRotation; From 52347a449fa3c5bc933854965f4e6cdf7443d98d Mon Sep 17 00:00:00 2001 From: Carter Meyers Date: Fri, 18 Aug 2023 20:04:08 -0700 Subject: [PATCH 32/60] Fix esp fusion add gyro calibration --- src/configuration/CalibrationConfig.cpp | 2 + src/configuration/CalibrationConfig.h | 11 +- src/sensors/lsm6dsv16xsensor.cpp | 596 +++++++++++------------- src/sensors/lsm6dsv16xsensor.h | 14 +- 4 files changed, 293 insertions(+), 330 deletions(-) diff --git a/src/configuration/CalibrationConfig.cpp b/src/configuration/CalibrationConfig.cpp index 75555ad34..9e5b889b1 100644 --- a/src/configuration/CalibrationConfig.cpp +++ b/src/configuration/CalibrationConfig.cpp @@ -37,6 +37,8 @@ namespace SlimeVR { return "MPU9250"; case ICM20948: return "ICM20948"; + case LSM6DSV16X: + return "LSM6DSV16X"; default: return "UNKNOWN"; } diff --git a/src/configuration/CalibrationConfig.h b/src/configuration/CalibrationConfig.h index 5b2a3859f..cf3a9d525 100644 --- a/src/configuration/CalibrationConfig.h +++ b/src/configuration/CalibrationConfig.h @@ -76,7 +76,15 @@ namespace SlimeVR { int32_t C[3]; }; - enum CalibrationConfigType { NONE, BMI160, MPU6050, MPU9250, ICM20948 }; + struct LSM6DSV16XCalibrationConfig { + // gyroscope bias + float G_off[3]; + + // accelerometer bias + float A_off[3]; + }; + + enum CalibrationConfigType { NONE, BMI160, MPU6050, MPU9250, ICM20948, LSM6DSV16X }; const char* calibrationConfigTypeToString(CalibrationConfigType type); @@ -88,6 +96,7 @@ namespace SlimeVR { MPU6050CalibrationConfig mpu6050; MPU9250CalibrationConfig mpu9250; ICM20948CalibrationConfig icm20948; + LSM6DSV16XCalibrationConfig lsm6dsv16x; } data; }; } diff --git a/src/sensors/lsm6dsv16xsensor.cpp b/src/sensors/lsm6dsv16xsensor.cpp index c51d76c43..c081489a1 100644 --- a/src/sensors/lsm6dsv16xsensor.cpp +++ b/src/sensors/lsm6dsv16xsensor.cpp @@ -28,8 +28,6 @@ #include "lsm6dsv16xsensor.h" #include "utils.h" -void IRAM_ATTR interruptHandler() {} - #ifdef LSM6DSV16X_INTERRUPT volatile bool imuEvent = false; @@ -45,10 +43,13 @@ LSM6DSV16XSensor::LSM6DSV16XSensor( uint8_t sdaPin, uint8_t intPin ) : Sensor("LSM6DSV16XSensor", type, id, address, rotation, sclPin, sdaPin), - imu(&Wire, addr << 1), // We shift the address left 1 to work with the library - m_IntPin(intPin), - sfusion(1.0f, -1.0f, -1.0f) - {}; + imu(&Wire, addr << 1), // We shift the address left 1 to work with the library + m_IntPin(intPin), + sfusion( + 1.0f / LSM6DSV16X_FIFO_DATA_RATE, + 1.0f / LSM6DSV16X_FIFO_DATA_RATE, + -1.0f + ){}; void LSM6DSV16XSensor::motionSetup() { uint8_t deviceId = 0; @@ -76,9 +77,12 @@ void LSM6DSV16XSensor::motionSetup() { return; } - imu.Enable_6D_Orientation(LSM6DSV16X_INT2_PIN); + int8_t status = 0; + + status |= imu.Enable_6D_Orientation(LSM6DSV16X_INT2_PIN); uint8_t isFaceDown; - imu.Get_6D_Orientation_ZL(&isFaceDown); //TODO: IMU rotation could be different (IMU upside down then isFaceUp) + // TODO: IMU rotation could be different (IMU upside down then isFaceUp) + status |= imu.Get_6D_Orientation_ZL(&isFaceDown); #ifndef LSM6DSV16X_NO_SELF_TEST_ON_FACEDOWN if (isFaceDown) { @@ -98,8 +102,6 @@ void LSM6DSV16XSensor::motionSetup() { m_Logger.info("Connected to %s on 0x%02x", getIMUNameByType(sensorType), addr); - int8_t status = 0; - status |= imu.begin(); // Restore defaults @@ -124,7 +126,9 @@ void LSM6DSV16XSensor::motionSetup() { status |= imu.FIFO_Set_G_BDR(LSM6DSV16X_FIFO_DATA_RATE); #endif - status |= imu.FIFO_Set_Timestamp_Decimation(lsm6dsv16x_fifo_timestamp_batch_t::LSM6DSV16X_TMSTMP_DEC_1); + status |= imu.FIFO_Set_Timestamp_Decimation( + lsm6dsv16x_fifo_timestamp_batch_t::LSM6DSV16X_TMSTMP_DEC_1 + ); // Enable IMU status |= imu.Enable_Timestamp(); @@ -140,8 +144,7 @@ void LSM6DSV16XSensor::motionSetup() { // Set FIFO mode to "continuous", so old data gets thrown away status |= imu.FIFO_Set_Mode(LSM6DSV16X_STREAM_MODE); - -#if defined(LSM6DSV16X_ONBOARD_FUSION) && defined(LSM6DSV16X_ESP_FUSION) +#if defined(LSM6DSV16X_ONBOARD_FUSION) && defined(LSM6DSV16X_ESP_FUSION) status |= imu.FIFO_Set_SFLP_Batch(true, true, false); status |= imu.Enable_Game_Rotation(); @@ -149,18 +152,19 @@ void LSM6DSV16XSensor::motionSetup() { if (isFaceDown) { startCalibration(0); // can not calibrate onboard fusion } - loadIMUCalibration(); // accel into imu and gryo into ram + loadIMUCalibration(); #elif defined(LSM6DSV16X_ONBOARD_FUSION) status |= imu.FIFO_Set_SFLP_Batch(true, true, false); status |= imu.Enable_Game_Rotation(); #elif defined(LSM6DSV16X_ESP_FUSION) status |= imu.FIFO_Set_SFLP_Batch(false, true, false); + status |= imu.Enable_Game_Rotation(); // Calibration if (isFaceDown) { startCalibration(0); // can not calibrate onboard fusion } - loadIMUCalibration(); // accel into imu and gryo into ram + loadIMUCalibration(); #endif status |= imu.Disable_6D_Orientation(); @@ -191,6 +195,7 @@ void LSM6DSV16XSensor::motionSetup() { constexpr float mgPerG = 1000.0f; constexpr float mdpsPerDps = 1000.0f; +constexpr float dpsPerRad = 57.295779578552f; void LSM6DSV16XSensor::motionLoop() { if (millis() - lastTempRead > LSM6DSV16X_TEMP_READ_INTERVAL * 1000) { @@ -246,151 +251,12 @@ void LSM6DSV16XSensor::motionLoop() { return; } -// Group all the data together //set the watermark level here for nrf sleep -#if defined(LSM6DSV16X_ONBOARD_FUSION) && defined(LSM6DSV16X_ESP_FUSION) - if (fifo_samples < 5) { // X BDR, G BDR, Game, Gravity, Timestamp + // Group all the data together //set the watermark level for nrf sleep + if (fifo_samples < LSM6DSV16X_FIFO_FRAME_SIZE) { return; } -#elif defined(LSM6DSV16X_ONBOARD_FUSION) - if (fifo_samples < 4) { // X BDR, GAME and Gravity, Timestamp - return; - } -#elif defined(LSM6DSV16X_ESP_FUSION) - if (fifo_samples < 4) { // X BDR, G BDR, Gravity, Timestamp - return; - } -#endif - - for (uint16_t i = 0; i < fifo_samples; i++) { - uint8_t tag; - if (imu.FIFO_Get_Tag(&tag) != LSM6DSV16X_OK) { - m_Logger.error( - "Failed to get FIFO data tag on %s at address 0x%02x", - getIMUNameByType(sensorType), - addr - ); - continue; - } - - switch (tag) { - case lsm6dsv16x_fifo_out_raw_t::LSM6DSV16X_TIMESTAMP_TAG: { - if (i != 0) { - return; // Group data together - } - previousDataTime = currentDataTime; - if (imu.FIFO_Get_Timestamp(¤tDataTime)) { - m_Logger.error( - "Failed to get timestamp data on %s at address 0x%02x", - getIMUNameByType(sensorType), - addr - ); - continue; - } - - // newTemperature = false; - newFusedGameRotation = false; - newRawAcceleration = false; - newRawGyro = false; - newGravityVector = false; - - // m_Logger.info("timestamp: %d", currentDataTime); - break; - } - -#if SEND_ACCELERATION || defined(LSM6DSV16X_ESP_FUSION) - case lsm6dsv16x_fifo_out_raw_t::LSM6DSV16X_XL_NC_TAG: { // accel - - int32_t acceleration[3]; - if (imu.FIFO_Get_X_Axes(acceleration) != LSM6DSV16X_OK) { - m_Logger.error( - "Failed to get accelerometer data on %s at address 0x%02x", - getIMUNameByType(sensorType), - addr - ); - continue; - } - rawAcceleration[0] = (acceleration[0] / mgPerG); - rawAcceleration[1] = (acceleration[1] / mgPerG); - rawAcceleration[2] = (acceleration[2] / mgPerG); - - newRawAcceleration = true; - // m_Logger.info("accel X: %f, Y: %f, Z: %f", rawAcceleration[0], rawAcceleration[1], rawAcceleration[2]); - break; - } -#endif // SEND_ACCELERATION - - case lsm6dsv16x_fifo_out_raw_t::LSM6DSV16X_SFLP_GRAVITY_VECTOR_TAG: { - if (imu.FIFO_Get_Gravity_Vector(gravityVector) != LSM6DSV16X_OK) { - m_Logger.error( - "Failed to get gravity vector on %s at address 0x%02x", - getIMUNameByType(sensorType), - addr - ); - continue; - } - gravityVector[0] /= mgPerG; - gravityVector[1] /= mgPerG; - gravityVector[2] /= mgPerG; - -#ifdef LSM6DSV16X_ESP_FUSION - // The SFLP block does not use the accelerometer calibration - gravityVector[0] += accelerationOffset[0]; - gravityVector[1] += accelerationOffset[1]; - gravityVector[2] += accelerationOffset[2]; -#endif - // m_Logger.info("gravity X: %f, Y: %f, Z: %f", gravityVector[0], gravityVector[1], gravityVector[2]); - newGravityVector = true; - break; - } - -#ifdef LSM6DSV16X_ESP_FUSION - case lsm6dsv16x_fifo_out_raw_t::LSM6DSV16X_GY_NC_TAG: { - int32_t angularVelocity[3]; - if (imu.FIFO_Get_G_Axes(angularVelocity) != LSM6DSV16X_OK) { - m_Logger.error( - "Failed to get accelerometer data on %s at address 0x%02x", - getIMUNameByType(sensorType), - addr - ); - continue; - } - rawGyro[0] = angularVelocity[0] / mdpsPerDps; - rawGyro[1] = angularVelocity[1] / mdpsPerDps; - rawGyro[2] = angularVelocity[2] / mdpsPerDps; - - rawGyro[0] += gyroOffset[0]; - rawGyro[1] += gyroOffset[1]; - rawGyro[2] += gyroOffset[2]; - - newRawGyro = true; - m_Logger.info("gyro X: %f, Y: %f, Z: %f", rawGyro[0], rawGyro[1], rawGyro[2]); - break; - } -#endif - -#ifdef LSM6DSV16X_ONBOARD_FUSION - case lsm6dsv16x_fifo_out_raw_t::LSM6DSV16X_SFLP_GAME_ROTATION_VECTOR_TAG: { - if (imu.FIFO_Get_Game_Vector(fusedGameRotation) != LSM6DSV16X_OK) { - m_Logger.error( - "Failed to get game rotation vector on %s at address 0x%02x", - getIMUNameByType(sensorType), - addr - ); - continue; - } - newFusedGameRotation = true; - // m_Logger.info("game"); - break; - } -#endif - - default: { // We don't use the data so remove from fifo - uint8_t data[6]; - imu.FIFO_Get_Data(data); - } - } - } + readFifo(fifo_samples); if (newRawAcceleration && newGravityVector) { acceleration[0] = (rawAcceleration[0] - gravityVector[0]) * CONST_EARTH_GRAVITY; @@ -402,18 +268,21 @@ void LSM6DSV16XSensor::motionLoop() { } #if defined(LSM6DSV16X_ONBOARD_FUSION) && defined(LSM6DSV16X_ESP_FUSION) - //TODO: fusion of fusion stuff + // TODO: fusion of fusion stuff fusedRotation = sfusion.getQuaternionQuat(); - newFusedRotation = true; lastFusedRotationSent = fusedRotation; + newFusedRotation = true; + newFusedGameRotation = false; + lastData = millis(); #elif defined(LSM6DSV16X_ONBOARD_FUSION) if (newFusedGameRotation) { - Quat fusedRotationGame = fusedRotationToQuaternion( + fusedRotation = fusedRotationToQuaternion( fusedGameRotation[0], fusedGameRotation[1], fusedGameRotation[2] ); - if (ENABLE_INSPECTION || !OPTIMIZE_UPDATES || !lastFusedRotationSent.equalsWithEpsilon(fusedRotation)) { + if (ENABLE_INSPECTION || !OPTIMIZE_UPDATES + || !lastFusedRotationSent.equalsWithEpsilon(fusedRotation)) { newFusedRotation = true; lastFusedRotationSent = fusedRotation; } @@ -421,12 +290,12 @@ void LSM6DSV16XSensor::motionLoop() { newFusedGameRotation = false; } #elif defined(LSM6DSV16X_ESP_FUSION) - fusedRotation = sfusion.getQuaternionQuat(); - if (ENABLE_INSPECTION || !OPTIMIZE_UPDATES || !lastFusedRotationSent.equalsWithEpsilon(fusedRotation)) { - newFusedRotation = true; - lastFusedRotationSent = fusedRotation; - } - lastData = millis(); + fusedRotation = sfusion.getQuaternionQuat(); + if (ENABLE_INSPECTION || !OPTIMIZE_UPDATES || !lastFusedRotationSent.equalsWithEpsilon(fusedRotation)) { + newFusedRotation = true; + lastFusedRotationSent = fusedRotation; + } + lastData = millis(); #endif #ifdef LSM6DSV16X_ESP_FUSION @@ -481,12 +350,31 @@ LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::runSelfTest() { } LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::loadIMUCalibration() { + SlimeVR::Configuration::CalibrationConfig sensorCalibration = configuration.getCalibration(sensorId); + // If no compatible calibration data is found, the calibration data will just be zero-ed out + switch (sensorCalibration.type) { + case SlimeVR::Configuration::CalibrationConfigType::LSM6DSV16X: + m_Calibration = sensorCalibration.data.lsm6dsv16x; + break; + + case SlimeVR::Configuration::CalibrationConfigType::NONE: + m_Logger.warn("No calibration data found for sensor %d, ignoring...", sensorId); + m_Logger.info("Calibration is advised"); + break; + + default: + m_Logger.warn("Incompatible calibration data found for sensor %d, ignoring...", sensorId); + m_Logger.info("Calibration is advised"); + } + + + int8_t status = 0; status |= imu.Enable_X_User_Offset(); status |= imu.Set_X_User_Offset( - accelerationOffset[0], - accelerationOffset[1], - accelerationOffset[2] + m_Calibration.A_off[0], + m_Calibration.A_off[1], + m_Calibration.A_off[2] ); return (LSM6DSV16XStatusTypeDef)status; } @@ -526,173 +414,233 @@ void LSM6DSV16XSensor::sendData() { } } -void LSM6DSV16XSensor::startCalibration(int calibrationType) { +LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::readFifo(uint8_t fifo_samples) { + for (uint16_t i = 0; i < fifo_samples; i++) { + uint8_t tag; + if (imu.FIFO_Get_Tag(&tag) != LSM6DSV16X_OK) { + m_Logger.error( + "Failed to get FIFO data tag on %s at address 0x%02x", + getIMUNameByType(sensorType), + addr + ); + return LSM6DSV16X_ERROR; + } - m_Logger.info("Flip right side up in the next 5 seconds to start calibration."); - delay(5000); - uint8_t isFaceUp; - imu.Get_6D_Orientation_ZH(&isFaceUp); + switch (tag) { + case lsm6dsv16x_fifo_out_raw_t::LSM6DSV16X_TIMESTAMP_TAG: { + if (i % LSM6DSV16X_FIFO_FRAME_SIZE != 0) { + return LSM6DSV16X_OK; // If we are not requesting a full data set + // then stop reading + } + previousDataTime = currentDataTime; + if (imu.FIFO_Get_Timestamp(¤tDataTime)) { + m_Logger.error( + "Failed to get timestamp data on %s at address 0x%02x", + getIMUNameByType(sensorType), + addr + ); + return LSM6DSV16X_ERROR; + } - if (!isFaceUp) { - return; - } + // newTemperature = false; + newRawAcceleration = false; + newGravityVector = false; - // give warning if temp is low ? (is this needed)? - m_Logger.info("Leave still for 30 seconds"); - delay(2000); - /* - double gbiasAvg[] = {0, 0, 0}; - float startingQuat[3]; - double quatDiffAvg[] = {0, 0, 0}; - uint16_t dataCountGbias = 0; - uint16_t dataCountQuatDiff = 0; - - while (dataCountGbias < 4000 && dataCountQuatDiff < 4000) { - uint16_t fifo_samples = 0; - if (imu.FIFO_Get_Num_Samples(&fifo_samples) == LSM6DSV16X_ERROR) { - m_Logger.error( - "Error getting number of samples in FIFO on %s at address 0x%02x", - getIMUNameByType(sensorType), - addr - ); - return; +#ifdef LSM6DSV16X_ONBOARD_FUSION + newFusedGameRotation = false; +#endif +#ifdef LSM6DSV16X_ESP_FUSION + newRawGyro = false; +#endif + break; } - for (uint16_t i = 0; i < fifo_samples; i++) { - uint8_t tag; - if (imu.FIFO_Get_Tag(&tag) != LSM6DSV16X_OK) { +#if SEND_ACCELERATION || defined(LSM6DSV16X_ESP_FUSION) + case lsm6dsv16x_fifo_out_raw_t::LSM6DSV16X_XL_NC_TAG: { // accel + + int32_t acceleration[3]; + if (imu.FIFO_Get_X_Axes(acceleration) != LSM6DSV16X_OK) { m_Logger.error( - "Failed to get FIFO data tag on %s at address 0x%02x", + "Failed to get accelerometer data on %s at address 0x%02x", getIMUNameByType(sensorType), addr ); - continue; + return LSM6DSV16X_ERROR; } + rawAcceleration[0] = (acceleration[0] / mgPerG); + rawAcceleration[1] = (acceleration[1] / mgPerG); + rawAcceleration[2] = (acceleration[2] / mgPerG); - switch (tag) { - case lsm6dsv16x_fifo_out_raw_t::LSM6DSV16X_XL_NC_TAG: { // accel - int32_t rawAccelerations[3]; - if (imu.FIFO_Get_X_Axes(rawAccelerations) != LSM6DSV16X_OK) { - m_Logger.error( - "Failed to get accelerometer data on %s at address - 0x%02x", getIMUNameByType(sensorType), addr - ); - continue; - } - acceleration[0] = (rawAccelerations[0] / mgPerG) - gravity[0]; - acceleration[1] = (rawAccelerations[1] / mgPerG) - gravity[1]; - acceleration[2] = (rawAccelerations[2] / mgPerG) - gravity[2]; - acceleration[0] *= CONST_EARTH_GRAVITY; - acceleration[1] *= CONST_EARTH_GRAVITY; - acceleration[2] *= CONST_EARTH_GRAVITY; - - // m_Logger.info( - // "Acceleration x: %f, y: %f, z: %f", - // acceleration[0], - // acceleration[1], - // acceleration[2] - //); - break; - } - case lsm6dsv16x_fifo_out_raw_t::LSM6DSV16X_SFLP_GYROSCOPE_BIAS_TAG: - { if (dataCountGbias < 100) { dataCountGbias++; uint8_t data[6]; - imu.FIFO_Get_Data(data); - break; - } - float gbias[3]; - if (imu.FIFO_Get_GBias_Vector(gbias) != LSM6DSV16X_OK) { - m_Logger.error( - "Failed to get gyro bias on %s at address 0x%02x", - getIMUNameByType(sensorType), - addr - ); - continue; - } - - gbiasAvg[0] += gbias[0] / mdpsPerDps; - gbiasAvg[1] += gbias[1] / mdpsPerDps; - gbiasAvg[2] += gbias[2] / mdpsPerDps; - dataCountGbias++; - break; - } - case lsm6dsv16x_fifo_out_raw_t:: - LSM6DSV16X_SFLP_GAME_ROTATION_VECTOR_TAG: { - if (dataCountQuatDiff < 99) { - dataCountQuatDiff++; - uint8_t data[6]; - imu.FIFO_Get_Data(data); - break; - } - - float data[3]; - if (imu.FIFO_Get_Game_Vector(data) != LSM6DSV16X_OK) { - m_Logger.error( - "Failed to get game rotation vector on %s at address " - "0x%02x", - getIMUNameByType(sensorType), - addr - ); - continue; - } - - if (dataCountQuatDiff == 99) { - startingQuat[0] = data[0]; - startingQuat[1] = data[1]; - startingQuat[2] = data[2]; - } - - quatDiffAvg[0] += (startingQuat[0] - data[0]); - quatDiffAvg[1] += (startingQuat[1] - data[1]); - quatDiffAvg[2] += (startingQuat[2] - data[2]); - - m_Logger.info( - "Game Diff X: %f, Y: %f, Z: %f", - (startingQuat[0] - data[0]), - (startingQuat[1] - data[1]), - (startingQuat[2] - data[2]) - ); - startingQuat[0] = data[0]; - startingQuat[1] = data[1]; - startingQuat[2] = data[2]; - dataCountQuatDiff++; - break; - } - default: { // We don't use the data so remove from fifo - uint8_t data[6]; - imu.FIFO_Get_Data(data); - } + newRawAcceleration = true; + break; + } +#endif // SEND_ACCELERATION + + case lsm6dsv16x_fifo_out_raw_t::LSM6DSV16X_SFLP_GRAVITY_VECTOR_TAG: { + if (imu.FIFO_Get_Gravity_Vector(gravityVector) != LSM6DSV16X_OK) { + m_Logger.error( + "Failed to get gravity vector on %s at address 0x%02x", + getIMUNameByType(sensorType), + addr + ); + return LSM6DSV16X_ERROR; + } + gravityVector[0] /= mgPerG; + gravityVector[1] /= mgPerG; + gravityVector[2] /= mgPerG; + newGravityVector = true; + +#ifdef LSM6DSV16X_ESP_FUSION + // The SFLP block does not use the accelerometer calibration + gravityVector[0] += m_Calibration.G_off[0]; + gravityVector[1] += m_Calibration.G_off[1]; + gravityVector[2] += m_Calibration.G_off[2]; +#endif + break; + } + +#ifdef LSM6DSV16X_ESP_FUSION + case lsm6dsv16x_fifo_out_raw_t::LSM6DSV16X_GY_NC_TAG: { + int32_t angularVelocity[3]; + if (imu.FIFO_Get_G_Axes(angularVelocity) != LSM6DSV16X_OK) { + m_Logger.error( + "Failed to get accelerometer data on %s at address 0x%02x", + getIMUNameByType(sensorType), + addr + ); + return LSM6DSV16X_ERROR; + } + rawGyro[0] = angularVelocity[0] / mdpsPerDps; + rawGyro[1] = angularVelocity[1] / mdpsPerDps; + rawGyro[2] = angularVelocity[2] / mdpsPerDps; + + // convert to rads/s + rawGyro[0] /= dpsPerRad; + rawGyro[1] /= dpsPerRad; + rawGyro[2] /= dpsPerRad; + + rawGyro[0] -= m_Calibration.G_off[0]; + rawGyro[1] -= m_Calibration.G_off[1]; + rawGyro[2] -= m_Calibration.G_off[2]; + + newRawGyro = true; + break; + } +#endif + +#ifdef LSM6DSV16X_ONBOARD_FUSION + case lsm6dsv16x_fifo_out_raw_t::LSM6DSV16X_SFLP_GAME_ROTATION_VECTOR_TAG: { + if (imu.FIFO_Get_Game_Vector(fusedGameRotation) != LSM6DSV16X_OK) { + m_Logger.error( + "Failed to get game rotation vector on %s at address 0x%02x", + getIMUNameByType(sensorType), + addr + ); + return LSM6DSV16X_ERROR; + } + + newFusedGameRotation = true; + break; + } +#endif + + default: { // We don't use the data so remove from fifo + uint8_t data[6]; + if (imu.FIFO_Get_Data(data) != LSM6DSV16X_OK) { + m_Logger.error( + "Failed to get unwanted data on %s at address 0x%02x", + getIMUNameByType(sensorType), + addr + ); + return LSM6DSV16X_ERROR; } } } - m_Logger.info("Gbias x: %f, y: %f, z: %f", gbiasAvg[0], gbiasAvg[1], - gbiasAvg[2]); m_Logger.info( "quatAvg x: %f, y: %f, z: %f", quatDiffAvg[0], - quatDiffAvg[1], - quatDiffAvg[2] - ); - dataCountGbias -= 100; - dataCountQuatDiff -= 100; - - gbiasAvg[0] /= dataCountGbias; - gbiasAvg[1] /= dataCountGbias; - gbiasAvg[2] /= dataCountGbias; - - quatDiffAvg[0] /= dataCountGbias; - quatDiffAvg[1] /= dataCountGbias; - quatDiffAvg[2] /= dataCountGbias; - - m_Logger.info( - "Gbias Calibration Data, X: %f, Y: %f, Z: %f", - gbiasAvg[0], - gbiasAvg[1], - gbiasAvg[2] - ); + } + return LSM6DSV16X_OK; +} - m_Logger.info( - "Quat Diff Calibration Data, X: %f, Y: %f, Z: %f", - quatDiffAvg[0], - quatDiffAvg[1], - quatDiffAvg[2] - ); - delay(10000); - */ +void LSM6DSV16XSensor::startCalibration(int calibrationType) { + m_Logger.info("Flip right side up in the next 5 seconds to start calibration."); + delay(5000); + uint8_t isFaceUp; + imu.Get_6D_Orientation_ZH(&isFaceUp); + + if (!isFaceUp) { + return; + } + + ledManager.on(); + m_Logger.debug("Gathering raw data for device calibration..."); + constexpr uint16_t calibrationSamples = 300; + // Reset values + float tempGxyz[3] = {0, 0, 0}; + m_Calibration.G_off[0] = 0.0f; + m_Calibration.G_off[1] = 0.0f; + m_Calibration.G_off[2] = 0.0f; + + // Wait for sensor to calm down before calibration + m_Logger.info("Put down the device and wait for baseline gyro reading calibration"); + delay(2000); + + imu.FIFO_Set_Mode(LSM6DSV16X_BYPASS_MODE); // add to lib but we need to + imu.FIFO_Set_Mode(LSM6DSV16X_STREAM_MODE); // get / keep track of current fifo type + uint16_t count = 0; + while (count < calibrationSamples) { + uint16_t fifo_samples = 0; + if (imu.FIFO_Get_Num_Samples(&fifo_samples) == LSM6DSV16X_ERROR) { + m_Logger.error( + "Error getting number of samples in FIFO on %s at address 0x%02x", + getIMUNameByType(sensorType), + addr + ); + return; + } + + // Group all the data together //set the watermark level here for nrf sleep + if (fifo_samples >= LSM6DSV16X_FIFO_FRAME_SIZE) { + readFifo(fifo_samples); + tempGxyz[0] += rawGyro[0]; + tempGxyz[1] += rawGyro[1]; + tempGxyz[2] += rawGyro[2]; + count++; + } + } + + tempGxyz[0] /= calibrationSamples; + tempGxyz[1] /= calibrationSamples; + tempGxyz[2] /= calibrationSamples; + +#ifdef DEBUG_SENSOR + m_Logger.trace( + "Gyro calibration results: %f %f %f", + m_Calibration.G_off[0] + tempGxyz[0], + m_Calibration.G_off[1] + tempGxyz[1], + m_Calibration.G_off[2] + tempGxyz[2] + ); +#endif + + m_Calibration.G_off[0] = tempGxyz[0]; + m_Calibration.G_off[1] = tempGxyz[1]; + m_Calibration.G_off[2] = tempGxyz[2]; + + // accel calibration + m_Calibration.G_off[0] = 0.0f; + m_Calibration.G_off[1] = 0.0f; + m_Calibration.G_off[2] = 0.0f; + + m_Logger.debug("Saving the calibration data"); + + SlimeVR::Configuration::CalibrationConfig calibration; + calibration.type = SlimeVR::Configuration::CalibrationConfigType::LSM6DSV16X; + calibration.data.lsm6dsv16x = m_Calibration; + configuration.setCalibration(sensorId, calibration); + configuration.save(); + + ledManager.off(); + m_Logger.debug("Saved the calibration data"); + m_Logger.info("Calibration data gathered"); + + imu.FIFO_Set_Mode(LSM6DSV16X_BYPASS_MODE); // add to lib but we need to + imu.FIFO_Set_Mode(LSM6DSV16X_STREAM_MODE); // get / keep track of current fifo type } diff --git a/src/sensors/lsm6dsv16xsensor.h b/src/sensors/lsm6dsv16xsensor.h index 88252ac6a..921ef11b6 100644 --- a/src/sensors/lsm6dsv16xsensor.h +++ b/src/sensors/lsm6dsv16xsensor.h @@ -54,17 +54,21 @@ // #define REINIT_ON_FAILURE // #define LSM6DSV16X_INTERRUPT // #define LSM6DSV16X_NO_SELF_TEST_ON_FACEDOWN +#define LSM6DSV16X_ONBOARD_FUSION +#define LSM6DSV16X_ESP_FUSION #ifdef REINIT_ON_FAILURE #define REINIT_RETRY_MAX_ATTEMPTS 5 #undef SELF_TEST_ON_INIT #endif +#if defined(LSM6DSV16X_ONBOARD_FUSION) && defined(LSM6DSV16X_ESP_FUSION) +#define LSM6DSV16X_FIFO_FRAME_SIZE 5 // X BDR, G BDR, Game, Gravity, Timestamp +#else +#define LSM6DSV16X_FIFO_FRAME_SIZE 4 // X BDR, (G BDR || Game), Gravity, Timestamp +#endif -#define LSM6DSV16X_ONBOARD_FUSION -#define LSM6DSV16X_ESP_FUSION - class LSM6DSV16XSensor : public Sensor { public: @@ -88,6 +92,7 @@ class LSM6DSV16XSensor : public Sensor { Quat fusedRotationToQuaternion(float x, float y, float z); LSM6DSV16XStatusTypeDef runSelfTest(); LSM6DSV16XStatusTypeDef loadIMUCalibration(); + LSM6DSV16XStatusTypeDef readFifo(uint8_t fifo_samples); LSM6DSV16X imu; uint8_t m_IntPin; @@ -99,7 +104,6 @@ class LSM6DSV16XSensor : public Sensor { float gravityVector[3]; bool newGravityVector = false; float rawAcceleration[3]; - float accelerationOffset[3] = {0, 0, 0}; //Put these in stored calibration bool newRawAcceleration = false; #ifdef LSM6DSV16X_ONBOARD_FUSION @@ -109,9 +113,9 @@ class LSM6DSV16XSensor : public Sensor { #endif #ifdef LSM6DSV16X_ESP_FUSION + SlimeVR::Configuration::LSM6DSV16XCalibrationConfig m_Calibration; SlimeVR::Sensors::SensorFusion sfusion; float rawGyro[3]; - float gyroOffset[3] = {0, 0, 0}; //Put these in stored calibration bool newRawGyro = false; Quat previousEspRotation; uint32_t previousDataTime = 0; From bd4f94a92a3bcc3a15ae82e60932394242412265 Mon Sep 17 00:00:00 2001 From: Carter Meyers Date: Sat, 19 Aug 2023 16:12:45 -0700 Subject: [PATCH 33/60] Add acceleration calibration --- src/sensors/lsm6dsv16xsensor.cpp | 175 ++++++++++++++++++++++++++----- src/sensors/lsm6dsv16xsensor.h | 8 +- 2 files changed, 156 insertions(+), 27 deletions(-) diff --git a/src/sensors/lsm6dsv16xsensor.cpp b/src/sensors/lsm6dsv16xsensor.cpp index c081489a1..e8f869776 100644 --- a/src/sensors/lsm6dsv16xsensor.cpp +++ b/src/sensors/lsm6dsv16xsensor.cpp @@ -176,6 +176,8 @@ void LSM6DSV16XSensor::motionSetup() { status |= imu.Enable_Single_Tap_Detection(LSM6DSV16X_INT1_PIN); // Tap requires an interrupt status |= imu.Enable_Double_Tap_Detection(LSM6DSV16X_INT1_PIN); #endif // LSM6DSV16X_INTERRUPT + status |= imu.FIFO_Set_Mode(LSM6DSV16X_BYPASS_MODE); // add to lib but we need to + status |= imu.FIFO_Set_Mode(LSM6DSV16X_STREAM_MODE); // get / keep track of current fifo type if (status != LSM6DSV16X_OK) { m_Logger.fatal( @@ -367,15 +369,13 @@ LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::loadIMUCalibration() { m_Logger.info("Calibration is advised"); } - - int8_t status = 0; - status |= imu.Enable_X_User_Offset(); status |= imu.Set_X_User_Offset( m_Calibration.A_off[0], m_Calibration.A_off[1], m_Calibration.A_off[2] ); + status |= imu.Enable_X_User_Offset(); return (LSM6DSV16XStatusTypeDef)status; } @@ -414,7 +414,7 @@ void LSM6DSV16XSensor::sendData() { } } -LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::readFifo(uint8_t fifo_samples) { +LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::readFifo(uint16_t fifo_samples) { for (uint16_t i = 0; i < fifo_samples; i++) { uint8_t tag; if (imu.FIFO_Get_Tag(&tag) != LSM6DSV16X_OK) { @@ -571,7 +571,6 @@ void LSM6DSV16XSensor::startCalibration(int calibrationType) { } ledManager.on(); - m_Logger.debug("Gathering raw data for device calibration..."); constexpr uint16_t calibrationSamples = 300; // Reset values float tempGxyz[3] = {0, 0, 0}; @@ -600,10 +599,12 @@ void LSM6DSV16XSensor::startCalibration(int calibrationType) { // Group all the data together //set the watermark level here for nrf sleep if (fifo_samples >= LSM6DSV16X_FIFO_FRAME_SIZE) { readFifo(fifo_samples); - tempGxyz[0] += rawGyro[0]; - tempGxyz[1] += rawGyro[1]; - tempGxyz[2] += rawGyro[2]; - count++; + if (newRawGyro) { + tempGxyz[0] += rawGyro[0]; + tempGxyz[1] += rawGyro[1]; + tempGxyz[2] += rawGyro[2]; + count++; + } } } @@ -614,20 +615,150 @@ void LSM6DSV16XSensor::startCalibration(int calibrationType) { #ifdef DEBUG_SENSOR m_Logger.trace( "Gyro calibration results: %f %f %f", - m_Calibration.G_off[0] + tempGxyz[0], - m_Calibration.G_off[1] + tempGxyz[1], - m_Calibration.G_off[2] + tempGxyz[2] + tempGxyz[0], + tempGxyz[1], + tempGxyz[2] ); #endif - m_Calibration.G_off[0] = tempGxyz[0]; - m_Calibration.G_off[1] = tempGxyz[1]; - m_Calibration.G_off[2] = tempGxyz[2]; - // accel calibration - m_Calibration.G_off[0] = 0.0f; - m_Calibration.G_off[1] = 0.0f; - m_Calibration.G_off[2] = 0.0f; + //Accelerometer Calibration + MagnetoCalibration* magneto = new MagnetoCalibration(); + imu.Disable_X_User_Offset(); + + m_Logger.info( + "Put the device into 6 unique orientations (all sides), leave it still and do " + "not hold/touch for 3 seconds each" + ); + constexpr uint8_t ACCEL_CALIBRATION_DELAY_SEC = 3; + ledManager.on(); + for (uint8_t i = ACCEL_CALIBRATION_DELAY_SEC; i > 0; i--) { + m_Logger.info("%i...", i); + delay(1000); + } + ledManager.off(); + + RestDetectionParams calibrationRestDetectionParams; + calibrationRestDetectionParams.restMinTimeMicros = 3 * 1e6; + calibrationRestDetectionParams.restThAcc = 0.01f; + RestDetection calibrationRestDetection( + calibrationRestDetectionParams, + 1.0f / LSM6DSV16X_FIFO_DATA_RATE, + 1.0f / LSM6DSV16X_FIFO_DATA_RATE + ); + + constexpr uint16_t expectedPositions = 6; + constexpr uint16_t numSamplesPerPosition = 96; + + uint16_t numPositionsRecorded = 0; + uint16_t numCurrentPositionSamples = 0; + bool waitForMotion = false; + + float* accelCalibrationChunk = new float[numSamplesPerPosition * 3]; + ledManager.pattern(100, 100, 6); + ledManager.on(); + m_Logger.info("Gathering accelerometer data..."); + m_Logger.info( + "Waiting for position %i, leave the device as is...", + numPositionsRecorded + 1 + ); + + imu.FIFO_Set_Mode(LSM6DSV16X_BYPASS_MODE); // add to lib but we need to + imu.FIFO_Set_Mode(LSM6DSV16X_STREAM_MODE); // get / keep track of current fifo type + while (true) { + uint16_t fifo_samples = 0; + if (imu.FIFO_Get_Num_Samples(&fifo_samples) == LSM6DSV16X_ERROR) { + m_Logger.error( + "Error getting number of samples in FIFO on %s at address 0x%02x", + getIMUNameByType(sensorType), + addr + ); + return; + } + + + // Group all the data together //set the watermark level here for nrf sleep + if (fifo_samples < LSM6DSV16X_FIFO_FRAME_SIZE) { + continue; + } + + readFifo(fifo_samples); + + // in microseconds + if (!newRawAcceleration) { + continue; + } + + calibrationRestDetection.updateAcc( + (currentDataTime - previousDataTime) * LSM6DSV16X_TIMESTAMP_LSB * 1e6, + rawAcceleration + ); + newRawAcceleration = false; + + if (waitForMotion) { + if (!calibrationRestDetection.getRestDetected()) { + waitForMotion = false; + } + continue; + } + + if (calibrationRestDetection.getRestDetected()) { + const uint16_t i = numCurrentPositionSamples * 3; + accelCalibrationChunk[i + 0] = rawAcceleration[0]; + accelCalibrationChunk[i + 1] = rawAcceleration[1]; + accelCalibrationChunk[i + 2] = rawAcceleration[2]; + numCurrentPositionSamples++; + + if (numCurrentPositionSamples >= numSamplesPerPosition) { + for (int i = 0; i < numSamplesPerPosition; i++) { + magneto->sample( + accelCalibrationChunk[i * 3 + 0], + accelCalibrationChunk[i * 3 + 1], + accelCalibrationChunk[i * 3 + 2] + ); + } + numPositionsRecorded++; + numCurrentPositionSamples = 0; + if (numPositionsRecorded < expectedPositions) { + ledManager.pattern(50, 50, 2); + ledManager.on(); + m_Logger.info( + "Recorded, waiting for position %i...", + numPositionsRecorded + 1 + ); + waitForMotion = true; + } + } + } else { + numCurrentPositionSamples = 0; + } + + if (numPositionsRecorded >= expectedPositions) { + break; + } + } + ledManager.off(); + m_Logger.debug("Calculating accelerometer calibration data..."); + delete[] accelCalibrationChunk; + + float A_BAinv[4][3]; + magneto->current_calibration(A_BAinv); + delete magneto; + + m_Logger.debug("Finished calculating accelerometer calibration"); + + m_Calibration.A_off[0] = A_BAinv[0][0]; + m_Calibration.A_off[1] = A_BAinv[0][1]; + m_Calibration.A_off[2] = A_BAinv[0][2]; + +#ifdef DEBUG_SENSOR + m_Logger.trace( + "Accel calibration results: %f %f %f", + A_BAinv[0][0], + A_BAinv[0][1], + A_BAinv[0][2] + ); +#endif m_Logger.debug("Saving the calibration data"); @@ -638,9 +769,5 @@ void LSM6DSV16XSensor::startCalibration(int calibrationType) { configuration.save(); ledManager.off(); - m_Logger.debug("Saved the calibration data"); - m_Logger.info("Calibration data gathered"); - - imu.FIFO_Set_Mode(LSM6DSV16X_BYPASS_MODE); // add to lib but we need to - imu.FIFO_Set_Mode(LSM6DSV16X_STREAM_MODE); // get / keep track of current fifo type + m_Logger.info("Calibration finished, enjoy"); } diff --git a/src/sensors/lsm6dsv16xsensor.h b/src/sensors/lsm6dsv16xsensor.h index 921ef11b6..9c5f0a123 100644 --- a/src/sensors/lsm6dsv16xsensor.h +++ b/src/sensors/lsm6dsv16xsensor.h @@ -27,6 +27,8 @@ #include "LSM6DSV16X.h" #include "sensor.h" #include "SensorFusion.h" +#include "magneto1.4.h" +#include "SensorFusionRestDetect.h" #ifndef LSM6DSV16X_ACCEL_MAX #define LSM6DSV16X_ACCEL_MAX 4 @@ -48,13 +50,13 @@ #define LSM6DSV16X_TEMP_READ_INTERVAL 1 #endif -#define LSM6DSV16X_TIMESTAMP_LSB 21.75e-6 +#define LSM6DSV16X_TIMESTAMP_LSB 21.75e-6f // #define SELF_TEST_ON_INIT // #define REINIT_ON_FAILURE // #define LSM6DSV16X_INTERRUPT // #define LSM6DSV16X_NO_SELF_TEST_ON_FACEDOWN -#define LSM6DSV16X_ONBOARD_FUSION +//#define LSM6DSV16X_ONBOARD_FUSION #define LSM6DSV16X_ESP_FUSION #ifdef REINIT_ON_FAILURE @@ -92,7 +94,7 @@ class LSM6DSV16XSensor : public Sensor { Quat fusedRotationToQuaternion(float x, float y, float z); LSM6DSV16XStatusTypeDef runSelfTest(); LSM6DSV16XStatusTypeDef loadIMUCalibration(); - LSM6DSV16XStatusTypeDef readFifo(uint8_t fifo_samples); + LSM6DSV16XStatusTypeDef readFifo(uint16_t fifo_samples); LSM6DSV16X imu; uint8_t m_IntPin; From 8870218dcec6629f2b30dee648158647a4e8a37a Mon Sep 17 00:00:00 2001 From: Carter Meyers Date: Sun, 20 Aug 2023 13:41:18 -0700 Subject: [PATCH 34/60] Added gyro scale calibration GSCALE serial command --- src/configuration/CalibrationConfig.h | 3 + src/motionprocessing/RestDetection.h | 16 +- src/sensors/SensorFusionRestDetect.cpp | 18 +- src/sensors/SensorFusionRestDetect.h | 2 + src/sensors/bmi160sensor.cpp | 2 +- src/sensors/lsm6dsv16xsensor.cpp | 276 ++++++++++++++++++++----- src/sensors/lsm6dsv16xsensor.h | 19 +- src/sensors/sensor.cpp | 9 + src/sensors/sensor.h | 5 + src/serial/serialcommands.cpp | 44 ++++ 10 files changed, 341 insertions(+), 53 deletions(-) diff --git a/src/configuration/CalibrationConfig.h b/src/configuration/CalibrationConfig.h index cf3a9d525..502f44682 100644 --- a/src/configuration/CalibrationConfig.h +++ b/src/configuration/CalibrationConfig.h @@ -80,6 +80,9 @@ namespace SlimeVR { // gyroscope bias float G_off[3]; + // gyroscope scale + float G_scale[3]; + // accelerometer bias float A_off[3]; }; diff --git a/src/motionprocessing/RestDetection.h b/src/motionprocessing/RestDetection.h index c0215baff..8d8ae1a9c 100644 --- a/src/motionprocessing/RestDetection.h +++ b/src/motionprocessing/RestDetection.h @@ -18,11 +18,11 @@ #define NaN std::numeric_limits::quiet_NaN() struct RestDetectionParams { - sensor_real_t biasClip; + sensor_real_t biasClip; //dps sensor_real_t biasSigmaRest; uint32_t restMinTimeMicros; sensor_real_t restFilterTau; - sensor_real_t restThGyr; + sensor_real_t restThGyr; //dps sensor_real_t restThAcc; RestDetectionParams(): biasClip(2.0f), @@ -50,6 +50,13 @@ class RestDetection { setup(); } + void updateRestDetectionParameters(RestDetectionParams newParams) { + params = newParams; +#ifndef REST_DETECTION_DISABLE_LPF + resetState(); +#endif // REST_DETECTION_DISABLE_LPF + } + #ifndef REST_DETECTION_DISABLE_LPF void filterInitialState(sensor_real_t x0, const double b[3], const double a[2], double out[]) { @@ -185,6 +192,11 @@ class RestDetection { #endif } + void update6D(uint32_t dtMicros, sensor_real_t acc[3], sensor_real_t gyr[3]) { + updateAcc(dtMicros, acc); + updateGyr(dtMicros, gyr); //gyro doesn't set restDection to true only false + } + bool getRestDetected() { return restDetected; } diff --git a/src/sensors/SensorFusionRestDetect.cpp b/src/sensors/SensorFusionRestDetect.cpp index f7c47fc5f..31232f4e0 100644 --- a/src/sensors/SensorFusionRestDetect.cpp +++ b/src/sensors/SensorFusionRestDetect.cpp @@ -8,7 +8,7 @@ namespace SlimeVR { #if !SENSOR_WITH_REST_DETECT if (deltat < 0) deltat = accTs; - restDetection.updateAcc(deltat, Axyz); + restDetection.updateAcc(deltat * 1e6, Axyz); #endif SensorFusion::updateAcc(Axyz, deltat); } @@ -17,11 +17,20 @@ namespace SlimeVR { #if !SENSOR_WITH_REST_DETECT if (deltat < 0) deltat = accTs; - restDetection.updateGyr(deltat, Gxyz); + restDetection.updateGyr(deltat * 1e6, Gxyz); #endif SensorFusion::updateGyro(Gxyz, deltat); } + void SensorFusionRestDetect::update6D(sensor_real_t Axyz[3], sensor_real_t Gxyz[3], sensor_real_t deltat) + { + #if !SENSOR_WITH_REST_DETECT + if (deltat < 0) deltat = accTs; //They need to be the same + restDetection.update6D(deltat * 1e6, Axyz, Gxyz); + #endif + SensorFusion::update6D(Axyz, Gxyz, deltat); + } + bool SensorFusionRestDetect::getRestDetected() { #if !SENSOR_WITH_REST_DETECT @@ -30,5 +39,10 @@ namespace SlimeVR return vqf.getRestDetected(); #endif } + + void SensorFusionRestDetect::updateRestDetectionParameters(RestDetectionParams newParams) + { + restDetection.updateRestDetectionParameters(newParams); + } } } diff --git a/src/sensors/SensorFusionRestDetect.h b/src/sensors/SensorFusionRestDetect.h index 684d8e640..e8c0371dd 100644 --- a/src/sensors/SensorFusionRestDetect.h +++ b/src/sensors/SensorFusionRestDetect.h @@ -41,6 +41,8 @@ namespace SlimeVR #if !SENSOR_WITH_REST_DETECT void updateAcc(sensor_real_t Axyz[3], sensor_real_t deltat); void updateGyro(sensor_real_t Gxyz[3], sensor_real_t deltat); + void update6D(sensor_real_t Axyz[3], sensor_real_t Gxyz[3], sensor_real_t deltat); + void updateRestDetectionParameters(RestDetectionParams newParams); #endif protected: #if !SENSOR_WITH_REST_DETECT diff --git a/src/sensors/bmi160sensor.cpp b/src/sensors/bmi160sensor.cpp index 78c21d633..d2d9bd360 100644 --- a/src/sensors/bmi160sensor.cpp +++ b/src/sensors/bmi160sensor.cpp @@ -567,7 +567,7 @@ void BMI160Sensor::onAccelRawSample(uint32_t dtMicros, int16_t x, int16_t y, int lastAxyz[1] = Axyz[1]; lastAxyz[2] = Axyz[2]; - sfusion.updateAcc(Axyz, dtMicros); + sfusion.updateAcc(Axyz, (double)dtMicros * 1.0e-6); optimistic_yield(100); } diff --git a/src/sensors/lsm6dsv16xsensor.cpp b/src/sensors/lsm6dsv16xsensor.cpp index e8f869776..165787e21 100644 --- a/src/sensors/lsm6dsv16xsensor.cpp +++ b/src/sensors/lsm6dsv16xsensor.cpp @@ -52,6 +52,14 @@ LSM6DSV16XSensor::LSM6DSV16XSensor( ){}; void LSM6DSV16XSensor::motionSetup() { + RestDetectionParams restDetectionParams; + restDetectionParams.restMinTimeMicros = 3 * 1e6; + //restDetectionParams.restThAcc = 0.01f; + //restDetectionParams.restThGyr = 0.15f; //dps + restDetectionParams.restThAcc = 0.05f; + restDetectionParams.restThGyr = 0.25f; //dps + sfusion.updateRestDetectionParameters(restDetectionParams); + uint8_t deviceId = 0; if (imu.ReadID(&deviceId) == LSM6DSV16X_ERROR) { m_Logger.fatal( @@ -147,24 +155,24 @@ void LSM6DSV16XSensor::motionSetup() { #if defined(LSM6DSV16X_ONBOARD_FUSION) && defined(LSM6DSV16X_ESP_FUSION) status |= imu.FIFO_Set_SFLP_Batch(true, true, false); status |= imu.Enable_Game_Rotation(); + loadIMUCalibration(); // Calibration if (isFaceDown) { startCalibration(0); // can not calibrate onboard fusion } - loadIMUCalibration(); #elif defined(LSM6DSV16X_ONBOARD_FUSION) status |= imu.FIFO_Set_SFLP_Batch(true, true, false); status |= imu.Enable_Game_Rotation(); #elif defined(LSM6DSV16X_ESP_FUSION) status |= imu.FIFO_Set_SFLP_Batch(false, true, false); status |= imu.Enable_Game_Rotation(); + loadIMUCalibration(); // Calibration if (isFaceDown) { startCalibration(0); // can not calibrate onboard fusion } - loadIMUCalibration(); #endif status |= imu.Disable_6D_Orientation(); @@ -369,6 +377,7 @@ LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::loadIMUCalibration() { m_Logger.info("Calibration is advised"); } +#ifdef LSM6DSV16X_ACCEL_OFFSET_CAL int8_t status = 0; status |= imu.Set_X_User_Offset( m_Calibration.A_off[0], @@ -377,6 +386,8 @@ LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::loadIMUCalibration() { ); status |= imu.Enable_X_User_Offset(); return (LSM6DSV16XStatusTypeDef)status; +#endif + return LSM6DSV16X_OK; } void LSM6DSV16XSensor::sendData() { @@ -414,8 +425,24 @@ void LSM6DSV16XSensor::sendData() { } } -LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::readFifo(uint16_t fifo_samples) { - for (uint16_t i = 0; i < fifo_samples; i++) { +// Used for calibration (Blocking) +LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::readNextFifoFrame() { + uint16_t fifo_samples = 0; + while (fifo_samples < LSM6DSV16X_FIFO_FRAME_SIZE) { + if (imu.FIFO_Get_Num_Samples(&fifo_samples) == LSM6DSV16X_ERROR) { + m_Logger.error( + "Error getting number of samples in FIFO on %s at address 0x%02x", + getIMUNameByType(sensorType), + addr + ); + return LSM6DSV16X_ERROR; + } + } + return readFifo(LSM6DSV16X_FIFO_FRAME_SIZE); +} + + LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::readFifo(uint16_t fifo_samples) { + for (uint16_t i = 0; i < fifo_samples; i++) { uint8_t tag; if (imu.FIFO_Get_Tag(&tag) != LSM6DSV16X_OK) { m_Logger.error( @@ -490,7 +517,7 @@ LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::readFifo(uint16_t fifo_samples) { gravityVector[2] /= mgPerG; newGravityVector = true; -#ifdef LSM6DSV16X_ESP_FUSION +#if defined(LSM6DSV16X_ESP_FUSION) && defined(LSM6DSV16X_ACCEL_OFFSET_CAL) // The SFLP block does not use the accelerometer calibration gravityVector[0] += m_Calibration.G_off[0]; gravityVector[1] += m_Calibration.G_off[1]; @@ -510,18 +537,26 @@ LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::readFifo(uint16_t fifo_samples) { ); return LSM6DSV16X_ERROR; } - rawGyro[0] = angularVelocity[0] / mdpsPerDps; - rawGyro[1] = angularVelocity[1] / mdpsPerDps; - rawGyro[2] = angularVelocity[2] / mdpsPerDps; + + rawGyro[0] = (float)angularVelocity[0] / mdpsPerDps; + rawGyro[1] = (float)angularVelocity[1] / mdpsPerDps; + rawGyro[2] = (float)angularVelocity[2] / mdpsPerDps; // convert to rads/s rawGyro[0] /= dpsPerRad; rawGyro[1] /= dpsPerRad; rawGyro[2] /= dpsPerRad; +#ifdef LSM6DSV16X_GYRO_OFFSET_CAL rawGyro[0] -= m_Calibration.G_off[0]; rawGyro[1] -= m_Calibration.G_off[1]; rawGyro[2] -= m_Calibration.G_off[2]; +#endif +#ifdef LSM6DSV16X_GYRO_SCALE_CAL + rawGyro[0] *= m_Calibration.G_scale[0]; + rawGyro[1] *= m_Calibration.G_scale[1]; + rawGyro[2] *= m_Calibration.G_scale[2]; +#endif newRawGyro = true; break; @@ -567,50 +602,64 @@ void LSM6DSV16XSensor::startCalibration(int calibrationType) { imu.Get_6D_Orientation_ZH(&isFaceUp); if (!isFaceUp) { + m_Logger.info("This is your last chance to flip the tracker over. I am warning you"); + delay(5000); + uint8_t isFaceDown; + imu.Get_6D_Orientation_ZL(&isFaceDown); + + if (!isFaceDown) { + return; + } + testGyroScaleCalibration(); return; } +#ifdef LSM6DSV16X_GYRO_OFFSET_CAL ledManager.on(); constexpr uint16_t calibrationSamples = 300; // Reset values float tempGxyz[3] = {0, 0, 0}; + m_Calibration.G_off[0] = 0.0f; m_Calibration.G_off[1] = 0.0f; m_Calibration.G_off[2] = 0.0f; + float tempGscale[3]; + tempGscale[0] = m_Calibration.G_scale[0]; + tempGscale[1] = m_Calibration.G_scale[1]; + tempGscale[2] = m_Calibration.G_scale[2]; + + m_Calibration.G_scale[0] = 1.0f; + m_Calibration.G_scale[1] = 1.0f; + m_Calibration.G_scale[2] = 1.0f; + + // Wait for sensor to calm down before calibration m_Logger.info("Put down the device and wait for baseline gyro reading calibration"); delay(2000); imu.FIFO_Set_Mode(LSM6DSV16X_BYPASS_MODE); // add to lib but we need to imu.FIFO_Set_Mode(LSM6DSV16X_STREAM_MODE); // get / keep track of current fifo type + waitForRest(); uint16_t count = 0; while (count < calibrationSamples) { - uint16_t fifo_samples = 0; - if (imu.FIFO_Get_Num_Samples(&fifo_samples) == LSM6DSV16X_ERROR) { - m_Logger.error( - "Error getting number of samples in FIFO on %s at address 0x%02x", - getIMUNameByType(sensorType), - addr - ); - return; - } - - // Group all the data together //set the watermark level here for nrf sleep - if (fifo_samples >= LSM6DSV16X_FIFO_FRAME_SIZE) { - readFifo(fifo_samples); - if (newRawGyro) { - tempGxyz[0] += rawGyro[0]; - tempGxyz[1] += rawGyro[1]; - tempGxyz[2] += rawGyro[2]; - count++; - } + readNextFifoFrame(); + if (newRawGyro) { + tempGxyz[0] += rawGyro[0]; + tempGxyz[1] += rawGyro[1]; + tempGxyz[2] += rawGyro[2]; + count++; } } - tempGxyz[0] /= calibrationSamples; - tempGxyz[1] /= calibrationSamples; - tempGxyz[2] /= calibrationSamples; + + m_Calibration.G_off[0] = tempGxyz[0] / calibrationSamples; + m_Calibration.G_off[1] = tempGxyz[1] / calibrationSamples; + m_Calibration.G_off[2] = tempGxyz[2] / calibrationSamples; + + m_Calibration.G_scale[0] = tempGscale[0]; + m_Calibration.G_scale[1] = tempGscale[1]; + m_Calibration.G_scale[2] = tempGscale[2]; #ifdef DEBUG_SENSOR m_Logger.trace( @@ -620,8 +669,10 @@ void LSM6DSV16XSensor::startCalibration(int calibrationType) { tempGxyz[2] ); #endif +#endif +#ifdef LSM6DSV16X_ACCEL_OFFSET_CAL //Accelerometer Calibration MagnetoCalibration* magneto = new MagnetoCalibration(); imu.Disable_X_User_Offset(); @@ -638,15 +689,6 @@ void LSM6DSV16XSensor::startCalibration(int calibrationType) { } ledManager.off(); - RestDetectionParams calibrationRestDetectionParams; - calibrationRestDetectionParams.restMinTimeMicros = 3 * 1e6; - calibrationRestDetectionParams.restThAcc = 0.01f; - RestDetection calibrationRestDetection( - calibrationRestDetectionParams, - 1.0f / LSM6DSV16X_FIFO_DATA_RATE, - 1.0f / LSM6DSV16X_FIFO_DATA_RATE - ); - constexpr uint16_t expectedPositions = 6; constexpr uint16_t numSamplesPerPosition = 96; @@ -689,20 +731,20 @@ void LSM6DSV16XSensor::startCalibration(int calibrationType) { continue; } - calibrationRestDetection.updateAcc( - (currentDataTime - previousDataTime) * LSM6DSV16X_TIMESTAMP_LSB * 1e6, - rawAcceleration + sfusion.updateAcc( + rawAcceleration, + (currentDataTime - previousDataTime) * LSM6DSV16X_TIMESTAMP_LSB ); newRawAcceleration = false; if (waitForMotion) { - if (!calibrationRestDetection.getRestDetected()) { + if (!sfusion.getRestDetected()) { waitForMotion = false; } continue; } - if (calibrationRestDetection.getRestDetected()) { + if (sfusion.getRestDetected()) { const uint16_t i = numCurrentPositionSamples * 3; accelCalibrationChunk[i + 0] = rawAcceleration[0]; accelCalibrationChunk[i + 1] = rawAcceleration[1]; @@ -759,7 +801,128 @@ void LSM6DSV16XSensor::startCalibration(int calibrationType) { A_BAinv[0][2] ); #endif +#endif + + saveCalibration(); + ledManager.off(); + m_Logger.info("Calibration finished, enjoy"); +} + +void LSM6DSV16XSensor::printGyroScaleCalibration() { + m_Logger.info(""); + m_Logger.info("Gyro Scale Calibration Values for %s on 0x%02x", getIMUNameByType(sensorType), addr); + m_Logger.info("X Scale: %f", m_Calibration.G_scale[0]); + m_Logger.info("Y Scale: %f", m_Calibration.G_scale[1]); + m_Logger.info("Z Scale: %f", m_Calibration.G_scale[2]); + + m_Logger.info("Gyro Offset Calibration Values for %s on 0x%02x", getIMUNameByType(sensorType), addr); + m_Logger.info("X Scale: %f", m_Calibration.G_off[0]); + m_Logger.info("Y Scale: %f", m_Calibration.G_off[1]); + m_Logger.info("Z Scale: %f", m_Calibration.G_off[2]); + + m_Logger.info("Accel Offset Calibration Values for %s on 0x%02x", getIMUNameByType(sensorType), addr); + m_Logger.info("X Scale: %f", m_Calibration.A_off[0]); + m_Logger.info("Y Scale: %f", m_Calibration.A_off[1]); + m_Logger.info("Z Scale: %f", m_Calibration.A_off[2]); +} + +void LSM6DSV16XSensor::setGyroScaleCalibration(float xScale, float yScale, float zScale) { + m_Logger.info("Gyro Scale Calibration Updated"); + m_Calibration.G_scale[0] = xScale; + m_Calibration.G_scale[1] = yScale; + m_Calibration.G_scale[2] = zScale; + saveCalibration(); +} + +void LSM6DSV16XSensor::resetGyroScaleCalibration() { + m_Calibration.G_scale[0] = 1.0f; + m_Calibration.G_scale[1] = 1.0f; + m_Calibration.G_scale[2] = 1.0f; + saveCalibration(); +} + + +void LSM6DSV16XSensor::testGyroScaleCalibration() { +#ifdef LSM6DSV16X_GYRO_SCALE_CAL + //Gyroscope scale + m_Logger.info(""); + m_Logger.info(""); + m_Logger.info( + "Congrats you made it to the advance calibrations, let the tracker sit still and don't " + "touch" + ); + + printGyroScaleCalibration(); + + Vector3 rawRotations[4]; + uint8_t count = 0; + + ledManager.off(); + waitForRest(); + + m_Logger.info(""); + m_Logger.info( + "\tStep 1: Move the tracker to a corner or edge that you can get it in the " + "same position every time" + ); + m_Logger.info("\tStep 2: Let the tracker rest until the light turns on"); + m_Logger.info( + "\tStep 3: Rotate the tracker in one axis 360 degrees and align with the " + "previous edge." + ); + m_Logger.info("\t\tNOTE: the light will turn off after you start moving it"); + + m_Logger.info( + "\tStep 4: Repeat step 2 and 3 in the same axis going the opposite direction" + ); + + ledManager.on(); + waitForMovement(); + + delay(2000); + imu.FIFO_Set_Mode(LSM6DSV16X_BYPASS_MODE); // add to lib but we need to + imu.FIFO_Set_Mode(LSM6DSV16X_STREAM_MODE); // get / keep track of current fifo type + + waitForRest(); + while (count < 2) { + ledManager.on(); // The user should rotate + m_Logger.info("Rotate the tracker 360 degrees"); + + waitForMovement(); + rawRotations[count * 2] = sfusion.getQuaternionQuat().get_euler() * dpsPerRad; + ledManager.off(); + + waitForRest(); + rawRotations[count * 2 + 1] = sfusion.getQuaternionQuat().get_euler() * dpsPerRad; + count++; + } + + m_Logger.info( + "Drift First Rotation: %f, %f, %f", + rawRotations[0].x - rawRotations[1].x, + rawRotations[0].y - rawRotations[1].y, + rawRotations[0].z - rawRotations[1].z + ); + + m_Logger.info( + "Drift Second Rotation: %f, %f, %f", + rawRotations[2].x - rawRotations[3].x, + rawRotations[2].y - rawRotations[3].y, + rawRotations[2].z - rawRotations[3].z + ); + + m_Logger.info( + "Drift Initial - Final: %f, %f, %f", + rawRotations[0].x - rawRotations[3].x, + rawRotations[0].y - rawRotations[3].y, + rawRotations[0].z - rawRotations[3].z + ); + lastData = millis(); +#endif +} + +void LSM6DSV16XSensor::saveCalibration() { m_Logger.debug("Saving the calibration data"); SlimeVR::Configuration::CalibrationConfig calibration; @@ -767,7 +930,28 @@ void LSM6DSV16XSensor::startCalibration(int calibrationType) { calibration.data.lsm6dsv16x = m_Calibration; configuration.setCalibration(sensorId, calibration); configuration.save(); +} - ledManager.off(); - m_Logger.info("Calibration finished, enjoy"); +void LSM6DSV16XSensor::apply6DToRestDetection() { + if (newRawGyro && newRawAcceleration) { + sfusion.update6D( + rawAcceleration, + rawGyro, + (double)(currentDataTime - previousDataTime) * LSM6DSV16X_TIMESTAMP_LSB + ); + } + newRawAcceleration = false; +} + +void LSM6DSV16XSensor::waitForRest() { + while (!sfusion.getRestDetected()) { + readNextFifoFrame(); + apply6DToRestDetection(); + } +} +void LSM6DSV16XSensor::waitForMovement() { + while (sfusion.getRestDetected()) { + readNextFifoFrame(); + apply6DToRestDetection(); + } } diff --git a/src/sensors/lsm6dsv16xsensor.h b/src/sensors/lsm6dsv16xsensor.h index 9c5f0a123..1c2c2b091 100644 --- a/src/sensors/lsm6dsv16xsensor.h +++ b/src/sensors/lsm6dsv16xsensor.h @@ -56,9 +56,14 @@ // #define REINIT_ON_FAILURE // #define LSM6DSV16X_INTERRUPT // #define LSM6DSV16X_NO_SELF_TEST_ON_FACEDOWN -//#define LSM6DSV16X_ONBOARD_FUSION +#define LSM6DSV16X_ONBOARD_FUSION #define LSM6DSV16X_ESP_FUSION + +#define LSM6DSV16X_GYRO_OFFSET_CAL +#define LSM6DSV16X_ACCEL_OFFSET_CAL +#define LSM6DSV16X_GYRO_SCALE_CAL + #ifdef REINIT_ON_FAILURE #define REINIT_RETRY_MAX_ATTEMPTS 5 #undef SELF_TEST_ON_INIT @@ -89,12 +94,22 @@ class LSM6DSV16XSensor : public Sensor { void sendData() override final; void startCalibration(int calibrationType) override final; SensorStatus getSensorState() override final; + void printGyroScaleCalibration() override final; + void setGyroScaleCalibration(float xScale, float yScale, float zScale) override final; + void resetGyroScaleCalibration() override final; + void testGyroScaleCalibration() override final; + private: Quat fusedRotationToQuaternion(float x, float y, float z); LSM6DSV16XStatusTypeDef runSelfTest(); LSM6DSV16XStatusTypeDef loadIMUCalibration(); LSM6DSV16XStatusTypeDef readFifo(uint16_t fifo_samples); + LSM6DSV16XStatusTypeDef readNextFifoFrame(); + void apply6DToRestDetection(); + void waitForRest(); + void waitForMovement(); + void saveCalibration(); LSM6DSV16X imu; uint8_t m_IntPin; @@ -116,7 +131,7 @@ class LSM6DSV16XSensor : public Sensor { #ifdef LSM6DSV16X_ESP_FUSION SlimeVR::Configuration::LSM6DSV16XCalibrationConfig m_Calibration; - SlimeVR::Sensors::SensorFusion sfusion; + SlimeVR::Sensors::SensorFusionRestDetect sfusion; float rawGyro[3]; bool newRawGyro = false; Quat previousEspRotation; diff --git a/src/sensors/sensor.cpp b/src/sensors/sensor.cpp index 54f1ad893..9ed8a1c43 100644 --- a/src/sensors/sensor.cpp +++ b/src/sensors/sensor.cpp @@ -55,6 +55,15 @@ void Sensor::printDebugTemperatureCalibrationState() { printTemperatureCalibrati void Sensor::saveTemperatureCalibration() { printTemperatureCalibrationUnsupported(); }; void Sensor::resetTemperatureCalibrationState() { printTemperatureCalibrationUnsupported(); }; +void Sensor::printGyroScaleCalibrationUnsupported() { + m_Logger.error("Gyroscope Scale calibration not supported for IMU %s", getIMUNameByType(sensorType)); +} +void Sensor::printGyroScaleCalibration() { printGyroScaleCalibrationUnsupported(); }; +void Sensor::setGyroScaleCalibration(float xScale, float yScale, float zScale) { printGyroScaleCalibrationUnsupported(); }; +void Sensor::resetGyroScaleCalibration() { printGyroScaleCalibrationUnsupported(); }; +void Sensor::testGyroScaleCalibration() { printGyroScaleCalibrationUnsupported(); }; + + const char * getIMUNameByType(int imuType) { switch(imuType) { case IMU_MPU9250: diff --git a/src/sensors/sensor.h b/src/sensors/sensor.h index b736756d9..8c0ca4c5c 100644 --- a/src/sensors/sensor.h +++ b/src/sensors/sensor.h @@ -64,6 +64,10 @@ class Sensor virtual void printDebugTemperatureCalibrationState(); virtual void resetTemperatureCalibrationState(); virtual void saveTemperatureCalibration(); + virtual void printGyroScaleCalibration(); + virtual void setGyroScaleCalibration(float xScale, float yScale, float zScale); + virtual void resetGyroScaleCalibration(); + virtual void testGyroScaleCalibration(); bool isWorking() { return working; }; @@ -108,6 +112,7 @@ class Sensor private: void printTemperatureCalibrationUnsupported(); + void printGyroScaleCalibrationUnsupported(); }; const char * getIMUNameByType(int imuType); diff --git a/src/serial/serialcommands.cpp b/src/serial/serialcommands.cpp index 2158ec469..ce5d53832 100644 --- a/src/serial/serialcommands.cpp +++ b/src/serial/serialcommands.cpp @@ -218,12 +218,56 @@ namespace SerialCommands { logger.info(" Temperature calibration config saves automatically when calibration percent is at 100%"); } + void cmdGyroScaleCalibration(CmdParser* parser) { + if (parser->getParamCount() > 1) { + if (parser->equalCmdParam(1, "GET")) { + for (auto sensor : sensorManager.getSensors()) { + sensor->printGyroScaleCalibration(); + } + return; + } else if (parser->equalCmdParam(1, "SET")) { + for (auto sensor : sensorManager.getSensors()) { + sensor->setGyroScaleCalibration( + std::atof(parser->getCmdParam(2)), + std::atof(parser->getCmdParam(3)), + std::atof(parser->getCmdParam(4)) + ); + } + return; + } else if (parser->equalCmdParam(1, "RESET")) { + for (auto sensor : sensorManager.getSensors()) { + sensor->resetGyroScaleCalibration(); + } + return; + } else if (parser->equalCmdParam(1, "TEST")) { + for (auto sensor : sensorManager.getSensors()) { + if (parser->getParamCount() > 2) { + sensor->setGyroScaleCalibration( + std::atof(parser->getCmdParam(2)), + std::atof(parser->getCmdParam(3)), + std::atof(parser->getCmdParam(4)) + ); + } + sensor->testGyroScaleCalibration(); + } + return; + } + } + logger.info("Usage:"); + logger.info(" GSCALE GET: Prints out current gyroscope scale calibration values"); + logger.info(" GSCALE SET \"X\" \"Y\" \"Z\": Sets the calibration after you guess a new value"); + logger.info(" GSCALE RESET: Resets the GSCALE calibration to X: 1.0, Y: 1.0, Z: 1.0"); + logger.info(" GSCALE TEST: Test the current configuration by following the instructions"); + logger.info(" GSCALE TEST \"X\" \"Y\" \"Z\": Sets then tests the new values"); + } + void setUp() { cmdCallbacks.addCmd("SET", &cmdSet); cmdCallbacks.addCmd("GET", &cmdGet); cmdCallbacks.addCmd("FRST", &cmdFactoryReset); cmdCallbacks.addCmd("REBOOT", &cmdReboot); cmdCallbacks.addCmd("TCAL", &cmdTemperatureCalibration); + cmdCallbacks.addCmd("GSCALE", &cmdGyroScaleCalibration); } void update() { From bf800a5eaf0c830e49d0d31f95fada7f2910c027 Mon Sep 17 00:00:00 2001 From: Carter Meyers Date: Mon, 21 Aug 2023 11:14:15 -0700 Subject: [PATCH 35/60] Put esp fusion features behind define flags --- src/sensors/lsm6dsv16xsensor.cpp | 148 ++++++++++++++++++------------- src/sensors/lsm6dsv16xsensor.h | 26 +++--- 2 files changed, 102 insertions(+), 72 deletions(-) diff --git a/src/sensors/lsm6dsv16xsensor.cpp b/src/sensors/lsm6dsv16xsensor.cpp index 165787e21..e80fa07fa 100644 --- a/src/sensors/lsm6dsv16xsensor.cpp +++ b/src/sensors/lsm6dsv16xsensor.cpp @@ -44,14 +44,17 @@ LSM6DSV16XSensor::LSM6DSV16XSensor( uint8_t intPin ) : Sensor("LSM6DSV16XSensor", type, id, address, rotation, sclPin, sdaPin), imu(&Wire, addr << 1), // We shift the address left 1 to work with the library - m_IntPin(intPin), - sfusion( + m_IntPin(intPin) +#ifdef LSM6DSV16X_ESP_FUSION + , sfusion( 1.0f / LSM6DSV16X_FIFO_DATA_RATE, 1.0f / LSM6DSV16X_FIFO_DATA_RATE, - -1.0f - ){}; + -1.0f ) +#endif + {}; void LSM6DSV16XSensor::motionSetup() { +#ifdef LSM6DSV16X_ESP_FUSION RestDetectionParams restDetectionParams; restDetectionParams.restMinTimeMicros = 3 * 1e6; //restDetectionParams.restThAcc = 0.01f; @@ -59,6 +62,7 @@ void LSM6DSV16XSensor::motionSetup() { restDetectionParams.restThAcc = 0.05f; restDetectionParams.restThGyr = 0.25f; //dps sfusion.updateRestDetectionParameters(restDetectionParams); +#endif uint8_t deviceId = 0; if (imu.ReadID(&deviceId) == LSM6DSV16X_ERROR) { @@ -153,20 +157,18 @@ void LSM6DSV16XSensor::motionSetup() { status |= imu.FIFO_Set_Mode(LSM6DSV16X_STREAM_MODE); #if defined(LSM6DSV16X_ONBOARD_FUSION) && defined(LSM6DSV16X_ESP_FUSION) - status |= imu.FIFO_Set_SFLP_Batch(true, true, false); - status |= imu.Enable_Game_Rotation(); - loadIMUCalibration(); + status |= imu.FIFO_Set_SFLP_Batch(true, true, false); // TODO: Don't use batch change for this + status |= imu.Enable_Game_Rotation(); // TODO: Rename to enable SFLP - // Calibration - if (isFaceDown) { - startCalibration(0); // can not calibrate onboard fusion - } #elif defined(LSM6DSV16X_ONBOARD_FUSION) status |= imu.FIFO_Set_SFLP_Batch(true, true, false); status |= imu.Enable_Game_Rotation(); #elif defined(LSM6DSV16X_ESP_FUSION) - status |= imu.FIFO_Set_SFLP_Batch(false, true, false); + status |= imu.FIFO_Set_SFLP_Batch(false, true, false); // TODO: we alread calc lin accel, maybe use that status |= imu.Enable_Game_Rotation(); +#endif + +#ifdef LSM6DSV16X_ESP_FUSION loadIMUCalibration(); // Calibration @@ -184,6 +186,7 @@ void LSM6DSV16XSensor::motionSetup() { status |= imu.Enable_Single_Tap_Detection(LSM6DSV16X_INT1_PIN); // Tap requires an interrupt status |= imu.Enable_Double_Tap_Detection(LSM6DSV16X_INT1_PIN); #endif // LSM6DSV16X_INTERRUPT + status |= imu.FIFO_Set_Mode(LSM6DSV16X_BYPASS_MODE); // add to lib but we need to status |= imu.FIFO_Set_Mode(LSM6DSV16X_STREAM_MODE); // get / keep track of current fifo type @@ -319,6 +322,23 @@ void LSM6DSV16XSensor::motionLoop() { newRawGyro = false; } #endif + +#ifdef DEBUG_SENSOR + Vector3 rotation = lastFusedRotationSent.get_euler() * dpsPerRad; + m_Logger.trace(",%f,%f,%f,%d,%f,%d,%f,%f,%f,%f,%f,%f", + rotation.x, + rotation.y, + rotation.z, + currentDataTime, + (float)(currentDataTime - previousDataTime) * LSM6DSV16X_TIMESTAMP_LSB, + millis(), + rawGyro[0], + rawGyro[1], + rawGyro[2], + rawAcceleration[0], + rawAcceleration[1], + rawAcceleration[2]); +#endif // DEBUG_SENSOR } SensorStatus LSM6DSV16XSensor::getSensorState() { @@ -359,37 +379,6 @@ LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::runSelfTest() { return LSM6DSV16X_OK; } -LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::loadIMUCalibration() { - SlimeVR::Configuration::CalibrationConfig sensorCalibration = configuration.getCalibration(sensorId); - // If no compatible calibration data is found, the calibration data will just be zero-ed out - switch (sensorCalibration.type) { - case SlimeVR::Configuration::CalibrationConfigType::LSM6DSV16X: - m_Calibration = sensorCalibration.data.lsm6dsv16x; - break; - - case SlimeVR::Configuration::CalibrationConfigType::NONE: - m_Logger.warn("No calibration data found for sensor %d, ignoring...", sensorId); - m_Logger.info("Calibration is advised"); - break; - - default: - m_Logger.warn("Incompatible calibration data found for sensor %d, ignoring...", sensorId); - m_Logger.info("Calibration is advised"); - } - -#ifdef LSM6DSV16X_ACCEL_OFFSET_CAL - int8_t status = 0; - status |= imu.Set_X_User_Offset( - m_Calibration.A_off[0], - m_Calibration.A_off[1], - m_Calibration.A_off[2] - ); - status |= imu.Enable_X_User_Offset(); - return (LSM6DSV16XStatusTypeDef)status; -#endif - return LSM6DSV16X_OK; -} - void LSM6DSV16XSensor::sendData() { if (newFusedRotation) { newFusedRotation = false; @@ -425,24 +414,8 @@ void LSM6DSV16XSensor::sendData() { } } -// Used for calibration (Blocking) -LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::readNextFifoFrame() { - uint16_t fifo_samples = 0; - while (fifo_samples < LSM6DSV16X_FIFO_FRAME_SIZE) { - if (imu.FIFO_Get_Num_Samples(&fifo_samples) == LSM6DSV16X_ERROR) { - m_Logger.error( - "Error getting number of samples in FIFO on %s at address 0x%02x", - getIMUNameByType(sensorType), - addr - ); - return LSM6DSV16X_ERROR; - } - } - return readFifo(LSM6DSV16X_FIFO_FRAME_SIZE); -} - - LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::readFifo(uint16_t fifo_samples) { - for (uint16_t i = 0; i < fifo_samples; i++) { +LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::readFifo(uint16_t fifo_samples) { + for (uint16_t i = 0; i < fifo_samples; i++) { uint8_t tag; if (imu.FIFO_Get_Tag(&tag) != LSM6DSV16X_OK) { m_Logger.error( @@ -517,7 +490,7 @@ LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::readNextFifoFrame() { gravityVector[2] /= mgPerG; newGravityVector = true; -#if defined(LSM6DSV16X_ESP_FUSION) && defined(LSM6DSV16X_ACCEL_OFFSET_CAL) +#ifdef LSM6DSV16X_ACCEL_OFFSET_CAL // The SFLP block does not use the accelerometer calibration gravityVector[0] += m_Calibration.G_off[0]; gravityVector[1] += m_Calibration.G_off[1]; @@ -595,6 +568,56 @@ LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::readNextFifoFrame() { return LSM6DSV16X_OK; } + + +#ifdef LSM6DSV16X_ESP_FUSION +// Used for calibration (Blocking) +LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::readNextFifoFrame() { + uint16_t fifo_samples = 0; + while (fifo_samples < LSM6DSV16X_FIFO_FRAME_SIZE) { + if (imu.FIFO_Get_Num_Samples(&fifo_samples) == LSM6DSV16X_ERROR) { + m_Logger.error( + "Error getting number of samples in FIFO on %s at address 0x%02x", + getIMUNameByType(sensorType), + addr + ); + return LSM6DSV16X_ERROR; + } + } + return readFifo(LSM6DSV16X_FIFO_FRAME_SIZE); +} + +LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::loadIMUCalibration() { + SlimeVR::Configuration::CalibrationConfig sensorCalibration = configuration.getCalibration(sensorId); + // If no compatible calibration data is found, the calibration data will just be zero-ed out + switch (sensorCalibration.type) { + case SlimeVR::Configuration::CalibrationConfigType::LSM6DSV16X: + m_Calibration = sensorCalibration.data.lsm6dsv16x; + break; + + case SlimeVR::Configuration::CalibrationConfigType::NONE: + m_Logger.warn("No calibration data found for sensor %d, ignoring...", sensorId); + m_Logger.info("Calibration is advised"); + break; + + default: + m_Logger.warn("Incompatible calibration data found for sensor %d, ignoring...", sensorId); + m_Logger.info("Calibration is advised"); + } + +#ifdef LSM6DSV16X_ACCEL_OFFSET_CAL + int8_t status = 0; + status |= imu.Set_X_User_Offset( + m_Calibration.A_off[0], + m_Calibration.A_off[1], + m_Calibration.A_off[2] + ); + status |= imu.Enable_X_User_Offset(); + return (LSM6DSV16XStatusTypeDef)status; +#endif + return LSM6DSV16X_OK; +} + void LSM6DSV16XSensor::startCalibration(int calibrationType) { m_Logger.info("Flip right side up in the next 5 seconds to start calibration."); delay(5000); @@ -955,3 +978,4 @@ void LSM6DSV16XSensor::waitForMovement() { apply6DToRestDetection(); } } +#endif // LSM6DSV16X_ESP_FUSION diff --git a/src/sensors/lsm6dsv16xsensor.h b/src/sensors/lsm6dsv16xsensor.h index 1c2c2b091..42d6e5740 100644 --- a/src/sensors/lsm6dsv16xsensor.h +++ b/src/sensors/lsm6dsv16xsensor.h @@ -60,9 +60,13 @@ #define LSM6DSV16X_ESP_FUSION + +#ifdef LSM6DSV16X_ESP_FUSION #define LSM6DSV16X_GYRO_OFFSET_CAL #define LSM6DSV16X_ACCEL_OFFSET_CAL #define LSM6DSV16X_GYRO_SCALE_CAL +#endif + #ifdef REINIT_ON_FAILURE #define REINIT_RETRY_MAX_ATTEMPTS 5 @@ -92,24 +96,20 @@ class LSM6DSV16XSensor : public Sensor { void motionSetup() override final; void motionLoop() override final; void sendData() override final; - void startCalibration(int calibrationType) override final; SensorStatus getSensorState() override final; + +#ifdef LSM6DSV16X_ESP_FUSION + void startCalibration(int calibrationType) override final; void printGyroScaleCalibration() override final; void setGyroScaleCalibration(float xScale, float yScale, float zScale) override final; void resetGyroScaleCalibration() override final; void testGyroScaleCalibration() override final; +#endif - private: Quat fusedRotationToQuaternion(float x, float y, float z); LSM6DSV16XStatusTypeDef runSelfTest(); - LSM6DSV16XStatusTypeDef loadIMUCalibration(); LSM6DSV16XStatusTypeDef readFifo(uint16_t fifo_samples); - LSM6DSV16XStatusTypeDef readNextFifoFrame(); - void apply6DToRestDetection(); - void waitForRest(); - void waitForMovement(); - void saveCalibration(); LSM6DSV16X imu; uint8_t m_IntPin; @@ -122,6 +122,8 @@ class LSM6DSV16XSensor : public Sensor { bool newGravityVector = false; float rawAcceleration[3]; bool newRawAcceleration = false; + uint32_t previousDataTime = 0; + uint32_t currentDataTime = 0; #ifdef LSM6DSV16X_ONBOARD_FUSION float fusedGameRotation[3]; @@ -130,13 +132,17 @@ class LSM6DSV16XSensor : public Sensor { #endif #ifdef LSM6DSV16X_ESP_FUSION + LSM6DSV16XStatusTypeDef readNextFifoFrame(); + LSM6DSV16XStatusTypeDef loadIMUCalibration(); + void apply6DToRestDetection(); + void waitForRest(); + void waitForMovement(); + void saveCalibration(); SlimeVR::Configuration::LSM6DSV16XCalibrationConfig m_Calibration; SlimeVR::Sensors::SensorFusionRestDetect sfusion; float rawGyro[3]; bool newRawGyro = false; Quat previousEspRotation; - uint32_t previousDataTime = 0; - uint32_t currentDataTime = 0; uint8_t previousTag = 0; #endif From 1a1948517009a93689a8414db275a463073bed7b Mon Sep 17 00:00:00 2001 From: wigwagwent Date: Mon, 21 Aug 2023 12:17:52 -0700 Subject: [PATCH 36/60] Set values with defines, gyro dsp 2000 --- src/sensors/lsm6dsv16xsensor.cpp | 8 ++++---- src/sensors/lsm6dsv16xsensor.h | 6 +++++- 2 files changed, 9 insertions(+), 5 deletions(-) diff --git a/src/sensors/lsm6dsv16xsensor.cpp b/src/sensors/lsm6dsv16xsensor.cpp index 79533e45a..561cd7d95 100644 --- a/src/sensors/lsm6dsv16xsensor.cpp +++ b/src/sensors/lsm6dsv16xsensor.cpp @@ -109,12 +109,12 @@ void LSM6DSV16XSensor::motionSetup() { status |= imu.Set_Auto_Increment(true); // Set maximums - status |= imu.Set_X_FS(16); - status |= imu.Set_G_FS(1000); + status |= imu.Set_X_FS(LSM6DSV16X_ACCEL_MAX); + status |= imu.Set_G_FS(LSM6DSV16X_GYRO_MAX); // Set data rate - status |= imu.Set_X_ODR(7680.0f, LSM6DSV16X_ACC_HIGH_PERFORMANCE_MODE); - status |= imu.Set_G_ODR(7680.0f, LSM6DSV16X_GYRO_HIGH_PERFORMANCE_MODE); + status |= imu.Set_X_ODR(LSM6DSV16X_GYRO_ACCEL_RATE, LSM6DSV16X_ACC_HIGH_PERFORMANCE_MODE); + status |= imu.Set_G_ODR(LSM6DSV16X_GYRO_ACCEL_RATE, LSM6DSV16X_GYRO_HIGH_PERFORMANCE_MODE); status |= imu.Set_SFLP_ODR(LSM6DSV16X_FIFO_DATA_RATE); status |= imu.FIFO_Set_X_BDR(LSM6DSV16X_FIFO_DATA_RATE); diff --git a/src/sensors/lsm6dsv16xsensor.h b/src/sensors/lsm6dsv16xsensor.h index 4479d8480..fbc19c8b7 100644 --- a/src/sensors/lsm6dsv16xsensor.h +++ b/src/sensors/lsm6dsv16xsensor.h @@ -28,7 +28,7 @@ #include "sensor.h" #ifndef LSM6DSV16X_ACCEL_MAX -#define LSM6DSV16X_ACCEL_MAX 4 +#define LSM6DSV16X_ACCEL_MAX 16 #endif #ifndef LSM6DSV16X_GYRO_MAX @@ -39,6 +39,10 @@ #define LSM6DSV16X_FIFO_DATA_RATE 120 #endif +#ifndef LSM6DSV16X_GYRO_ACCEL_RATE +#define LSM6DSV16X_GYRO_ACCEL_RATE 7680.0f +#endif + #ifndef LSM6DSV16X_FIFO_TEMP_DATA_RATE #define LSM6DSV16X_FIFO_TEMP_DATA_RATE 1.875f #endif From fd1a8bda9ad0799ddfeb07be7893fc03708a0854 Mon Sep 17 00:00:00 2001 From: wigwagwent Date: Wed, 30 Aug 2023 15:40:55 -0700 Subject: [PATCH 37/60] Better Gyroscope sensitivity calibration --- src/configuration/CalibrationConfig.h | 4 +- src/sensors/lsm6dsv16xsensor.cpp | 431 +++++++++++++++++--------- src/sensors/lsm6dsv16xsensor.h | 9 +- src/sensors/sensor.cpp | 45 ++- src/sensors/sensor.h | 38 ++- src/serial/serialcommands.cpp | 100 ++++-- 6 files changed, 433 insertions(+), 194 deletions(-) diff --git a/src/configuration/CalibrationConfig.h b/src/configuration/CalibrationConfig.h index 502f44682..386ffb778 100644 --- a/src/configuration/CalibrationConfig.h +++ b/src/configuration/CalibrationConfig.h @@ -80,8 +80,8 @@ namespace SlimeVR { // gyroscope bias float G_off[3]; - // gyroscope scale - float G_scale[3]; + // gyroscope sensitivity + float G_sensitivity[3]; // accelerometer bias float A_off[3]; diff --git a/src/sensors/lsm6dsv16xsensor.cpp b/src/sensors/lsm6dsv16xsensor.cpp index e166cf5af..bc8448cca 100644 --- a/src/sensors/lsm6dsv16xsensor.cpp +++ b/src/sensors/lsm6dsv16xsensor.cpp @@ -59,8 +59,8 @@ void LSM6DSV16XSensor::motionSetup() { restDetectionParams.restMinTimeMicros = 3 * 1e6; //restDetectionParams.restThAcc = 0.01f; //restDetectionParams.restThGyr = 0.15f; //dps - restDetectionParams.restThAcc = 0.05f; - restDetectionParams.restThGyr = 0.25f; //dps + restDetectionParams.restThAcc = 0.1f; + restDetectionParams.restThGyr = 0.5f; //dps sfusion.updateRestDetectionParameters(restDetectionParams); #endif @@ -526,9 +526,9 @@ LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::readFifo(uint16_t fifo_samples) { rawGyro[2] -= m_Calibration.G_off[2]; #endif #ifdef LSM6DSV16X_GYRO_SCALE_CAL - rawGyro[0] *= m_Calibration.G_scale[0]; - rawGyro[1] *= m_Calibration.G_scale[1]; - rawGyro[2] *= m_Calibration.G_scale[2]; + rawGyro[0] *= m_Calibration.G_sensitivity[0]; + rawGyro[1] *= m_Calibration.G_sensitivity[1]; + rawGyro[2] *= m_Calibration.G_sensitivity[2]; #endif newRawGyro = true; @@ -618,85 +618,22 @@ LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::loadIMUCalibration() { return LSM6DSV16X_OK; } -void LSM6DSV16XSensor::startCalibration(int calibrationType) { - m_Logger.info("Flip right side up in the next 5 seconds to start calibration."); - delay(5000); - uint8_t isFaceUp; - imu.Get_6D_Orientation_ZH(&isFaceUp); - - if (!isFaceUp) { - m_Logger.info("This is your last chance to flip the tracker over. I am warning you"); - delay(5000); - uint8_t isFaceDown; - imu.Get_6D_Orientation_ZL(&isFaceDown); - - if (!isFaceDown) { - return; - } - testGyroScaleCalibration(); - return; - } - -#ifdef LSM6DSV16X_GYRO_OFFSET_CAL - ledManager.on(); - constexpr uint16_t calibrationSamples = 300; - // Reset values - float tempGxyz[3] = {0, 0, 0}; - - m_Calibration.G_off[0] = 0.0f; - m_Calibration.G_off[1] = 0.0f; - m_Calibration.G_off[2] = 0.0f; - float tempGscale[3]; - tempGscale[0] = m_Calibration.G_scale[0]; - tempGscale[1] = m_Calibration.G_scale[1]; - tempGscale[2] = m_Calibration.G_scale[2]; - - m_Calibration.G_scale[0] = 1.0f; - m_Calibration.G_scale[1] = 1.0f; - m_Calibration.G_scale[2] = 1.0f; - - // Wait for sensor to calm down before calibration - m_Logger.info("Put down the device and wait for baseline gyro reading calibration"); - delay(2000); - - imu.FIFO_Set_Mode(LSM6DSV16X_BYPASS_MODE); // add to lib but we need to - imu.FIFO_Set_Mode(LSM6DSV16X_STREAM_MODE); // get / keep track of current fifo type - waitForRest(); - uint16_t count = 0; - while (count < calibrationSamples) { - readNextFifoFrame(); - if (newRawGyro) { - tempGxyz[0] += rawGyro[0]; - tempGxyz[1] += rawGyro[1]; - tempGxyz[2] += rawGyro[2]; - count++; - } - } - m_Calibration.G_off[0] = tempGxyz[0] / calibrationSamples; - m_Calibration.G_off[1] = tempGxyz[1] / calibrationSamples; - m_Calibration.G_off[2] = tempGxyz[2] / calibrationSamples; - m_Calibration.G_scale[0] = tempGscale[0]; - m_Calibration.G_scale[1] = tempGscale[1]; - m_Calibration.G_scale[2] = tempGscale[2]; -#ifdef DEBUG_SENSOR - m_Logger.trace( - "Gyro calibration results: %f %f %f", - tempGxyz[0], - tempGxyz[1], - tempGxyz[2] - ); -#endif -#endif #ifdef LSM6DSV16X_ACCEL_OFFSET_CAL - //Accelerometer Calibration +void LSM6DSV16XSensor::calibrateAccel() { + m_Logger.info( + "Accel offset calibration started on sensor #%d of type %s at address 0x%02x", + getSensorId(), + getIMUNameByType(sensorType), + addr + ); MagnetoCalibration* magneto = new MagnetoCalibration(); imu.Disable_X_User_Offset(); @@ -815,6 +752,14 @@ void LSM6DSV16XSensor::startCalibration(int calibrationType) { m_Calibration.A_off[0] = A_BAinv[0][0]; m_Calibration.A_off[1] = A_BAinv[0][1]; m_Calibration.A_off[2] = A_BAinv[0][2]; + saveCalibration(); + + imu.Set_X_User_Offset( + m_Calibration.A_off[0], + m_Calibration.A_off[1], + m_Calibration.A_off[2] + ); + lastData = millis(); #ifdef DEBUG_SENSOR m_Logger.trace( @@ -824,64 +769,114 @@ void LSM6DSV16XSensor::startCalibration(int calibrationType) { A_BAinv[0][2] ); #endif +} #endif - saveCalibration(); - ledManager.off(); - m_Logger.info("Calibration finished, enjoy"); -} -void LSM6DSV16XSensor::printGyroScaleCalibration() { - m_Logger.info(""); - m_Logger.info("Gyro Scale Calibration Values for %s on 0x%02x", getIMUNameByType(sensorType), addr); - m_Logger.info("X Scale: %f", m_Calibration.G_scale[0]); - m_Logger.info("Y Scale: %f", m_Calibration.G_scale[1]); - m_Logger.info("Z Scale: %f", m_Calibration.G_scale[2]); - - m_Logger.info("Gyro Offset Calibration Values for %s on 0x%02x", getIMUNameByType(sensorType), addr); - m_Logger.info("X Scale: %f", m_Calibration.G_off[0]); - m_Logger.info("Y Scale: %f", m_Calibration.G_off[1]); - m_Logger.info("Z Scale: %f", m_Calibration.G_off[2]); - - m_Logger.info("Accel Offset Calibration Values for %s on 0x%02x", getIMUNameByType(sensorType), addr); - m_Logger.info("X Scale: %f", m_Calibration.A_off[0]); - m_Logger.info("Y Scale: %f", m_Calibration.A_off[1]); - m_Logger.info("Z Scale: %f", m_Calibration.A_off[2]); -} -void LSM6DSV16XSensor::setGyroScaleCalibration(float xScale, float yScale, float zScale) { - m_Logger.info("Gyro Scale Calibration Updated"); - m_Calibration.G_scale[0] = xScale; - m_Calibration.G_scale[1] = yScale; - m_Calibration.G_scale[2] = zScale; - saveCalibration(); -} -void LSM6DSV16XSensor::resetGyroScaleCalibration() { - m_Calibration.G_scale[0] = 1.0f; - m_Calibration.G_scale[1] = 1.0f; - m_Calibration.G_scale[2] = 1.0f; + + + + + + + + + + + + +#ifdef LSM6DSV16X_GYRO_OFFSET_CAL +void LSM6DSV16XSensor::calibrateGyro() { + m_Logger.info( + "Gyro offset calibration started on sensor #%d of type %s at address 0x%02x", + getSensorId(), + getIMUNameByType(sensorType), + addr + ); + ledManager.on(); + constexpr uint16_t calibrationSamples = 300; + // Reset values + float tempGxyz[3] = {0, 0, 0}; + + m_Calibration.G_off[0] = 0.0f; + m_Calibration.G_off[1] = 0.0f; + m_Calibration.G_off[2] = 0.0f; + + float tempGsensitivity[3]; + tempGsensitivity[0] = m_Calibration.G_sensitivity[0]; + tempGsensitivity[1] = m_Calibration.G_sensitivity[1]; + tempGsensitivity[2] = m_Calibration.G_sensitivity[2]; + + m_Calibration.G_sensitivity[0] = 1.0f; + m_Calibration.G_sensitivity[1] = 1.0f; + m_Calibration.G_sensitivity[2] = 1.0f; + + + // Wait for sensor to calm down before calibration + m_Logger.info("Put down the device and wait for baseline gyro reading calibration"); + delay(2000); + + imu.FIFO_Set_Mode(LSM6DSV16X_BYPASS_MODE); // TODO: add to lib but we need to + imu.FIFO_Set_Mode(LSM6DSV16X_STREAM_MODE); // get / keep track of current fifo type + waitForRest(); + uint16_t count = 0; + while (count < calibrationSamples) { + readNextFifoFrame(); + if (newRawGyro) { + tempGxyz[0] += rawGyro[0]; + tempGxyz[1] += rawGyro[1]; + tempGxyz[2] += rawGyro[2]; + count++; + } + } + ledManager.off(); + m_Calibration.G_off[0] = tempGxyz[0] / calibrationSamples; + m_Calibration.G_off[1] = tempGxyz[1] / calibrationSamples; + m_Calibration.G_off[2] = tempGxyz[2] / calibrationSamples; + + m_Calibration.G_sensitivity[0] = tempGsensitivity[0]; + m_Calibration.G_sensitivity[1] = tempGsensitivity[1]; + m_Calibration.G_sensitivity[2] = tempGsensitivity[2]; saveCalibration(); + lastData = millis(); + + +#ifdef DEBUG_SENSOR + m_Logger.trace( + "Gyro calibration results: %f %f %f", + tempGxyz[0], + tempGxyz[1], + tempGxyz[2] + ); +#endif } +#endif -void LSM6DSV16XSensor::testGyroScaleCalibration() { #ifdef LSM6DSV16X_GYRO_SCALE_CAL - //Gyroscope scale - m_Logger.info(""); - m_Logger.info(""); +void LSM6DSV16XSensor::calibrateGyroSensitivity() { m_Logger.info( - "Congrats you made it to the advance calibrations, let the tracker sit still and don't " - "touch" + "Gyro sensitivity calibration started on sensor #%d of type %s at address 0x%02x", + getSensorId(), + getIMUNameByType(sensorType), + addr ); - printGyroScaleCalibration(); + m_Calibration.G_sensitivity[0] = 1.0f; + m_Calibration.G_sensitivity[1] = 1.0f; + m_Calibration.G_sensitivity[2] = 1.0f; - Vector3 rawRotations[4]; + imu.Enable_6D_Orientation(LSM6DSV16X_INT2_PIN); + Vector3 rawRotationInit; + Vector3 rawRotationFinal; uint8_t count = 0; + float gyroCount[3]; //Use this to determine the axis spun + float calculatedScale[3] = {1.0f, 1.0f, 1.0f}; + constexpr uint8_t fullRotations = 1; ledManager.off(); - waitForRest(); m_Logger.info(""); m_Logger.info( @@ -890,61 +885,199 @@ void LSM6DSV16XSensor::testGyroScaleCalibration() { ); m_Logger.info("\tStep 2: Let the tracker rest until the light turns on"); m_Logger.info( - "\tStep 3: Rotate the tracker in one axis 360 degrees and align with the " - "previous edge." + "\tStep 3: Rotate the tracker about one axis %d full rotations and align with the " + "previous edge.", + fullRotations ); m_Logger.info("\t\tNOTE: the light will turn off after you start moving it"); m_Logger.info( - "\tStep 4: Repeat step 2 and 3 in the same axis going the opposite direction" + "\tStep 4: Repeat step 1 - 3 in the other 2 axis. When the light is on you should move it" ); - ledManager.on(); - waitForMovement(); - - delay(2000); - imu.FIFO_Set_Mode(LSM6DSV16X_BYPASS_MODE); // add to lib but we need to - imu.FIFO_Set_Mode(LSM6DSV16X_STREAM_MODE); // get / keep track of current fifo type - waitForRest(); - while (count < 2) { - ledManager.on(); // The user should rotate - m_Logger.info("Rotate the tracker 360 degrees"); + while (count < 3) { + ledManager.on(); + waitForMovement(); //position tracker + ledManager.off(); + waitForRest(); + ledManager.on(); // The user should rotate + m_Logger.info("Rotate the tracker %d times", fullRotations); + gyroCount[0] = 0.0f; + gyroCount[1] = 0.0f; + gyroCount[2] = 0.0f; + rawRotationInit = sfusion.getQuaternionQuat().get_euler() * dpsPerRad; waitForMovement(); - rawRotations[count * 2] = sfusion.getQuaternionQuat().get_euler() * dpsPerRad; ledManager.off(); - waitForRest(); - rawRotations[count * 2 + 1] = sfusion.getQuaternionQuat().get_euler() * dpsPerRad; + + + while (!sfusion.getRestDetected()) { //wait for rest + readNextFifoFrame(); + gyroCount[0] += rawGyro[0]; + gyroCount[1] += rawGyro[1]; + gyroCount[2] += rawGyro[2]; + apply6DToRestDetection(); + } + + uint8_t isUp; + + + rawRotationFinal = sfusion.getQuaternionQuat().get_euler() * dpsPerRad; + + + + + if (abs(gyroCount[0]) > abs(gyroCount[1]) && abs(gyroCount[0]) > abs(gyroCount[2])) { //Spun in X + imu.Get_6D_Orientation_XH(&isUp); + if((!isUp && gyroCount[0] > 0) || (isUp && gyroCount[0] < 0)) { + calculatedScale[0] = (1.0 / (1.0 - ((rawRotationInit.y - rawRotationFinal.y)/(360.0f * fullRotations)))); + m_Logger.info("X, Diff: %f", rawRotationInit.y - rawRotationFinal.y); + } + else { + calculatedScale[0] = (1.0 / (1.0 - ((rawRotationFinal.y - rawRotationInit.y)/(360.0f * fullRotations)))); + m_Logger.info("-X, Diff: %f", rawRotationFinal.y - rawRotationInit.y); + } + } + + else if (abs(gyroCount[1]) > abs(gyroCount[0]) && abs(gyroCount[1]) > abs(gyroCount[2])) { //Spun in Y + imu.Get_6D_Orientation_YH(&isUp); + if((isUp && gyroCount[1] > 0) || (!isUp && gyroCount[1] < 0)) { + calculatedScale[1] = (1.0 / (1.0 - ((rawRotationInit.y - rawRotationFinal.y)/(360.0f * fullRotations)))); + m_Logger.info("Y, Diff: %f", rawRotationInit.y - rawRotationFinal.y); + } + else { + calculatedScale[1] = (1.0 / (1.0 - ((rawRotationFinal.y - rawRotationInit.y)/(360.0f * fullRotations)))); + m_Logger.info("-Y, Diff: %f", rawRotationFinal.y - rawRotationInit.y); + } + } + + else if (abs(gyroCount[2]) > abs(gyroCount[0]) && abs(gyroCount[2]) > abs(gyroCount[1])) { //Spun in Z + imu.Get_6D_Orientation_ZH(&isUp); + if((isUp && gyroCount[2] > 0) || (!isUp && gyroCount[2] < 0)) { + calculatedScale[2] = (1.0 / (1.0 - ((rawRotationInit.y - rawRotationFinal.y)/(360.0f * fullRotations)))); + m_Logger.info("Z, Diff: %f", rawRotationInit.y - rawRotationFinal.y); + } + else { + calculatedScale[2] = (1.0 / (1.0 - ((rawRotationFinal.y - rawRotationInit.y)/(360.0f * fullRotations)))); + m_Logger.info("-Z, Diff: %f", rawRotationFinal.y - rawRotationInit.y); + } + } count++; } + m_Calibration.G_sensitivity[0] = calculatedScale[0]; + m_Calibration.G_sensitivity[1] = calculatedScale[1]; + m_Calibration.G_sensitivity[2] = calculatedScale[2]; + saveCalibration(); - m_Logger.info( - "Drift First Rotation: %f, %f, %f", - rawRotations[0].x - rawRotations[1].x, - rawRotations[0].y - rawRotations[1].y, - rawRotations[0].z - rawRotations[1].z + m_Logger.debug( + "Gyro Sensitivity calibration results: %f %f %f", + calculatedScale[0], + calculatedScale[1], + calculatedScale[2] ); - m_Logger.info( - "Drift Second Rotation: %f, %f, %f", - rawRotations[2].x - rawRotations[3].x, - rawRotations[2].y - rawRotations[3].y, - rawRotations[2].z - rawRotations[3].z - ); + imu.Disable_6D_Orientation(); - m_Logger.info( - "Drift Initial - Final: %f, %f, %f", - rawRotations[0].x - rawRotations[3].x, - rawRotations[0].y - rawRotations[3].y, - rawRotations[0].z - rawRotations[3].z - ); lastData = millis(); + +#ifdef DEBUG_SENSOR + m_Logger.trace( + "Gyro Sensitivity calibration results: %f %f %f", + calculatedScale[0], + calculatedScale[1], + calculatedScale[2] + ); #endif } +#endif + + + + + + + + + + + + + + + + + + + + + +void LSM6DSV16XSensor::startCalibration(int calibrationType) { + m_Logger.info("Flip right side up in the next 5 seconds to start calibration."); + delay(5000); + uint8_t isFaceUp; + imu.Get_6D_Orientation_ZH(&isFaceUp); + + if (!isFaceUp) { + m_Logger.info("Flip the tracker over now for gyro sensitivity calibration"); + delay(5000); + imu.Get_6D_Orientation_ZH(&isFaceUp); + + if (!isFaceUp) { + return; + } + calibrateGyroSensitivity(); + return; + } + + +#ifdef LSM6DSV16X_GYRO_OFFSET_CAL + calibrateGyro(); +#endif + +#ifdef LSM6DSV16X_ACCEL_OFFSET_CAL + calibrateAccel(); +#endif + m_Logger.info("Calibration finished, enjoy"); +} + +void LSM6DSV16XSensor::printCalibration() { + m_Logger.info("Sensor #%d Calibration Data", getSensorId()); + m_Logger.info(" Accel Offset Calibration Values for %s on 0x%02x", getIMUNameByType(sensorType), addr); + m_Logger.info(" X Scale: %f", m_Calibration.A_off[0]); + m_Logger.info(" Y Scale: %f", m_Calibration.A_off[1]); + m_Logger.info(" Z Scale: %f", m_Calibration.A_off[2]); + + m_Logger.info(" Gyro Offset Calibration Values for %s on 0x%02x", getIMUNameByType(sensorType), addr); + m_Logger.info(" X Scale: %f", m_Calibration.G_off[0]); + m_Logger.info(" Y Scale: %f", m_Calibration.G_off[1]); + m_Logger.info(" Z Scale: %f", m_Calibration.G_off[2]); + + m_Logger.info(" Gyro Sensitivity Calibration Values for %s on 0x%02x", getIMUNameByType(sensorType), addr); + m_Logger.info(" X Scale: %f", m_Calibration.G_sensitivity[0]); + m_Logger.info(" Y Scale: %f", m_Calibration.G_sensitivity[1]); + m_Logger.info(" Z Scale: %f", m_Calibration.G_sensitivity[2]); +} + +void LSM6DSV16XSensor::resetCalibration() { + m_Logger.info("Sensor #%d Calibration Reset", getSensorId()); + m_Calibration.A_off[0] = 0.0f; + m_Calibration.A_off[1] = 0.0f; + m_Calibration.A_off[2] = 0.0f; + + m_Calibration.G_off[0] = 0.0f; + m_Calibration.G_off[1] = 0.0f; + m_Calibration.G_off[2] = 0.0f; + + m_Calibration.G_sensitivity[0] = 1.0f; + m_Calibration.G_sensitivity[1] = 1.0f; + m_Calibration.G_sensitivity[2] = 1.0f; + saveCalibration(); +} + void LSM6DSV16XSensor::saveCalibration() { m_Logger.debug("Saving the calibration data"); diff --git a/src/sensors/lsm6dsv16xsensor.h b/src/sensors/lsm6dsv16xsensor.h index 3b029a849..65705c052 100644 --- a/src/sensors/lsm6dsv16xsensor.h +++ b/src/sensors/lsm6dsv16xsensor.h @@ -104,10 +104,11 @@ class LSM6DSV16XSensor : public Sensor { #ifdef LSM6DSV16X_ESP_FUSION void startCalibration(int calibrationType) override final; - void printGyroScaleCalibration() override final; - void setGyroScaleCalibration(float xScale, float yScale, float zScale) override final; - void resetGyroScaleCalibration() override final; - void testGyroScaleCalibration() override final; + void calibrateAccel() override final; + void calibrateGyro() override final; + void calibrateGyroSensitivity() override final; + void printCalibration() override final; + void resetCalibration() override final; #endif private: diff --git a/src/sensors/sensor.cpp b/src/sensors/sensor.cpp index 9ed8a1c43..1bb9f27d6 100644 --- a/src/sensors/sensor.cpp +++ b/src/sensors/sensor.cpp @@ -55,13 +55,22 @@ void Sensor::printDebugTemperatureCalibrationState() { printTemperatureCalibrati void Sensor::saveTemperatureCalibration() { printTemperatureCalibrationUnsupported(); }; void Sensor::resetTemperatureCalibrationState() { printTemperatureCalibrationUnsupported(); }; -void Sensor::printGyroScaleCalibrationUnsupported() { - m_Logger.error("Gyroscope Scale calibration not supported for IMU %s", getIMUNameByType(sensorType)); +void Sensor::printCalibrationUnsupported(CalibrationType type) { + m_Logger.warn( + "Calibration of type %s is not supported for IMU %s, sensor # %d, skipping", + getCalibrationNameByType(type), + getIMUNameByType(sensorType), + getSensorId() + ); } -void Sensor::printGyroScaleCalibration() { printGyroScaleCalibrationUnsupported(); }; -void Sensor::setGyroScaleCalibration(float xScale, float yScale, float zScale) { printGyroScaleCalibrationUnsupported(); }; -void Sensor::resetGyroScaleCalibration() { printGyroScaleCalibrationUnsupported(); }; -void Sensor::testGyroScaleCalibration() { printGyroScaleCalibrationUnsupported(); }; +void Sensor::printCalibration() { printCalibrationUnsupported(CalibrationType::CALIBRATION_TYPE_ALL); }; +void Sensor::resetCalibration() { printCalibrationUnsupported(CalibrationType::CALIBRATION_TYPE_ALL); }; +void Sensor::startCalibration(int type) { printCalibrationUnsupported((CalibrationType)type); }; +void Sensor::calibrateAccel() { printCalibrationUnsupported(CalibrationType::CALIBRATION_TYPE_ACCEL); }; +void Sensor::calibrateGyro() { printCalibrationUnsupported(CalibrationType::CALIBRATION_TYPE_GYRO); }; +void Sensor::calibrateMag() { printCalibrationUnsupported(CalibrationType::CALIBRATION_TYPE_MAG); }; +void Sensor::calibrateTemp() { printCalibrationUnsupported(CalibrationType::CALIBRATION_TYPE_TEMP); }; +void Sensor::calibrateGyroSensitivity() { printCalibrationUnsupported(CalibrationType::CALIBRATION_TYPE_GYRO_SENSITIVITY); }; const char * getIMUNameByType(int imuType) { @@ -89,3 +98,27 @@ const char * getIMUNameByType(int imuType) { } return "Unknown"; } + +const char * getCalibrationNameByType(CalibrationType calibrationType) { + switch(calibrationType) { + case CalibrationType::CALIBRATION_TYPE_NONE: + return "None"; + case CalibrationType::CALIBRATION_TYPE_ALL: + return "All"; + case CalibrationType::CALIBRATION_TYPE_ACCEL: + return "ACCEL"; + case CalibrationType::CALIBRATION_TYPE_GYRO: + return "GYRO"; + case CalibrationType::CALIBRATION_TYPE_MAG: + return "MAG"; + case CalibrationType::CALIBRATION_TYPE_6DOF: + return "6DOF"; + case CalibrationType::CALIBRATION_TYPE_9DOF: + return "9DOF"; + case CalibrationType::CALIBRATION_TYPE_TEMP: + return "Temp"; + case CalibrationType::CALIBRATION_TYPE_GYRO_SENSITIVITY: + return "Gyro Sensitivity"; + } + return "Unknown"; +} diff --git a/src/sensors/sensor.h b/src/sensors/sensor.h index 8c0ca4c5c..8c1273296 100644 --- a/src/sensors/sensor.h +++ b/src/sensors/sensor.h @@ -41,6 +41,28 @@ enum class SensorStatus : uint8_t { SENSOR_ERROR = 2 }; +enum class CalibrationType: uint8_t { + CALIBRATION_TYPE_NONE = 0, + CALIBRATION_TYPE_ALL = 1, + CALIBRATION_TYPE_ACCEL = 2, + CALIBRATION_TYPE_GYRO = 3, + CALIBRATION_TYPE_MAG = 4, + CALIBRATION_TYPE_6DOF = 5, + CALIBRATION_TYPE_9DOF = 6, + + CALIBRATION_TYPE_TEMP = 7, + CALIBRATION_TYPE_GYRO_SENSITIVITY = 8 +}; + +enum class CalibrationStatus : uint8_t { + CALIBRATION_STATUS_CALIBRATED = 0, + CALIBRATION_STATUS_FULLY_CALIBRATED = 1, //Like temp cal, not required but possible + CALIBRATION_STATUS_REQUIRED = 2, + CALIBRATION_STATUS_RECOMMENED = 3, //If temp cal is enabled then recommend + CALIBRATION_STATUS_IN_PROGRESS = 4, + CALIBRATION_STATUS_ERROR = 5 +}; + class Sensor { public: @@ -58,16 +80,19 @@ class Sensor virtual void postSetup(){}; virtual void motionLoop(){}; virtual void sendData(); - virtual void startCalibration(int calibrationType){}; + virtual void startCalibration(int calibrationType); + virtual void calibrateAccel(); + virtual void calibrateGyro(); + virtual void calibrateMag(); + virtual void calibrateTemp(); + virtual void calibrateGyroSensitivity(); virtual SensorStatus getSensorState(); virtual void printTemperatureCalibrationState(); virtual void printDebugTemperatureCalibrationState(); virtual void resetTemperatureCalibrationState(); virtual void saveTemperatureCalibration(); - virtual void printGyroScaleCalibration(); - virtual void setGyroScaleCalibration(float xScale, float yScale, float zScale); - virtual void resetGyroScaleCalibration(); - virtual void testGyroScaleCalibration(); + virtual void printCalibration(); + virtual void resetCalibration(); bool isWorking() { return working; }; @@ -112,9 +137,10 @@ class Sensor private: void printTemperatureCalibrationUnsupported(); - void printGyroScaleCalibrationUnsupported(); + void printCalibrationUnsupported(CalibrationType calibrationType); }; const char * getIMUNameByType(int imuType); +const char * getCalibrationNameByType(CalibrationType calibrationType); #endif // SLIMEVR_SENSOR_H_ diff --git a/src/serial/serialcommands.cpp b/src/serial/serialcommands.cpp index ce5d53832..5bf403bd0 100644 --- a/src/serial/serialcommands.cpp +++ b/src/serial/serialcommands.cpp @@ -218,47 +218,93 @@ namespace SerialCommands { logger.info(" Temperature calibration config saves automatically when calibration percent is at 100%"); } - void cmdGyroScaleCalibration(CmdParser* parser) { + void cmdCalibration(CmdParser* parser) { if (parser->getParamCount() > 1) { if (parser->equalCmdParam(1, "GET")) { for (auto sensor : sensorManager.getSensors()) { - sensor->printGyroScaleCalibration(); + sensor->printCalibration(); } return; - } else if (parser->equalCmdParam(1, "SET")) { - for (auto sensor : sensorManager.getSensors()) { - sensor->setGyroScaleCalibration( - std::atof(parser->getCmdParam(2)), - std::atof(parser->getCmdParam(3)), - std::atof(parser->getCmdParam(4)) - ); - } - return; } else if (parser->equalCmdParam(1, "RESET")) { for (auto sensor : sensorManager.getSensors()) { - sensor->resetGyroScaleCalibration(); + if (parser->getParamCount() > 2) { + if (sensor->getSensorId() == std::atoi(parser->getCmdParam(2))) { + sensor->resetCalibration(); + return; + } + } else { + + sensor->resetCalibration(); + } } + if (parser->getParamCount() > 2) { + logger.warn("Sensor with Id %d does not exist, nothing reset", std::atoi(parser->getCmdParam(2))); + } return; - } else if (parser->equalCmdParam(1, "TEST")) { + } else if (parser->equalCmdParam(1, "START") && parser->getParamCount() > 3) { for (auto sensor : sensorManager.getSensors()) { - if (parser->getParamCount() > 2) { - sensor->setGyroScaleCalibration( - std::atof(parser->getCmdParam(2)), - std::atof(parser->getCmdParam(3)), - std::atof(parser->getCmdParam(4)) - ); - } - sensor->testGyroScaleCalibration(); + if (sensor->getSensorId() == std::atoi(parser->getCmdParam(2))) { + switch ((CalibrationType)std::atoi(parser->getCmdParam(3))) + { + case CalibrationType::CALIBRATION_TYPE_NONE: + return; + case CalibrationType::CALIBRATION_TYPE_ALL: + sensor->calibrateGyro(); + sensor->calibrateAccel(); + sensor->calibrateMag(); + sensor->calibrateTemp(); + sensor->calibrateGyroSensitivity(); + return; + case CalibrationType::CALIBRATION_TYPE_ACCEL: + sensor->calibrateAccel(); + return; + case CalibrationType::CALIBRATION_TYPE_GYRO: + sensor->calibrateGyro(); + return; + case CalibrationType::CALIBRATION_TYPE_MAG: + sensor->calibrateMag(); + return; + case CalibrationType::CALIBRATION_TYPE_6DOF: + sensor->calibrateGyro(); + sensor->calibrateAccel(); + return; + case CalibrationType::CALIBRATION_TYPE_9DOF: + sensor->calibrateGyro(); + sensor->calibrateAccel(); + sensor->calibrateMag(); + return; + case CalibrationType::CALIBRATION_TYPE_TEMP: + sensor->calibrateTemp(); + return; + case CalibrationType::CALIBRATION_TYPE_GYRO_SENSITIVITY: + sensor->calibrateGyroSensitivity(); + return; + + default: + logger.warn( + "The calibration type of %d can not be called from serial", + (CalibrationType)std::atoi(parser->getCmdParam(3)) + ); + return; + } + } } return; } } logger.info("Usage:"); - logger.info(" GSCALE GET: Prints out current gyroscope scale calibration values"); - logger.info(" GSCALE SET \"X\" \"Y\" \"Z\": Sets the calibration after you guess a new value"); - logger.info(" GSCALE RESET: Resets the GSCALE calibration to X: 1.0, Y: 1.0, Z: 1.0"); - logger.info(" GSCALE TEST: Test the current configuration by following the instructions"); - logger.info(" GSCALE TEST \"X\" \"Y\" \"Z\": Sets then tests the new values"); + logger.info(" CALIBRATION GET: Prints out current stored calibration values"); + logger.info(" CALIBRATION RESET: Resets the calibration values to the default"); + logger.info(" CALIBRATION RESET \"Sensor#\": Resets the calibration values on sensor # (0 or 1) to the default"); + logger.info(" CALIBRATION START \"Sensor#\" \"Type$$\": Starts the calibration of type $$ on sensor # (0 or 1)"); + logger.info(" 1 - All"); + logger.info(" 2 - Accel"); + logger.info(" 3 - Gyro"); + logger.info(" 4 - Mag"); + logger.info(" 5 - 6DOF"); + logger.info(" 6 - 9DOF"); + logger.info(" 7 - Temp"); + logger.info(" 8 - Gyro Sensitivity"); } void setUp() { @@ -267,7 +313,7 @@ namespace SerialCommands { cmdCallbacks.addCmd("FRST", &cmdFactoryReset); cmdCallbacks.addCmd("REBOOT", &cmdReboot); cmdCallbacks.addCmd("TCAL", &cmdTemperatureCalibration); - cmdCallbacks.addCmd("GSCALE", &cmdGyroScaleCalibration); + cmdCallbacks.addCmd("CALIBRATION", &cmdCalibration); } void update() { From 1516ec48559ce515acb49400c664fe5c5ff50229 Mon Sep 17 00:00:00 2001 From: wigwagwent Date: Wed, 30 Aug 2023 17:29:43 -0700 Subject: [PATCH 38/60] Reduce the library imu read --- lib/LSM6DSV16X/LSM6DSV16X.cpp | 122 +++++++++++++++++++------------ lib/LSM6DSV16X/LSM6DSV16X.h | 11 ++- src/sensors/lsm6dsv16xsensor.cpp | 35 ++++----- src/sensors/lsm6dsv16xsensor.h | 5 +- 4 files changed, 101 insertions(+), 72 deletions(-) diff --git a/lib/LSM6DSV16X/LSM6DSV16X.cpp b/lib/LSM6DSV16X/LSM6DSV16X.cpp index 1ca623b32..58a433137 100644 --- a/lib/LSM6DSV16X/LSM6DSV16X.cpp +++ b/lib/LSM6DSV16X/LSM6DSV16X.cpp @@ -97,7 +97,8 @@ LSM6DSV16XStatusTypeDef LSM6DSV16X::begin() } /* FIFO mode selection */ - if (lsm6dsv16x_fifo_mode_set(®_ctx, LSM6DSV16X_BYPASS_MODE) != LSM6DSV16X_OK) { + fifo_mode = LSM6DSV16X_BYPASS_MODE; + if (lsm6dsv16x_fifo_mode_set(®_ctx, fifo_mode) != LSM6DSV16X_OK) { return LSM6DSV16X_ERROR; } @@ -110,7 +111,8 @@ LSM6DSV16XStatusTypeDef LSM6DSV16X::begin() } /* Full scale selection. */ - if (lsm6dsv16x_xl_full_scale_set(®_ctx, LSM6DSV16X_2g) != LSM6DSV16X_OK) { + acc_fs = LSM6DSV16X_2g; + if (lsm6dsv16x_xl_full_scale_set(®_ctx, acc_fs) != LSM6DSV16X_OK) { return LSM6DSV16X_ERROR; } @@ -123,7 +125,8 @@ LSM6DSV16XStatusTypeDef LSM6DSV16X::begin() } /* Full scale selection. */ - if (lsm6dsv16x_gy_full_scale_set(®_ctx, LSM6DSV16X_2000dps) != LSM6DSV16X_OK) { + gyro_fs = LSM6DSV16X_2000dps; + if (lsm6dsv16x_gy_full_scale_set(®_ctx, gyro_fs) != LSM6DSV16X_OK) { return LSM6DSV16X_ERROR; } @@ -241,7 +244,6 @@ LSM6DSV16XStatusTypeDef LSM6DSV16X::Disable_X() */ LSM6DSV16XStatusTypeDef LSM6DSV16X::Get_X_Sensitivity(float *Sensitivity) { - LSM6DSV16XStatusTypeDef ret = LSM6DSV16X_OK; lsm6dsv16x_xl_full_scale_t full_scale; /* Read actual full scale selection from sensor. */ @@ -249,30 +251,34 @@ LSM6DSV16XStatusTypeDef LSM6DSV16X::Get_X_Sensitivity(float *Sensitivity) return LSM6DSV16X_ERROR; } + *Sensitivity = Convert_X_Sensitivity(full_scale); + if (*Sensitivity == 0.0f) { + return LSM6DSV16X_ERROR; + } + return LSM6DSV16X_OK; +} + +float LSM6DSV16X::Convert_X_Sensitivity(lsm6dsv16x_xl_full_scale_t full_scale) { + float Sensitivity = 0.0f; /* Store the Sensitivity based on actual full scale. */ switch (full_scale) { case LSM6DSV16X_2g: - *Sensitivity = LSM6DSV16X_ACC_SENSITIVITY_FS_2G; + Sensitivity = LSM6DSV16X_ACC_SENSITIVITY_FS_2G; break; case LSM6DSV16X_4g: - *Sensitivity = LSM6DSV16X_ACC_SENSITIVITY_FS_4G; + Sensitivity = LSM6DSV16X_ACC_SENSITIVITY_FS_4G; break; case LSM6DSV16X_8g: - *Sensitivity = LSM6DSV16X_ACC_SENSITIVITY_FS_8G; + Sensitivity = LSM6DSV16X_ACC_SENSITIVITY_FS_8G; break; case LSM6DSV16X_16g: - *Sensitivity = LSM6DSV16X_ACC_SENSITIVITY_FS_16G; - break; - - default: - ret = LSM6DSV16X_ERROR; + Sensitivity = LSM6DSV16X_ACC_SENSITIVITY_FS_16G; break; } - - return ret; + return Sensitivity; } /** @@ -359,7 +365,7 @@ LSM6DSV16XStatusTypeDef LSM6DSV16X::Get_X_ODR(float *Odr) LSM6DSV16XStatusTypeDef LSM6DSV16X::Get_X_Axes(int32_t *Acceleration) { lsm6dsv16x_axis3bit16_t data_raw; - float sensitivity = 0.0f; + float sensitivity = Convert_X_Sensitivity(acc_fs); /* Read raw data values. */ if (lsm6dsv16x_acceleration_raw_get(®_ctx, data_raw.i16bit) != LSM6DSV16X_OK) { @@ -367,7 +373,7 @@ LSM6DSV16XStatusTypeDef LSM6DSV16X::Get_X_Axes(int32_t *Acceleration) } /* Get LSM6DSV16X actual sensitivity. */ - if (Get_X_Sensitivity(&sensitivity) != LSM6DSV16X_OK) { + if (sensitivity == 0.0f) { return LSM6DSV16X_ERROR; } @@ -576,7 +582,12 @@ LSM6DSV16XStatusTypeDef LSM6DSV16X::Set_X_FS(int32_t FullScale) : (FullScale <= 8) ? LSM6DSV16X_8g : LSM6DSV16X_16g; - if (lsm6dsv16x_xl_full_scale_set(®_ctx, new_fs) != LSM6DSV16X_OK) { + if (new_fs == acc_fs) { + return LSM6DSV16X_OK; + } + acc_fs = new_fs; + + if (lsm6dsv16x_xl_full_scale_set(®_ctx, acc_fs) != LSM6DSV16X_OK) { return LSM6DSV16X_ERROR; } @@ -2634,6 +2645,18 @@ LSM6DSV16XStatusTypeDef LSM6DSV16X::Disable_Tilt_Detection() return ret; } + +LSM6DSV16XStatusTypeDef LSM6DSV16X::FIFO_Reset() { + if (lsm6dsv16x_fifo_mode_set(®_ctx, LSM6DSV16X_BYPASS_MODE) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + + if (lsm6dsv16x_fifo_mode_set(®_ctx, fifo_mode) != LSM6DSV16X_OK) { + return LSM6DSV16X_ERROR; + } + return LSM6DSV16X_OK; +} + /** * @brief Get the LSM6DSV16X FIFO number of samples @@ -2744,7 +2767,6 @@ LSM6DSV16XStatusTypeDef LSM6DSV16X::FIFO_Set_Stop_On_Fth(uint8_t Status) */ LSM6DSV16XStatusTypeDef LSM6DSV16X::FIFO_Set_Mode(uint8_t Mode) { - LSM6DSV16XStatusTypeDef ret = LSM6DSV16X_OK; lsm6dsv16x_fifo_mode_t newMode = LSM6DSV16X_BYPASS_MODE; switch (Mode) { @@ -2767,19 +2789,14 @@ LSM6DSV16XStatusTypeDef LSM6DSV16X::FIFO_Set_Mode(uint8_t Mode) newMode = LSM6DSV16X_BYPASS_TO_FIFO_MODE; break; default: - ret = LSM6DSV16X_ERROR; - break; - } - - if (ret == LSM6DSV16X_ERROR) { - return LSM6DSV16X_ERROR; + return LSM6DSV16X_ERROR; } - - if (lsm6dsv16x_fifo_mode_set(®_ctx, newMode) != LSM6DSV16X_OK) { + fifo_mode = newMode; + if (lsm6dsv16x_fifo_mode_set(®_ctx, fifo_mode) != LSM6DSV16X_OK) { return LSM6DSV16X_ERROR; } - return ret; + return LSM6DSV16X_OK; } /** @@ -2820,14 +2837,14 @@ LSM6DSV16XStatusTypeDef LSM6DSV16X::FIFO_Get_Data(uint8_t *Data) LSM6DSV16XStatusTypeDef LSM6DSV16X::FIFO_Get_X_Axes(int32_t *Acceleration) { lsm6dsv16x_axis3bit16_t data_raw; - float sensitivity = 0.0f; + float sensitivity = Convert_X_Sensitivity(acc_fs); float acceleration_float[3]; if (FIFO_Get_Data(data_raw.u8bit) != LSM6DSV16X_OK) { return LSM6DSV16X_ERROR; } - if (Get_X_Sensitivity(&sensitivity) != LSM6DSV16X_OK) { + if (sensitivity == 0.0f) { return LSM6DSV16X_ERROR; } acceleration_float[0] = (float)data_raw.i16bit[0] * sensitivity; @@ -2878,14 +2895,14 @@ LSM6DSV16XStatusTypeDef LSM6DSV16X::FIFO_Set_X_BDR(float Bdr) LSM6DSV16XStatusTypeDef LSM6DSV16X::FIFO_Get_G_Axes(int32_t *AngularVelocity) { lsm6dsv16x_axis3bit16_t data_raw; - float sensitivity = 0.0f; + float sensitivity = Convert_G_Sensitivity(gyro_fs); float angular_velocity_float[3]; if (FIFO_Get_Data(data_raw.u8bit) != LSM6DSV16X_OK) { return LSM6DSV16X_ERROR; } - if (Get_G_Sensitivity(&sensitivity) != LSM6DSV16X_OK) { + if (sensitivity == 0.0f) { return LSM6DSV16X_ERROR; } @@ -3072,38 +3089,44 @@ LSM6DSV16XStatusTypeDef LSM6DSV16X::Get_G_Sensitivity(float *Sensitivity) return LSM6DSV16X_ERROR; } + *Sensitivity = Convert_G_Sensitivity(full_scale); + if (*Sensitivity == 0.0f) { + return LSM6DSV16X_ERROR; + } + + return LSM6DSV16X_OK; +} + + +float LSM6DSV16X::Convert_G_Sensitivity(lsm6dsv16x_gy_full_scale_t full_scale) { + float Sensitivity = 0.0f; /* Store the sensitivity based on actual full scale. */ switch (full_scale) { case LSM6DSV16X_125dps: - *Sensitivity = LSM6DSV16X_GYRO_SENSITIVITY_FS_125DPS; + Sensitivity = LSM6DSV16X_GYRO_SENSITIVITY_FS_125DPS; break; case LSM6DSV16X_250dps: - *Sensitivity = LSM6DSV16X_GYRO_SENSITIVITY_FS_250DPS; + Sensitivity = LSM6DSV16X_GYRO_SENSITIVITY_FS_250DPS; break; case LSM6DSV16X_500dps: - *Sensitivity = LSM6DSV16X_GYRO_SENSITIVITY_FS_500DPS; + Sensitivity = LSM6DSV16X_GYRO_SENSITIVITY_FS_500DPS; break; case LSM6DSV16X_1000dps: - *Sensitivity = LSM6DSV16X_GYRO_SENSITIVITY_FS_1000DPS; + Sensitivity = LSM6DSV16X_GYRO_SENSITIVITY_FS_1000DPS; break; case LSM6DSV16X_2000dps: - *Sensitivity = LSM6DSV16X_GYRO_SENSITIVITY_FS_2000DPS; + Sensitivity = LSM6DSV16X_GYRO_SENSITIVITY_FS_2000DPS; break; case LSM6DSV16X_4000dps: - *Sensitivity = LSM6DSV16X_GYRO_SENSITIVITY_FS_4000DPS; - break; - - default: - ret = LSM6DSV16X_ERROR; + Sensitivity = LSM6DSV16X_GYRO_SENSITIVITY_FS_4000DPS; break; } - - return ret; + return Sensitivity; } /** @@ -3343,7 +3366,12 @@ LSM6DSV16XStatusTypeDef LSM6DSV16X::Set_G_FS(int32_t FullScale) : (FullScale <= 2000) ? LSM6DSV16X_2000dps : LSM6DSV16X_4000dps; - return (LSM6DSV16XStatusTypeDef) lsm6dsv16x_gy_full_scale_set(®_ctx, new_fs); + if (new_fs == gyro_fs) { + return LSM6DSV16X_OK; + } + gyro_fs = new_fs; + + return (LSM6DSV16XStatusTypeDef) lsm6dsv16x_gy_full_scale_set(®_ctx, gyro_fs); } /** @@ -3395,7 +3423,7 @@ LSM6DSV16XStatusTypeDef LSM6DSV16X::Get_G_AxesRaw_When_Aval(int16_t *Value) { LSM6DSV16XStatusTypeDef LSM6DSV16X::Get_G_Axes(int32_t *AngularRate) { lsm6dsv16x_axis3bit16_t data_raw; - float sensitivity; + float sensitivity = Convert_G_Sensitivity(gyro_fs); /* Read raw data values. */ if (lsm6dsv16x_angular_rate_raw_get(®_ctx, data_raw.i16bit) != LSM6DSV16X_OK) { @@ -3403,7 +3431,7 @@ LSM6DSV16XStatusTypeDef LSM6DSV16X::Get_G_Axes(int32_t *AngularRate) } /* Get LSM6DSV16X actual sensitivity. */ - if (Get_G_Sensitivity(&sensitivity) != LSM6DSV16X_OK) { + if (sensitivity == 0.0f) { return LSM6DSV16X_ERROR; } @@ -3684,7 +3712,7 @@ LSM6DSV16XStatusTypeDef LSM6DSV16X::Reset_Set(uint8_t flags) return LSM6DSV16X_OK; } -LSM6DSV16XStatusTypeDef LSM6DSV16X::Enable_Game_Rotation(bool enable) +LSM6DSV16XStatusTypeDef LSM6DSV16X::Enable_SFLP_Block(bool enable) { return (LSM6DSV16XStatusTypeDef)lsm6dsv16x_sflp_game_rotation_set( ®_ctx, diff --git a/lib/LSM6DSV16X/LSM6DSV16X.h b/lib/LSM6DSV16X/LSM6DSV16X.h index f0faa09b4..232f50690 100644 --- a/lib/LSM6DSV16X/LSM6DSV16X.h +++ b/lib/LSM6DSV16X/LSM6DSV16X.h @@ -192,6 +192,7 @@ class LSM6DSV16X { LSM6DSV16XStatusTypeDef Get_T_ODR(float *Odr); LSM6DSV16XStatusTypeDef Set_T_ODR(float Odr); + LSM6DSV16XStatusTypeDef Enable_SFLP_Block(bool enable = true); LSM6DSV16XStatusTypeDef Set_SFLP_ODR(float Odr); LSM6DSV16XStatusTypeDef Set_SFLP_GBIAS(float x, float y, float z); @@ -232,6 +233,7 @@ class LSM6DSV16X { LSM6DSV16XStatusTypeDef Enable_Tilt_Detection(LSM6DSV16X_SensorIntPin_t IntPin); LSM6DSV16XStatusTypeDef Disable_Tilt_Detection(); + LSM6DSV16XStatusTypeDef FIFO_Reset(); LSM6DSV16XStatusTypeDef FIFO_Get_Num_Samples(uint16_t *NumSamples); LSM6DSV16XStatusTypeDef FIFO_Get_Full_Status(uint8_t *Status); LSM6DSV16XStatusTypeDef FIFO_Set_INT1_FIFO_Full(uint8_t Status); @@ -267,8 +269,6 @@ class LSM6DSV16X { LSM6DSV16XStatusTypeDef Reset_Set(uint8_t flags); - LSM6DSV16XStatusTypeDef Enable_Game_Rotation(bool enable = true); - LSM6DSV16XStatusTypeDef Enable_Block_Data_Update(bool enable = true); LSM6DSV16XStatusTypeDef Set_Auto_Increment(bool enable); @@ -396,6 +396,9 @@ class LSM6DSV16X { LSM6DSV16XStatusTypeDef Set_G_ODR_When_Enabled(float Odr); LSM6DSV16XStatusTypeDef Set_G_ODR_When_Disabled(float Odr); + float Convert_X_Sensitivity(lsm6dsv16x_xl_full_scale_t full_scale); + float Convert_G_Sensitivity(lsm6dsv16x_gy_full_scale_t full_scale); + /* Helper classes. */ TwoWire *dev_i2c; SPIClass *dev_spi; @@ -407,6 +410,10 @@ class LSM6DSV16X { lsm6dsv16x_data_rate_t acc_odr; lsm6dsv16x_data_rate_t gyro_odr; + lsm6dsv16x_xl_full_scale_t acc_fs; + lsm6dsv16x_gy_full_scale_t gyro_fs; + lsm6dsv16x_fifo_mode_t fifo_mode; + uint8_t acc_is_enabled; uint8_t gyro_is_enabled; uint8_t initialized; diff --git a/src/sensors/lsm6dsv16xsensor.cpp b/src/sensors/lsm6dsv16xsensor.cpp index bc8448cca..cfeb6ed4f 100644 --- a/src/sensors/lsm6dsv16xsensor.cpp +++ b/src/sensors/lsm6dsv16xsensor.cpp @@ -158,7 +158,7 @@ void LSM6DSV16XSensor::motionSetup() { #if defined(LSM6DSV16X_ONBOARD_FUSION) && defined(LSM6DSV16X_ESP_FUSION) status |= imu.FIFO_Set_SFLP_Batch(true, true, false); // TODO: Don't use batch change for this - status |= imu.Enable_Game_Rotation(); // TODO: Rename to enable SFLP + status |= imu.Enable_SFLP_Block(); #elif defined(LSM6DSV16X_ONBOARD_FUSION) status |= imu.FIFO_Set_SFLP_Batch(true, true, false); @@ -187,8 +187,7 @@ void LSM6DSV16XSensor::motionSetup() { status |= imu.Enable_Double_Tap_Detection(LSM6DSV16X_INT1_PIN); #endif // LSM6DSV16X_INTERRUPT - status |= imu.FIFO_Set_Mode(LSM6DSV16X_BYPASS_MODE); // add to lib but we need to - status |= imu.FIFO_Set_Mode(LSM6DSV16X_STREAM_MODE); // get / keep track of current fifo type + status |= imu.FIFO_Reset(); if (status != LSM6DSV16X_OK) { m_Logger.fatal( @@ -665,8 +664,7 @@ void LSM6DSV16XSensor::calibrateAccel() { numPositionsRecorded + 1 ); - imu.FIFO_Set_Mode(LSM6DSV16X_BYPASS_MODE); // add to lib but we need to - imu.FIFO_Set_Mode(LSM6DSV16X_STREAM_MODE); // get / keep track of current fifo type + imu.FIFO_Reset(); while (true) { uint16_t fifo_samples = 0; if (imu.FIFO_Get_Num_Samples(&fifo_samples) == LSM6DSV16X_ERROR) { @@ -818,8 +816,7 @@ void LSM6DSV16XSensor::calibrateGyro() { m_Logger.info("Put down the device and wait for baseline gyro reading calibration"); delay(2000); - imu.FIFO_Set_Mode(LSM6DSV16X_BYPASS_MODE); // TODO: add to lib but we need to - imu.FIFO_Set_Mode(LSM6DSV16X_STREAM_MODE); // get / keep track of current fifo type + imu.FIFO_Reset(); waitForRest(); uint16_t count = 0; while (count < calibrationSamples) { @@ -855,7 +852,7 @@ void LSM6DSV16XSensor::calibrateGyro() { #endif -#ifdef LSM6DSV16X_GYRO_SCALE_CAL +#ifdef LSM6DSV16X_GYRO_SENSITIVITY_CAL void LSM6DSV16XSensor::calibrateGyroSensitivity() { m_Logger.info( "Gyro sensitivity calibration started on sensor #%d of type %s at address 0x%02x", @@ -874,7 +871,6 @@ void LSM6DSV16XSensor::calibrateGyroSensitivity() { uint8_t count = 0; float gyroCount[3]; //Use this to determine the axis spun float calculatedScale[3] = {1.0f, 1.0f, 1.0f}; - constexpr uint8_t fullRotations = 1; ledManager.off(); @@ -887,7 +883,7 @@ void LSM6DSV16XSensor::calibrateGyroSensitivity() { m_Logger.info( "\tStep 3: Rotate the tracker about one axis %d full rotations and align with the " "previous edge.", - fullRotations + LSM6DSV16X_GYRO_SENSITIVITY_SPINS ); m_Logger.info("\t\tNOTE: the light will turn off after you start moving it"); @@ -903,7 +899,7 @@ void LSM6DSV16XSensor::calibrateGyroSensitivity() { waitForRest(); ledManager.on(); // The user should rotate - m_Logger.info("Rotate the tracker %d times", fullRotations); + m_Logger.info("Rotate the tracker %d times", LSM6DSV16X_GYRO_SENSITIVITY_SPINS); gyroCount[0] = 0.0f; gyroCount[1] = 0.0f; gyroCount[2] = 0.0f; @@ -922,21 +918,16 @@ void LSM6DSV16XSensor::calibrateGyroSensitivity() { } uint8_t isUp; - - rawRotationFinal = sfusion.getQuaternionQuat().get_euler() * dpsPerRad; - - - if (abs(gyroCount[0]) > abs(gyroCount[1]) && abs(gyroCount[0]) > abs(gyroCount[2])) { //Spun in X imu.Get_6D_Orientation_XH(&isUp); if((!isUp && gyroCount[0] > 0) || (isUp && gyroCount[0] < 0)) { - calculatedScale[0] = (1.0 / (1.0 - ((rawRotationInit.y - rawRotationFinal.y)/(360.0f * fullRotations)))); + calculatedScale[0] = (1.0 / (1.0 - ((rawRotationInit.y - rawRotationFinal.y)/(360.0f * LSM6DSV16X_GYRO_SENSITIVITY_SPINS)))); m_Logger.info("X, Diff: %f", rawRotationInit.y - rawRotationFinal.y); } else { - calculatedScale[0] = (1.0 / (1.0 - ((rawRotationFinal.y - rawRotationInit.y)/(360.0f * fullRotations)))); + calculatedScale[0] = (1.0 / (1.0 - ((rawRotationFinal.y - rawRotationInit.y)/(360.0f * LSM6DSV16X_GYRO_SENSITIVITY_SPINS)))); m_Logger.info("-X, Diff: %f", rawRotationFinal.y - rawRotationInit.y); } } @@ -944,11 +935,11 @@ void LSM6DSV16XSensor::calibrateGyroSensitivity() { else if (abs(gyroCount[1]) > abs(gyroCount[0]) && abs(gyroCount[1]) > abs(gyroCount[2])) { //Spun in Y imu.Get_6D_Orientation_YH(&isUp); if((isUp && gyroCount[1] > 0) || (!isUp && gyroCount[1] < 0)) { - calculatedScale[1] = (1.0 / (1.0 - ((rawRotationInit.y - rawRotationFinal.y)/(360.0f * fullRotations)))); + calculatedScale[1] = (1.0 / (1.0 - ((rawRotationInit.y - rawRotationFinal.y)/(360.0f * LSM6DSV16X_GYRO_SENSITIVITY_SPINS)))); m_Logger.info("Y, Diff: %f", rawRotationInit.y - rawRotationFinal.y); } else { - calculatedScale[1] = (1.0 / (1.0 - ((rawRotationFinal.y - rawRotationInit.y)/(360.0f * fullRotations)))); + calculatedScale[1] = (1.0 / (1.0 - ((rawRotationFinal.y - rawRotationInit.y)/(360.0f * LSM6DSV16X_GYRO_SENSITIVITY_SPINS)))); m_Logger.info("-Y, Diff: %f", rawRotationFinal.y - rawRotationInit.y); } } @@ -956,11 +947,11 @@ void LSM6DSV16XSensor::calibrateGyroSensitivity() { else if (abs(gyroCount[2]) > abs(gyroCount[0]) && abs(gyroCount[2]) > abs(gyroCount[1])) { //Spun in Z imu.Get_6D_Orientation_ZH(&isUp); if((isUp && gyroCount[2] > 0) || (!isUp && gyroCount[2] < 0)) { - calculatedScale[2] = (1.0 / (1.0 - ((rawRotationInit.y - rawRotationFinal.y)/(360.0f * fullRotations)))); + calculatedScale[2] = (1.0 / (1.0 - ((rawRotationInit.y - rawRotationFinal.y)/(360.0f * LSM6DSV16X_GYRO_SENSITIVITY_SPINS)))); m_Logger.info("Z, Diff: %f", rawRotationInit.y - rawRotationFinal.y); } else { - calculatedScale[2] = (1.0 / (1.0 - ((rawRotationFinal.y - rawRotationInit.y)/(360.0f * fullRotations)))); + calculatedScale[2] = (1.0 / (1.0 - ((rawRotationFinal.y - rawRotationInit.y)/(360.0f * LSM6DSV16X_GYRO_SENSITIVITY_SPINS)))); m_Logger.info("-Z, Diff: %f", rawRotationFinal.y - rawRotationInit.y); } } diff --git a/src/sensors/lsm6dsv16xsensor.h b/src/sensors/lsm6dsv16xsensor.h index 65705c052..c26f79255 100644 --- a/src/sensors/lsm6dsv16xsensor.h +++ b/src/sensors/lsm6dsv16xsensor.h @@ -68,9 +68,12 @@ #ifdef LSM6DSV16X_ESP_FUSION #define LSM6DSV16X_GYRO_OFFSET_CAL #define LSM6DSV16X_ACCEL_OFFSET_CAL -#define LSM6DSV16X_GYRO_SCALE_CAL +#define LSM6DSV16X_GYRO_SENSITIVITY_CAL #endif +#ifdef LSM6DSV16X_GYRO_SENSITIVITY_CAL +#define LSM6DSV16X_GYRO_SENSITIVITY_SPINS 5 +#endif #ifdef REINIT_ON_FAILURE #define REINIT_RETRY_MAX_ATTEMPTS 5 From 86cda956e0c6dfd066a78ff4cef1b269df65dc4d Mon Sep 17 00:00:00 2001 From: wigwagwent Date: Sun, 3 Sep 2023 15:20:09 -0700 Subject: [PATCH 39/60] Make the merge build --- lib/LSM6DSV16X/LSM6DSV16X.cpp | 1 - src/configuration/Configuration.cpp | 7 +++++++ 2 files changed, 7 insertions(+), 1 deletion(-) diff --git a/lib/LSM6DSV16X/LSM6DSV16X.cpp b/lib/LSM6DSV16X/LSM6DSV16X.cpp index 58a433137..f5801271b 100644 --- a/lib/LSM6DSV16X/LSM6DSV16X.cpp +++ b/lib/LSM6DSV16X/LSM6DSV16X.cpp @@ -3081,7 +3081,6 @@ LSM6DSV16XStatusTypeDef LSM6DSV16X::Disable_G() */ LSM6DSV16XStatusTypeDef LSM6DSV16X::Get_G_Sensitivity(float *Sensitivity) { - LSM6DSV16XStatusTypeDef ret = LSM6DSV16X_OK; lsm6dsv16x_gy_full_scale_t full_scale; /* Read actual full scale selection from sensor. */ diff --git a/src/configuration/Configuration.cpp b/src/configuration/Configuration.cpp index a0264d4b5..34b988e97 100644 --- a/src/configuration/Configuration.cpp +++ b/src/configuration/Configuration.cpp @@ -442,6 +442,13 @@ namespace SlimeVR { m_Logger.info(" A_B : %f, %f, %f", UNPACK_VECTOR_ARRAY(c.data.mpu6050.A_B)); m_Logger.info(" G_off: %f, %f, %f", UNPACK_VECTOR_ARRAY(c.data.mpu6050.G_off)); + break; + + case CalibrationConfigType::LSM6DSV16X: + m_Logger.info(" A_off: %f, %f, %f", UNPACK_VECTOR_ARRAY(c.data.lsm6dsv16x.A_off)); + m_Logger.info(" G_off: %f, %f, %f", UNPACK_VECTOR_ARRAY(c.data.lsm6dsv16x.G_off)); + m_Logger.info(" G_sen: %f, %f, %f", UNPACK_VECTOR_ARRAY(c.data.lsm6dsv16x.G_sensitivity)); + break; } } From d99ea15ed008de78bc97346719b833b66e84217e Mon Sep 17 00:00:00 2001 From: wigwagwent Date: Sun, 3 Sep 2023 15:59:31 -0700 Subject: [PATCH 40/60] Add tap detection --- src/sensors/lsm6dsv16xsensor.cpp | 28 +++++++++++++++++++++++++--- src/sensors/lsm6dsv16xsensor.h | 2 +- 2 files changed, 26 insertions(+), 4 deletions(-) diff --git a/src/sensors/lsm6dsv16xsensor.cpp b/src/sensors/lsm6dsv16xsensor.cpp index cfeb6ed4f..944a3e5c1 100644 --- a/src/sensors/lsm6dsv16xsensor.cpp +++ b/src/sensors/lsm6dsv16xsensor.cpp @@ -28,12 +28,13 @@ #include "lsm6dsv16xsensor.h" #include "utils.h" + #ifdef LSM6DSV16X_INTERRUPT volatile bool imuEvent = false; - void IRAM_ATTR interruptHandler() { imuEvent = true; } #endif + LSM6DSV16XSensor::LSM6DSV16XSensor( uint8_t id, uint8_t type, @@ -183,8 +184,9 @@ void LSM6DSV16XSensor::motionSetup() { #ifdef LSM6DSV16X_INTERRUPT attachInterrupt(m_IntPin, interruptHandler, RISING); - status |= imu.Enable_Single_Tap_Detection(LSM6DSV16X_INT1_PIN); // Tap requires an interrupt - status |= imu.Enable_Double_Tap_Detection(LSM6DSV16X_INT1_PIN); + status |= imu.Enable_Single_Tap_Detection(LSM6DSV16X_INT1_PIN); // Tap requires an interrupt +#else + status |= imu.Enable_Single_Tap_Detection(LSM6DSV16X_INT2_PIN); //Just poll to see if an event happened #endif // LSM6DSV16X_INTERRUPT status |= imu.FIFO_Reset(); @@ -210,6 +212,26 @@ constexpr float mdpsPerDps = 1000.0f; constexpr float dpsPerRad = 57.295779578552f; void LSM6DSV16XSensor::motionLoop() { +#ifdef LSM6DSV16X_INTERRUPT + if (imuEvent) { + LSM6DSV16X_Event_Status_t status; + imu.Get_X_Event_Status(&status); + + if (status.TapStatus) { + tap++; + } + imuEvent = false; + } +#else + LSM6DSV16X_Event_Status_t status; + imu.Get_X_Event_Status(&status); + + if (status.TapStatus) { + tap++; + } +#endif + + if (millis() - lastTempRead > LSM6DSV16X_TEMP_READ_INTERVAL * 1000) { lastTempRead = millis(); diff --git a/src/sensors/lsm6dsv16xsensor.h b/src/sensors/lsm6dsv16xsensor.h index c26f79255..afcfa1277 100644 --- a/src/sensors/lsm6dsv16xsensor.h +++ b/src/sensors/lsm6dsv16xsensor.h @@ -58,7 +58,7 @@ // #define SELF_TEST_ON_INIT // #define REINIT_ON_FAILURE -// #define LSM6DSV16X_INTERRUPT +// #define LSM6DSV16X_INTERRUPT //recommended for tap detect // #define LSM6DSV16X_NO_SELF_TEST_ON_FACEDOWN #define LSM6DSV16X_ONBOARD_FUSION #define LSM6DSV16X_ESP_FUSION From 4906d14f89b8f772e0257fc79127e983303e6df3 Mon Sep 17 00:00:00 2001 From: wigwagwent Date: Sun, 3 Sep 2023 17:58:03 -0700 Subject: [PATCH 41/60] Tune tap detection and add error handling --- src/sensors/lsm6dsv16xsensor.cpp | 35 ++++++++++++++++++++++---------- src/sensors/lsm6dsv16xsensor.h | 16 ++++++++++++++- 2 files changed, 39 insertions(+), 12 deletions(-) diff --git a/src/sensors/lsm6dsv16xsensor.cpp b/src/sensors/lsm6dsv16xsensor.cpp index 944a3e5c1..2a6029454 100644 --- a/src/sensors/lsm6dsv16xsensor.cpp +++ b/src/sensors/lsm6dsv16xsensor.cpp @@ -74,6 +74,7 @@ void LSM6DSV16XSensor::motionSetup() { deviceId ); ledManager.pattern(50, 50, 200); + status = LSM6DSV16X_ERROR; return; } @@ -90,7 +91,6 @@ void LSM6DSV16XSensor::motionSetup() { return; } - int8_t status = 0; status |= imu.Enable_6D_Orientation(LSM6DSV16X_INT2_PIN); uint8_t isFaceDown; @@ -106,8 +106,8 @@ void LSM6DSV16XSensor::motionSetup() { getIMUNameByType(sensorType), addr ); - ledManager.pattern(50, 50, 200); + status = LSM6DSV16X_ERROR; return; } } @@ -182,6 +182,8 @@ void LSM6DSV16XSensor::motionSetup() { // status |= imu.beginPreconfigured(); + + #ifdef LSM6DSV16X_INTERRUPT attachInterrupt(m_IntPin, interruptHandler, RISING); status |= imu.Enable_Single_Tap_Detection(LSM6DSV16X_INT1_PIN); // Tap requires an interrupt @@ -189,8 +191,12 @@ void LSM6DSV16XSensor::motionSetup() { status |= imu.Enable_Single_Tap_Detection(LSM6DSV16X_INT2_PIN); //Just poll to see if an event happened #endif // LSM6DSV16X_INTERRUPT - status |= imu.FIFO_Reset(); + status |= imu.Set_Tap_Threshold(LSM6DSV16X_TAP_THRESHOLD); + status |= imu.Set_Tap_Shock_Time(LSM6DSV16X_TAP_SHOCK_TIME); + status |= imu.Set_Tap_Quiet_Time(LSM6DSV16X_TAP_QUITE_TIME); + status |= imu.FIFO_Reset(); + if (status != LSM6DSV16X_OK) { m_Logger.fatal( "Errors occured enabling imu features on %s at address 0x%02x", @@ -214,24 +220,24 @@ constexpr float dpsPerRad = 57.295779578552f; void LSM6DSV16XSensor::motionLoop() { #ifdef LSM6DSV16X_INTERRUPT if (imuEvent) { - LSM6DSV16X_Event_Status_t status; - imu.Get_X_Event_Status(&status); + LSM6DSV16X_Event_Status_t eventStatus; + status |= imu.Get_X_Event_Status(&eventStatus); - if (status.TapStatus) { + if (eventStatus.TapStatus) { tap++; + m_Logger.info("Tap: %d", millis()); } imuEvent = false; } #else - LSM6DSV16X_Event_Status_t status; - imu.Get_X_Event_Status(&status); + LSM6DSV16X_Event_Status_t eventStatus; + status |= imu.Get_X_Event_Status(&eventStatus); - if (status.TapStatus) { + if (eventStatus.TapStatus) { tap++; } #endif - if (millis() - lastTempRead > LSM6DSV16X_TEMP_READ_INTERVAL * 1000) { lastTempRead = millis(); @@ -242,6 +248,7 @@ void LSM6DSV16XSensor::motionLoop() { getIMUNameByType(sensorType), addr ); + status = LSM6DSV16X_ERROR; } else { float actualTemp = lsm6dsv16x_from_lsb_to_celsius(rawTemp); if (fabsf(actualTemp - temperature) > 0.01f) { @@ -282,6 +289,7 @@ void LSM6DSV16XSensor::motionLoop() { getIMUNameByType(sensorType), addr ); + status = LSM6DSV16X_ERROR; return; } @@ -302,6 +310,7 @@ void LSM6DSV16XSensor::motionLoop() { } #if defined(LSM6DSV16X_ONBOARD_FUSION) && defined(LSM6DSV16X_ESP_FUSION) + // TODO: fusion of fusion stuff fusedRotation = sfusion.getQuaternionQuat(); lastFusedRotationSent = fusedRotation; @@ -363,7 +372,10 @@ void LSM6DSV16XSensor::motionLoop() { } SensorStatus LSM6DSV16XSensor::getSensorState() { - return isWorking() ? SensorStatus::SENSOR_OK : SensorStatus::SENSOR_OFFLINE; + return isWorking() + ? (status == LSM6DSV16X_OK ? SensorStatus::SENSOR_OK + : SensorStatus::SENSOR_ERROR) + : SensorStatus::SENSOR_OFFLINE; } Quat LSM6DSV16XSensor::fusedRotationToQuaternion(float x, float y, float z) { @@ -695,6 +707,7 @@ void LSM6DSV16XSensor::calibrateAccel() { getIMUNameByType(sensorType), addr ); + status = LSM6DSV16X_ERROR; return; } diff --git a/src/sensors/lsm6dsv16xsensor.h b/src/sensors/lsm6dsv16xsensor.h index afcfa1277..6e29e4f7b 100644 --- a/src/sensors/lsm6dsv16xsensor.h +++ b/src/sensors/lsm6dsv16xsensor.h @@ -54,11 +54,24 @@ #define LSM6DSV16X_TEMP_READ_INTERVAL 1 #endif + +#ifndef LSM6DSV16X_TAP_THRESHOLD +#define LSM6DSV16X_TAP_THRESHOLD 5 //0-32 +#endif + +#ifndef LSM6DSV16X_TAP_SHOCK_TIME +#define LSM6DSV16X_TAP_SHOCK_TIME 3 //0-3 +#endif + +#ifndef LSM6DSV16X_TAP_QUITE_TIME +#define LSM6DSV16X_TAP_QUITE_TIME 3 //0-3 +#endif + #define LSM6DSV16X_TIMESTAMP_LSB 21.75e-6f // #define SELF_TEST_ON_INIT // #define REINIT_ON_FAILURE -// #define LSM6DSV16X_INTERRUPT //recommended for tap detect + #define LSM6DSV16X_INTERRUPT //recommended for tap detect // #define LSM6DSV16X_NO_SELF_TEST_ON_FACEDOWN #define LSM6DSV16X_ONBOARD_FUSION #define LSM6DSV16X_ESP_FUSION @@ -122,6 +135,7 @@ class LSM6DSV16XSensor : public Sensor { LSM6DSV16X imu; uint8_t m_IntPin; uint8_t tap = 0; + int8_t status = 0; unsigned long lastData = 0; float temperature = 0; bool newTemperature = false; From 01bd912d7fda1605995c2426ab20452ab78d474a Mon Sep 17 00:00:00 2001 From: wigwagwent Date: Mon, 16 Oct 2023 10:45:35 -0700 Subject: [PATCH 42/60] Rename to lsm6dsv except lib --- src/configuration/CalibrationConfig.cpp | 4 +- src/configuration/CalibrationConfig.h | 6 +- src/configuration/Configuration.cpp | 8 +- src/consts.h | 2 +- src/sensors/SensorManager.cpp | 6 +- ...lsm6dsv16xsensor.cpp => lsm6dsvsensor.cpp} | 184 +++++++++--------- .../{lsm6dsv16xsensor.h => lsm6dsvsensor.h} | 82 ++++---- src/sensors/sensor.cpp | 4 +- src/sensors/sensoraddresses.h | 4 +- 9 files changed, 150 insertions(+), 150 deletions(-) rename src/sensors/{lsm6dsv16xsensor.cpp => lsm6dsvsensor.cpp} (85%) rename src/sensors/{lsm6dsv16xsensor.h => lsm6dsvsensor.h} (65%) diff --git a/src/configuration/CalibrationConfig.cpp b/src/configuration/CalibrationConfig.cpp index 9e5b889b1..f1332dc31 100644 --- a/src/configuration/CalibrationConfig.cpp +++ b/src/configuration/CalibrationConfig.cpp @@ -37,8 +37,8 @@ namespace SlimeVR { return "MPU9250"; case ICM20948: return "ICM20948"; - case LSM6DSV16X: - return "LSM6DSV16X"; + case LSM6DSV: + return "LSM6DSV"; default: return "UNKNOWN"; } diff --git a/src/configuration/CalibrationConfig.h b/src/configuration/CalibrationConfig.h index 386ffb778..5a44f41c6 100644 --- a/src/configuration/CalibrationConfig.h +++ b/src/configuration/CalibrationConfig.h @@ -76,7 +76,7 @@ namespace SlimeVR { int32_t C[3]; }; - struct LSM6DSV16XCalibrationConfig { + struct LSM6DSVCalibrationConfig { // gyroscope bias float G_off[3]; @@ -87,7 +87,7 @@ namespace SlimeVR { float A_off[3]; }; - enum CalibrationConfigType { NONE, BMI160, MPU6050, MPU9250, ICM20948, LSM6DSV16X }; + enum CalibrationConfigType { NONE, BMI160, MPU6050, MPU9250, ICM20948, LSM6DSV }; const char* calibrationConfigTypeToString(CalibrationConfigType type); @@ -99,7 +99,7 @@ namespace SlimeVR { MPU6050CalibrationConfig mpu6050; MPU9250CalibrationConfig mpu9250; ICM20948CalibrationConfig icm20948; - LSM6DSV16XCalibrationConfig lsm6dsv16x; + LSM6DSVCalibrationConfig lsm6dsv; } data; }; } diff --git a/src/configuration/Configuration.cpp b/src/configuration/Configuration.cpp index 34b988e97..0e46ace7d 100644 --- a/src/configuration/Configuration.cpp +++ b/src/configuration/Configuration.cpp @@ -444,10 +444,10 @@ namespace SlimeVR { break; - case CalibrationConfigType::LSM6DSV16X: - m_Logger.info(" A_off: %f, %f, %f", UNPACK_VECTOR_ARRAY(c.data.lsm6dsv16x.A_off)); - m_Logger.info(" G_off: %f, %f, %f", UNPACK_VECTOR_ARRAY(c.data.lsm6dsv16x.G_off)); - m_Logger.info(" G_sen: %f, %f, %f", UNPACK_VECTOR_ARRAY(c.data.lsm6dsv16x.G_sensitivity)); + case CalibrationConfigType::LSM6DSV: + m_Logger.info(" A_off: %f, %f, %f", UNPACK_VECTOR_ARRAY(c.data.lsm6dsv.A_off)); + m_Logger.info(" G_off: %f, %f, %f", UNPACK_VECTOR_ARRAY(c.data.lsm6dsv.G_off)); + m_Logger.info(" G_sen: %f, %f, %f", UNPACK_VECTOR_ARRAY(c.data.lsm6dsv.G_sensitivity)); break; } diff --git a/src/consts.h b/src/consts.h index 89ac2085b..db3c91186 100644 --- a/src/consts.h +++ b/src/consts.h @@ -34,7 +34,7 @@ #define IMU_BNO086 7 #define IMU_BMI160 8 #define IMU_ICM20948 9 -#define IMU_LSM6DSV16X 11 +#define IMU_LSM6DSV 12 #define IMU_DEV_RESERVED 250 // Reserved, should not be used in any release firmware #define BOARD_UNKNOWN 0 diff --git a/src/sensors/SensorManager.cpp b/src/sensors/SensorManager.cpp index 497fbafae..c90e74413 100644 --- a/src/sensors/SensorManager.cpp +++ b/src/sensors/SensorManager.cpp @@ -29,7 +29,7 @@ #include "mpu6050sensor.h" #include "bmi160sensor.h" #include "icm20948sensor.h" -#include "lsm6dsv16xsensor.h" +#include "lsm6dsvsensor.h" #include "ErroneousSensor.h" #include "sensoraddresses.h" #include "GlobalVars.h" @@ -98,10 +98,10 @@ namespace SlimeVR case IMU_ICM20948: sensor = new ICM20948Sensor(sensorID, address, rotation, sclPin, sdaPin); break; - case IMU_LSM6DSV16X: + case IMU_LSM6DSV: { uint8_t intPin = extraParam; - sensor = new LSM6DSV16XSensor(sensorID, imuType, address, rotation, sclPin, sdaPin, intPin); + sensor = new LSM6DSVSensor(sensorID, imuType, address, rotation, sclPin, sdaPin, intPin); } break; default: diff --git a/src/sensors/lsm6dsv16xsensor.cpp b/src/sensors/lsm6dsvsensor.cpp similarity index 85% rename from src/sensors/lsm6dsv16xsensor.cpp rename to src/sensors/lsm6dsvsensor.cpp index 2a6029454..73776e499 100644 --- a/src/sensors/lsm6dsv16xsensor.cpp +++ b/src/sensors/lsm6dsvsensor.cpp @@ -21,21 +21,21 @@ THE SOFTWARE. */ -#include "sensors/lsm6dsv16xsensor.h" +#include "sensors/lsm6dsvsensor.h" #include "GlobalVars.h" #include "customConversions.h" -#include "lsm6dsv16xsensor.h" +#include "lsm6dsvsensor.h" #include "utils.h" -#ifdef LSM6DSV16X_INTERRUPT +#ifdef LSM6DSV_INTERRUPT volatile bool imuEvent = false; void IRAM_ATTR interruptHandler() { imuEvent = true; } #endif -LSM6DSV16XSensor::LSM6DSV16XSensor( +LSM6DSVSensor::LSM6DSVSensor( uint8_t id, uint8_t type, uint8_t address, @@ -43,19 +43,19 @@ LSM6DSV16XSensor::LSM6DSV16XSensor( uint8_t sclPin, uint8_t sdaPin, uint8_t intPin -) : Sensor("LSM6DSV16XSensor", type, id, address, rotation, sclPin, sdaPin), +) : Sensor("LSM6DSVSensor", type, id, address, rotation, sclPin, sdaPin), imu(&Wire, addr << 1), // We shift the address left 1 to work with the library m_IntPin(intPin) -#ifdef LSM6DSV16X_ESP_FUSION +#ifdef LSM6DSV_ESP_FUSION , sfusion( - 1.0f / LSM6DSV16X_FIFO_DATA_RATE, - 1.0f / LSM6DSV16X_FIFO_DATA_RATE, + 1.0f / LSM6DSV_FIFO_DATA_RATE, + 1.0f / LSM6DSV_FIFO_DATA_RATE, -1.0f ) #endif {}; -void LSM6DSV16XSensor::motionSetup() { -#ifdef LSM6DSV16X_ESP_FUSION +void LSM6DSVSensor::motionSetup() { +#ifdef LSM6DSV_ESP_FUSION RestDetectionParams restDetectionParams; restDetectionParams.restMinTimeMicros = 3 * 1e6; //restDetectionParams.restThAcc = 0.01f; @@ -97,7 +97,7 @@ void LSM6DSV16XSensor::motionSetup() { // TODO: IMU rotation could be different (IMU upside down then isFaceUp) status |= imu.Get_6D_Orientation_ZL(&isFaceDown); -#ifndef LSM6DSV16X_NO_SELF_TEST_ON_FACEDOWN +#ifndef LSM6DSV_NO_SELF_TEST_ON_FACEDOWN if (isFaceDown) { if (runSelfTest() != LSM6DSV16X_OK) { m_Logger.fatal( @@ -111,7 +111,7 @@ void LSM6DSV16XSensor::motionSetup() { return; } } -#endif // LSM6DSV16X_NO_SELF_TEST_ON_FACEDOWN +#endif // LSM6DSV_NO_SELF_TEST_ON_FACEDOWN m_Logger.info("Connected to %s on 0x%02x", getIMUNameByType(sensorType), addr); @@ -125,18 +125,18 @@ void LSM6DSV16XSensor::motionSetup() { status |= imu.Set_Auto_Increment(true); // Set maximums - status |= imu.Set_X_FS(LSM6DSV16X_ACCEL_MAX); - status |= imu.Set_G_FS(LSM6DSV16X_GYRO_MAX); + status |= imu.Set_X_FS(LSM6DSV_ACCEL_MAX); + status |= imu.Set_G_FS(LSM6DSV_GYRO_MAX); // Set data rate - status |= imu.Set_X_ODR(LSM6DSV16X_GYRO_ACCEL_RATE, LSM6DSV16X_ACC_HIGH_PERFORMANCE_MODE); - status |= imu.Set_G_ODR(LSM6DSV16X_GYRO_ACCEL_RATE, LSM6DSV16X_GYRO_HIGH_PERFORMANCE_MODE); + status |= imu.Set_X_ODR(LSM6DSV_GYRO_ACCEL_RATE, LSM6DSV16X_ACC_HIGH_PERFORMANCE_MODE); + status |= imu.Set_G_ODR(LSM6DSV_GYRO_ACCEL_RATE, LSM6DSV16X_GYRO_HIGH_PERFORMANCE_MODE); - status |= imu.Set_SFLP_ODR(LSM6DSV16X_FIFO_DATA_RATE); // Linear accel or full fusion - status |= imu.FIFO_Set_X_BDR(LSM6DSV16X_FIFO_DATA_RATE); + status |= imu.Set_SFLP_ODR(LSM6DSV_FIFO_DATA_RATE); // Linear accel or full fusion + status |= imu.FIFO_Set_X_BDR(LSM6DSV_FIFO_DATA_RATE); -#ifdef LSM6DSV16X_ESP_FUSION - status |= imu.FIFO_Set_G_BDR(LSM6DSV16X_FIFO_DATA_RATE); +#ifdef LSM6DSV_ESP_FUSION + status |= imu.FIFO_Set_G_BDR(LSM6DSV_FIFO_DATA_RATE); #endif status |= imu.FIFO_Set_Timestamp_Decimation( @@ -157,19 +157,19 @@ void LSM6DSV16XSensor::motionSetup() { // Set FIFO mode to "continuous", so old data gets thrown away status |= imu.FIFO_Set_Mode(LSM6DSV16X_STREAM_MODE); -#if defined(LSM6DSV16X_ONBOARD_FUSION) && defined(LSM6DSV16X_ESP_FUSION) +#if defined(LSM6DSV_ONBOARD_FUSION) && defined(LSM6DSV_ESP_FUSION) status |= imu.FIFO_Set_SFLP_Batch(true, true, false); // TODO: Don't use batch change for this status |= imu.Enable_SFLP_Block(); -#elif defined(LSM6DSV16X_ONBOARD_FUSION) +#elif defined(LSM6DSV_ONBOARD_FUSION) status |= imu.FIFO_Set_SFLP_Batch(true, true, false); status |= imu.Enable_Game_Rotation(); -#elif defined(LSM6DSV16X_ESP_FUSION) +#elif defined(LSM6DSV_ESP_FUSION) status |= imu.FIFO_Set_SFLP_Batch(false, true, false); // TODO: we alread calc lin accel, maybe use that status |= imu.Enable_Game_Rotation(); #endif -#ifdef LSM6DSV16X_ESP_FUSION +#ifdef LSM6DSV_ESP_FUSION loadIMUCalibration(); // Calibration @@ -184,16 +184,16 @@ void LSM6DSV16XSensor::motionSetup() { -#ifdef LSM6DSV16X_INTERRUPT +#ifdef LSM6DSV_INTERRUPT attachInterrupt(m_IntPin, interruptHandler, RISING); status |= imu.Enable_Single_Tap_Detection(LSM6DSV16X_INT1_PIN); // Tap requires an interrupt #else - status |= imu.Enable_Single_Tap_Detection(LSM6DSV16X_INT2_PIN); //Just poll to see if an event happened -#endif // LSM6DSV16X_INTERRUPT + status |= imu.Enable_Single_Tap_Detection(LSM6DSV_INT2_PIN); //Just poll to see if an event happened +#endif // LSM6DSV_INTERRUPT - status |= imu.Set_Tap_Threshold(LSM6DSV16X_TAP_THRESHOLD); - status |= imu.Set_Tap_Shock_Time(LSM6DSV16X_TAP_SHOCK_TIME); - status |= imu.Set_Tap_Quiet_Time(LSM6DSV16X_TAP_QUITE_TIME); + status |= imu.Set_Tap_Threshold(LSM6DSV_TAP_THRESHOLD); + status |= imu.Set_Tap_Shock_Time(LSM6DSV_TAP_SHOCK_TIME); + status |= imu.Set_Tap_Quiet_Time(LSM6DSV_TAP_QUITE_TIME); status |= imu.FIFO_Reset(); @@ -217,8 +217,8 @@ constexpr float mgPerG = 1000.0f; constexpr float mdpsPerDps = 1000.0f; constexpr float dpsPerRad = 57.295779578552f; -void LSM6DSV16XSensor::motionLoop() { -#ifdef LSM6DSV16X_INTERRUPT +void LSM6DSVSensor::motionLoop() { +#ifdef LSM6DSV_INTERRUPT if (imuEvent) { LSM6DSV16X_Event_Status_t eventStatus; status |= imu.Get_X_Event_Status(&eventStatus); @@ -230,7 +230,7 @@ void LSM6DSV16XSensor::motionLoop() { imuEvent = false; } #else - LSM6DSV16X_Event_Status_t eventStatus; + LSM6DSV_Event_Status_t eventStatus; status |= imu.Get_X_Event_Status(&eventStatus); if (eventStatus.TapStatus) { @@ -238,7 +238,7 @@ void LSM6DSV16XSensor::motionLoop() { } #endif - if (millis() - lastTempRead > LSM6DSV16X_TEMP_READ_INTERVAL * 1000) { + if (millis() - lastTempRead > LSM6DSV_TEMP_READ_INTERVAL * 1000) { lastTempRead = millis(); int16_t rawTemp; @@ -294,7 +294,7 @@ void LSM6DSV16XSensor::motionLoop() { } // Group all the data together //set the watermark level for nrf sleep - if (fifo_samples < LSM6DSV16X_FIFO_FRAME_SIZE) { + if (fifo_samples < LSM6DSV_FIFO_FRAME_SIZE) { return; } @@ -309,7 +309,7 @@ void LSM6DSV16XSensor::motionLoop() { newGravityVector = false; } -#if defined(LSM6DSV16X_ONBOARD_FUSION) && defined(LSM6DSV16X_ESP_FUSION) +#if defined(LSM6DSV_ONBOARD_FUSION) && defined(LSM6DSV_ESP_FUSION) // TODO: fusion of fusion stuff fusedRotation = sfusion.getQuaternionQuat(); @@ -317,7 +317,7 @@ void LSM6DSV16XSensor::motionLoop() { newFusedRotation = true; newFusedGameRotation = false; lastData = millis(); -#elif defined(LSM6DSV16X_ONBOARD_FUSION) +#elif defined(LSM6DSV_ONBOARD_FUSION) if (newFusedGameRotation) { fusedRotation = fusedRotationToQuaternion( fusedGameRotation[0], @@ -332,7 +332,7 @@ void LSM6DSV16XSensor::motionLoop() { lastData = millis(); newFusedGameRotation = false; } -#elif defined(LSM6DSV16X_ESP_FUSION) +#elif defined(LSM6DSV_ESP_FUSION) fusedRotation = sfusion.getQuaternionQuat(); if (ENABLE_INSPECTION || !OPTIMIZE_UPDATES || !lastFusedRotationSent.equalsWithEpsilon(fusedRotation)) { newFusedRotation = true; @@ -341,12 +341,12 @@ void LSM6DSV16XSensor::motionLoop() { lastData = millis(); #endif -#ifdef LSM6DSV16X_ESP_FUSION +#ifdef LSM6DSV_ESP_FUSION if (newRawAcceleration && newRawGyro) { sfusion.update6D( rawAcceleration, rawGyro, - (double)(currentDataTime - previousDataTime) * LSM6DSV16X_TIMESTAMP_LSB + (double)(currentDataTime - previousDataTime) * LSM6DSV_TIMESTAMP_LSB ); newRawAcceleration = false; newRawGyro = false; @@ -360,7 +360,7 @@ void LSM6DSV16XSensor::motionLoop() { rotation.y, rotation.z, currentDataTime, - (float)(currentDataTime - previousDataTime) * LSM6DSV16X_TIMESTAMP_LSB, + (float)(currentDataTime - previousDataTime) * LSM6DSV_TIMESTAMP_LSB, millis(), rawGyro[0], rawGyro[1], @@ -371,14 +371,14 @@ void LSM6DSV16XSensor::motionLoop() { #endif // DEBUG_SENSOR } -SensorStatus LSM6DSV16XSensor::getSensorState() { +SensorStatus LSM6DSVSensor::getSensorState() { return isWorking() ? (status == LSM6DSV16X_OK ? SensorStatus::SENSOR_OK : SensorStatus::SENSOR_ERROR) : SensorStatus::SENSOR_OFFLINE; } -Quat LSM6DSV16XSensor::fusedRotationToQuaternion(float x, float y, float z) { +Quat LSM6DSVSensor::fusedRotationToQuaternion(float x, float y, float z) { float length2 = x * x + y * y + z * z; if (length2 > 1) { @@ -393,7 +393,7 @@ Quat LSM6DSV16XSensor::fusedRotationToQuaternion(float x, float y, float z) { return Quat(x, y, z, w); } -LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::runSelfTest() { +LSM6DSV16XStatusTypeDef LSM6DSVSensor::runSelfTest() { m_Logger.info( "%s Self Test started on address: 0x%02x", getIMUNameByType(sensorType), @@ -412,7 +412,7 @@ LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::runSelfTest() { return LSM6DSV16X_OK; } -void LSM6DSV16XSensor::sendData() { +void LSM6DSVSensor::sendData() { if (newFusedRotation) { newFusedRotation = false; networkConnection.sendRotationData( @@ -447,7 +447,7 @@ void LSM6DSV16XSensor::sendData() { } } -LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::readFifo(uint16_t fifo_samples) { +LSM6DSV16XStatusTypeDef LSM6DSVSensor::readFifo(uint16_t fifo_samples) { for (uint16_t i = 0; i < fifo_samples; i++) { uint8_t tag; if (imu.FIFO_Get_Tag(&tag) != LSM6DSV16X_OK) { @@ -461,7 +461,7 @@ LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::readFifo(uint16_t fifo_samples) { switch (tag) { case lsm6dsv16x_fifo_out_raw_t::LSM6DSV16X_TIMESTAMP_TAG: { - if (i % LSM6DSV16X_FIFO_FRAME_SIZE != 0) { + if (i % LSM6DSV_FIFO_FRAME_SIZE != 0) { return LSM6DSV16X_OK; // If we are not requesting a full data set // then stop reading } @@ -479,16 +479,16 @@ LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::readFifo(uint16_t fifo_samples) { newRawAcceleration = false; newGravityVector = false; -#ifdef LSM6DSV16X_ONBOARD_FUSION +#ifdef LSM6DSV_ONBOARD_FUSION newFusedGameRotation = false; #endif -#ifdef LSM6DSV16X_ESP_FUSION +#ifdef LSM6DSV_ESP_FUSION newRawGyro = false; #endif break; } -#if SEND_ACCELERATION || defined(LSM6DSV16X_ESP_FUSION) +#if SEND_ACCELERATION || defined(LSM6DSV_ESP_FUSION) case lsm6dsv16x_fifo_out_raw_t::LSM6DSV16X_XL_NC_TAG: { // accel int32_t acceleration[3]; @@ -523,7 +523,7 @@ LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::readFifo(uint16_t fifo_samples) { gravityVector[2] /= mgPerG; newGravityVector = true; -#ifdef LSM6DSV16X_ACCEL_OFFSET_CAL +#ifdef LSM6DSV_ACCEL_OFFSET_CAL // The SFLP block does not use the accelerometer calibration gravityVector[0] += m_Calibration.G_off[0]; gravityVector[1] += m_Calibration.G_off[1]; @@ -532,7 +532,7 @@ LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::readFifo(uint16_t fifo_samples) { break; } -#ifdef LSM6DSV16X_ESP_FUSION +#ifdef LSM6DSV_ESP_FUSION case lsm6dsv16x_fifo_out_raw_t::LSM6DSV16X_GY_NC_TAG: { int32_t angularVelocity[3]; if (imu.FIFO_Get_G_Axes(angularVelocity) != LSM6DSV16X_OK) { @@ -553,12 +553,12 @@ LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::readFifo(uint16_t fifo_samples) { rawGyro[1] /= dpsPerRad; rawGyro[2] /= dpsPerRad; -#ifdef LSM6DSV16X_GYRO_OFFSET_CAL +#ifdef LSM6DSV_GYRO_OFFSET_CAL rawGyro[0] -= m_Calibration.G_off[0]; rawGyro[1] -= m_Calibration.G_off[1]; rawGyro[2] -= m_Calibration.G_off[2]; #endif -#ifdef LSM6DSV16X_GYRO_SCALE_CAL +#ifdef LSM6DSV_GYRO_SCALE_CAL rawGyro[0] *= m_Calibration.G_sensitivity[0]; rawGyro[1] *= m_Calibration.G_sensitivity[1]; rawGyro[2] *= m_Calibration.G_sensitivity[2]; @@ -569,7 +569,7 @@ LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::readFifo(uint16_t fifo_samples) { } #endif -#ifdef LSM6DSV16X_ONBOARD_FUSION +#ifdef LSM6DSV_ONBOARD_FUSION case lsm6dsv16x_fifo_out_raw_t::LSM6DSV16X_SFLP_GAME_ROTATION_VECTOR_TAG: { if (imu.FIFO_Get_Game_Vector(fusedGameRotation) != LSM6DSV16X_OK) { m_Logger.error( @@ -603,11 +603,11 @@ LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::readFifo(uint16_t fifo_samples) { -#ifdef LSM6DSV16X_ESP_FUSION +#ifdef LSM6DSV_ESP_FUSION // Used for calibration (Blocking) -LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::readNextFifoFrame() { +LSM6DSV16XStatusTypeDef LSM6DSVSensor::readNextFifoFrame() { uint16_t fifo_samples = 0; - while (fifo_samples < LSM6DSV16X_FIFO_FRAME_SIZE) { + while (fifo_samples < LSM6DSV_FIFO_FRAME_SIZE) { if (imu.FIFO_Get_Num_Samples(&fifo_samples) == LSM6DSV16X_ERROR) { m_Logger.error( "Error getting number of samples in FIFO on %s at address 0x%02x", @@ -617,15 +617,15 @@ LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::readNextFifoFrame() { return LSM6DSV16X_ERROR; } } - return readFifo(LSM6DSV16X_FIFO_FRAME_SIZE); + return readFifo(LSM6DSV_FIFO_FRAME_SIZE); } -LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::loadIMUCalibration() { +LSM6DSV16XStatusTypeDef LSM6DSVSensor::loadIMUCalibration() { SlimeVR::Configuration::CalibrationConfig sensorCalibration = configuration.getCalibration(sensorId); // If no compatible calibration data is found, the calibration data will just be zero-ed out switch (sensorCalibration.type) { - case SlimeVR::Configuration::CalibrationConfigType::LSM6DSV16X: - m_Calibration = sensorCalibration.data.lsm6dsv16x; + case SlimeVR::Configuration::CalibrationConfigType::LSM6DSV: + m_Calibration = sensorCalibration.data.lsm6dsv; break; case SlimeVR::Configuration::CalibrationConfigType::NONE: @@ -638,7 +638,7 @@ LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::loadIMUCalibration() { m_Logger.info("Calibration is advised"); } -#ifdef LSM6DSV16X_ACCEL_OFFSET_CAL +#ifdef LSM6DSV_ACCEL_OFFSET_CAL int8_t status = 0; status |= imu.Set_X_User_Offset( m_Calibration.A_off[0], @@ -659,8 +659,8 @@ LSM6DSV16XStatusTypeDef LSM6DSV16XSensor::loadIMUCalibration() { -#ifdef LSM6DSV16X_ACCEL_OFFSET_CAL -void LSM6DSV16XSensor::calibrateAccel() { +#ifdef LSM6DSV_ACCEL_OFFSET_CAL +void LSM6DSVSensor::calibrateAccel() { m_Logger.info( "Accel offset calibration started on sensor #%d of type %s at address 0x%02x", getSensorId(), @@ -713,7 +713,7 @@ void LSM6DSV16XSensor::calibrateAccel() { // Group all the data together //set the watermark level here for nrf sleep - if (fifo_samples < LSM6DSV16X_FIFO_FRAME_SIZE) { + if (fifo_samples < LSM6DSV_FIFO_FRAME_SIZE) { continue; } @@ -726,7 +726,7 @@ void LSM6DSV16XSensor::calibrateAccel() { sfusion.updateAcc( rawAcceleration, - (currentDataTime - previousDataTime) * LSM6DSV16X_TIMESTAMP_LSB + (currentDataTime - previousDataTime) * LSM6DSV_TIMESTAMP_LSB ); newRawAcceleration = false; @@ -820,8 +820,8 @@ void LSM6DSV16XSensor::calibrateAccel() { -#ifdef LSM6DSV16X_GYRO_OFFSET_CAL -void LSM6DSV16XSensor::calibrateGyro() { +#ifdef LSM6DSV_GYRO_OFFSET_CAL +void LSM6DSVSensor::calibrateGyro() { m_Logger.info( "Gyro offset calibration started on sensor #%d of type %s at address 0x%02x", getSensorId(), @@ -887,8 +887,8 @@ void LSM6DSV16XSensor::calibrateGyro() { #endif -#ifdef LSM6DSV16X_GYRO_SENSITIVITY_CAL -void LSM6DSV16XSensor::calibrateGyroSensitivity() { +#ifdef LSM6DSV_GYRO_SENSITIVITY_CAL +void LSM6DSVSensor::calibrateGyroSensitivity() { m_Logger.info( "Gyro sensitivity calibration started on sensor #%d of type %s at address 0x%02x", getSensorId(), @@ -918,7 +918,7 @@ void LSM6DSV16XSensor::calibrateGyroSensitivity() { m_Logger.info( "\tStep 3: Rotate the tracker about one axis %d full rotations and align with the " "previous edge.", - LSM6DSV16X_GYRO_SENSITIVITY_SPINS + LSM6DSV_GYRO_SENSITIVITY_SPINS ); m_Logger.info("\t\tNOTE: the light will turn off after you start moving it"); @@ -934,7 +934,7 @@ void LSM6DSV16XSensor::calibrateGyroSensitivity() { waitForRest(); ledManager.on(); // The user should rotate - m_Logger.info("Rotate the tracker %d times", LSM6DSV16X_GYRO_SENSITIVITY_SPINS); + m_Logger.info("Rotate the tracker %d times", LSM6DSV_GYRO_SENSITIVITY_SPINS); gyroCount[0] = 0.0f; gyroCount[1] = 0.0f; gyroCount[2] = 0.0f; @@ -958,11 +958,11 @@ void LSM6DSV16XSensor::calibrateGyroSensitivity() { if (abs(gyroCount[0]) > abs(gyroCount[1]) && abs(gyroCount[0]) > abs(gyroCount[2])) { //Spun in X imu.Get_6D_Orientation_XH(&isUp); if((!isUp && gyroCount[0] > 0) || (isUp && gyroCount[0] < 0)) { - calculatedScale[0] = (1.0 / (1.0 - ((rawRotationInit.y - rawRotationFinal.y)/(360.0f * LSM6DSV16X_GYRO_SENSITIVITY_SPINS)))); + calculatedScale[0] = (1.0 / (1.0 - ((rawRotationInit.y - rawRotationFinal.y)/(360.0f * LSM6DSV_GYRO_SENSITIVITY_SPINS)))); m_Logger.info("X, Diff: %f", rawRotationInit.y - rawRotationFinal.y); } else { - calculatedScale[0] = (1.0 / (1.0 - ((rawRotationFinal.y - rawRotationInit.y)/(360.0f * LSM6DSV16X_GYRO_SENSITIVITY_SPINS)))); + calculatedScale[0] = (1.0 / (1.0 - ((rawRotationFinal.y - rawRotationInit.y)/(360.0f * LSM6DSV_GYRO_SENSITIVITY_SPINS)))); m_Logger.info("-X, Diff: %f", rawRotationFinal.y - rawRotationInit.y); } } @@ -970,11 +970,11 @@ void LSM6DSV16XSensor::calibrateGyroSensitivity() { else if (abs(gyroCount[1]) > abs(gyroCount[0]) && abs(gyroCount[1]) > abs(gyroCount[2])) { //Spun in Y imu.Get_6D_Orientation_YH(&isUp); if((isUp && gyroCount[1] > 0) || (!isUp && gyroCount[1] < 0)) { - calculatedScale[1] = (1.0 / (1.0 - ((rawRotationInit.y - rawRotationFinal.y)/(360.0f * LSM6DSV16X_GYRO_SENSITIVITY_SPINS)))); + calculatedScale[1] = (1.0 / (1.0 - ((rawRotationInit.y - rawRotationFinal.y)/(360.0f * LSM6DSV_GYRO_SENSITIVITY_SPINS)))); m_Logger.info("Y, Diff: %f", rawRotationInit.y - rawRotationFinal.y); } else { - calculatedScale[1] = (1.0 / (1.0 - ((rawRotationFinal.y - rawRotationInit.y)/(360.0f * LSM6DSV16X_GYRO_SENSITIVITY_SPINS)))); + calculatedScale[1] = (1.0 / (1.0 - ((rawRotationFinal.y - rawRotationInit.y)/(360.0f * LSM6DSV_GYRO_SENSITIVITY_SPINS)))); m_Logger.info("-Y, Diff: %f", rawRotationFinal.y - rawRotationInit.y); } } @@ -982,11 +982,11 @@ void LSM6DSV16XSensor::calibrateGyroSensitivity() { else if (abs(gyroCount[2]) > abs(gyroCount[0]) && abs(gyroCount[2]) > abs(gyroCount[1])) { //Spun in Z imu.Get_6D_Orientation_ZH(&isUp); if((isUp && gyroCount[2] > 0) || (!isUp && gyroCount[2] < 0)) { - calculatedScale[2] = (1.0 / (1.0 - ((rawRotationInit.y - rawRotationFinal.y)/(360.0f * LSM6DSV16X_GYRO_SENSITIVITY_SPINS)))); + calculatedScale[2] = (1.0 / (1.0 - ((rawRotationInit.y - rawRotationFinal.y)/(360.0f * LSM6DSV_GYRO_SENSITIVITY_SPINS)))); m_Logger.info("Z, Diff: %f", rawRotationInit.y - rawRotationFinal.y); } else { - calculatedScale[2] = (1.0 / (1.0 - ((rawRotationFinal.y - rawRotationInit.y)/(360.0f * LSM6DSV16X_GYRO_SENSITIVITY_SPINS)))); + calculatedScale[2] = (1.0 / (1.0 - ((rawRotationFinal.y - rawRotationInit.y)/(360.0f * LSM6DSV_GYRO_SENSITIVITY_SPINS)))); m_Logger.info("-Z, Diff: %f", rawRotationFinal.y - rawRotationInit.y); } } @@ -1041,7 +1041,7 @@ void LSM6DSV16XSensor::calibrateGyroSensitivity() { -void LSM6DSV16XSensor::startCalibration(int calibrationType) { +void LSM6DSVSensor::startCalibration(int calibrationType) { m_Logger.info("Flip right side up in the next 5 seconds to start calibration."); delay(5000); uint8_t isFaceUp; @@ -1060,17 +1060,17 @@ void LSM6DSV16XSensor::startCalibration(int calibrationType) { } -#ifdef LSM6DSV16X_GYRO_OFFSET_CAL +#ifdef LSM6DSV_GYRO_OFFSET_CAL calibrateGyro(); #endif -#ifdef LSM6DSV16X_ACCEL_OFFSET_CAL +#ifdef LSM6DSV_ACCEL_OFFSET_CAL calibrateAccel(); #endif m_Logger.info("Calibration finished, enjoy"); } -void LSM6DSV16XSensor::printCalibration() { +void LSM6DSVSensor::printCalibration() { m_Logger.info("Sensor #%d Calibration Data", getSensorId()); m_Logger.info(" Accel Offset Calibration Values for %s on 0x%02x", getIMUNameByType(sensorType), addr); m_Logger.info(" X Scale: %f", m_Calibration.A_off[0]); @@ -1088,7 +1088,7 @@ void LSM6DSV16XSensor::printCalibration() { m_Logger.info(" Z Scale: %f", m_Calibration.G_sensitivity[2]); } -void LSM6DSV16XSensor::resetCalibration() { +void LSM6DSVSensor::resetCalibration() { m_Logger.info("Sensor #%d Calibration Reset", getSensorId()); m_Calibration.A_off[0] = 0.0f; m_Calibration.A_off[1] = 0.0f; @@ -1104,37 +1104,37 @@ void LSM6DSV16XSensor::resetCalibration() { saveCalibration(); } -void LSM6DSV16XSensor::saveCalibration() { +void LSM6DSVSensor::saveCalibration() { m_Logger.debug("Saving the calibration data"); SlimeVR::Configuration::CalibrationConfig calibration; - calibration.type = SlimeVR::Configuration::CalibrationConfigType::LSM6DSV16X; - calibration.data.lsm6dsv16x = m_Calibration; + calibration.type = SlimeVR::Configuration::CalibrationConfigType::LSM6DSV; + calibration.data.lsm6dsv = m_Calibration; configuration.setCalibration(sensorId, calibration); configuration.save(); } -void LSM6DSV16XSensor::apply6DToRestDetection() { +void LSM6DSVSensor::apply6DToRestDetection() { if (newRawGyro && newRawAcceleration) { sfusion.update6D( rawAcceleration, rawGyro, - (double)(currentDataTime - previousDataTime) * LSM6DSV16X_TIMESTAMP_LSB + (double)(currentDataTime - previousDataTime) * LSM6DSV_TIMESTAMP_LSB ); } newRawAcceleration = false; } -void LSM6DSV16XSensor::waitForRest() { +void LSM6DSVSensor::waitForRest() { while (!sfusion.getRestDetected()) { readNextFifoFrame(); apply6DToRestDetection(); } } -void LSM6DSV16XSensor::waitForMovement() { +void LSM6DSVSensor::waitForMovement() { while (sfusion.getRestDetected()) { readNextFifoFrame(); apply6DToRestDetection(); } } -#endif // LSM6DSV16X_ESP_FUSION +#endif // LSM6DSV_ESP_FUSION diff --git a/src/sensors/lsm6dsv16xsensor.h b/src/sensors/lsm6dsvsensor.h similarity index 65% rename from src/sensors/lsm6dsv16xsensor.h rename to src/sensors/lsm6dsvsensor.h index 6e29e4f7b..665d21510 100644 --- a/src/sensors/lsm6dsv16xsensor.h +++ b/src/sensors/lsm6dsvsensor.h @@ -21,8 +21,8 @@ THE SOFTWARE. */ -#ifndef SENSORS_LSM6DSV16X_H -#define SENSORS_LSM6DSV16X_H +#ifndef SENSORS_LSM6DSV_H +#define SENSORS_LSM6DSV_H #include "LSM6DSV16X.h" #include "sensor.h" @@ -30,62 +30,62 @@ #include "magneto1.4.h" #include "SensorFusionRestDetect.h" -#ifndef LSM6DSV16X_ACCEL_MAX -#define LSM6DSV16X_ACCEL_MAX 16 +#ifndef LSM6DSV_ACCEL_MAX +#define LSM6DSV_ACCEL_MAX 16 #endif -#ifndef LSM6DSV16X_GYRO_MAX -#define LSM6DSV16X_GYRO_MAX 2000 +#ifndef LSM6DSV_GYRO_MAX +#define LSM6DSV_GYRO_MAX 2000 #endif -#ifndef LSM6DSV16X_FIFO_DATA_RATE -#define LSM6DSV16X_FIFO_DATA_RATE 120 +#ifndef LSM6DSV_FIFO_DATA_RATE +#define LSM6DSV_FIFO_DATA_RATE 120 #endif -#ifndef LSM6DSV16X_GYRO_ACCEL_RATE -#define LSM6DSV16X_GYRO_ACCEL_RATE 7680.0f +#ifndef LSM6DSV_GYRO_ACCEL_RATE +#define LSM6DSV_GYRO_ACCEL_RATE 7680.0f #endif -#ifndef LSM6DSV16X_FIFO_TEMP_DATA_RATE -#define LSM6DSV16X_FIFO_TEMP_DATA_RATE 1.875f +#ifndef LSM6DSV_FIFO_TEMP_DATA_RATE +#define LSM6DSV_FIFO_TEMP_DATA_RATE 1.875f #endif -#ifndef LSM6DSV16X_TEMP_READ_INTERVAL -#define LSM6DSV16X_TEMP_READ_INTERVAL 1 +#ifndef LSM6DSV_TEMP_READ_INTERVAL +#define LSM6DSV_TEMP_READ_INTERVAL 1 #endif -#ifndef LSM6DSV16X_TAP_THRESHOLD -#define LSM6DSV16X_TAP_THRESHOLD 5 //0-32 +#ifndef LSM6DSV_TAP_THRESHOLD +#define LSM6DSV_TAP_THRESHOLD 5 //0-32 #endif -#ifndef LSM6DSV16X_TAP_SHOCK_TIME -#define LSM6DSV16X_TAP_SHOCK_TIME 3 //0-3 +#ifndef LSM6DSV_TAP_SHOCK_TIME +#define LSM6DSV_TAP_SHOCK_TIME 3 //0-3 #endif -#ifndef LSM6DSV16X_TAP_QUITE_TIME -#define LSM6DSV16X_TAP_QUITE_TIME 3 //0-3 +#ifndef LSM6DSV_TAP_QUITE_TIME +#define LSM6DSV_TAP_QUITE_TIME 3 //0-3 #endif -#define LSM6DSV16X_TIMESTAMP_LSB 21.75e-6f +#define LSM6DSV_TIMESTAMP_LSB 21.75e-6f // #define SELF_TEST_ON_INIT // #define REINIT_ON_FAILURE - #define LSM6DSV16X_INTERRUPT //recommended for tap detect -// #define LSM6DSV16X_NO_SELF_TEST_ON_FACEDOWN -#define LSM6DSV16X_ONBOARD_FUSION -#define LSM6DSV16X_ESP_FUSION + #define LSM6DSV_INTERRUPT //recommended for tap detect +// #define LSM6DSV_NO_SELF_TEST_ON_FACEDOWN +#define LSM6DSV_ONBOARD_FUSION +#define LSM6DSV_ESP_FUSION -#ifdef LSM6DSV16X_ESP_FUSION -#define LSM6DSV16X_GYRO_OFFSET_CAL -#define LSM6DSV16X_ACCEL_OFFSET_CAL -#define LSM6DSV16X_GYRO_SENSITIVITY_CAL +#ifdef LSM6DSV_ESP_FUSION +#define LSM6DSV_GYRO_OFFSET_CAL +#define LSM6DSV_ACCEL_OFFSET_CAL +#define LSM6DSV_GYRO_SENSITIVITY_CAL #endif -#ifdef LSM6DSV16X_GYRO_SENSITIVITY_CAL -#define LSM6DSV16X_GYRO_SENSITIVITY_SPINS 5 +#ifdef LSM6DSV_GYRO_SENSITIVITY_CAL +#define LSM6DSV_GYRO_SENSITIVITY_SPINS 5 #endif #ifdef REINIT_ON_FAILURE @@ -93,17 +93,17 @@ #undef SELF_TEST_ON_INIT #endif -#if defined(LSM6DSV16X_ONBOARD_FUSION) && defined(LSM6DSV16X_ESP_FUSION) -#define LSM6DSV16X_FIFO_FRAME_SIZE 5 // X BDR, G BDR, Game, Gravity, Timestamp +#if defined(LSM6DSV_ONBOARD_FUSION) && defined(LSM6DSV_ESP_FUSION) +#define LSM6DSV_FIFO_FRAME_SIZE 5 // X BDR, G BDR, Game, Gravity, Timestamp #else -#define LSM6DSV16X_FIFO_FRAME_SIZE 4 // X BDR, (G BDR || Game), Gravity, Timestamp +#define LSM6DSV_FIFO_FRAME_SIZE 4 // X BDR, (G BDR || Game), Gravity, Timestamp #endif -class LSM6DSV16XSensor : public Sensor { +class LSM6DSVSensor : public Sensor { public: - LSM6DSV16XSensor( + LSM6DSVSensor( uint8_t id, uint8_t type, uint8_t address, @@ -112,13 +112,13 @@ class LSM6DSV16XSensor : public Sensor { uint8_t sdaPin, uint8_t intPin ); - ~LSM6DSV16XSensor(){}; + ~LSM6DSVSensor(){}; void motionSetup() override final; void motionLoop() override final; void sendData() override final; SensorStatus getSensorState() override final; -#ifdef LSM6DSV16X_ESP_FUSION +#ifdef LSM6DSV_ESP_FUSION void startCalibration(int calibrationType) override final; void calibrateAccel() override final; void calibrateGyro() override final; @@ -147,20 +147,20 @@ class LSM6DSV16XSensor : public Sensor { uint32_t previousDataTime = 0; uint32_t currentDataTime = 0; -#ifdef LSM6DSV16X_ONBOARD_FUSION +#ifdef LSM6DSV_ONBOARD_FUSION float fusedGameRotation[3]; Quat previousGameRotation; bool newFusedGameRotation = false; #endif -#ifdef LSM6DSV16X_ESP_FUSION +#ifdef LSM6DSV_ESP_FUSION LSM6DSV16XStatusTypeDef readNextFifoFrame(); LSM6DSV16XStatusTypeDef loadIMUCalibration(); void apply6DToRestDetection(); void waitForRest(); void waitForMovement(); void saveCalibration(); - SlimeVR::Configuration::LSM6DSV16XCalibrationConfig m_Calibration; + SlimeVR::Configuration::LSM6DSVCalibrationConfig m_Calibration; SlimeVR::Sensors::SensorFusionRestDetect sfusion; float rawGyro[3]; bool newRawGyro = false; diff --git a/src/sensors/sensor.cpp b/src/sensors/sensor.cpp index 1bb9f27d6..10033238e 100644 --- a/src/sensors/sensor.cpp +++ b/src/sensors/sensor.cpp @@ -93,8 +93,8 @@ const char * getIMUNameByType(int imuType) { return "BMI160"; case IMU_ICM20948: return "ICM20948"; - case IMU_LSM6DSV16X: - return "LSM6DSV16X"; + case IMU_LSM6DSV: + return "LSM6DSV"; } return "Unknown"; } diff --git a/src/sensors/sensoraddresses.h b/src/sensors/sensoraddresses.h index f9ba44460..76c91082f 100644 --- a/src/sensors/sensoraddresses.h +++ b/src/sensors/sensoraddresses.h @@ -13,7 +13,7 @@ #elif IMU == IMU_MPU9250 || IMU == IMU_BMI160 || IMU == IMU_MPU6500 || IMU == IMU_MPU6050 || IMU == IMU_ICM20948 #define PRIMARY_IMU_ADDRESS_ONE 0x68 #define PRIMARY_IMU_ADDRESS_TWO 0x69 - #elif IMU == IMU_LSM6DSV16X + #elif IMU == IMU_LSM6DSV #define PRIMARY_IMU_ADDRESS_ONE 0x6A #define PRIMARY_IMU_ADDRESS_TWO 0x6B #endif @@ -27,7 +27,7 @@ #elif SECOND_IMU == IMU_MPU9250 || SECOND_IMU == IMU_BMI160 || SECOND_IMU == IMU_MPU6500 || SECOND_IMU == IMU_MPU6050 || SECOND_IMU == IMU_ICM20948 #define SECONDARY_IMU_ADDRESS_ONE 0x68 #define SECONDARY_IMU_ADDRESS_TWO 0x69 - #elif SECOND_IMU == IMU_LSM6DSV16X + #elif SECOND_IMU == IMU_LSM6DSV #define SECONDARY_IMU_ADDRESS_ONE 0x6A #define SECONDARY_IMU_ADDRESS_TWO 0x6B #endif From 992cffd485ab3ce4eac55c45a96e62aa94f58a2c Mon Sep 17 00:00:00 2001 From: wigwagwent Date: Mon, 16 Oct 2023 12:45:23 -0700 Subject: [PATCH 43/60] Code cleanup --- src/sensors/SensorFusion.cpp | 6 +- src/sensors/lsm6dsvsensor.cpp | 217 +++++++++------------------------- src/sensors/lsm6dsvsensor.h | 53 +++------ 3 files changed, 77 insertions(+), 199 deletions(-) diff --git a/src/sensors/SensorFusion.cpp b/src/sensors/SensorFusion.cpp index 809dbf10d..faba9551a 100644 --- a/src/sensors/SensorFusion.cpp +++ b/src/sensors/SensorFusion.cpp @@ -165,9 +165,9 @@ namespace SlimeVR void SensorFusion::calcLinearAcc(const sensor_real_t accin[3], const sensor_real_t gravVec[3], sensor_real_t accout[3]) { - accout[0] = accin[0] - gravVec[0] * CONST_EARTH_GRAVITY; - accout[1] = accin[1] - gravVec[1] * CONST_EARTH_GRAVITY; - accout[2] = accin[2] - gravVec[2] * CONST_EARTH_GRAVITY; + accout[0] = (accin[0] - gravVec[0]) * CONST_EARTH_GRAVITY; + accout[1] = (accin[1] - gravVec[1]) * CONST_EARTH_GRAVITY; + accout[2] = (accin[2] - gravVec[2]) * CONST_EARTH_GRAVITY; } } } diff --git a/src/sensors/lsm6dsvsensor.cpp b/src/sensors/lsm6dsvsensor.cpp index 73776e499..8a1bdb7df 100644 --- a/src/sensors/lsm6dsvsensor.cpp +++ b/src/sensors/lsm6dsvsensor.cpp @@ -46,7 +46,7 @@ LSM6DSVSensor::LSM6DSVSensor( ) : Sensor("LSM6DSVSensor", type, id, address, rotation, sclPin, sdaPin), imu(&Wire, addr << 1), // We shift the address left 1 to work with the library m_IntPin(intPin) -#ifdef LSM6DSV_ESP_FUSION +#if (LSM6DSV_FUSION_SOURCE == LSM6DSV_FUSION_ESP) , sfusion( 1.0f / LSM6DSV_FIFO_DATA_RATE, 1.0f / LSM6DSV_FIFO_DATA_RATE, @@ -55,16 +55,6 @@ LSM6DSVSensor::LSM6DSVSensor( {}; void LSM6DSVSensor::motionSetup() { -#ifdef LSM6DSV_ESP_FUSION - RestDetectionParams restDetectionParams; - restDetectionParams.restMinTimeMicros = 3 * 1e6; - //restDetectionParams.restThAcc = 0.01f; - //restDetectionParams.restThGyr = 0.15f; //dps - restDetectionParams.restThAcc = 0.1f; - restDetectionParams.restThGyr = 0.5f; //dps - sfusion.updateRestDetectionParameters(restDetectionParams); -#endif - uint8_t deviceId = 0; if (imu.ReadID(&deviceId) == LSM6DSV16X_ERROR) { m_Logger.fatal( @@ -91,7 +81,6 @@ void LSM6DSVSensor::motionSetup() { return; } - status |= imu.Enable_6D_Orientation(LSM6DSV16X_INT2_PIN); uint8_t isFaceDown; // TODO: IMU rotation could be different (IMU upside down then isFaceUp) @@ -132,10 +121,9 @@ void LSM6DSVSensor::motionSetup() { status |= imu.Set_X_ODR(LSM6DSV_GYRO_ACCEL_RATE, LSM6DSV16X_ACC_HIGH_PERFORMANCE_MODE); status |= imu.Set_G_ODR(LSM6DSV_GYRO_ACCEL_RATE, LSM6DSV16X_GYRO_HIGH_PERFORMANCE_MODE); - status |= imu.Set_SFLP_ODR(LSM6DSV_FIFO_DATA_RATE); // Linear accel or full fusion status |= imu.FIFO_Set_X_BDR(LSM6DSV_FIFO_DATA_RATE); -#ifdef LSM6DSV_ESP_FUSION +#if (LSM6DSV_FUSION_SOURCE == LSM6DSV_FUSION_ESP) status |= imu.FIFO_Set_G_BDR(LSM6DSV_FIFO_DATA_RATE); #endif @@ -157,19 +145,15 @@ void LSM6DSVSensor::motionSetup() { // Set FIFO mode to "continuous", so old data gets thrown away status |= imu.FIFO_Set_Mode(LSM6DSV16X_STREAM_MODE); -#if defined(LSM6DSV_ONBOARD_FUSION) && defined(LSM6DSV_ESP_FUSION) - status |= imu.FIFO_Set_SFLP_Batch(true, true, false); // TODO: Don't use batch change for this - status |= imu.Enable_SFLP_Block(); - -#elif defined(LSM6DSV_ONBOARD_FUSION) +#if (LSM6DSV_FUSION_SOURCE == LSM6DSV_FUSION_ONBOARD) status |= imu.FIFO_Set_SFLP_Batch(true, true, false); - status |= imu.Enable_Game_Rotation(); -#elif defined(LSM6DSV_ESP_FUSION) - status |= imu.FIFO_Set_SFLP_Batch(false, true, false); // TODO: we alread calc lin accel, maybe use that - status |= imu.Enable_Game_Rotation(); + status |= imu.Set_SFLP_ODR(LSM6DSV_FIFO_DATA_RATE); + status |= imu.Enable_SFLP_Block(); #endif + + -#ifdef LSM6DSV_ESP_FUSION +#if (LSM6DSV_FUSION_SOURCE == LSM6DSV_FUSION_ESP) loadIMUCalibration(); // Calibration @@ -186,9 +170,9 @@ void LSM6DSVSensor::motionSetup() { #ifdef LSM6DSV_INTERRUPT attachInterrupt(m_IntPin, interruptHandler, RISING); - status |= imu.Enable_Single_Tap_Detection(LSM6DSV16X_INT1_PIN); // Tap requires an interrupt + status |= imu.Enable_Single_Tap_Detection(LSM6DSV16X_INT1_PIN); // Tap recommends an interrupt #else - status |= imu.Enable_Single_Tap_Detection(LSM6DSV_INT2_PIN); //Just poll to see if an event happened + status |= imu.Enable_Single_Tap_Detection(LSM6DSV16X_INT2_PIN); //Just poll to see if an event happened jank but works #endif // LSM6DSV_INTERRUPT status |= imu.Set_Tap_Threshold(LSM6DSV_TAP_THRESHOLD); @@ -216,6 +200,7 @@ void LSM6DSVSensor::motionSetup() { constexpr float mgPerG = 1000.0f; constexpr float mdpsPerDps = 1000.0f; constexpr float dpsPerRad = 57.295779578552f; +constexpr uint8_t fifoFramSize = 4; // X BDR, (G BDR || Game), Gravity, Timestamp void LSM6DSVSensor::motionLoop() { #ifdef LSM6DSV_INTERRUPT @@ -225,7 +210,6 @@ void LSM6DSVSensor::motionLoop() { if (eventStatus.TapStatus) { tap++; - m_Logger.info("Tap: %d", millis()); } imuEvent = false; } @@ -267,19 +251,6 @@ void LSM6DSVSensor::motionLoop() { statusManager.setStatus(SlimeVR::Status::IMU_ERROR, true); working = false; lastData = millis(); // reset time counter for error message - -#ifdef REINIT_ON_FAILURE // Most likely will not fix the imu (maybe remove) - if (reinitOnFailAttempts < REINIT_RETRY_MAX_ATTEMPTS) { - motionSetup(); - } else { - m_Logger.error( - "The %s at address 0x%02x, could not be revived", - getIMUNameByType(sensorType), - addr - ); - } - reinitOnFailAttempts++; // buf overflow will make it try again in about 4 min -#endif // REINIT_ON_FAILURE } uint16_t fifo_samples = 0; @@ -294,59 +265,30 @@ void LSM6DSVSensor::motionLoop() { } // Group all the data together //set the watermark level for nrf sleep - if (fifo_samples < LSM6DSV_FIFO_FRAME_SIZE) { + // TODO: add the interupt code to this + + if (fifo_samples < fifoFramSize) { return; } readFifo(fifo_samples); - if (newRawAcceleration && newGravityVector) { - acceleration[0] = (rawAcceleration[0] - gravityVector[0]) * CONST_EARTH_GRAVITY; - acceleration[1] = (rawAcceleration[1] - gravityVector[1]) * CONST_EARTH_GRAVITY; - acceleration[2] = (rawAcceleration[2] - gravityVector[2]) * CONST_EARTH_GRAVITY; - - newAcceleration = true; - newGravityVector = false; +#if (LSM6DSV_FUSION_SOURCE == LSM6DSV_FUSION_ESP) + fusedRotation = sfusion.getQuaternionQuat() * sensorOffset; + if (ENABLE_INSPECTION || !OPTIMIZE_UPDATES || !lastFusedRotationSent.equalsWithEpsilon(fusedRotation)) { + newFusedRotation = true; + lastFusedRotationSent = fusedRotation; } - -#if defined(LSM6DSV_ONBOARD_FUSION) && defined(LSM6DSV_ESP_FUSION) - - // TODO: fusion of fusion stuff - fusedRotation = sfusion.getQuaternionQuat(); - lastFusedRotationSent = fusedRotation; - newFusedRotation = true; - newFusedGameRotation = false; lastData = millis(); -#elif defined(LSM6DSV_ONBOARD_FUSION) - if (newFusedGameRotation) { - fusedRotation = fusedRotationToQuaternion( - fusedGameRotation[0], - fusedGameRotation[1], - fusedGameRotation[2] - ); - if (ENABLE_INSPECTION || !OPTIMIZE_UPDATES - || !lastFusedRotationSent.equalsWithEpsilon(fusedRotation)) { - newFusedRotation = true; - lastFusedRotationSent = fusedRotation; - } - lastData = millis(); - newFusedGameRotation = false; - } -#elif defined(LSM6DSV_ESP_FUSION) - fusedRotation = sfusion.getQuaternionQuat(); - if (ENABLE_INSPECTION || !OPTIMIZE_UPDATES || !lastFusedRotationSent.equalsWithEpsilon(fusedRotation)) { - newFusedRotation = true; - lastFusedRotationSent = fusedRotation; - } - lastData = millis(); -#endif -#ifdef LSM6DSV_ESP_FUSION + sfusion.getLinearAcc(acceleration); + newAcceleration = true; + if (newRawAcceleration && newRawGyro) { sfusion.update6D( rawAcceleration, rawGyro, - (double)(currentDataTime - previousDataTime) * LSM6DSV_TIMESTAMP_LSB + lsm6dsv16x_from_lsb_to_nsec(currentDataTime - previousDataTime) * 1e-9 ); newRawAcceleration = false; newRawGyro = false; @@ -360,7 +302,7 @@ void LSM6DSVSensor::motionLoop() { rotation.y, rotation.z, currentDataTime, - (float)(currentDataTime - previousDataTime) * LSM6DSV_TIMESTAMP_LSB, + lsm6dsv16x_from_lsb_to_nsec(currentDataTime - previousDataTime) * 1e-9, millis(), rawGyro[0], rawGyro[1], @@ -435,7 +377,7 @@ void LSM6DSVSensor::sendData() { } #endif // SEND_ACCELERATION - if (tap != 0) // chip supports tap and double tap + if (tap != 0) { networkConnection.sendSensorTap(sensorId, tap); tap = 0; @@ -461,7 +403,7 @@ LSM6DSV16XStatusTypeDef LSM6DSVSensor::readFifo(uint16_t fifo_samples) { switch (tag) { case lsm6dsv16x_fifo_out_raw_t::LSM6DSV16X_TIMESTAMP_TAG: { - if (i % LSM6DSV_FIFO_FRAME_SIZE != 0) { + if (i % fifoFramSize != 0) { return LSM6DSV16X_OK; // If we are not requesting a full data set // then stop reading } @@ -477,22 +419,17 @@ LSM6DSV16XStatusTypeDef LSM6DSVSensor::readFifo(uint16_t fifo_samples) { // newTemperature = false; newRawAcceleration = false; - newGravityVector = false; -#ifdef LSM6DSV_ONBOARD_FUSION - newFusedGameRotation = false; -#endif -#ifdef LSM6DSV_ESP_FUSION +#if (LSM6DSV_FUSION_SOURCE == LSM6DSV_FUSION_ESP) newRawGyro = false; #endif break; } -#if SEND_ACCELERATION || defined(LSM6DSV_ESP_FUSION) case lsm6dsv16x_fifo_out_raw_t::LSM6DSV16X_XL_NC_TAG: { // accel - int32_t acceleration[3]; - if (imu.FIFO_Get_X_Axes(acceleration) != LSM6DSV16X_OK) { + int32_t intAcceleration[3]; + if (imu.FIFO_Get_X_Axes(intAcceleration) != LSM6DSV16X_OK) { m_Logger.error( "Failed to get accelerometer data on %s at address 0x%02x", getIMUNameByType(sensorType), @@ -500,16 +437,16 @@ LSM6DSV16XStatusTypeDef LSM6DSVSensor::readFifo(uint16_t fifo_samples) { ); return LSM6DSV16X_ERROR; } - rawAcceleration[0] = (acceleration[0] / mgPerG); - rawAcceleration[1] = (acceleration[1] / mgPerG); - rawAcceleration[2] = (acceleration[2] / mgPerG); + rawAcceleration[0] = (intAcceleration[0] / mgPerG); + rawAcceleration[1] = (intAcceleration[1] / mgPerG); + rawAcceleration[2] = (intAcceleration[2] / mgPerG); newRawAcceleration = true; break; } -#endif // SEND_ACCELERATION case lsm6dsv16x_fifo_out_raw_t::LSM6DSV16X_SFLP_GRAVITY_VECTOR_TAG: { + float gravityVector[3]; if (imu.FIFO_Get_Gravity_Vector(gravityVector) != LSM6DSV16X_OK) { m_Logger.error( "Failed to get gravity vector on %s at address 0x%02x", @@ -521,18 +458,13 @@ LSM6DSV16XStatusTypeDef LSM6DSVSensor::readFifo(uint16_t fifo_samples) { gravityVector[0] /= mgPerG; gravityVector[1] /= mgPerG; gravityVector[2] /= mgPerG; - newGravityVector = true; -#ifdef LSM6DSV_ACCEL_OFFSET_CAL - // The SFLP block does not use the accelerometer calibration - gravityVector[0] += m_Calibration.G_off[0]; - gravityVector[1] += m_Calibration.G_off[1]; - gravityVector[2] += m_Calibration.G_off[2]; -#endif + SlimeVR::Sensors::SensorFusion::calcLinearAcc(rawAcceleration, gravityVector, acceleration); + newAcceleration = true; break; } -#ifdef LSM6DSV_ESP_FUSION +#if (LSM6DSV_FUSION_SOURCE == LSM6DSV_FUSION_ESP) case lsm6dsv16x_fifo_out_raw_t::LSM6DSV16X_GY_NC_TAG: { int32_t angularVelocity[3]; if (imu.FIFO_Get_G_Axes(angularVelocity) != LSM6DSV16X_OK) { @@ -569,8 +501,9 @@ LSM6DSV16XStatusTypeDef LSM6DSVSensor::readFifo(uint16_t fifo_samples) { } #endif -#ifdef LSM6DSV_ONBOARD_FUSION +#if (LSM6DSV_FUSION_SOURCE == LSM6DSV_FUSION_ONBOARD) case lsm6dsv16x_fifo_out_raw_t::LSM6DSV16X_SFLP_GAME_ROTATION_VECTOR_TAG: { + float fusedGameRotation[3]; if (imu.FIFO_Get_Game_Vector(fusedGameRotation) != LSM6DSV16X_OK) { m_Logger.error( "Failed to get game rotation vector on %s at address 0x%02x", @@ -580,7 +513,18 @@ LSM6DSV16XStatusTypeDef LSM6DSVSensor::readFifo(uint16_t fifo_samples) { return LSM6DSV16X_ERROR; } - newFusedGameRotation = true; + fusedRotation = fusedRotationToQuaternion( + fusedGameRotation[0], + fusedGameRotation[1], + fusedGameRotation[2] + ) + * sensorOffset; + if (ENABLE_INSPECTION || !OPTIMIZE_UPDATES + || !lastFusedRotationSent.equalsWithEpsilon(fusedRotation)) { + newFusedRotation = true; + lastFusedRotationSent = fusedRotation; + } + lastData = millis(); break; } #endif @@ -603,11 +547,11 @@ LSM6DSV16XStatusTypeDef LSM6DSVSensor::readFifo(uint16_t fifo_samples) { -#ifdef LSM6DSV_ESP_FUSION +#if (LSM6DSV_FUSION_SOURCE == LSM6DSV_FUSION_ESP) // Used for calibration (Blocking) LSM6DSV16XStatusTypeDef LSM6DSVSensor::readNextFifoFrame() { uint16_t fifo_samples = 0; - while (fifo_samples < LSM6DSV_FIFO_FRAME_SIZE) { + while (fifo_samples < fifoFramSize) { if (imu.FIFO_Get_Num_Samples(&fifo_samples) == LSM6DSV16X_ERROR) { m_Logger.error( "Error getting number of samples in FIFO on %s at address 0x%02x", @@ -617,7 +561,7 @@ LSM6DSV16XStatusTypeDef LSM6DSVSensor::readNextFifoFrame() { return LSM6DSV16X_ERROR; } } - return readFifo(LSM6DSV_FIFO_FRAME_SIZE); + return readFifo(fifoFramSize); } LSM6DSV16XStatusTypeDef LSM6DSVSensor::loadIMUCalibration() { @@ -651,14 +595,6 @@ LSM6DSV16XStatusTypeDef LSM6DSVSensor::loadIMUCalibration() { return LSM6DSV16X_OK; } - - - - - - - - #ifdef LSM6DSV_ACCEL_OFFSET_CAL void LSM6DSVSensor::calibrateAccel() { m_Logger.info( @@ -713,7 +649,7 @@ void LSM6DSVSensor::calibrateAccel() { // Group all the data together //set the watermark level here for nrf sleep - if (fifo_samples < LSM6DSV_FIFO_FRAME_SIZE) { + if (fifo_samples < fifoFramSize) { continue; } @@ -726,7 +662,7 @@ void LSM6DSVSensor::calibrateAccel() { sfusion.updateAcc( rawAcceleration, - (currentDataTime - previousDataTime) * LSM6DSV_TIMESTAMP_LSB + lsm6dsv16x_from_lsb_to_nsec(currentDataTime - previousDataTime) * 1e-9 ); newRawAcceleration = false; @@ -805,21 +741,6 @@ void LSM6DSVSensor::calibrateAccel() { } #endif - - - - - - - - - - - - - - - #ifdef LSM6DSV_GYRO_OFFSET_CAL void LSM6DSVSensor::calibrateGyro() { m_Logger.info( @@ -887,7 +808,7 @@ void LSM6DSVSensor::calibrateGyro() { #endif -#ifdef LSM6DSV_GYRO_SENSITIVITY_CAL +#ifdef LSM6DSV_GYRO_SENSITIVITY_CAL //Redo based on the bmi implementation void LSM6DSVSensor::calibrateGyroSensitivity() { m_Logger.info( "Gyro sensitivity calibration started on sensor #%d of type %s at address 0x%02x", @@ -1019,28 +940,6 @@ void LSM6DSVSensor::calibrateGyroSensitivity() { } #endif - - - - - - - - - - - - - - - - - - - - - - void LSM6DSVSensor::startCalibration(int calibrationType) { m_Logger.info("Flip right side up in the next 5 seconds to start calibration."); delay(5000); @@ -1119,7 +1018,7 @@ void LSM6DSVSensor::apply6DToRestDetection() { sfusion.update6D( rawAcceleration, rawGyro, - (double)(currentDataTime - previousDataTime) * LSM6DSV_TIMESTAMP_LSB + lsm6dsv16x_from_lsb_to_nsec(currentDataTime - previousDataTime) * 1e-9 //seconds ); } newRawAcceleration = false; @@ -1137,4 +1036,4 @@ void LSM6DSVSensor::waitForMovement() { apply6DToRestDetection(); } } -#endif // LSM6DSV_ESP_FUSION +#endif // (LSM6DSV_FUSION_SOURCE == LSM6DSV_FUSION_ESP) diff --git a/src/sensors/lsm6dsvsensor.h b/src/sensors/lsm6dsvsensor.h index 665d21510..1bdf68cbf 100644 --- a/src/sensors/lsm6dsvsensor.h +++ b/src/sensors/lsm6dsvsensor.h @@ -46,9 +46,9 @@ #define LSM6DSV_GYRO_ACCEL_RATE 7680.0f #endif -#ifndef LSM6DSV_FIFO_TEMP_DATA_RATE -#define LSM6DSV_FIFO_TEMP_DATA_RATE 1.875f -#endif +//#ifndef LSM6DSV_FIFO_TEMP_DATA_RATE //We should use this instead +//#define LSM6DSV_FIFO_TEMP_DATA_RATE 1.875f +//#endif #ifndef LSM6DSV_TEMP_READ_INTERVAL #define LSM6DSV_TEMP_READ_INTERVAL 1 @@ -67,37 +67,31 @@ #define LSM6DSV_TAP_QUITE_TIME 3 //0-3 #endif -#define LSM6DSV_TIMESTAMP_LSB 21.75e-6f -// #define SELF_TEST_ON_INIT -// #define REINIT_ON_FAILURE - #define LSM6DSV_INTERRUPT //recommended for tap detect + +#define LSM6DSV_INTERRUPT //recommended for tap detect // #define LSM6DSV_NO_SELF_TEST_ON_FACEDOWN -#define LSM6DSV_ONBOARD_FUSION -#define LSM6DSV_ESP_FUSION -#ifdef LSM6DSV_ESP_FUSION +#define LSM6DSV_FUSION_ESP 0 +#define LSM6DSV_FUSION_ONBOARD 1 + +#define LSM6DSV_FUSION_SOURCE LSM6DSV_FUSION_ESP + + + +#if (LSM6DSV_FUSION_SOURCE == LSM6DSV_FUSION_ESP) #define LSM6DSV_GYRO_OFFSET_CAL #define LSM6DSV_ACCEL_OFFSET_CAL #define LSM6DSV_GYRO_SENSITIVITY_CAL #endif #ifdef LSM6DSV_GYRO_SENSITIVITY_CAL -#define LSM6DSV_GYRO_SENSITIVITY_SPINS 5 +#define LSM6DSV_GYRO_SENSITIVITY_SPINS 2 #endif -#ifdef REINIT_ON_FAILURE -#define REINIT_RETRY_MAX_ATTEMPTS 5 -#undef SELF_TEST_ON_INIT -#endif -#if defined(LSM6DSV_ONBOARD_FUSION) && defined(LSM6DSV_ESP_FUSION) -#define LSM6DSV_FIFO_FRAME_SIZE 5 // X BDR, G BDR, Game, Gravity, Timestamp -#else -#define LSM6DSV_FIFO_FRAME_SIZE 4 // X BDR, (G BDR || Game), Gravity, Timestamp -#endif @@ -118,7 +112,7 @@ class LSM6DSVSensor : public Sensor { void sendData() override final; SensorStatus getSensorState() override final; -#ifdef LSM6DSV_ESP_FUSION +#if (LSM6DSV_FUSION_SOURCE == LSM6DSV_FUSION_ESP) void startCalibration(int calibrationType) override final; void calibrateAccel() override final; void calibrateGyro() override final; @@ -140,20 +134,12 @@ class LSM6DSVSensor : public Sensor { float temperature = 0; bool newTemperature = false; uint32_t lastTempRead = 0; - float gravityVector[3]; - bool newGravityVector = false; float rawAcceleration[3]; bool newRawAcceleration = false; uint32_t previousDataTime = 0; uint32_t currentDataTime = 0; -#ifdef LSM6DSV_ONBOARD_FUSION - float fusedGameRotation[3]; - Quat previousGameRotation; - bool newFusedGameRotation = false; -#endif - -#ifdef LSM6DSV_ESP_FUSION +#if (LSM6DSV_FUSION_SOURCE == LSM6DSV_FUSION_ESP) LSM6DSV16XStatusTypeDef readNextFifoFrame(); LSM6DSV16XStatusTypeDef loadIMUCalibration(); void apply6DToRestDetection(); @@ -164,13 +150,6 @@ class LSM6DSVSensor : public Sensor { SlimeVR::Sensors::SensorFusionRestDetect sfusion; float rawGyro[3]; bool newRawGyro = false; - Quat previousEspRotation; - uint8_t previousTag = 0; -#endif - - -#ifdef REINIT_ON_FAILURE - uint8_t reinitOnFailAttempts = 0; #endif }; From 33b741cbbadc441473aeba88ca40701fa91ff3c5 Mon Sep 17 00:00:00 2001 From: wigwagwent Date: Sun, 29 Oct 2023 09:30:51 -0700 Subject: [PATCH 44/60] Finish Renaming everything to lsm6dsv --- lib/LSM6DSV16X/LSM6DSV.cpp | 3754 +++++++++++++ lib/LSM6DSV16X/LSM6DSV.h | 425 ++ lib/LSM6DSV16X/LSM6DSV16X.cpp | 3900 -------------- lib/LSM6DSV16X/LSM6DSV16X.h | 433 -- .../{lsm6dsv16x_reg.c => lsm6dsv_reg.c} | 4623 ++++++++--------- .../{lsm6dsv16x_reg.h => lsm6dsv_reg.h} | 3405 ++++++------ src/sensors/SensorManager.cpp | 2 +- .../{lsm6dsvsensor.cpp => lsm6dsvSensor.cpp} | 136 +- .../{lsm6dsvsensor.h => lsm6dsvSensor.h} | 12 +- 9 files changed, 7842 insertions(+), 8848 deletions(-) create mode 100644 lib/LSM6DSV16X/LSM6DSV.cpp create mode 100644 lib/LSM6DSV16X/LSM6DSV.h delete mode 100644 lib/LSM6DSV16X/LSM6DSV16X.cpp delete mode 100644 lib/LSM6DSV16X/LSM6DSV16X.h rename lib/LSM6DSV16X/{lsm6dsv16x_reg.c => lsm6dsv_reg.c} (54%) rename lib/LSM6DSV16X/{lsm6dsv16x_reg.h => lsm6dsv_reg.h} (51%) rename src/sensors/{lsm6dsvsensor.cpp => lsm6dsvSensor.cpp} (87%) rename src/sensors/{lsm6dsvsensor.h => lsm6dsvSensor.h} (94%) diff --git a/lib/LSM6DSV16X/LSM6DSV.cpp b/lib/LSM6DSV16X/LSM6DSV.cpp new file mode 100644 index 000000000..578435314 --- /dev/null +++ b/lib/LSM6DSV16X/LSM6DSV.cpp @@ -0,0 +1,3754 @@ +/** + ****************************************************************************** + * @file LSM6DSV.cpp + * @author SRA + * @version V1.5.1 + * @date July 2022 + * @brief Implementation of a LSM6DSV inertial measurement sensor. + ****************************************************************************** + * @attention + * + *

© COPYRIGHT(c) 2022 STMicroelectronics

+ * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * 3. Neither the name of STMicroelectronics nor the names of its contributors + * may be used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************** + */ + + +/* Includes ------------------------------------------------------------------*/ + +#include "LSM6DSV.h" + + +/* Class Implementation ------------------------------------------------------*/ + +/** Constructor + * @param i2c object of an helper class which handles the I2C peripheral + * @param address the address of the component's instance + */ +LSM6DSV::LSM6DSV(TwoWire *i2c, uint8_t address) : dev_i2c(i2c), address(address) +{ + reg_ctx.write_reg = LSM6DSV_io_write; + reg_ctx.read_reg = LSM6DSV_io_read; + reg_ctx.mdelay = LSM6DSV_sleep; + reg_ctx.handle = (void *)this; + dev_spi = NULL; + acc_is_enabled = 0L; + gyro_is_enabled = 0L; +} + +/** Constructor + * @param spi object of an helper class which handles the SPI peripheral + * @param cs_pin the chip select pin + * @param spi_speed the SPI speed + */ +LSM6DSV::LSM6DSV(SPIClass *spi, int cs_pin, uint32_t spi_speed) : dev_spi(spi), cs_pin(cs_pin), spi_speed(spi_speed) +{ + reg_ctx.write_reg = LSM6DSV_io_write; + reg_ctx.read_reg = LSM6DSV_io_read; + reg_ctx.handle = (void *)this; + dev_i2c = NULL; + acc_is_enabled = 0L; + gyro_is_enabled = 0L; +} + +/** + * @brief Initialize the LSM6DSV sensor + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSVStatusTypeDef LSM6DSV::begin() +{ + if (dev_spi) { + // Configure CS pin + pinMode(cs_pin, OUTPUT); + digitalWrite(cs_pin, HIGH); + } + + /* Enable register address automatically incremented during a multiple byte + access with a serial interface. */ + if (lsm6dsv_auto_increment_set(®_ctx, PROPERTY_ENABLE) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + /* Enable BDU */ + if (Enable_Block_Data_Update() != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + /* FIFO mode selection */ + fifo_mode = LSM6DSV_BYPASS_MODE; + if (lsm6dsv_fifo_mode_set(®_ctx, fifo_mode) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + /* Select default output data rate. */ + acc_odr = LSM6DSV_ODR_AT_120Hz; + + /* Output data rate selection - power down. */ + if (lsm6dsv_xl_data_rate_set(®_ctx, LSM6DSV_ODR_OFF) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + /* Full scale selection. */ + acc_fs = LSM6DSV_2g; + if (lsm6dsv_xl_full_scale_set(®_ctx, acc_fs) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + /* Select default output data rate. */ + gyro_odr = LSM6DSV_ODR_AT_120Hz; + + /* Output data rate selection - power down. */ + if (lsm6dsv_gy_data_rate_set(®_ctx, LSM6DSV_ODR_OFF) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + /* Full scale selection. */ + gyro_fs = LSM6DSV_2000dps; + if (lsm6dsv_gy_full_scale_set(®_ctx, gyro_fs) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + initialized = 1U; + + return LSM6DSV_OK; +} + +/** + * @brief Deinitialize the LSM6DSV sensor + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSVStatusTypeDef LSM6DSV::end() +{ + /* Disable the component */ + if (Disable_X() != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + if (Disable_G() != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + /* Reset output data rate. */ + acc_odr = LSM6DSV_ODR_OFF; + gyro_odr = LSM6DSV_ODR_OFF; + + initialized = 0U; + + return LSM6DSV_OK; +} + +/** + * @brief Read component ID + * @param Id the WHO_AM_I value + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSVStatusTypeDef LSM6DSV::ReadID(uint8_t *Id) +{ + if (lsm6dsv_device_id_get(®_ctx, Id) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + return LSM6DSV_OK; +} + +/** + * @brief Enable the LSM6DSV accelerometer sensor + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSVStatusTypeDef LSM6DSV::Enable_X() +{ + /* Check if the component is already enabled */ + if (acc_is_enabled == 1U) { + return LSM6DSV_OK; + } + + /* Output data rate selection. */ + if (lsm6dsv_xl_data_rate_set(®_ctx, acc_odr) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + acc_is_enabled = 1U; + + return LSM6DSV_OK; +} + +/** + * @brief Disable the LSM6DSV accelerometer sensor + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSVStatusTypeDef LSM6DSV::Disable_X() +{ + /* Check if the component is already disabled */ + if (acc_is_enabled == 0U) { + return LSM6DSV_OK; + } + + /* Get current output data rate. */ + if (lsm6dsv_xl_data_rate_get(®_ctx, &acc_odr) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + /* Output data rate selection - power down. */ + if (lsm6dsv_xl_data_rate_set(®_ctx, LSM6DSV_ODR_OFF) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + acc_is_enabled = 0U; + + return LSM6DSV_OK; +} + +/** + * @brief Get the LSM6DSV accelerometer sensor sensitivity + * @param Sensitivity pointer + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSVStatusTypeDef LSM6DSV::Get_X_Sensitivity(float *Sensitivity) +{ + lsm6dsv_xl_full_scale_t full_scale; + + /* Read actual full scale selection from sensor. */ + if (lsm6dsv_xl_full_scale_get(®_ctx, &full_scale) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + *Sensitivity = Convert_X_Sensitivity(full_scale); + if (*Sensitivity == 0.0f) { + return LSM6DSV_ERROR; + } + return LSM6DSV_OK; +} + +float LSM6DSV::Convert_X_Sensitivity(lsm6dsv_xl_full_scale_t full_scale) { + float Sensitivity = 0.0f; + /* Store the Sensitivity based on actual full scale. */ + switch (full_scale) { + case LSM6DSV_2g: + Sensitivity = LSM6DSV_ACC_SENSITIVITY_FS_2G; + break; + + case LSM6DSV_4g: + Sensitivity = LSM6DSV_ACC_SENSITIVITY_FS_4G; + break; + + case LSM6DSV_8g: + Sensitivity = LSM6DSV_ACC_SENSITIVITY_FS_8G; + break; + + case LSM6DSV_16g: + Sensitivity = LSM6DSV_ACC_SENSITIVITY_FS_16G; + break; + } + return Sensitivity; +} + +/** + * @brief Get the LSM6DSV accelerometer sensor output data rate + * @param Odr pointer where the output data rate is written + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSVStatusTypeDef LSM6DSV::Get_X_ODR(float *Odr) +{ + LSM6DSVStatusTypeDef ret = LSM6DSV_OK; + lsm6dsv_data_rate_t odr_low_level; + + /* Get current output data rate. */ + if (lsm6dsv_xl_data_rate_get(®_ctx, &odr_low_level) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + switch (odr_low_level) { + case LSM6DSV_ODR_OFF: + *Odr = 0.0f; + break; + + case LSM6DSV_ODR_AT_1Hz875: + *Odr = 1.875f; + break; + + case LSM6DSV_ODR_AT_7Hz5: + *Odr = 7.5f; + break; + + case LSM6DSV_ODR_AT_15Hz: + *Odr = 15.0f; + break; + + case LSM6DSV_ODR_AT_30Hz: + *Odr = 30.0f; + break; + + case LSM6DSV_ODR_AT_60Hz: + *Odr = 60.0f; + break; + + case LSM6DSV_ODR_AT_120Hz: + *Odr = 120.0f; + break; + + case LSM6DSV_ODR_AT_240Hz: + *Odr = 240.0f; + break; + + case LSM6DSV_ODR_AT_480Hz: + *Odr = 480.0f; + break; + + case LSM6DSV_ODR_AT_960Hz: + *Odr = 960.0f; + break; + + case LSM6DSV_ODR_AT_1920Hz: + *Odr = 1920.0f; + break; + + case LSM6DSV_ODR_AT_3840Hz: + *Odr = 3840.0f; + break; + + case LSM6DSV_ODR_AT_7680Hz: + *Odr = 7680.0f; + break; + + default: + ret = LSM6DSV_ERROR; + break; + } + + return ret; +} + +/** + * @brief Get the LSM6DSV accelerometer sensor axes + * @param Acceleration pointer where the values of the axes are written + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSVStatusTypeDef LSM6DSV::Get_X_Axes(int32_t *Acceleration) +{ + lsm6dsv_axis3bit16_t data_raw; + float sensitivity = Convert_X_Sensitivity(acc_fs); + + /* Read raw data values. */ + if (lsm6dsv_acceleration_raw_get(®_ctx, data_raw.i16bit) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + /* Get LSM6DSV actual sensitivity. */ + if (sensitivity == 0.0f) { + return LSM6DSV_ERROR; + } + + /* Calculate the data. */ + Acceleration[0] = (int32_t)((float)((float)data_raw.i16bit[0] * sensitivity)); + Acceleration[1] = (int32_t)((float)((float)data_raw.i16bit[1] * sensitivity)); + Acceleration[2] = (int32_t)((float)((float)data_raw.i16bit[2] * sensitivity)); + + return LSM6DSV_OK; +} + +/** + * @brief Set the LSM6DSV accelerometer sensor output data rate + * @param Odr the output data rate value to be set + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSVStatusTypeDef LSM6DSV::Set_X_ODR(float Odr, LSM6DSV_ACC_Operating_Mode_t Mode) +{ + switch (Mode) { + case LSM6DSV_ACC_HIGH_PERFORMANCE_MODE: { + if (lsm6dsv_xl_mode_set(®_ctx, LSM6DSV_XL_HIGH_PERFORMANCE_MD) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + /* Valid ODR: 7.5Hz <= Odr <= 7.68kHz */ + Odr = (Odr < 7.5f) ? 7.5f + : (Odr > 7680.0f) ? 7680.0f + : Odr; + break; + } + + case LSM6DSV_ACC_HIGH_ACCURACY_MODE: + // TODO: Not implemented. + // NOTE: According to datasheet, section `6.5 High-accuracy ODR mode`: + // "... the other sensor also has to be configured in high-accuracy ODR (HAODR) mode." + return LSM6DSV_ERROR; + + case LSM6DSV_ACC_NORMAL_MODE: { + if (lsm6dsv_xl_mode_set(®_ctx, LSM6DSV_XL_NORMAL_MD) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + /* Valid ODR: 7.5Hz <= Odr <= 1.92kHz */ + Odr = (Odr < 7.5f) ? 7.5f + : (Odr > 1920.0f) ? 1920.0f + : Odr; + break; + } + + case LSM6DSV_ACC_LOW_POWER_MODE1: { + if (lsm6dsv_xl_mode_set(®_ctx, LSM6DSV_XL_LOW_POWER_2_AVG_MD) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + /* Valid ODR: 1.875Hz; 15Hz <= Odr <= 240kHz */ + Odr = (Odr == 1.875f) ? Odr + : (Odr < 15.000f) ? 15.0f + : (Odr > 240.000f) ? 240.0f + : Odr; + break; + } + + case LSM6DSV_ACC_LOW_POWER_MODE2: { + if (lsm6dsv_xl_mode_set(®_ctx, LSM6DSV_XL_LOW_POWER_4_AVG_MD) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + /* Valid ODR: 1.875Hz; 15Hz <= Odr <= 240kHz */ + Odr = (Odr == 1.875f) ? Odr + : (Odr < 15.000f) ? 15.0f + : (Odr > 240.000f) ? 240.0f + : Odr; + break; + } + + case LSM6DSV_ACC_LOW_POWER_MODE3: { + if (lsm6dsv_xl_mode_set(®_ctx, LSM6DSV_XL_LOW_POWER_8_AVG_MD) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + /* Valid ODR: 1.875Hz; 15Hz <= Odr <= 240kHz */ + Odr = (Odr == 1.875f) ? Odr + : (Odr < 15.000f) ? 15.0f + : (Odr > 240.000f) ? 240.0f + : Odr; + break; + } + + default: + return LSM6DSV_ERROR; + } + + if (acc_is_enabled == 1U) { + return Set_X_ODR_When_Enabled(Odr); + } else { + return Set_X_ODR_When_Disabled(Odr); + } +} + +/** + * @brief Set the LSM6DSV accelerometer sensor output data rate when enabled + * @param Odr the functional output data rate to be set + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSVStatusTypeDef LSM6DSV::Set_X_ODR_When_Enabled(float Odr) +{ + lsm6dsv_data_rate_t new_odr; + + new_odr = (Odr <= 1.875f) ? LSM6DSV_ODR_AT_1Hz875 + : (Odr <= 7.5f) ? LSM6DSV_ODR_AT_7Hz5 + : (Odr <= 15.0f) ? LSM6DSV_ODR_AT_15Hz + : (Odr <= 30.0f) ? LSM6DSV_ODR_AT_30Hz + : (Odr <= 60.0f) ? LSM6DSV_ODR_AT_60Hz + : (Odr <= 120.0f) ? LSM6DSV_ODR_AT_120Hz + : (Odr <= 240.0f) ? LSM6DSV_ODR_AT_240Hz + : (Odr <= 480.0f) ? LSM6DSV_ODR_AT_480Hz + : (Odr <= 960.0f) ? LSM6DSV_ODR_AT_960Hz + : (Odr <= 1920.0f) ? LSM6DSV_ODR_AT_1920Hz + : (Odr <= 3840.0f) ? LSM6DSV_ODR_AT_3840Hz + : LSM6DSV_ODR_AT_7680Hz; + + /* Output data rate selection. */ + if (lsm6dsv_xl_data_rate_set(®_ctx, new_odr) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + return LSM6DSV_OK; +} + +/** + * @brief Set the LSM6DSV accelerometer sensor output data rate when disabled + * @param Odr the functional output data rate to be set + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSVStatusTypeDef LSM6DSV::Set_X_ODR_When_Disabled(float Odr) +{ + acc_odr = (Odr <= 1.875f) ? LSM6DSV_ODR_AT_1Hz875 + : (Odr <= 7.5f) ? LSM6DSV_ODR_AT_7Hz5 + : (Odr <= 15.0f) ? LSM6DSV_ODR_AT_15Hz + : (Odr <= 30.0f) ? LSM6DSV_ODR_AT_30Hz + : (Odr <= 60.0f) ? LSM6DSV_ODR_AT_60Hz + : (Odr <= 120.0f) ? LSM6DSV_ODR_AT_120Hz + : (Odr <= 240.0f) ? LSM6DSV_ODR_AT_240Hz + : (Odr <= 480.0f) ? LSM6DSV_ODR_AT_480Hz + : (Odr <= 960.0f) ? LSM6DSV_ODR_AT_960Hz + : (Odr <= 1920.0f) ? LSM6DSV_ODR_AT_1920Hz + : (Odr <= 3840.0f) ? LSM6DSV_ODR_AT_3840Hz + : LSM6DSV_ODR_AT_7680Hz; + + return LSM6DSV_OK; +} + +/** + * @brief Get the LSM6DSV accelerometer sensor full scale + * @param FullScale pointer where the full scale is written + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSVStatusTypeDef LSM6DSV::Get_X_FS(int32_t *FullScale) +{ + LSM6DSVStatusTypeDef ret = LSM6DSV_OK; + lsm6dsv_xl_full_scale_t fs_low_level; + + /* Read actual full scale selection from sensor. */ + if (lsm6dsv_xl_full_scale_get(®_ctx, &fs_low_level) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + switch (fs_low_level) { + case LSM6DSV_2g: + *FullScale = 2; + break; + + case LSM6DSV_4g: + *FullScale = 4; + break; + + case LSM6DSV_8g: + *FullScale = 8; + break; + + case LSM6DSV_16g: + *FullScale = 16; + break; + + default: + ret = LSM6DSV_ERROR; + break; + } + + return ret; +} + +/** + * @brief Set the LSM6DSV accelerometer sensor full scale + * @param FullScale the functional full scale to be set + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSVStatusTypeDef LSM6DSV::Set_X_FS(int32_t FullScale) +{ + lsm6dsv_xl_full_scale_t new_fs; + + /* Seems like MISRA C-2012 rule 14.3a violation but only from single file statical analysis point of view because + the parameter passed to the function is not known at the moment of analysis */ + new_fs = (FullScale <= 2) ? LSM6DSV_2g + : (FullScale <= 4) ? LSM6DSV_4g + : (FullScale <= 8) ? LSM6DSV_8g + : LSM6DSV_16g; + + if (new_fs == acc_fs) { + return LSM6DSV_OK; + } + acc_fs = new_fs; + + if (lsm6dsv_xl_full_scale_set(®_ctx, acc_fs) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + return LSM6DSV_OK; +} + +/** + * @brief Get the LSM6DSV accelerometer sensor raw axes + * @param Value pointer where the raw values of the axes are written + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSVStatusTypeDef LSM6DSV::Get_X_AxesRaw(int16_t *Value) +{ + lsm6dsv_axis3bit16_t data_raw; + + /* Read raw data values. */ + if (lsm6dsv_acceleration_raw_get(®_ctx, data_raw.i16bit) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + /* Format the data. */ + Value[0] = data_raw.i16bit[0]; + Value[1] = data_raw.i16bit[1]; + Value[2] = data_raw.i16bit[2]; + + return LSM6DSV_OK; +} + +/** + * @brief Get the LSM6DSV accelerometer sensor raw axes when avaiable (Blocking) + * @param Value pointer where the raw values of the axes are written + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSVStatusTypeDef LSM6DSV::Get_X_AxesRaw_When_Aval(int16_t *Value) { + lsm6dsv_data_ready_t drdy; + do { + if (lsm6dsv_flag_data_ready_get(®_ctx, &drdy) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + } while (!drdy.drdy_xl); + + if (Get_X_AxesRaw(Value) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + return LSM6DSV_OK; +} + +/** + * @brief Get the LSM6DSV ACC data ready bit value + * @param Status the status of data ready bit + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSVStatusTypeDef LSM6DSV::Get_X_DRDY_Status(uint8_t *Status) +{ + lsm6dsv_all_sources_t val; + + if (lsm6dsv_all_sources_get(®_ctx, &val) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + *Status = val.drdy_xl; + return LSM6DSV_OK; +} + +/** + * @brief Get the status of all hardware events + * @param Status the status of all hardware events + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSVStatusTypeDef LSM6DSV::Get_X_Event_Status(LSM6DSV_Event_Status_t *Status) +{ + lsm6dsv_emb_func_status_t emb_func_status; + lsm6dsv_wake_up_src_t wake_up_src; + lsm6dsv_tap_src_t tap_src; + lsm6dsv_d6d_src_t d6d_src; + lsm6dsv_emb_func_src_t func_src; + lsm6dsv_md1_cfg_t md1_cfg; + lsm6dsv_md2_cfg_t md2_cfg; + lsm6dsv_emb_func_int1_t int1_ctrl; + lsm6dsv_emb_func_int2_t int2_ctrl; + + + (void)memset((void *)Status, 0x0, sizeof(LSM6DSV_Event_Status_t)); + + if (lsm6dsv_read_reg(®_ctx, LSM6DSV_WAKE_UP_SRC, (uint8_t *)&wake_up_src, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + if (lsm6dsv_read_reg(®_ctx, LSM6DSV_TAP_SRC, (uint8_t *)&tap_src, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + if (lsm6dsv_read_reg(®_ctx, LSM6DSV_D6D_SRC, (uint8_t *)&d6d_src, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + if (lsm6dsv_mem_bank_set(®_ctx, LSM6DSV_EMBED_FUNC_MEM_BANK) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + if (lsm6dsv_read_reg(®_ctx, LSM6DSV_EMB_FUNC_SRC, (uint8_t *)&func_src, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + if (lsm6dsv_read_reg(®_ctx, LSM6DSV_EMB_FUNC_INT1, (uint8_t *)&int1_ctrl, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + if (lsm6dsv_read_reg(®_ctx, LSM6DSV_EMB_FUNC_INT2, (uint8_t *)&int2_ctrl, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + if (lsm6dsv_read_reg(®_ctx, LSM6DSV_EMB_FUNC_STATUS, (uint8_t *)&emb_func_status, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + if (lsm6dsv_mem_bank_set(®_ctx, LSM6DSV_MAIN_MEM_BANK) != 0) { + return LSM6DSV_ERROR; + } + + if (lsm6dsv_read_reg(®_ctx, LSM6DSV_MD1_CFG, (uint8_t *)&md1_cfg, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + if (lsm6dsv_read_reg(®_ctx, LSM6DSV_MD2_CFG, (uint8_t *)&md2_cfg, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + + if ((md1_cfg.int1_ff == 1U) || (md2_cfg.int2_ff == 1U)) { + if (wake_up_src.ff_ia == 1U) { + Status->FreeFallStatus = 1; + } + } + + if ((md1_cfg.int1_wu == 1U) || (md2_cfg.int2_wu == 1U)) { + if (wake_up_src.wu_ia == 1U) { + Status->WakeUpStatus = 1; + } + } + + if ((md1_cfg.int1_single_tap == 1U) || (md2_cfg.int2_single_tap == 1U)) { + if (tap_src.single_tap == 1U) { + Status->TapStatus = 1; + } + } + + if ((md1_cfg.int1_double_tap == 1U) || (md2_cfg.int2_double_tap == 1U)) { + if (tap_src.double_tap == 1U) { + Status->DoubleTapStatus = 1; + } + } + + if ((md1_cfg.int1_6d == 1U) || (md2_cfg.int2_6d == 1U)) { + if (d6d_src.d6d_ia == 1U) { + Status->D6DOrientationStatus = 1; + } + } + + if (int1_ctrl.int1_step_detector == 1U || int2_ctrl.int2_step_detector == 1U) { + if (func_src.step_detected == 1U) { + Status->StepStatus = 1; + } + } + + if ((int1_ctrl.int1_tilt == 1U) || (int2_ctrl.int2_tilt == 1U)) { + if (emb_func_status.is_tilt == 1U) { + Status->TiltStatus = 1; + } + } + + return LSM6DSV_OK; +} + +/** + * @brief Set the LSM6DSV accelerometer power mode + * @param PowerMode Value of the powerMode + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSVStatusTypeDef LSM6DSV::Set_X_Power_Mode(uint8_t PowerMode) +{ + if (lsm6dsv_xl_mode_set(®_ctx, (lsm6dsv_xl_mode_t)PowerMode) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + return LSM6DSV_OK; +} + +/** + * @brief Set the LSM6DSV accelerometer filter mode + * @param LowHighPassFlag 0/1 for setting low-pass/high-pass filter mode + * @param FilterMode Value of the filter Mode + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSVStatusTypeDef LSM6DSV::Set_X_Filter_Mode(uint8_t LowHighPassFlag, uint8_t FilterMode) +{ + if (LowHighPassFlag == 0) { + /*Set accelerometer low_pass filter-mode*/ + + /*Set to 1 LPF2 bit (CTRL8_XL)*/ + if (lsm6dsv_filt_xl_lp2_set(®_ctx, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + if (lsm6dsv_filt_xl_lp2_bandwidth_set(®_ctx, (lsm6dsv_filt_xl_lp2_bandwidth_t)FilterMode) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + } else { + if (lsm6dsv_filt_xl_lp2_set(®_ctx, 0) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + /*Set accelerometer high_pass filter-mode*/ + if (lsm6dsv_filt_xl_lp2_bandwidth_set(®_ctx, (lsm6dsv_filt_xl_lp2_bandwidth_t)FilterMode) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + } + return LSM6DSV_OK; +} + +/** + * @brief Runs the ST specified accelerometer and gyroscope self test + * @param XTestType LSM6DSV_XL_ST_DISABLE = 0x0, LSM6DSV_XL_ST_POSITIVE = 0x1, LSM6DSV_XL_ST_NEGATIVE = 0x2 + * @param GTestType LSM6DSV_GY_ST_DISABLE = 0x0, LSM6DSV_GY_ST_POSITIVE = 0x1, LSM6DSV_GY_ST_NEGATIVE = 0x2 + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSVStatusTypeDef LSM6DSV::Test_IMU(uint8_t XTestType, uint8_t GTestType) +{ + uint8_t whoamI; + + if (ReadID(&whoamI) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + if (whoamI != LSM6DSV_ID) { + return LSM6DSV_ERROR; + } + + if (Test_X_IMU(XTestType) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + if (Test_G_IMU(GTestType) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + printf("IMU Test OK\n"); + return LSM6DSV_OK; +} + +/** + * @brief Runs the ST specified accelerometer self test + * @param TestType LSM6DSV_XL_ST_DISABLE = 0x0, LSM6DSV_XL_ST_POSITIVE = 0x1, LSM6DSV_XL_ST_NEGATIVE = 0x2 + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSVStatusTypeDef LSM6DSV::Test_X_IMU(uint8_t TestType) +{ + int16_t data_raw[3]; + float val_st_off[3]; + float val_st_on[3]; + float test_val[3]; + + if (Reset_Set(LSM6DSV_RESTORE_CTRL_REGS) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + if (lsm6dsv_block_data_update_set(®_ctx, PROPERTY_ENABLE) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + /* + * Accelerometer Self Test + */ + if (lsm6dsv_xl_data_rate_set(®_ctx, LSM6DSV_ODR_AT_60Hz) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + if (lsm6dsv_xl_full_scale_set(®_ctx, LSM6DSV_4g) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + delay(100); //Wait for Accelerometer to stabalize; + memset(val_st_off, 0x00, 3 * sizeof(float)); + memset(val_st_on, 0x00, 3 * sizeof(float)); + + /*Ignore First Data*/ + if (Get_X_AxesRaw_When_Aval(data_raw) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + for (uint8_t i = 0; i < 5; i++) { + if (Get_X_AxesRaw_When_Aval(data_raw) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + /*Average the data in each axis*/ + for (uint8_t j = 0; j < 3; j++) { + val_st_off[j] += lsm6dsv_from_fs4_to_mg(data_raw[j]); + } + } + + /* Calculate the mg average values */ + for (uint8_t i = 0; i < 3; i++) { + val_st_off[i] /= 5.0f; + } + + if (lsm6dsv_xl_self_test_set(®_ctx, (lsm6dsv_xl_self_test_t)TestType) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + delay(100); + + /*Ignore First Data*/ + if (Get_X_AxesRaw_When_Aval(data_raw) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + for (uint8_t i = 0; i < 5; i++) { + if (Get_X_AxesRaw_When_Aval(data_raw) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + /*Average the data in each axis*/ + for (uint8_t j = 0; j < 3; j++) { + val_st_on[j] += lsm6dsv_from_fs4_to_mg(data_raw[j]); + } + } + + /* Calculate the mg average values */ + for (uint8_t i = 0; i < 3; i++) { + val_st_on[i] /= 5.0f; + } + + /* Calculate the mg values for self test */ + for (uint8_t i = 0; i < 3; i++) { + test_val[i] = fabs((val_st_on[i] - val_st_off[i])); + } + + for (uint8_t i = 0; i < 3; i++) { + if (( LSM6DSV_MIN_ST_LIMIT_mg > test_val[i] ) || ( test_val[i] > LSM6DSV_MAX_ST_LIMIT_mg)) + return LSM6DSV_ERROR; + } + + if (lsm6dsv_xl_self_test_set(®_ctx, LSM6DSV_XL_ST_DISABLE) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + if (lsm6dsv_xl_data_rate_set(®_ctx, LSM6DSV_ODR_OFF) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + return LSM6DSV_OK; +} + +/** + * @brief Runs the ST specified self test on the acceleration axis of the IMU + * @param TestType LSM6DSV_GY_ST_DISABLE = 0x0, LSM6DSV_GY_ST_POSITIVE = 0x1, LSM6DSV_GY_ST_NEGATIVE = 0x2 + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSVStatusTypeDef LSM6DSV::Test_G_IMU(uint8_t TestType = LSM6DSV_GY_ST_POSITIVE) +{ + int16_t data_raw[3]; + float test_val[3]; + + if (Reset_Set(LSM6DSV_RESTORE_CTRL_REGS) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + if (lsm6dsv_block_data_update_set(®_ctx, PROPERTY_ENABLE) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + /* + * Gyroscope Self Test + */ + + if (lsm6dsv_gy_data_rate_set(®_ctx, LSM6DSV_ODR_AT_240Hz) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + if (lsm6dsv_gy_full_scale_set(®_ctx, LSM6DSV_2000dps) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + delay(100); + + /*Ignore First Data*/ + if (Get_G_AxesRaw_When_Aval(data_raw) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + float val_st_off[3] = {0}; + float val_st_on[3] = {0}; + + for (uint8_t i = 0; i < 5; i++) { + if (Get_G_AxesRaw_When_Aval(data_raw) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + /*Average the data in each axis*/ + for (uint8_t j = 0; j < 3; j++) { + val_st_off[j] += lsm6dsv_from_fs2000_to_mdps(data_raw[j]); + } + } + + /* Calculate the mg average values */ + for (uint8_t i = 0; i < 3; i++) { + val_st_off[i] /= 5.0f; + } + + if (lsm6dsv_gy_self_test_set(®_ctx, (lsm6dsv_gy_self_test_t)TestType) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + delay(100); + + /*Ignore First Data*/ + if (Get_G_AxesRaw_When_Aval(data_raw) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + for (uint8_t i = 0; i < 5; i++) { + if (Get_G_AxesRaw_When_Aval(data_raw) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + /*Average the data in each axis*/ + for (uint8_t j = 0; j < 3; j++) { + val_st_on[j] += lsm6dsv_from_fs2000_to_mdps(data_raw[j]); + } + } + + /* Calculate the mg average values */ + for (uint8_t i = 0; i < 3; i++) { + val_st_on[i] /= 5.0f; + } + + /* Calculate the mg values for self test */ + for (uint8_t i = 0; i < 3; i++) { + test_val[i] = fabs((val_st_on[i] - val_st_off[i])); + } + + /* Check self test limit */ + for (uint8_t i = 0; i < 3; i++) { + if (( LSM6DSV_MIN_ST_LIMIT_mdps > test_val[i] ) || + ( test_val[i] > LSM6DSV_MAX_ST_LIMIT_mdps)) { + return LSM6DSV_ERROR; + } + } + + + if (lsm6dsv_gy_self_test_set(®_ctx, LSM6DSV_GY_ST_DISABLE) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + if (lsm6dsv_xl_data_rate_set(®_ctx, LSM6DSV_ODR_OFF) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + return LSM6DSV_OK; +} + + +LSM6DSVStatusTypeDef LSM6DSV::Get_T_ODR(float *Odr) { + lsm6dsv_fifo_ctrl4_t ctrl4; + + if (lsm6dsv_read_reg(®_ctx, LSM6DSV_FIFO_CTRL4, (uint8_t *)&ctrl4, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + switch (ctrl4.odr_t_batch) + { + case LSM6DSV_TEMP_NOT_BATCHED: + *Odr = 0; + break; + case LSM6DSV_TEMP_BATCHED_AT_1Hz875: + *Odr = 1.875f; + break; + case LSM6DSV_TEMP_BATCHED_AT_15Hz: + *Odr = 15; + break; + case LSM6DSV_TEMP_BATCHED_AT_60Hz: + *Odr = 60; + break; + default: + break; + } + + return LSM6DSV_OK; +} + + + +LSM6DSVStatusTypeDef LSM6DSV::Set_T_ODR(float Odr) { + lsm6dsv_fifo_ctrl4_t ctrl4; + + if (lsm6dsv_read_reg(®_ctx, LSM6DSV_FIFO_CTRL4, (uint8_t *)&ctrl4, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + if (Odr == 0.0F) { + ctrl4.odr_t_batch = LSM6DSV_TEMP_NOT_BATCHED; + } else if (Odr <= 1.875F) { + ctrl4.odr_t_batch = LSM6DSV_TEMP_BATCHED_AT_1Hz875; + } else if (Odr <= 15.0F) { + ctrl4.odr_t_batch = LSM6DSV_TEMP_BATCHED_AT_15Hz; + } else { + ctrl4.odr_t_batch = LSM6DSV_TEMP_BATCHED_AT_60Hz; + } + + return (LSM6DSVStatusTypeDef)lsm6dsv_write_reg( + ®_ctx, + LSM6DSV_FIFO_CTRL4, + (uint8_t *)&ctrl4, + 1 + ); +} + +/** + * @brief Enable 6D orientation detection + * @param IntPin interrupt pin line to be used + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSVStatusTypeDef LSM6DSV::Enable_6D_Orientation(LSM6DSV_SensorIntPin_t IntPin) +{ + LSM6DSVStatusTypeDef ret = LSM6DSV_OK; + lsm6dsv_md1_cfg_t val1; + lsm6dsv_md2_cfg_t val2; + lsm6dsv_functions_enable_t functions_enable; + + /* Output Data Rate selection */ + if (Set_X_ODR(480.0f) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + /* Full scale selection */ + if (Set_X_FS(2) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + /* 6D orientation enabled. */ + if (Set_6D_Orientation_Threshold(2) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + /* Enable 6D orientation event on either INT1 or INT2 pin */ + switch (IntPin) { + case LSM6DSV_INT1_PIN: + if (lsm6dsv_read_reg(®_ctx, LSM6DSV_MD1_CFG, (uint8_t *)&val1, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + val1.int1_6d = PROPERTY_ENABLE; + + if (lsm6dsv_write_reg(®_ctx, LSM6DSV_MD1_CFG, (uint8_t *)&val1, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + if (lsm6dsv_read_reg(®_ctx, LSM6DSV_FUNCTIONS_ENABLE, (uint8_t *)&functions_enable, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + functions_enable.interrupts_enable = PROPERTY_ENABLE; + + if (lsm6dsv_write_reg(®_ctx, LSM6DSV_FUNCTIONS_ENABLE, (uint8_t *)&functions_enable, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + break; + + case LSM6DSV_INT2_PIN: + if (lsm6dsv_read_reg(®_ctx, LSM6DSV_MD2_CFG, (uint8_t *)&val2, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + val2.int2_6d = PROPERTY_ENABLE; + + if (lsm6dsv_write_reg(®_ctx, LSM6DSV_MD2_CFG, (uint8_t *)&val2, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + if (lsm6dsv_read_reg(®_ctx, LSM6DSV_FUNCTIONS_ENABLE, (uint8_t *)&functions_enable, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + functions_enable.interrupts_enable = PROPERTY_ENABLE; + + if (lsm6dsv_write_reg(®_ctx, LSM6DSV_FUNCTIONS_ENABLE, (uint8_t *)&functions_enable, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + break; + + default: + ret = LSM6DSV_ERROR; + break; + } + + return ret; +} + +/** + * @brief Disable 6D orientation detection + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSVStatusTypeDef LSM6DSV::Disable_6D_Orientation() +{ + lsm6dsv_md1_cfg_t val1; + lsm6dsv_md2_cfg_t val2; + + /* Reset threshold */ + if (Set_6D_Orientation_Threshold(0) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + /* Disable 6D orientation event on both INT1 and INT2 pins */ + if (lsm6dsv_read_reg(®_ctx, LSM6DSV_MD1_CFG, (uint8_t *)&val1, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + val1.int1_6d = PROPERTY_DISABLE; + + if (lsm6dsv_write_reg(®_ctx, LSM6DSV_MD1_CFG, (uint8_t *)&val1, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + if (lsm6dsv_read_reg(®_ctx, LSM6DSV_MD2_CFG, (uint8_t *)&val2, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + val2.int2_6d = PROPERTY_DISABLE; + + if (lsm6dsv_write_reg(®_ctx, LSM6DSV_MD2_CFG, (uint8_t *)&val2, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + return LSM6DSV_OK; +} + +/** + * @brief Set 6D orientation threshold + * @param Threshold 6D Orientation detection threshold + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSVStatusTypeDef LSM6DSV::Set_6D_Orientation_Threshold(uint8_t Threshold) +{ + LSM6DSVStatusTypeDef ret = LSM6DSV_OK; + lsm6dsv_6d_threshold_t newThreshold = LSM6DSV_DEG_80; + + switch (Threshold) { + case 0: + newThreshold = LSM6DSV_DEG_80; + break; + case 1: + newThreshold = LSM6DSV_DEG_70; + break; + case 2: + newThreshold = LSM6DSV_DEG_60; + break; + case 3: + newThreshold = LSM6DSV_DEG_50; + break; + default: + ret = LSM6DSV_ERROR; + break; + } + + if (ret == LSM6DSV_ERROR) { + return LSM6DSV_ERROR; + } + + if (lsm6dsv_6d_threshold_set(®_ctx, newThreshold) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + return LSM6DSV_OK; + +} + +/** + * @brief Get the status of XLow orientation + * @param XLow the status of XLow orientation + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSVStatusTypeDef LSM6DSV::Get_6D_Orientation_XL(uint8_t *XLow) +{ + lsm6dsv_d6d_src_t data; + + if (lsm6dsv_read_reg(®_ctx, LSM6DSV_D6D_SRC, (uint8_t *)&data, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + *XLow = data.xl; + + return LSM6DSV_OK; +} + +/** + * @brief Get the status of XHigh orientation + * @param XHigh the status of XHigh orientation + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSVStatusTypeDef LSM6DSV::Get_6D_Orientation_XH(uint8_t *XHigh) +{ + lsm6dsv_d6d_src_t data; + + if (lsm6dsv_read_reg(®_ctx, LSM6DSV_D6D_SRC, (uint8_t *)&data, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + *XHigh = data.xh; + + return LSM6DSV_OK; +} + +/** + * @brief Get the status of YLow orientation + * @param YLow the status of YLow orientation + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSVStatusTypeDef LSM6DSV::Get_6D_Orientation_YL(uint8_t *YLow) +{ + lsm6dsv_d6d_src_t data; + + if (lsm6dsv_read_reg(®_ctx, LSM6DSV_D6D_SRC, (uint8_t *)&data, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + *YLow = data.yl; + + return LSM6DSV_OK; +} + +/** + * @brief Get the status of YHigh orientation + * @param YHigh the status of YHigh orientation + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSVStatusTypeDef LSM6DSV::Get_6D_Orientation_YH(uint8_t *YHigh) +{ + lsm6dsv_d6d_src_t data; + + if (lsm6dsv_read_reg(®_ctx, LSM6DSV_D6D_SRC, (uint8_t *)&data, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + *YHigh = data.yh; + + return LSM6DSV_OK; +} + +/** + * @brief Get the status of ZLow orientation + * @param ZLow the status of ZLow orientation + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSVStatusTypeDef LSM6DSV::Get_6D_Orientation_ZL(uint8_t *ZLow) +{ + lsm6dsv_d6d_src_t data; + + if (lsm6dsv_read_reg(®_ctx, LSM6DSV_D6D_SRC, (uint8_t *)&data, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + *ZLow = data.zl; + + return LSM6DSV_OK; +} + +/** + * @brief Get the status of ZHigh orientation + * @param ZHigh the status of ZHigh orientation + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSVStatusTypeDef LSM6DSV::Get_6D_Orientation_ZH(uint8_t *ZHigh) +{ + lsm6dsv_d6d_src_t data; + + if (lsm6dsv_read_reg(®_ctx, LSM6DSV_D6D_SRC, (uint8_t *)&data, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + *ZHigh = data.zh; + + return LSM6DSV_OK; +} + + +/** + * @brief Enable free fall detection + * @param IntPin interrupt pin line to be used + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSVStatusTypeDef LSM6DSV::Enable_Free_Fall_Detection(LSM6DSV_SensorIntPin_t IntPin) +{ + LSM6DSVStatusTypeDef ret = LSM6DSV_OK; + lsm6dsv_md1_cfg_t val1; + lsm6dsv_md2_cfg_t val2; + lsm6dsv_functions_enable_t functions_enable; + + /* Output Data Rate selection */ + if (Set_X_ODR(480) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + /* Full scale selection */ + if (Set_X_FS(2) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + /* Set free fall duration.*/ + if (Set_Free_Fall_Duration(3) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + /* Set free fall threshold. */ + if (Set_Free_Fall_Threshold(3) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + /* Enable free fall event on either INT1 or INT2 pin */ + switch (IntPin) { + case LSM6DSV_INT1_PIN: + if (lsm6dsv_read_reg(®_ctx, LSM6DSV_MD1_CFG, (uint8_t *)&val1, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + val1.int1_ff = PROPERTY_ENABLE; + + if (lsm6dsv_write_reg(®_ctx, LSM6DSV_MD1_CFG, (uint8_t *)&val1, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + if (lsm6dsv_read_reg(®_ctx, LSM6DSV_FUNCTIONS_ENABLE, (uint8_t *)&functions_enable, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + functions_enable.interrupts_enable = PROPERTY_ENABLE; + + if (lsm6dsv_write_reg(®_ctx, LSM6DSV_FUNCTIONS_ENABLE, (uint8_t *)&functions_enable, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + break; + + case LSM6DSV_INT2_PIN: + if (lsm6dsv_read_reg(®_ctx, LSM6DSV_MD2_CFG, (uint8_t *)&val2, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + val2.int2_ff = PROPERTY_ENABLE; + + if (lsm6dsv_write_reg(®_ctx, LSM6DSV_MD2_CFG, (uint8_t *)&val2, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + if (lsm6dsv_read_reg(®_ctx, LSM6DSV_FUNCTIONS_ENABLE, (uint8_t *)&functions_enable, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + functions_enable.interrupts_enable = PROPERTY_ENABLE; + + if (lsm6dsv_write_reg(®_ctx, LSM6DSV_FUNCTIONS_ENABLE, (uint8_t *)&functions_enable, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + break; + + default: + ret = LSM6DSV_ERROR; + break; + } + + return ret; +} + +/** + * @brief Disable free fall detection + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSVStatusTypeDef LSM6DSV::Disable_Free_Fall_Detection() +{ + lsm6dsv_md1_cfg_t val1; + lsm6dsv_md2_cfg_t val2; + + /* Disable free fall event on both INT1 and INT2 pins */ + if (lsm6dsv_read_reg(®_ctx, LSM6DSV_MD1_CFG, (uint8_t *)&val1, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + val1.int1_ff = PROPERTY_DISABLE; + + if (lsm6dsv_write_reg(®_ctx, LSM6DSV_MD1_CFG, (uint8_t *)&val1, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + if (lsm6dsv_read_reg(®_ctx, LSM6DSV_MD2_CFG, (uint8_t *)&val2, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + val2.int2_ff = PROPERTY_DISABLE; + + if (lsm6dsv_write_reg(®_ctx, LSM6DSV_MD2_CFG, (uint8_t *)&val2, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + /* Reset free fall threshold. */ + if (Set_Free_Fall_Threshold(0) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + /* Reset free fall duration */ + if (Set_Free_Fall_Duration(0) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + return LSM6DSV_OK; +} + +/** + * @brief Set free fall threshold + * @param Threshold free fall detection threshold + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSVStatusTypeDef LSM6DSV::Set_Free_Fall_Threshold(uint8_t Threshold) +{ + lsm6dsv_ff_thresholds_t val; + switch (Threshold) { + case LSM6DSV_156_mg: + val = LSM6DSV_156_mg; + break; + + case LSM6DSV_219_mg: + val = LSM6DSV_219_mg; + break; + + case LSM6DSV_250_mg: + val = LSM6DSV_250_mg; + break; + + case LSM6DSV_312_mg: + val = LSM6DSV_312_mg; + break; + + case LSM6DSV_344_mg: + val = LSM6DSV_344_mg; + break; + + case LSM6DSV_406_mg: + val = LSM6DSV_406_mg; + break; + + case LSM6DSV_469_mg: + val = LSM6DSV_469_mg; + break; + + case LSM6DSV_500_mg: + val = LSM6DSV_500_mg; + break; + + default: + val = LSM6DSV_156_mg; + break; + } + + /* Set free fall threshold. */ + if (lsm6dsv_ff_thresholds_set(®_ctx, val) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + return LSM6DSV_OK; +} + + +/** + * @brief Set free fall duration + * @param Duration free fall detection duration + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSVStatusTypeDef LSM6DSV::Set_Free_Fall_Duration(uint8_t Duration) +{ + if (lsm6dsv_ff_time_windows_set(®_ctx, Duration) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + return LSM6DSV_OK; +} + +/** + * @brief Enable wake up detection + * @param IntPin interrupt pin line to be used + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSVStatusTypeDef LSM6DSV::Enable_Wake_Up_Detection(LSM6DSV_SensorIntPin_t IntPin) +{ + LSM6DSVStatusTypeDef ret = LSM6DSV_OK; + lsm6dsv_md1_cfg_t val1; + lsm6dsv_md2_cfg_t val2; + lsm6dsv_functions_enable_t functions_enable; + + /* Output Data Rate selection */ + if (Set_X_ODR(480) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + /* Full scale selection */ + if (Set_X_FS(2) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + /* Set wake-up threshold */ + if (Set_Wake_Up_Threshold(63) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + /* Set wake-up durantion */ + if (Set_Wake_Up_Duration(0) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + /* Enable wake up event on either INT1 or INT2 pin */ + switch (IntPin) { + case LSM6DSV_INT1_PIN: + if (lsm6dsv_read_reg(®_ctx, LSM6DSV_MD1_CFG, (uint8_t *)&val1, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + val1.int1_wu = PROPERTY_ENABLE; + + if (lsm6dsv_write_reg(®_ctx, LSM6DSV_MD1_CFG, (uint8_t *)&val1, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + if (lsm6dsv_read_reg(®_ctx, LSM6DSV_FUNCTIONS_ENABLE, (uint8_t *)&functions_enable, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + functions_enable.interrupts_enable = PROPERTY_ENABLE; + + if (lsm6dsv_write_reg(®_ctx, LSM6DSV_FUNCTIONS_ENABLE, (uint8_t *)&functions_enable, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + break; + + case LSM6DSV_INT2_PIN: + if (lsm6dsv_read_reg(®_ctx, LSM6DSV_MD2_CFG, (uint8_t *)&val2, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + val2.int2_wu = PROPERTY_ENABLE; + + if (lsm6dsv_write_reg(®_ctx, LSM6DSV_MD2_CFG, (uint8_t *)&val2, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + if (lsm6dsv_read_reg(®_ctx, LSM6DSV_FUNCTIONS_ENABLE, (uint8_t *)&functions_enable, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + functions_enable.interrupts_enable = PROPERTY_ENABLE; + + if (lsm6dsv_write_reg(®_ctx, LSM6DSV_FUNCTIONS_ENABLE, (uint8_t *)&functions_enable, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + break; + + default: + ret = LSM6DSV_ERROR; + break; + } + + return ret; +} + + +/** + * @brief Disable wake up detection + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSVStatusTypeDef LSM6DSV::Disable_Wake_Up_Detection() +{ + lsm6dsv_md1_cfg_t val1; + lsm6dsv_md2_cfg_t val2; + + /* Disable wake up event on both INT1 and INT2 pins */ + if (lsm6dsv_read_reg(®_ctx, LSM6DSV_MD1_CFG, (uint8_t *)&val1, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + val1.int1_wu = PROPERTY_DISABLE; + + if (lsm6dsv_write_reg(®_ctx, LSM6DSV_MD1_CFG, (uint8_t *)&val1, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + if (lsm6dsv_read_reg(®_ctx, LSM6DSV_MD2_CFG, (uint8_t *)&val2, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + val2.int2_wu = PROPERTY_DISABLE; + + if (lsm6dsv_write_reg(®_ctx, LSM6DSV_MD2_CFG, (uint8_t *)&val2, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + /* Reset wake-up threshold */ + if (Set_Wake_Up_Threshold(0) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + /* Reset wake-up durantion */ + if (Set_Wake_Up_Duration(0) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + return LSM6DSV_OK; +} + +/** + * @brief Set wake up threshold + * @param Threshold wake up detection threshold + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSVStatusTypeDef LSM6DSV::Set_Wake_Up_Threshold(uint32_t Threshold) +{ + lsm6dsv_act_thresholds_t wake_up_ths; + + if (lsm6dsv_act_thresholds_get(®_ctx, &wake_up_ths) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + wake_up_ths.threshold = Threshold; + + if (lsm6dsv_act_thresholds_set(®_ctx, &wake_up_ths) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + return LSM6DSV_OK; +} + +/** + * @brief Set wake up duration + * @param Duration wake up detection duration + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSVStatusTypeDef LSM6DSV::Set_Wake_Up_Duration(uint8_t Duration) +{ + lsm6dsv_act_wkup_time_windows_t dur_t; + + if (lsm6dsv_act_wkup_time_windows_get(®_ctx, &dur_t) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + dur_t.shock = Duration; + + if (lsm6dsv_act_wkup_time_windows_set(®_ctx, dur_t) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + return LSM6DSV_OK; +} + +/** + * @brief Enable single tap detection + * @param IntPin interrupt pin line to be used + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSVStatusTypeDef LSM6DSV::Enable_Single_Tap_Detection(LSM6DSV_SensorIntPin_t IntPin) +{ + LSM6DSVStatusTypeDef ret = LSM6DSV_OK; + lsm6dsv_md1_cfg_t val1; + lsm6dsv_md2_cfg_t val2; + lsm6dsv_functions_enable_t functions_enable; + + lsm6dsv_tap_dur_t tap_dur; + lsm6dsv_tap_cfg0_t tap_cfg0; + lsm6dsv_tap_ths_6d_t tap_ths_6d; + + /* Output Data Rate selection */ + if (Set_X_ODR(480) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + /* Full scale selection */ + if (Set_X_FS(8) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + /* Enable tap detection on Z-axis. */ + if (lsm6dsv_read_reg(®_ctx, LSM6DSV_TAP_CFG0, (uint8_t *)&tap_cfg0, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + tap_cfg0.tap_z_en = 0x01U; + + if (lsm6dsv_write_reg(®_ctx, LSM6DSV_TAP_CFG0, (uint8_t *)&tap_cfg0, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + /* Set Z-axis threshold. */ + if (lsm6dsv_read_reg(®_ctx, LSM6DSV_TAP_THS_6D, (uint8_t *)&tap_ths_6d, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + tap_ths_6d.tap_ths_z = 0x2U; + + if (lsm6dsv_write_reg(®_ctx, LSM6DSV_TAP_THS_6D, (uint8_t *)&tap_ths_6d, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + /* Set quiet and shock time windows. */ + if (lsm6dsv_read_reg(®_ctx, LSM6DSV_TAP_DUR, (uint8_t *)&tap_dur, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + tap_dur.quiet = (uint8_t)0x02U; + tap_dur.shock = (uint8_t)0x01U; + + if (lsm6dsv_write_reg(®_ctx, LSM6DSV_TAP_DUR, (uint8_t *)&tap_dur, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + /* Set tap mode. */ + if (lsm6dsv_tap_mode_set(®_ctx, LSM6DSV_ONLY_SINGLE) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + /* Enable single tap event on either INT1 or INT2 pin */ + switch (IntPin) { + case LSM6DSV_INT1_PIN: + if (lsm6dsv_read_reg(®_ctx, LSM6DSV_MD1_CFG, (uint8_t *)&val1, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + val1.int1_single_tap = PROPERTY_ENABLE; + + if (lsm6dsv_write_reg(®_ctx, LSM6DSV_MD1_CFG, (uint8_t *)&val1, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + if (lsm6dsv_read_reg(®_ctx, LSM6DSV_FUNCTIONS_ENABLE, (uint8_t *)&functions_enable, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + functions_enable.interrupts_enable = PROPERTY_ENABLE; + + if (lsm6dsv_write_reg(®_ctx, LSM6DSV_FUNCTIONS_ENABLE, (uint8_t *)&functions_enable, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + break; + + case LSM6DSV_INT2_PIN: + if (lsm6dsv_read_reg(®_ctx, LSM6DSV_MD2_CFG, (uint8_t *)&val2, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + val2.int2_single_tap = PROPERTY_ENABLE; + + if (lsm6dsv_write_reg(®_ctx, LSM6DSV_MD2_CFG, (uint8_t *)&val2, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + if (lsm6dsv_read_reg(®_ctx, LSM6DSV_FUNCTIONS_ENABLE, (uint8_t *)&functions_enable, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + functions_enable.interrupts_enable = PROPERTY_ENABLE; + + if (lsm6dsv_write_reg(®_ctx, LSM6DSV_FUNCTIONS_ENABLE, (uint8_t *)&functions_enable, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + break; + + default: + ret = LSM6DSV_ERROR; + break; + } + + return ret; +} + +/** + * @brief Disable single tap detection + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSVStatusTypeDef LSM6DSV::Disable_Single_Tap_Detection() +{ + lsm6dsv_md1_cfg_t val1; + lsm6dsv_md2_cfg_t val2; + lsm6dsv_tap_dur_t tap_dur; + lsm6dsv_tap_cfg0_t tap_cfg0; + lsm6dsv_tap_ths_6d_t tap_ths_6d; + + + /* Disable single tap event on both INT1 and INT2 pins */ + if (lsm6dsv_read_reg(®_ctx, LSM6DSV_MD1_CFG, (uint8_t *)&val1, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + val1.int1_single_tap = PROPERTY_DISABLE; + + if (lsm6dsv_write_reg(®_ctx, LSM6DSV_MD1_CFG, (uint8_t *)&val1, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + if (lsm6dsv_read_reg(®_ctx, LSM6DSV_MD2_CFG, (uint8_t *)&val2, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + val2.int2_single_tap = PROPERTY_DISABLE; + + if (lsm6dsv_write_reg(®_ctx, LSM6DSV_MD2_CFG, (uint8_t *)&val2, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + /* Disable tap detection on Z-axis. */ + if (lsm6dsv_read_reg(®_ctx, LSM6DSV_TAP_CFG0, (uint8_t *)&tap_cfg0, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + tap_cfg0.tap_z_en = 0x0U; + + if (lsm6dsv_write_reg(®_ctx, LSM6DSV_TAP_CFG0, (uint8_t *)&tap_cfg0, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + /* Reset Z-axis threshold. */ + if (lsm6dsv_read_reg(®_ctx, LSM6DSV_TAP_THS_6D, (uint8_t *)&tap_ths_6d, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + tap_ths_6d.tap_ths_z = 0x0U; + + if (lsm6dsv_write_reg(®_ctx, LSM6DSV_TAP_THS_6D, (uint8_t *)&tap_ths_6d, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + /* Reset quiet and shock time windows. */ + if (lsm6dsv_read_reg(®_ctx, LSM6DSV_TAP_DUR, (uint8_t *)&tap_dur, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + tap_dur.quiet = (uint8_t)0x0U; + tap_dur.shock = (uint8_t)0x0U; + + if (lsm6dsv_write_reg(®_ctx, LSM6DSV_TAP_DUR, (uint8_t *)&tap_dur, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + return LSM6DSV_OK; +} + +/** + * @brief Enable double tap detection + * @param IntPin interrupt pin line to be used + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSVStatusTypeDef LSM6DSV::Enable_Double_Tap_Detection(LSM6DSV_SensorIntPin_t IntPin) +{ + LSM6DSVStatusTypeDef ret = LSM6DSV_OK; + lsm6dsv_md1_cfg_t val1; + lsm6dsv_md2_cfg_t val2; + lsm6dsv_functions_enable_t functions_enable; + + lsm6dsv_tap_dur_t tap_dur; + lsm6dsv_tap_cfg0_t tap_cfg0; + lsm6dsv_tap_ths_6d_t tap_ths_6d; + + + /* Enable tap detection on Z-axis. */ + if (lsm6dsv_read_reg(®_ctx, LSM6DSV_TAP_CFG0, (uint8_t *)&tap_cfg0, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + tap_cfg0.tap_z_en = 0x01U; + + if (lsm6dsv_write_reg(®_ctx, LSM6DSV_TAP_CFG0, (uint8_t *)&tap_cfg0, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + /* Set Z-axis threshold. */ + if (lsm6dsv_read_reg(®_ctx, LSM6DSV_TAP_THS_6D, (uint8_t *)&tap_ths_6d, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + tap_ths_6d.tap_ths_z = 0x03U; + + if (lsm6dsv_write_reg(®_ctx, LSM6DSV_TAP_THS_6D, (uint8_t *)&tap_ths_6d, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + /* Set quiet shock and duration. */ + if (lsm6dsv_read_reg(®_ctx, LSM6DSV_TAP_DUR, (uint8_t *)&tap_dur, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + tap_dur.dur = (uint8_t)0x03U; + tap_dur.quiet = (uint8_t)0x02U; + tap_dur.shock = (uint8_t)0x02U; + + if (lsm6dsv_write_reg(®_ctx, LSM6DSV_TAP_DUR, (uint8_t *)&tap_dur, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + /* Set tap mode. */ + if (lsm6dsv_tap_mode_set(®_ctx, LSM6DSV_BOTH_SINGLE_DOUBLE) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + /* Output Data Rate selection */ + if (Set_X_ODR(480) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + /* Full scale selection */ + if (Set_X_FS(8) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + /* Enable double tap event on either INT1 or INT2 pin */ + switch (IntPin) { + case LSM6DSV_INT1_PIN: + if (lsm6dsv_read_reg(®_ctx, LSM6DSV_MD1_CFG, (uint8_t *)&val1, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + val1.int1_double_tap = PROPERTY_ENABLE; + + if (lsm6dsv_write_reg(®_ctx, LSM6DSV_MD1_CFG, (uint8_t *)&val1, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + if (lsm6dsv_read_reg(®_ctx, LSM6DSV_FUNCTIONS_ENABLE, (uint8_t *)&functions_enable, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + functions_enable.interrupts_enable = PROPERTY_ENABLE; + + if (lsm6dsv_write_reg(®_ctx, LSM6DSV_FUNCTIONS_ENABLE, (uint8_t *)&functions_enable, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + break; + + case LSM6DSV_INT2_PIN: + if (lsm6dsv_read_reg(®_ctx, LSM6DSV_MD2_CFG, (uint8_t *)&val2, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + val2.int2_double_tap = PROPERTY_ENABLE; + + if (lsm6dsv_write_reg(®_ctx, LSM6DSV_MD2_CFG, (uint8_t *)&val2, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + if (lsm6dsv_read_reg(®_ctx, LSM6DSV_FUNCTIONS_ENABLE, (uint8_t *)&functions_enable, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + functions_enable.interrupts_enable = PROPERTY_ENABLE; + + if (lsm6dsv_write_reg(®_ctx, LSM6DSV_FUNCTIONS_ENABLE, (uint8_t *)&functions_enable, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + break; + + default: + ret = LSM6DSV_ERROR; + break; + } + + return ret; +} + +/** + * @brief Disable double tap detection + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSVStatusTypeDef LSM6DSV::Disable_Double_Tap_Detection() +{ + lsm6dsv_md1_cfg_t val1; + lsm6dsv_md2_cfg_t val2; + + lsm6dsv_tap_dur_t tap_dur; + lsm6dsv_tap_cfg0_t tap_cfg0; + lsm6dsv_tap_ths_6d_t tap_ths_6d; + + /* Disable double tap event on both INT1 and INT2 pins */ + if (lsm6dsv_read_reg(®_ctx, LSM6DSV_MD1_CFG, (uint8_t *)&val1, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + val1.int1_ff = PROPERTY_DISABLE; + + if (lsm6dsv_write_reg(®_ctx, LSM6DSV_MD1_CFG, (uint8_t *)&val1, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + if (lsm6dsv_read_reg(®_ctx, LSM6DSV_MD2_CFG, (uint8_t *)&val2, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + val2.int2_ff = PROPERTY_DISABLE; + + if (lsm6dsv_write_reg(®_ctx, LSM6DSV_MD2_CFG, (uint8_t *)&val2, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + /* Disable tap detection on Z-axis. */ + if (lsm6dsv_read_reg(®_ctx, LSM6DSV_TAP_CFG0, (uint8_t *)&tap_cfg0, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + tap_cfg0.tap_z_en = 0x0U; + + if (lsm6dsv_write_reg(®_ctx, LSM6DSV_TAP_CFG0, (uint8_t *)&tap_cfg0, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + /* Reset Z-axis threshold. */ + if (lsm6dsv_read_reg(®_ctx, LSM6DSV_TAP_THS_6D, (uint8_t *)&tap_ths_6d, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + tap_ths_6d.tap_ths_z = 0x0U; + + if (lsm6dsv_write_reg(®_ctx, LSM6DSV_TAP_THS_6D, (uint8_t *)&tap_ths_6d, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + /* Reset quiet shock and duratio. */ + if (lsm6dsv_read_reg(®_ctx, LSM6DSV_TAP_DUR, (uint8_t *)&tap_dur, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + tap_dur.dur = (uint8_t)0x0U; + tap_dur.quiet = (uint8_t)0x0U; + tap_dur.shock = (uint8_t)0x0U; + + if (lsm6dsv_write_reg(®_ctx, LSM6DSV_TAP_DUR, (uint8_t *)&tap_dur, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + /* Set tap mode. */ + if (lsm6dsv_tap_mode_set(®_ctx, LSM6DSV_ONLY_SINGLE) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + + return LSM6DSV_OK; +} + +/** + * @brief Set tap threshold + * @param Threshold tap threshold + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSVStatusTypeDef LSM6DSV::Set_Tap_Threshold(uint8_t Threshold) +{ + lsm6dsv_tap_ths_6d_t tap_ths_6d; + + /* Set Z-axis threshold */ + if (lsm6dsv_read_reg(®_ctx, LSM6DSV_TAP_THS_6D, (uint8_t *)&tap_ths_6d, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + tap_ths_6d.tap_ths_z = Threshold; + + if (lsm6dsv_write_reg(®_ctx, LSM6DSV_TAP_THS_6D, (uint8_t *)&tap_ths_6d, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + return LSM6DSV_OK; +} + +/** + * @brief Set tap shock time + * @param Time tap shock time + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSVStatusTypeDef LSM6DSV::Set_Tap_Shock_Time(uint8_t Time) +{ + lsm6dsv_tap_dur_t tap_dur; + + /* Set shock time */ + if (lsm6dsv_read_reg(®_ctx, LSM6DSV_TAP_DUR, (uint8_t *)&tap_dur, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + tap_dur.shock = Time; + + if (lsm6dsv_write_reg(®_ctx, LSM6DSV_TAP_DUR, (uint8_t *)&tap_dur, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + return LSM6DSV_OK; +} + +/** + * @brief Set tap quiet time + * @param Time tap quiet time + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSVStatusTypeDef LSM6DSV::Set_Tap_Quiet_Time(uint8_t Time) +{ + lsm6dsv_tap_dur_t tap_dur; + + /* Set quiet time */ + if (lsm6dsv_read_reg(®_ctx, LSM6DSV_TAP_DUR, (uint8_t *)&tap_dur, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + tap_dur.quiet = Time; + + if (lsm6dsv_write_reg(®_ctx, LSM6DSV_TAP_DUR, (uint8_t *)&tap_dur, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + return LSM6DSV_OK; +} + +/** + * @brief Set tap duration time + * @param Time tap duration time + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSVStatusTypeDef LSM6DSV::Set_Tap_Duration_Time(uint8_t Time) +{ + lsm6dsv_tap_dur_t tap_dur; + + /* Set duration time */ + if (lsm6dsv_read_reg(®_ctx, LSM6DSV_TAP_DUR, (uint8_t *)&tap_dur, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + tap_dur.dur = Time; + + if (lsm6dsv_write_reg(®_ctx, LSM6DSV_TAP_DUR, (uint8_t *)&tap_dur, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + return LSM6DSV_OK; +} + +/** + * @brief Enable pedometer + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSVStatusTypeDef LSM6DSV::Enable_Pedometer(LSM6DSV_SensorIntPin_t IntPin) +{ + lsm6dsv_stpcnt_mode_t mode; + lsm6dsv_md1_cfg_t val1; + lsm6dsv_md2_cfg_t val2; + lsm6dsv_emb_func_int1_t emb_func_int1; + lsm6dsv_emb_func_int2_t emb_func_int2; + + /* Output Data Rate selection */ + if (Set_X_ODR(30) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + /* Full scale selection */ + if (Set_X_FS(2) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + if (lsm6dsv_stpcnt_mode_get(®_ctx, &mode) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + /* Enable pedometer algorithm. */ + mode.step_counter_enable = PROPERTY_ENABLE; + + /* Turn on embedded features */ + if (lsm6dsv_stpcnt_mode_set(®_ctx, mode) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + /* Enable free fall event on either INT1 or INT2 pin */ + switch (IntPin) { + case LSM6DSV_INT1_PIN: + /* Enable access to embedded functions registers. */ + if (lsm6dsv_mem_bank_set(®_ctx, LSM6DSV_EMBED_FUNC_MEM_BANK) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + /* Step detector interrupt driven to INT1 pin */ + if (lsm6dsv_read_reg(®_ctx, LSM6DSV_EMB_FUNC_INT1, (uint8_t *)&emb_func_int1, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + emb_func_int1.int1_step_detector = PROPERTY_ENABLE; + + if (lsm6dsv_write_reg(®_ctx, LSM6DSV_EMB_FUNC_INT1, (uint8_t *)&emb_func_int1, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + /* Disable access to embedded functions registers */ + if (lsm6dsv_mem_bank_set(®_ctx, LSM6DSV_MAIN_MEM_BANK) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + if (lsm6dsv_read_reg(®_ctx, LSM6DSV_MD1_CFG, (uint8_t *)&val1, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + val1.int1_emb_func = PROPERTY_ENABLE; + + if (lsm6dsv_write_reg(®_ctx, LSM6DSV_MD1_CFG, (uint8_t *)&val1, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + break; + + case LSM6DSV_INT2_PIN: + /* Enable access to embedded functions registers. */ + if (lsm6dsv_mem_bank_set(®_ctx, LSM6DSV_EMBED_FUNC_MEM_BANK) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + /* Step detector interrupt driven to INT1 pin */ + if (lsm6dsv_read_reg(®_ctx, LSM6DSV_EMB_FUNC_INT2, (uint8_t *)&emb_func_int2, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + emb_func_int2.int2_step_detector = PROPERTY_ENABLE; + + if (lsm6dsv_write_reg(®_ctx, LSM6DSV_EMB_FUNC_INT2, (uint8_t *)&emb_func_int2, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + /* Disable access to embedded functions registers */ + if (lsm6dsv_mem_bank_set(®_ctx, LSM6DSV_MAIN_MEM_BANK) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + if (lsm6dsv_read_reg(®_ctx, LSM6DSV_MD2_CFG, (uint8_t *)&val2, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + val2.int2_emb_func = PROPERTY_ENABLE; + + if (lsm6dsv_write_reg(®_ctx, LSM6DSV_MD2_CFG, (uint8_t *)&val2, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + break; + + default: + return LSM6DSV_ERROR; + break; + } + + return LSM6DSV_OK; +} + + +/** + * @brief Disable pedometer + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSVStatusTypeDef LSM6DSV::Disable_Pedometer() +{ + lsm6dsv_md1_cfg_t val1; + lsm6dsv_md2_cfg_t val2; + + lsm6dsv_emb_func_int1_t emb_func_int1; + lsm6dsv_emb_func_int2_t emb_func_int2; + + lsm6dsv_stpcnt_mode_t mode; + + + if (lsm6dsv_stpcnt_mode_get(®_ctx, &mode) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + /* Enable pedometer algorithm. */ + mode.step_counter_enable = PROPERTY_DISABLE; + + /* Turn off embedded features */ + if (lsm6dsv_stpcnt_mode_set(®_ctx, mode) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + /* Disable emb func event on either INT1 or INT2 pin */ + if (lsm6dsv_read_reg(®_ctx, LSM6DSV_MD1_CFG, (uint8_t *)&val1, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + val1.int1_emb_func = PROPERTY_DISABLE; + + if (lsm6dsv_write_reg(®_ctx, LSM6DSV_MD1_CFG, (uint8_t *)&val1, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + if (lsm6dsv_read_reg(®_ctx, LSM6DSV_MD2_CFG, (uint8_t *)&val2, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + val2.int2_emb_func = PROPERTY_DISABLE; + + if (lsm6dsv_write_reg(®_ctx, LSM6DSV_MD2_CFG, (uint8_t *)&val2, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + /* Enable access to embedded functions registers. */ + if (lsm6dsv_mem_bank_set(®_ctx, LSM6DSV_EMBED_FUNC_MEM_BANK) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + /* Reset interrupt driven to INT1 pin. */ + if (lsm6dsv_read_reg(®_ctx, LSM6DSV_EMB_FUNC_INT1, (uint8_t *)&emb_func_int1, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + emb_func_int1.int1_step_detector = PROPERTY_DISABLE; + + if (lsm6dsv_write_reg(®_ctx, LSM6DSV_EMB_FUNC_INT1, (uint8_t *)&emb_func_int1, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + /* Reset interrupt driven to INT2 pin. */ + if (lsm6dsv_read_reg(®_ctx, LSM6DSV_EMB_FUNC_INT2, (uint8_t *)&emb_func_int2, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + emb_func_int2.int2_step_detector = PROPERTY_DISABLE; + + if (lsm6dsv_write_reg(®_ctx, LSM6DSV_EMB_FUNC_INT2, (uint8_t *)&emb_func_int2, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + /* Disable access to embedded functions registers. */ + if (lsm6dsv_mem_bank_set(®_ctx, LSM6DSV_MAIN_MEM_BANK) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + + return LSM6DSV_OK; +} + +/** + * @brief Get step count + * @param StepCount step counter + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSVStatusTypeDef LSM6DSV::Get_Step_Count(uint16_t *StepCount) +{ + if (lsm6dsv_stpcnt_steps_get(®_ctx, StepCount) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + return LSM6DSV_OK; +} + +/** + * @brief Enable step counter reset + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSVStatusTypeDef LSM6DSV::Step_Counter_Reset() +{ + if (lsm6dsv_stpcnt_rst_step_set(®_ctx, PROPERTY_ENABLE) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + return LSM6DSV_OK; +} + +/** + * @brief Enable tilt detection + * @param IntPin interrupt pin line to be used + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSVStatusTypeDef LSM6DSV::Enable_Tilt_Detection(LSM6DSV_SensorIntPin_t IntPin) +{ + LSM6DSVStatusTypeDef ret = LSM6DSV_OK; + lsm6dsv_md1_cfg_t val1; + lsm6dsv_md2_cfg_t val2; + lsm6dsv_emb_func_en_a_t emb_func_en_a; + lsm6dsv_emb_func_int1_t emb_func_int1; + lsm6dsv_emb_func_int2_t emb_func_int2; + + /* Output Data Rate selection */ + if (Set_X_ODR(30) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + /* Full scale selection */ + if (Set_X_FS(2) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + switch (IntPin) { + case LSM6DSV_INT1_PIN: + /* Enable access to embedded functions registers */ + if (lsm6dsv_mem_bank_set(®_ctx, LSM6DSV_EMBED_FUNC_MEM_BANK) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + /* Enable tilt detection */ + if (lsm6dsv_read_reg(®_ctx, LSM6DSV_EMB_FUNC_EN_A, (uint8_t *)&emb_func_en_a, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + emb_func_en_a.tilt_en = PROPERTY_ENABLE; + + if (lsm6dsv_write_reg(®_ctx, LSM6DSV_EMB_FUNC_EN_A, (uint8_t *)&emb_func_en_a, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + /* Tilt interrupt driven to INT1 pin */ + if (lsm6dsv_read_reg(®_ctx, LSM6DSV_EMB_FUNC_INT1, (uint8_t *)&emb_func_int1, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + emb_func_int1.int1_tilt = PROPERTY_ENABLE; + + if (lsm6dsv_write_reg(®_ctx, LSM6DSV_EMB_FUNC_INT1, (uint8_t *)&emb_func_int1, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + /* Disable access to embedded functions registers */ + if (lsm6dsv_mem_bank_set(®_ctx, LSM6DSV_MAIN_MEM_BANK) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + /* Enable routing the embedded functions interrupt */ + if (lsm6dsv_read_reg(®_ctx, LSM6DSV_MD1_CFG, (uint8_t *)&val1, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + val1.int1_emb_func = PROPERTY_ENABLE; + + if (lsm6dsv_write_reg(®_ctx, LSM6DSV_MD1_CFG, (uint8_t *)&val1, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + break; + + + case LSM6DSV_INT2_PIN: + /* Enable access to embedded functions registers */ + if (lsm6dsv_mem_bank_set(®_ctx, LSM6DSV_EMBED_FUNC_MEM_BANK) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + /* Enable tilt detection */ + if (lsm6dsv_read_reg(®_ctx, LSM6DSV_EMB_FUNC_EN_A, (uint8_t *)&emb_func_en_a, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + emb_func_en_a.tilt_en = PROPERTY_ENABLE; + + if (lsm6dsv_write_reg(®_ctx, LSM6DSV_EMB_FUNC_EN_A, (uint8_t *)&emb_func_en_a, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + /* Tilt interrupt driven to INT2 pin */ + if (lsm6dsv_read_reg(®_ctx, LSM6DSV_EMB_FUNC_INT2, (uint8_t *)&emb_func_int2, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + emb_func_int2.int2_tilt = PROPERTY_ENABLE; + + if (lsm6dsv_write_reg(®_ctx, LSM6DSV_EMB_FUNC_INT2, (uint8_t *)&emb_func_int2, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + /* Disable access to embedded functions registers */ + if (lsm6dsv_mem_bank_set(®_ctx, LSM6DSV_MAIN_MEM_BANK) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + /* Enable routing the embedded functions interrupt */ + if (lsm6dsv_read_reg(®_ctx, LSM6DSV_MD2_CFG, (uint8_t *)&val2, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + val2.int2_emb_func = PROPERTY_ENABLE; + + if (lsm6dsv_write_reg(®_ctx, LSM6DSV_MD2_CFG, (uint8_t *)&val2, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + break; + + default: + ret = LSM6DSV_ERROR; + break; + } + + return ret; +} + +/** + * @brief Disable tilt detection + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSVStatusTypeDef LSM6DSV::Disable_Tilt_Detection() +{ + LSM6DSVStatusTypeDef ret = LSM6DSV_OK; + lsm6dsv_md1_cfg_t val1; + lsm6dsv_md2_cfg_t val2; + + lsm6dsv_emb_func_en_a_t emb_func_en_a; + lsm6dsv_emb_func_int1_t emb_func_int1; + lsm6dsv_emb_func_int2_t emb_func_int2; + + /* Disable emb func event on either INT1 or INT2 pin */ + if (lsm6dsv_read_reg(®_ctx, LSM6DSV_MD1_CFG, (uint8_t *)&val1, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + val1.int1_emb_func = PROPERTY_DISABLE; + + if (lsm6dsv_write_reg(®_ctx, LSM6DSV_MD1_CFG, (uint8_t *)&val1, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + if (lsm6dsv_read_reg(®_ctx, LSM6DSV_MD2_CFG, (uint8_t *)&val2, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + val2.int2_emb_func = PROPERTY_DISABLE; + + if (lsm6dsv_write_reg(®_ctx, LSM6DSV_MD2_CFG, (uint8_t *)&val2, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + /* Enable access to embedded functions registers. */ + if (lsm6dsv_mem_bank_set(®_ctx, LSM6DSV_EMBED_FUNC_MEM_BANK) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + /* Disable tilt detection. */ + if (lsm6dsv_read_reg(®_ctx, LSM6DSV_EMB_FUNC_EN_A, (uint8_t *)&emb_func_en_a, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + emb_func_en_a.tilt_en = PROPERTY_DISABLE; + + if (lsm6dsv_write_reg(®_ctx, LSM6DSV_EMB_FUNC_EN_A, (uint8_t *)&emb_func_en_a, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + /* Reset interrupt driven to INT1 pin. */ + if (lsm6dsv_read_reg(®_ctx, LSM6DSV_EMB_FUNC_INT1, (uint8_t *)&emb_func_int1, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + emb_func_int1.int1_tilt = PROPERTY_DISABLE; + + if (lsm6dsv_write_reg(®_ctx, LSM6DSV_EMB_FUNC_INT1, (uint8_t *)&emb_func_int1, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + /* Reset interrupt driven to INT2 pin. */ + if (lsm6dsv_read_reg(®_ctx, LSM6DSV_EMB_FUNC_INT2, (uint8_t *)&emb_func_int2, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + emb_func_int2.int2_tilt = PROPERTY_DISABLE; + + if (lsm6dsv_write_reg(®_ctx, LSM6DSV_EMB_FUNC_INT2, (uint8_t *)&emb_func_int2, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + /* Disable access to embedded functions registers. */ + if (lsm6dsv_mem_bank_set(®_ctx, LSM6DSV_MAIN_MEM_BANK) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + return ret; +} + + +LSM6DSVStatusTypeDef LSM6DSV::FIFO_Reset() { + if (lsm6dsv_fifo_mode_set(®_ctx, LSM6DSV_BYPASS_MODE) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + if (lsm6dsv_fifo_mode_set(®_ctx, fifo_mode) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + return LSM6DSV_OK; +} + +/** + * @brief Get the LSM6DSV FIFO number of samples + + * @param NumSamples number of samples + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSVStatusTypeDef LSM6DSV::FIFO_Get_Num_Samples(uint16_t *NumSamples) +{ + lsm6dsv_fifo_status_t fifo_status; + LSM6DSVStatusTypeDef result = + (LSM6DSVStatusTypeDef) lsm6dsv_fifo_status_get(®_ctx, &fifo_status); + *NumSamples = fifo_status.fifo_level; + return result; +} + +/** + * @brief Get the LSM6DSV FIFO full status + + * @param Status FIFO full status + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSVStatusTypeDef LSM6DSV::FIFO_Get_Full_Status(uint8_t *Status) +{ + lsm6dsv_fifo_status2_t val; + + if (lsm6dsv_read_reg(®_ctx, LSM6DSV_FIFO_STATUS2, (uint8_t *)&val, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + *Status = val.fifo_full_ia; + + return LSM6DSV_OK; +} + +/** + * @brief Set the LSM6DSV FIFO full interrupt on INT1 pin + + * @param Status FIFO full interrupt on INT1 pin status + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSVStatusTypeDef LSM6DSV::FIFO_Set_INT1_FIFO_Full(uint8_t Status) +{ + lsm6dsv_int1_ctrl_t reg; + + if (lsm6dsv_read_reg(®_ctx, LSM6DSV_INT1_CTRL, (uint8_t *)®, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + reg.int1_fifo_full = Status; + + if (lsm6dsv_write_reg(®_ctx, LSM6DSV_INT1_CTRL, (uint8_t *)®, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + return LSM6DSV_OK; +} + +/** + * @brief Set the LSM6DSV FIFO full interrupt on INT2 pin + + * @param Status FIFO full interrupt on INT1 pin status + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSVStatusTypeDef LSM6DSV::FIFO_Set_INT2_FIFO_Full(uint8_t Status) +{ + lsm6dsv_int2_ctrl_t reg; + + if (lsm6dsv_read_reg(®_ctx, LSM6DSV_INT2_CTRL, (uint8_t *)®, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + reg.int2_fifo_full = Status; + + if (lsm6dsv_write_reg(®_ctx, LSM6DSV_INT2_CTRL, (uint8_t *)®, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + return LSM6DSV_OK; +} + +/** + * @brief Set the LSM6DSV FIFO watermark level + + * @param Watermark FIFO watermark level + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSVStatusTypeDef LSM6DSV::FIFO_Set_Watermark_Level(uint8_t Watermark) +{ + return (LSM6DSVStatusTypeDef) lsm6dsv_fifo_watermark_set(®_ctx, Watermark); +} + +/** + * @brief Set the LSM6DSV FIFO stop on watermark + + * @param Status FIFO stop on watermark status + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSVStatusTypeDef LSM6DSV::FIFO_Set_Stop_On_Fth(uint8_t Status) +{ + return (LSM6DSVStatusTypeDef) lsm6dsv_fifo_stop_on_wtm_set(®_ctx, Status); +} + +/** + * @brief Set the LSM6DSV FIFO mode + + * @param Mode FIFO mode + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSVStatusTypeDef LSM6DSV::FIFO_Set_Mode(uint8_t Mode) +{ + lsm6dsv_fifo_mode_t newMode = LSM6DSV_BYPASS_MODE; + + switch (Mode) { + case 0: + newMode = LSM6DSV_BYPASS_MODE; + break; + case 1: + newMode = LSM6DSV_FIFO_MODE; + break; + case 3: + newMode = LSM6DSV_STREAM_TO_FIFO_MODE; + break; + case 4: + newMode = LSM6DSV_BYPASS_TO_STREAM_MODE; + break; + case 6: + newMode = LSM6DSV_STREAM_MODE; + break; + case 7: + newMode = LSM6DSV_BYPASS_TO_FIFO_MODE; + break; + default: + return LSM6DSV_ERROR; + } + fifo_mode = newMode; + if (lsm6dsv_fifo_mode_set(®_ctx, fifo_mode) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + return LSM6DSV_OK; +} + +/** + * @brief Get the LSM6DSV FIFO tag + + * @param Tag FIFO tag + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSVStatusTypeDef LSM6DSV::FIFO_Get_Tag(uint8_t *Tag) +{ + lsm6dsv_fifo_data_out_tag_t tag_local; + + if (lsm6dsv_read_reg(®_ctx, LSM6DSV_FIFO_DATA_OUT_TAG, (uint8_t *)&tag_local, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + *Tag = (uint8_t)tag_local.tag_sensor; + + return LSM6DSV_OK; +} + +/** + * @brief Get the LSM6DSV FIFO raw data + + * @param Data FIFO raw data array [6] + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSVStatusTypeDef LSM6DSV::FIFO_Get_Data(uint8_t *Data) +{ + return (LSM6DSVStatusTypeDef) lsm6dsv_read_reg(®_ctx, LSM6DSV_FIFO_DATA_OUT_X_L, Data, 6); +} + +/** + * @brief Get the LSM6DSV FIFO accelero single sample (16-bit data per 3 axes) and calculate acceleration [mg] + * @param Acceleration FIFO accelero axes [mg] + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSVStatusTypeDef LSM6DSV::FIFO_Get_X_Axes(int32_t *Acceleration) +{ + lsm6dsv_axis3bit16_t data_raw; + float sensitivity = Convert_X_Sensitivity(acc_fs); + float acceleration_float[3]; + + if (FIFO_Get_Data(data_raw.u8bit) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + if (sensitivity == 0.0f) { + return LSM6DSV_ERROR; + } + acceleration_float[0] = (float)data_raw.i16bit[0] * sensitivity; + acceleration_float[1] = (float)data_raw.i16bit[1] * sensitivity; + acceleration_float[2] = (float)data_raw.i16bit[2] * sensitivity; + + Acceleration[0] = (int32_t)acceleration_float[0]; + Acceleration[1] = (int32_t)acceleration_float[1]; + Acceleration[2] = (int32_t)acceleration_float[2]; + + return LSM6DSV_OK; + +} + +/** + * @brief Set the LSM6DSV FIFO accelero BDR value + + * @param Bdr FIFO accelero BDR value + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSVStatusTypeDef LSM6DSV::FIFO_Set_X_BDR(float Bdr) +{ + lsm6dsv_fifo_xl_batch_t new_bdr; + + new_bdr = (Bdr <= 0.0f) ? LSM6DSV_XL_NOT_BATCHED + : (Bdr <= 1.8f) ? LSM6DSV_XL_BATCHED_AT_1Hz875 + : (Bdr <= 7.5f) ? LSM6DSV_XL_BATCHED_AT_7Hz5 + : (Bdr <= 15.0f) ? LSM6DSV_XL_BATCHED_AT_15Hz + : (Bdr <= 30.0f) ? LSM6DSV_XL_BATCHED_AT_30Hz + : (Bdr <= 60.0f) ? LSM6DSV_XL_BATCHED_AT_60Hz + : (Bdr <= 120.0f) ? LSM6DSV_XL_BATCHED_AT_120Hz + : (Bdr <= 240.0f) ? LSM6DSV_XL_BATCHED_AT_240Hz + : (Bdr <= 480.0f) ? LSM6DSV_XL_BATCHED_AT_480Hz + : (Bdr <= 960.0f) ? LSM6DSV_XL_BATCHED_AT_960Hz + : (Bdr <= 1920.0f) ? LSM6DSV_XL_BATCHED_AT_1920Hz + : (Bdr <= 3840.0f) ? LSM6DSV_XL_BATCHED_AT_3840Hz + : LSM6DSV_XL_BATCHED_AT_7680Hz; + + return (LSM6DSVStatusTypeDef) lsm6dsv_fifo_xl_batch_set(®_ctx, new_bdr); +} + +/** + * @brief Get the LSM6DSV FIFO gyro single sample (16-bit data per 3 axes) and calculate angular velocity [mDPS] + + * @param AngularVelocity FIFO gyro axes [mDPS] + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSVStatusTypeDef LSM6DSV::FIFO_Get_G_Axes(int32_t *AngularVelocity) +{ + lsm6dsv_axis3bit16_t data_raw; + float sensitivity = Convert_G_Sensitivity(gyro_fs); + float angular_velocity_float[3]; + + if (FIFO_Get_Data(data_raw.u8bit) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + if (sensitivity == 0.0f) { + return LSM6DSV_ERROR; + } + + angular_velocity_float[0] = (float)data_raw.i16bit[0] * sensitivity; + angular_velocity_float[1] = (float)data_raw.i16bit[1] * sensitivity; + angular_velocity_float[2] = (float)data_raw.i16bit[2] * sensitivity; + + AngularVelocity[0] = (int32_t)angular_velocity_float[0]; + AngularVelocity[1] = (int32_t)angular_velocity_float[1]; + AngularVelocity[2] = (int32_t)angular_velocity_float[2]; + + return LSM6DSV_OK; +} + +/** + * @brief Set the LSM6DSV FIFO gyro BDR value + + * @param Bdr FIFO gyro BDR value + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSVStatusTypeDef LSM6DSV::FIFO_Set_G_BDR(float Bdr) +{ + lsm6dsv_fifo_gy_batch_t new_bdr; + + new_bdr = (Bdr <= 0.0f) ? LSM6DSV_GY_NOT_BATCHED + : (Bdr <= 1.8f) ? LSM6DSV_GY_BATCHED_AT_1Hz875 + : (Bdr <= 7.5f) ? LSM6DSV_GY_BATCHED_AT_7Hz5 + : (Bdr <= 15.0f) ? LSM6DSV_GY_BATCHED_AT_15Hz + : (Bdr <= 30.0f) ? LSM6DSV_GY_BATCHED_AT_30Hz + : (Bdr <= 60.0f) ? LSM6DSV_GY_BATCHED_AT_60Hz + : (Bdr <= 120.0f) ? LSM6DSV_GY_BATCHED_AT_120Hz + : (Bdr <= 240.0f) ? LSM6DSV_GY_BATCHED_AT_240Hz + : (Bdr <= 480.0f) ? LSM6DSV_GY_BATCHED_AT_480Hz + : (Bdr <= 960.0f) ? LSM6DSV_GY_BATCHED_AT_960Hz + : (Bdr <= 1920.0f) ? LSM6DSV_GY_BATCHED_AT_1920Hz + : (Bdr <= 3840.0f) ? LSM6DSV_GY_BATCHED_AT_3840Hz + : LSM6DSV_GY_BATCHED_AT_7680Hz; + + return (LSM6DSVStatusTypeDef) lsm6dsv_fifo_gy_batch_set(®_ctx, new_bdr); +} + +LSM6DSVStatusTypeDef LSM6DSV::FIFO_Set_SFLP_Batch(bool GameRotation, bool Gravity, bool gBias) +{ + lsm6dsv_fifo_sflp_raw_t fifo_sflp; + fifo_sflp.game_rotation = GameRotation; + fifo_sflp.gravity = Gravity; + fifo_sflp.gbias = gBias; + return (LSM6DSVStatusTypeDef)lsm6dsv_fifo_sflp_batch_set(®_ctx, fifo_sflp); +} + +LSM6DSVStatusTypeDef LSM6DSV::FIFO_Get_Status(lsm6dsv_fifo_status_t * Status) +{ + return (LSM6DSVStatusTypeDef) lsm6dsv_fifo_status_get(®_ctx, Status); +} + +LSM6DSVStatusTypeDef LSM6DSV::FIFO_Get_Gravity_Vector(float * data) +{ + uint16_t raw_data[6]; + if (FIFO_Get_Data((uint8_t *)raw_data) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + data[0] = lsm6dsv_from_sflp_to_mg(raw_data[0]); + data[1] = lsm6dsv_from_sflp_to_mg(raw_data[1]); + data[2] = lsm6dsv_from_sflp_to_mg(raw_data[2]); + return LSM6DSV_OK; +} + +LSM6DSVStatusTypeDef LSM6DSV::FIFO_Get_Game_Vector(float * data) +{ + uint16_t raw_data[3]; + if (FIFO_Get_Data((uint8_t *)raw_data) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + union bits_to_float { + uint32_t bits; + float value; + }; + + bits_to_float float_bits[3]; + float_bits[0].bits = Half_Bits_To_Float_Bits(raw_data[0]); + float_bits[1].bits = Half_Bits_To_Float_Bits(raw_data[1]); + float_bits[2].bits = Half_Bits_To_Float_Bits(raw_data[2]); + data[0] = float_bits[0].value; + data[1] = float_bits[1].value; + data[2] = float_bits[2].value; + return LSM6DSV_OK; +} + +LSM6DSVStatusTypeDef LSM6DSV::FIFO_Get_GBias_Vector(float * data) +{ + uint16_t raw_data[3]; + if (FIFO_Get_Data((uint8_t *)raw_data) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + data[0] = lsm6dsv_from_fs125_to_mdps(raw_data[0]); + data[1] = lsm6dsv_from_fs125_to_mdps(raw_data[1]); + data[2] = lsm6dsv_from_fs125_to_mdps(raw_data[2]); + return LSM6DSV_OK; +} + +LSM6DSVStatusTypeDef LSM6DSV::FIFO_Get_Timestamp(uint32_t *timestamp) +{ + uint32_t raw_data[2]; //first is timestamp second is half full of meta data + if (FIFO_Get_Data((uint8_t *)raw_data) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + *timestamp = raw_data[0]; + return LSM6DSV_OK; +} + +LSM6DSVStatusTypeDef LSM6DSV::FIFO_Set_Timestamp_Decimation(uint8_t decimation) +{ + return (LSM6DSVStatusTypeDef)lsm6dsv_fifo_timestamp_batch_set(®_ctx, (lsm6dsv_fifo_timestamp_batch_t)decimation); + +} + +LSM6DSVStatusTypeDef LSM6DSV::Enable_Timestamp() +{ + return (LSM6DSVStatusTypeDef)lsm6dsv_timestamp_set(®_ctx, 1); +} + + +/** + * @brief Enable the LSM6DSV gyroscope sensor + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSVStatusTypeDef LSM6DSV::Enable_G() +{ + /* Check if the component is already enabled */ + if (gyro_is_enabled == 1U) { + return LSM6DSV_OK; + } + + /* Output data rate selection. */ + if (lsm6dsv_gy_data_rate_set(®_ctx, gyro_odr) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + gyro_is_enabled = 1U; + + return LSM6DSV_OK; +} + +/** + * @brief Disable the LSM6DSV gyroscope sensor + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSVStatusTypeDef LSM6DSV::Disable_G() +{ + /* Check if the component is already disabled */ + if (gyro_is_enabled == 0U) { + return LSM6DSV_OK; + } + + /* Get current output data rate. */ + if (lsm6dsv_gy_data_rate_get(®_ctx, &gyro_odr) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + /* Output data rate selection - power down. */ + if (lsm6dsv_gy_data_rate_set(®_ctx, LSM6DSV_ODR_OFF) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + gyro_is_enabled = 0U; + + return LSM6DSV_OK; +} + +/** + * @brief Get the LSM6DSV gyroscope sensor sensitivity + * @param Sensitivity pointer + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSVStatusTypeDef LSM6DSV::Get_G_Sensitivity(float *Sensitivity) +{ + lsm6dsv_gy_full_scale_t full_scale; + + /* Read actual full scale selection from sensor. */ + if (lsm6dsv_gy_full_scale_get(®_ctx, &full_scale) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + *Sensitivity = Convert_G_Sensitivity(full_scale); + if (*Sensitivity == 0.0f) { + return LSM6DSV_ERROR; + } + + return LSM6DSV_OK; +} + + +float LSM6DSV::Convert_G_Sensitivity(lsm6dsv_gy_full_scale_t full_scale) { + float Sensitivity = 0.0f; + /* Store the sensitivity based on actual full scale. */ + switch (full_scale) { + case LSM6DSV_125dps: + Sensitivity = LSM6DSV_GYRO_SENSITIVITY_FS_125DPS; + break; + + case LSM6DSV_250dps: + Sensitivity = LSM6DSV_GYRO_SENSITIVITY_FS_250DPS; + break; + + case LSM6DSV_500dps: + Sensitivity = LSM6DSV_GYRO_SENSITIVITY_FS_500DPS; + break; + + case LSM6DSV_1000dps: + Sensitivity = LSM6DSV_GYRO_SENSITIVITY_FS_1000DPS; + break; + + case LSM6DSV_2000dps: + Sensitivity = LSM6DSV_GYRO_SENSITIVITY_FS_2000DPS; + break; + + case LSM6DSV_4000dps: + Sensitivity = LSM6DSV_GYRO_SENSITIVITY_FS_4000DPS; + break; + } + return Sensitivity; +} + +/** + * @brief Get the LSM6DSV gyroscope sensor output data rate + * @param Odr pointer where the output data rate is written + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSVStatusTypeDef LSM6DSV::Get_G_ODR(float *Odr) +{ + LSM6DSVStatusTypeDef ret = LSM6DSV_OK; + lsm6dsv_data_rate_t odr_low_level; + + /* Get current output data rate. */ + if (lsm6dsv_gy_data_rate_get(®_ctx, &odr_low_level) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + switch (odr_low_level) { + case LSM6DSV_ODR_OFF: + *Odr = 0.0f; + break; + + case LSM6DSV_ODR_AT_7Hz5: + *Odr = 7.5f; + break; + + case LSM6DSV_ODR_AT_15Hz: + *Odr = 15.0f; + break; + + case LSM6DSV_ODR_AT_30Hz: + *Odr = 30.0f; + break; + + case LSM6DSV_ODR_AT_60Hz: + *Odr = 60.0f; + break; + + case LSM6DSV_ODR_AT_120Hz: + *Odr = 120.0f; + break; + + case LSM6DSV_ODR_AT_240Hz: + *Odr = 240.0f; + break; + + case LSM6DSV_ODR_AT_480Hz: + *Odr = 480.0f; + break; + + case LSM6DSV_ODR_AT_960Hz: + *Odr = 960.0f; + break; + + case LSM6DSV_ODR_AT_1920Hz: + *Odr = 1920.0f; + break; + + case LSM6DSV_ODR_AT_3840Hz: + *Odr = 3840.0f; + break; + + case LSM6DSV_ODR_AT_7680Hz: + *Odr = 7680.0f; + break; + + default: + ret = LSM6DSV_ERROR; + break; + } + + return ret; +} + +/** + * @brief Set the LSM6DSV gyroscope sensor output data rate with operating mode + * @param Odr the output data rate value to be set + * @param Mode the gyroscope operating mode + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSVStatusTypeDef LSM6DSV::Set_G_ODR(float Odr, LSM6DSV_GYRO_Operating_Mode_t Mode) +{ + switch (Mode) { + case LSM6DSV_GYRO_HIGH_PERFORMANCE_MODE: { + if (lsm6dsv_gy_mode_set(®_ctx, LSM6DSV_GY_HIGH_PERFORMANCE_MD) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + /* Valid ODR: 7.5Hz <= Odr <= 7.68kHz */ + Odr = (Odr < 7.5f) ? 7.5f + : (Odr > 7680.0f) ? 7680.0f + : Odr; + break; + } + + case LSM6DSV_GYRO_HIGH_ACCURACY_MODE: + // TODO: Not implemented. + // NOTE: According to datasheet, section `6.5 High-accuracy ODR mode`: + // "... the other sensor also has to be configured in high-accuracy ODR (HAODR) mode." + return LSM6DSV_ERROR; + + case LSM6DSV_GYRO_SLEEP_MODE: + // TODO: Not implemented. + // NOTE: Unknown ODR validity for this mode + return LSM6DSV_ERROR; + + case LSM6DSV_GYRO_LOW_POWER_MODE: { + if (lsm6dsv_gy_mode_set(®_ctx, LSM6DSV_GY_LOW_POWER_MD) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + /* Valid ODR: 7.5Hz <= Odr <= 240kHz */ + Odr = (Odr < 7.5f) ? 7.5f + : (Odr > 240.0f) ? 240.0f + : Odr; + break; + } + + default: + return LSM6DSV_ERROR; + } + + if (gyro_is_enabled == 1U) { + return Set_G_ODR_When_Enabled(Odr); + } else { + return Set_G_ODR_When_Disabled(Odr); + } +} + +/** + * @brief Set the LSM6DSV gyroscope sensor output data rate when enabled + * @param Odr the functional output data rate to be set + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSVStatusTypeDef LSM6DSV::Set_G_ODR_When_Enabled(float Odr) +{ + lsm6dsv_data_rate_t new_odr; + + new_odr = (Odr <= 7.5f) ? LSM6DSV_ODR_AT_7Hz5 + : (Odr <= 15.0f) ? LSM6DSV_ODR_AT_15Hz + : (Odr <= 30.0f) ? LSM6DSV_ODR_AT_30Hz + : (Odr <= 60.0f) ? LSM6DSV_ODR_AT_60Hz + : (Odr <= 120.0f) ? LSM6DSV_ODR_AT_120Hz + : (Odr <= 240.0f) ? LSM6DSV_ODR_AT_240Hz + : (Odr <= 480.0f) ? LSM6DSV_ODR_AT_480Hz + : (Odr <= 960.0f) ? LSM6DSV_ODR_AT_960Hz + : (Odr <= 1920.0f) ? LSM6DSV_ODR_AT_1920Hz + : (Odr <= 3840.0f) ? LSM6DSV_ODR_AT_3840Hz + : LSM6DSV_ODR_AT_7680Hz; + + return (LSM6DSVStatusTypeDef) lsm6dsv_gy_data_rate_set(®_ctx, new_odr); +} + +/** + * @brief Set the LSM6DSV gyroscope sensor output data rate when disabled + * @param Odr the functional output data rate to be set + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSVStatusTypeDef LSM6DSV::Set_G_ODR_When_Disabled(float Odr) +{ + gyro_odr = (Odr <= 7.5f) ? LSM6DSV_ODR_AT_7Hz5 + : (Odr <= 15.0f) ? LSM6DSV_ODR_AT_15Hz + : (Odr <= 30.0f) ? LSM6DSV_ODR_AT_30Hz + : (Odr <= 60.0f) ? LSM6DSV_ODR_AT_60Hz + : (Odr <= 120.0f) ? LSM6DSV_ODR_AT_120Hz + : (Odr <= 240.0f) ? LSM6DSV_ODR_AT_240Hz + : (Odr <= 480.0f) ? LSM6DSV_ODR_AT_480Hz + : (Odr <= 960.0f) ? LSM6DSV_ODR_AT_960Hz + : (Odr <= 1920.0f) ? LSM6DSV_ODR_AT_1920Hz + : (Odr <= 3840.0f) ? LSM6DSV_ODR_AT_3840Hz + : LSM6DSV_ODR_AT_7680Hz; + + return LSM6DSV_OK; +} + +/** + * @brief Get the LSM6DSV gyroscope sensor full scale + * @param FullScale pointer where the full scale is written + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSVStatusTypeDef LSM6DSV::Get_G_FS(int32_t *FullScale) +{ + LSM6DSVStatusTypeDef ret = LSM6DSV_OK; + lsm6dsv_gy_full_scale_t fs_low_level; + + /* Read actual full scale selection from sensor. */ + if (lsm6dsv_gy_full_scale_get(®_ctx, &fs_low_level) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + switch (fs_low_level) { + case LSM6DSV_125dps: + *FullScale = 125; + break; + + case LSM6DSV_250dps: + *FullScale = 250; + break; + + case LSM6DSV_500dps: + *FullScale = 500; + break; + + case LSM6DSV_1000dps: + *FullScale = 1000; + break; + + case LSM6DSV_2000dps: + *FullScale = 2000; + break; + + case LSM6DSV_4000dps: + *FullScale = 4000; + break; + + default: + ret = LSM6DSV_ERROR; + break; + } + + return ret; +} + +/** + * @brief Set the LSM6DSV gyroscope sensor full scale + * @param FullScale the functional full scale to be set + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSVStatusTypeDef LSM6DSV::Set_G_FS(int32_t FullScale) +{ + lsm6dsv_gy_full_scale_t new_fs; + + new_fs = (FullScale <= 125) ? LSM6DSV_125dps + : (FullScale <= 250) ? LSM6DSV_250dps + : (FullScale <= 500) ? LSM6DSV_500dps + : (FullScale <= 1000) ? LSM6DSV_1000dps + : (FullScale <= 2000) ? LSM6DSV_2000dps + : LSM6DSV_4000dps; + + if (new_fs == gyro_fs) { + return LSM6DSV_OK; + } + gyro_fs = new_fs; + + return (LSM6DSVStatusTypeDef) lsm6dsv_gy_full_scale_set(®_ctx, gyro_fs); +} + +/** + * @brief Get the LSM6DSV gyroscope sensor raw axes + * @param Value pointer where the raw values of the axes are written + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSVStatusTypeDef LSM6DSV::Get_G_AxesRaw(int16_t *Value) +{ + lsm6dsv_axis3bit16_t data_raw; + + /* Read raw data values. */ + if (lsm6dsv_angular_rate_raw_get(®_ctx, data_raw.i16bit) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + /* Format the data. */ + Value[0] = data_raw.i16bit[0]; + Value[1] = data_raw.i16bit[1]; + Value[2] = data_raw.i16bit[2]; + + return LSM6DSV_OK; +} + +/** + * @brief Get the LSM6DSV gyroscope sensor raw axes when data available (Blocking) + * @param Value pointer where the raw values of the axes are written + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSVStatusTypeDef LSM6DSV::Get_G_AxesRaw_When_Aval(int16_t *Value) { + lsm6dsv_data_ready_t drdy; + do { + if (lsm6dsv_flag_data_ready_get(®_ctx, &drdy) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + } while (!drdy.drdy_gy); + + if (Get_G_AxesRaw(Value) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + return LSM6DSV_OK; +} + +/** + * @brief Get the LSM6DSV gyroscope sensor axes + * @param AngularRate pointer where the values of the axes are written + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSVStatusTypeDef LSM6DSV::Get_G_Axes(int32_t *AngularRate) +{ + lsm6dsv_axis3bit16_t data_raw; + float sensitivity = Convert_G_Sensitivity(gyro_fs); + + /* Read raw data values. */ + if (lsm6dsv_angular_rate_raw_get(®_ctx, data_raw.i16bit) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + /* Get LSM6DSV actual sensitivity. */ + if (sensitivity == 0.0f) { + return LSM6DSV_ERROR; + } + + /* Calculate the data. */ + AngularRate[0] = (int32_t)((float)((float)data_raw.i16bit[0] * sensitivity)); + AngularRate[1] = (int32_t)((float)((float)data_raw.i16bit[1] * sensitivity)); + AngularRate[2] = (int32_t)((float)((float)data_raw.i16bit[2] * sensitivity)); + + return LSM6DSV_OK; +} + +/** + * @brief Get the LSM6DSV GYRO data ready bit value + * @param Status the status of data ready bit + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSVStatusTypeDef LSM6DSV::Get_G_DRDY_Status(uint8_t *Status) +{ + lsm6dsv_all_sources_t val; + + if (lsm6dsv_all_sources_get(®_ctx, &val) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + *Status = val.drdy_gy; + return LSM6DSV_OK; +} + +/** + * @brief Set the LSM6DSV gyroscope power mode + * @param PowerMode Value of the powerMode + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSVStatusTypeDef LSM6DSV::Set_G_Power_Mode(uint8_t PowerMode) +{ + return (LSM6DSVStatusTypeDef)lsm6dsv_gy_mode_set( + ®_ctx, + (lsm6dsv_gy_mode_t)PowerMode + ); +} + +/** + * @brief Set the LSM6DSV gyroscope filter mode + * @param LowHighPassFlag 0/1 for setting low-pass/high-pass filter mode + * @param FilterMode Value of the filter Mode + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSVStatusTypeDef LSM6DSV::Set_G_Filter_Mode(uint8_t LowHighPassFlag, uint8_t FilterMode) +{ + if (LowHighPassFlag == 0) { + /*Set gyroscope low_pass 1 filter-mode*/ + /* Enable low-pass filter */ + if (lsm6dsv_filt_gy_lp1_set(®_ctx, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + if (lsm6dsv_filt_gy_lp1_bandwidth_set(®_ctx, (lsm6dsv_filt_gy_lp1_bandwidth_t)FilterMode) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + } else { + /*Set gyroscope high_pass filter-mode*/ + /* Enable high-pass filter */ + if (lsm6dsv_filt_gy_lp1_set(®_ctx, 0) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + if (lsm6dsv_filt_gy_lp1_bandwidth_set(®_ctx, (lsm6dsv_filt_gy_lp1_bandwidth_t)FilterMode) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + } + return LSM6DSV_OK; +} + +LSM6DSVStatusTypeDef LSM6DSV::Set_G_Bias(float x, float y, float z) +{ + lsm6dsv_sflp_gbias_t gbias; + gbias.gbias_x = x; + gbias.gbias_y = y; + gbias.gbias_z = z; + return (LSM6DSVStatusTypeDef)lsm6dsv_sflp_game_gbias_set( + ®_ctx, + &gbias + ); +} + +LSM6DSVStatusTypeDef LSM6DSV::Set_SFLP_ODR(float odr) { + lsm6dsv_sflp_data_rate_t rate = odr <= 15 ? LSM6DSV_SFLP_15Hz + : odr <= 30 ? LSM6DSV_SFLP_30Hz + : odr <= 60 ? LSM6DSV_SFLP_60Hz + : odr <= 120 ? LSM6DSV_SFLP_120Hz + : odr <= 240 ? LSM6DSV_SFLP_240Hz + : LSM6DSV_SFLP_480Hz; + + return (LSM6DSVStatusTypeDef)lsm6dsv_sflp_data_rate_set( + ®_ctx, + rate + ); +} + + +/** + * @brief Get the LSM6DSV register value + * @param Reg address to be read + * @param Data pointer where the value is written + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSVStatusTypeDef LSM6DSV::Read_Reg(uint8_t Reg, uint8_t *Data) +{ + if (lsm6dsv_read_reg(®_ctx, Reg, Data, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + return LSM6DSV_OK; +} + +/** + * @brief Set the LSM6DSV register value + * @param Reg address to be written + * @param Data value to be written + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSVStatusTypeDef LSM6DSV::Write_Reg(uint8_t Reg, uint8_t Data) +{ + if (lsm6dsv_write_reg(®_ctx, Reg, &Data, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + return LSM6DSV_OK; +} + +int32_t LSM6DSV_io_write(void *handle, uint8_t WriteAddr, const uint8_t *pBuffer, uint16_t nBytesToWrite) +{ + return ((LSM6DSV *)handle)->IO_Write(pBuffer, WriteAddr, nBytesToWrite); +} + +int32_t LSM6DSV_io_read(void *handle, uint8_t ReadAddr, uint8_t *pBuffer, uint16_t nBytesToRead) +{ + return ((LSM6DSV *)handle)->IO_Read(pBuffer, ReadAddr, nBytesToRead); +} + +void LSM6DSV_sleep(uint32_t ms) { + delay(ms); +} + +LSM6DSVStatusTypeDef LSM6DSV::Reset_Set(uint8_t flags) +{ + if (lsm6dsv_reset_set(®_ctx, (lsm6dsv_reset_t)flags) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + lsm6dsv_reset_t rst; + do { + if (lsm6dsv_reset_get(®_ctx, &rst) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + } while (rst != LSM6DSV_READY); + return LSM6DSV_OK; +} + +LSM6DSVStatusTypeDef LSM6DSV::Enable_SFLP_Block(bool enable) +{ + return (LSM6DSVStatusTypeDef)lsm6dsv_sflp_game_rotation_set( + ®_ctx, + enable ? PROPERTY_ENABLE : PROPERTY_DISABLE + ); +} + +LSM6DSVStatusTypeDef LSM6DSV::Enable_Block_Data_Update(bool enable) +{ + return (LSM6DSVStatusTypeDef)lsm6dsv_block_data_update_set( + ®_ctx, + enable ? PROPERTY_ENABLE : PROPERTY_DISABLE + ); +} + +/** + * @brief Enable register address automatically incremented during a multiple byte + access with a serial interface. + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSVStatusTypeDef LSM6DSV::Set_Auto_Increment(bool enable) +{ + return (LSM6DSVStatusTypeDef)lsm6dsv_auto_increment_set( + ®_ctx, + enable ? PROPERTY_ENABLE : PROPERTY_DISABLE + ); +} + +LSM6DSVStatusTypeDef LSM6DSV::Get_Temperature_Raw(int16_t * value) +{ + return (LSM6DSVStatusTypeDef)lsm6dsv_temperature_raw_get( + ®_ctx, + value + ); +} + +LSM6DSVStatusTypeDef LSM6DSV::Is_New_Temperature_Data(bool * available) +{ + lsm6dsv_status_reg_t status; + if (lsm6dsv_read_reg(®_ctx, LSM6DSV_STATUS_REG, (uint8_t *)&status, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + *available = status.tda; + return LSM6DSV_OK; +} + +LSM6DSVStatusTypeDef LSM6DSV::Set_SFLP_GBIAS(float x, float y, float z) { + lsm6dsv_sflp_gbias_t val = {0, 0, 0}; + val.gbias_x = x; + val.gbias_y = y; + val.gbias_z = z; + if(lsm6dsv_sflp_game_gbias_set(®_ctx, &val) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + return LSM6DSV_OK; +} + +LSM6DSVStatusTypeDef LSM6DSV::Enable_X_User_Offset() { + lsm6dsv_ctrl9_t ctrl9; + if (lsm6dsv_read_reg(®_ctx, LSM6DSV_CTRL9, (uint8_t *)&ctrl9, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + ctrl9.usr_off_on_out = true; //1 On, 0 Off + + if (lsm6dsv_write_reg(®_ctx, LSM6DSV_CTRL9, (uint8_t *)&ctrl9, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + // lsm6dsv_wake_up_ths_t wake_up_ths; + // if (lsm6dsv_read_reg(®_ctx, LSM6DSV_CTRL9, (uint8_t *)&wake_up_ths, 1) != LSM6DSV_OK) { + // return LSM6DSV_ERROR; + //} + //wake_up_ths.usr_off_on_wu = true; //1 On, 0 Off + + //if (lsm6dsv_write_reg(®_ctx, LSM6DSV_CTRL9, (uint8_t *)&wake_up_ths, 1) != LSM6DSV_OK) { + // return LSM6DSV_ERROR; + //} + + return LSM6DSV_OK; +} + +LSM6DSVStatusTypeDef LSM6DSV::Disable_X_User_Offset() { + lsm6dsv_ctrl9_t ctrl9; + if (lsm6dsv_read_reg(®_ctx, LSM6DSV_CTRL9, (uint8_t *)&ctrl9, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + ctrl9.usr_off_on_out = false; //1 On, 0 Off + + if (lsm6dsv_write_reg(®_ctx, LSM6DSV_CTRL9, (uint8_t *)&ctrl9, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + // lsm6dsv_wake_up_ths_t wake_up_ths; + // if (lsm6dsv_read_reg(®_ctx, LSM6DSV_CTRL9, (uint8_t *)&wake_up_ths, 1) != LSM6DSV_OK) { + // return LSM6DSV_ERROR; + //} + //wake_up_ths.usr_off_on_wu = false; //1 On, 0 Off + + //if (lsm6dsv_write_reg(®_ctx, LSM6DSV_CTRL9, (uint8_t *)&wake_up_ths, 1) != LSM6DSV_OK) { + // return LSM6DSV_ERROR; + //} + + return LSM6DSV_OK; +} + +LSM6DSVStatusTypeDef LSM6DSV::Set_X_User_Offset(float x, float y, float z) { + lsm6dsv_ctrl9_t ctrl9; + if (lsm6dsv_read_reg(®_ctx, LSM6DSV_CTRL9, (uint8_t *)&ctrl9, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + int8_t xyz[3]; + + if ( // about +- 2 G's for high and +- 0.124 G's for low + (x <= LSM6DSV_ACC_USR_OFF_W_LOW_MAX && x >= -LSM6DSV_ACC_USR_OFF_W_LOW_MAX) && + (y <= LSM6DSV_ACC_USR_OFF_W_LOW_MAX && y >= -LSM6DSV_ACC_USR_OFF_W_LOW_MAX) && + (z <= LSM6DSV_ACC_USR_OFF_W_LOW_MAX && z >= -LSM6DSV_ACC_USR_OFF_W_LOW_MAX) + ) { //Then we are under the low requirements + xyz[0] = (int8_t)(x / LSM6DSV_ACC_USR_OFF_W_LOW_LSB); + xyz[1] = (int8_t)(y / LSM6DSV_ACC_USR_OFF_W_LOW_LSB); + xyz[2] = (int8_t)(z / LSM6DSV_ACC_USR_OFF_W_LOW_LSB); + ctrl9.usr_off_w = false; //(0: 2^-10 g/LSB; 1: 2^-6 g/LSB) + } + + else if ( // about +- 2 G's for high and +- 0.124 G's for low + (x <= LSM6DSV_ACC_USR_OFF_W_HIGH_MAX && x >= -LSM6DSV_ACC_USR_OFF_W_HIGH_MAX) && + (y <= LSM6DSV_ACC_USR_OFF_W_HIGH_MAX && y >= -LSM6DSV_ACC_USR_OFF_W_HIGH_MAX) && + (z <= LSM6DSV_ACC_USR_OFF_W_HIGH_MAX && z >= -LSM6DSV_ACC_USR_OFF_W_HIGH_MAX) + ) { //Then we are under the high requirements + xyz[0] = (int8_t)(x / LSM6DSV_ACC_USR_OFF_W_HIGH_LSB); + xyz[1] = (int8_t)(y / LSM6DSV_ACC_USR_OFF_W_HIGH_LSB); + xyz[2] = (int8_t)(z / LSM6DSV_ACC_USR_OFF_W_HIGH_LSB); + ctrl9.usr_off_w = true; //(0: 2^-10 g/LSB; 1: 2^-6 g/LSB) + } else { + return LSM6DSV_ERROR; //Value too big + } + + + if (lsm6dsv_write_reg(®_ctx, LSM6DSV_CTRL9, (uint8_t *)&ctrl9, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + + //convert from float in G's to what it wants. Signed byte + printf("x: %d, y: %d, z: %d", xyz[0], xyz[1], xyz[2]); + if (lsm6dsv_write_reg(®_ctx, LSM6DSV_X_OFS_USR, (uint8_t*)&xyz, 3) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + return LSM6DSV_OK; +} + +uint32_t LSM6DSV::Half_Bits_To_Float_Bits(uint16_t h) +{ + uint16_t h_exp, h_sig; + uint32_t f_sgn, f_exp, f_sig; + + h_exp = (h&0x7c00u); + f_sgn = ((uint32_t)h&0x8000u) << 16; + switch (h_exp) { + case 0x0000u: /* 0 or subnormal */ + h_sig = (h&0x03ffu); + /* Signed zero */ + if (h_sig == 0) { + return f_sgn; + } + /* Subnormal */ + h_sig <<= 1; + while ((h_sig&0x0400u) == 0) { + h_sig <<= 1; + h_exp++; + } + f_exp = ((uint32_t)(127 - 15 - h_exp)) << 23; + f_sig = ((uint32_t)(h_sig&0x03ffu)) << 13; + return f_sgn + f_exp + f_sig; + case 0x7c00u: /* inf or NaN */ + /* All-ones exponent and a copy of the significand */ + return f_sgn + 0x7f800000u + (((uint32_t)(h&0x03ffu)) << 13); + default: /* normalized */ + /* Just need to adjust the exponent and shift */ + return f_sgn + (((uint32_t)(h&0x7fffu) + 0x1c000u) << 13); + } +} diff --git a/lib/LSM6DSV16X/LSM6DSV.h b/lib/LSM6DSV16X/LSM6DSV.h new file mode 100644 index 000000000..5cab610e0 --- /dev/null +++ b/lib/LSM6DSV16X/LSM6DSV.h @@ -0,0 +1,425 @@ +/** + ****************************************************************************** + * @file LSM6DSV.h + * @author SRA + * @version V1.5.1 + * @date July 2022 + * @brief Abstract Class of a LSM6DSV inertial measurement sensor. + ****************************************************************************** + * @attention + * + *

© COPYRIGHT(c) 2022 STMicroelectronics

+ * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * 3. Neither the name of STMicroelectronics nor the names of its contributors + * may be used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************** + */ + + +/* Prevent recursive inclusion -----------------------------------------------*/ + +#ifndef __LSM6DSV_H__ +#define __LSM6DSV_H__ + + +/* Includes ------------------------------------------------------------------*/ + +#include "Wire.h" +#include "SPI.h" +#include "lsm6dsv_reg.h" + + +/* Defines -------------------------------------------------------------------*/ + +#define LSM6DSV_ACC_SENSITIVITY_FS_2G 0.061f +#define LSM6DSV_ACC_SENSITIVITY_FS_4G 0.122f +#define LSM6DSV_ACC_SENSITIVITY_FS_8G 0.244f +#define LSM6DSV_ACC_SENSITIVITY_FS_16G 0.488f + +#define LSM6DSV_GYRO_SENSITIVITY_FS_125DPS 4.375f +#define LSM6DSV_GYRO_SENSITIVITY_FS_250DPS 8.750f +#define LSM6DSV_GYRO_SENSITIVITY_FS_500DPS 17.500f +#define LSM6DSV_GYRO_SENSITIVITY_FS_1000DPS 35.000f +#define LSM6DSV_GYRO_SENSITIVITY_FS_2000DPS 70.000f +#define LSM6DSV_GYRO_SENSITIVITY_FS_4000DPS 140.000f + +#define LSM6DSV_MIN_ST_LIMIT_mg 50.0f +#define LSM6DSV_MAX_ST_LIMIT_mg 1700.0f +#define LSM6DSV_MIN_ST_LIMIT_mdps 150000.0f +#define LSM6DSV_MAX_ST_LIMIT_mdps 700000.0f + +#define LSM6DSV_QVAR_GAIN 78.000f + +#define LSM6DSV_ACC_USR_OFF_W_HIGH_LSB (float)(pow(2, -6)) +#define LSM6DSV_ACC_USR_OFF_W_LOW_LSB (float)(pow(2, -10)) +#define LSM6DSV_ACC_USR_OFF_W_HIGH_MAX LSM6DSV_ACC_USR_OFF_W_HIGH_LSB * INT8_MAX +#define LSM6DSV_ACC_USR_OFF_W_LOW_MAX LSM6DSV_ACC_USR_OFF_W_LOW_LSB * INT8_MAX + + + + +//#define ENABLE_SPI +//#define I2C_LIB_DEBUG + +/* Typedefs ------------------------------------------------------------------*/ + +typedef enum { + LSM6DSV_OK = 0, + LSM6DSV_ERROR = -1 +} LSM6DSVStatusTypeDef; + +typedef enum { + LSM6DSV_INT1_PIN, + LSM6DSV_INT2_PIN, +} LSM6DSV_SensorIntPin_t; + +typedef struct { + unsigned int FreeFallStatus : 1; + unsigned int TapStatus : 1; + unsigned int DoubleTapStatus : 1; + unsigned int WakeUpStatus : 1; + unsigned int StepStatus : 1; + unsigned int TiltStatus : 1; + unsigned int D6DOrientationStatus : 1; + unsigned int SleepStatus : 1; +} LSM6DSV_Event_Status_t; + +typedef union { + int16_t i16bit[3]; + uint8_t u8bit[6]; +} lsm6dsv_axis3bit16_t; + +typedef union { + int16_t i16bit; + uint8_t u8bit[2]; +} lsm6dsv_axis1bit16_t; + +enum LSM6DSV_Reset { + LSM6DSV_RESET_GLOBAL = 0x1, + LSM6DSV_RESET_CAL_PARAM = 0x2, + LSM6DSV_RESET_CTRL_REGS = 0x4, +}; + +typedef enum { + LSM6DSV_ACC_HIGH_PERFORMANCE_MODE, + LSM6DSV_ACC_HIGH_ACCURACY_MODE, + LSM6DSV_ACC_NORMAL_MODE, + LSM6DSV_ACC_LOW_POWER_MODE1, + LSM6DSV_ACC_LOW_POWER_MODE2, + LSM6DSV_ACC_LOW_POWER_MODE3 +} LSM6DSV_ACC_Operating_Mode_t; + +typedef enum { + LSM6DSV_GYRO_HIGH_PERFORMANCE_MODE, + LSM6DSV_GYRO_HIGH_ACCURACY_MODE, + LSM6DSV_GYRO_SLEEP_MODE, + LSM6DSV_GYRO_LOW_POWER_MODE +} LSM6DSV_GYRO_Operating_Mode_t; + + +/* Class Declaration ---------------------------------------------------------*/ + +/** + * Abstract class of a LSM6DSV sensor. + */ +class LSM6DSV { + public: + LSM6DSV(TwoWire *i2c, uint8_t address = LSM6DSV_I2C_ADD_H); + LSM6DSV(SPIClass *spi, int cs_pin, uint32_t spi_speed = 2000000); + + LSM6DSVStatusTypeDef begin(); + LSM6DSVStatusTypeDef beginPreconfigured(); + LSM6DSVStatusTypeDef end(); + LSM6DSVStatusTypeDef ReadID(uint8_t *Id); + + LSM6DSVStatusTypeDef Enable_X(); + LSM6DSVStatusTypeDef Disable_X(); + LSM6DSVStatusTypeDef Enable_X_User_Offset(); + LSM6DSVStatusTypeDef Disable_X_User_Offset(); + LSM6DSVStatusTypeDef Get_X_Sensitivity(float *Sensitivity); + LSM6DSVStatusTypeDef Get_X_ODR(float *Odr); + LSM6DSVStatusTypeDef Set_X_ODR(float Odr, LSM6DSV_ACC_Operating_Mode_t Mode = LSM6DSV_ACC_HIGH_PERFORMANCE_MODE); + LSM6DSVStatusTypeDef Get_X_FS(int32_t *FullScale); + LSM6DSVStatusTypeDef Set_X_FS(int32_t FullScale); + LSM6DSVStatusTypeDef Get_X_AxesRaw(int16_t *Value); + LSM6DSVStatusTypeDef Get_X_AxesRaw_When_Aval(int16_t *Value); + LSM6DSVStatusTypeDef Get_X_Axes(int32_t *Acceleration); + LSM6DSVStatusTypeDef Get_X_DRDY_Status(uint8_t *Status); + LSM6DSVStatusTypeDef Get_X_Event_Status(LSM6DSV_Event_Status_t *Status); + LSM6DSVStatusTypeDef Set_X_Power_Mode(uint8_t PowerMode); + LSM6DSVStatusTypeDef Set_X_Filter_Mode(uint8_t LowHighPassFlag, uint8_t FilterMode); + LSM6DSVStatusTypeDef Set_X_User_Offset(float x, float y, float z); + + LSM6DSVStatusTypeDef Enable_G(); + LSM6DSVStatusTypeDef Disable_G(); + LSM6DSVStatusTypeDef Get_G_Sensitivity(float *Sensitivity); + LSM6DSVStatusTypeDef Get_G_ODR(float *Odr); + LSM6DSVStatusTypeDef Set_G_ODR(float Odr, LSM6DSV_GYRO_Operating_Mode_t Mode = LSM6DSV_GYRO_HIGH_PERFORMANCE_MODE); + LSM6DSVStatusTypeDef Get_G_FS(int32_t *FullScale); + LSM6DSVStatusTypeDef Set_G_FS(int32_t FullScale); + LSM6DSVStatusTypeDef Get_G_AxesRaw(int16_t *Value); + LSM6DSVStatusTypeDef Get_G_AxesRaw_When_Aval(int16_t *Value); + LSM6DSVStatusTypeDef Get_G_Axes(int32_t *AngularRate); + LSM6DSVStatusTypeDef Get_G_DRDY_Status(uint8_t *Status); + LSM6DSVStatusTypeDef Set_G_Power_Mode(uint8_t PowerMode); + LSM6DSVStatusTypeDef Set_G_Filter_Mode(uint8_t LowHighPassFlag, uint8_t FilterMode); + LSM6DSVStatusTypeDef Set_G_Bias(float x, float y, float z); + + LSM6DSVStatusTypeDef Test_IMU(uint8_t XTestType, uint8_t GTestType); + LSM6DSVStatusTypeDef Test_X_IMU(uint8_t TestType); + LSM6DSVStatusTypeDef Test_G_IMU(uint8_t TestType); + + LSM6DSVStatusTypeDef Get_T_ODR(float *Odr); + LSM6DSVStatusTypeDef Set_T_ODR(float Odr); + + LSM6DSVStatusTypeDef Enable_SFLP_Block(bool enable = true); + LSM6DSVStatusTypeDef Set_SFLP_ODR(float Odr); + LSM6DSVStatusTypeDef Set_SFLP_GBIAS(float x, float y, float z); + + LSM6DSVStatusTypeDef Enable_6D_Orientation(LSM6DSV_SensorIntPin_t IntPin); + LSM6DSVStatusTypeDef Disable_6D_Orientation(); + LSM6DSVStatusTypeDef Set_6D_Orientation_Threshold(uint8_t Threshold); + LSM6DSVStatusTypeDef Get_6D_Orientation_XL(uint8_t *XLow); + LSM6DSVStatusTypeDef Get_6D_Orientation_XH(uint8_t *XHigh); + LSM6DSVStatusTypeDef Get_6D_Orientation_YL(uint8_t *YLow); + LSM6DSVStatusTypeDef Get_6D_Orientation_YH(uint8_t *YHigh); + LSM6DSVStatusTypeDef Get_6D_Orientation_ZL(uint8_t *ZLow); + LSM6DSVStatusTypeDef Get_6D_Orientation_ZH(uint8_t *ZHigh); + + LSM6DSVStatusTypeDef Enable_Free_Fall_Detection(LSM6DSV_SensorIntPin_t IntPin); + LSM6DSVStatusTypeDef Disable_Free_Fall_Detection(); + LSM6DSVStatusTypeDef Set_Free_Fall_Threshold(uint8_t Threshold); + LSM6DSVStatusTypeDef Set_Free_Fall_Duration(uint8_t Duration); + + LSM6DSVStatusTypeDef Enable_Wake_Up_Detection(LSM6DSV_SensorIntPin_t IntPin); + LSM6DSVStatusTypeDef Disable_Wake_Up_Detection(); + LSM6DSVStatusTypeDef Set_Wake_Up_Threshold(uint32_t Threshold); + LSM6DSVStatusTypeDef Set_Wake_Up_Duration(uint8_t Duration); + + LSM6DSVStatusTypeDef Enable_Single_Tap_Detection(LSM6DSV_SensorIntPin_t IntPin); + LSM6DSVStatusTypeDef Disable_Single_Tap_Detection(); + LSM6DSVStatusTypeDef Enable_Double_Tap_Detection(LSM6DSV_SensorIntPin_t IntPin); + LSM6DSVStatusTypeDef Disable_Double_Tap_Detection(); + LSM6DSVStatusTypeDef Set_Tap_Threshold(uint8_t Threshold); + LSM6DSVStatusTypeDef Set_Tap_Shock_Time(uint8_t Time); + LSM6DSVStatusTypeDef Set_Tap_Quiet_Time(uint8_t Time); + LSM6DSVStatusTypeDef Set_Tap_Duration_Time(uint8_t Time); + + LSM6DSVStatusTypeDef Enable_Pedometer(LSM6DSV_SensorIntPin_t IntPin); + LSM6DSVStatusTypeDef Disable_Pedometer(); + LSM6DSVStatusTypeDef Get_Step_Count(uint16_t *StepCount); + LSM6DSVStatusTypeDef Step_Counter_Reset(); + + LSM6DSVStatusTypeDef Enable_Tilt_Detection(LSM6DSV_SensorIntPin_t IntPin); + LSM6DSVStatusTypeDef Disable_Tilt_Detection(); + + LSM6DSVStatusTypeDef FIFO_Reset(); + LSM6DSVStatusTypeDef FIFO_Get_Num_Samples(uint16_t *NumSamples); + LSM6DSVStatusTypeDef FIFO_Get_Full_Status(uint8_t *Status); + LSM6DSVStatusTypeDef FIFO_Set_INT1_FIFO_Full(uint8_t Status); + LSM6DSVStatusTypeDef FIFO_Set_INT2_FIFO_Full(uint8_t Status); + LSM6DSVStatusTypeDef FIFO_Set_Watermark_Level(uint8_t Watermark); + LSM6DSVStatusTypeDef FIFO_Set_Stop_On_Fth(uint8_t Status); + LSM6DSVStatusTypeDef FIFO_Set_Mode(uint8_t Mode); + LSM6DSVStatusTypeDef FIFO_Get_Tag(uint8_t *Tag); + LSM6DSVStatusTypeDef FIFO_Get_Data(uint8_t *Data); + LSM6DSVStatusTypeDef FIFO_Get_X_Axes(int32_t *Acceleration); + LSM6DSVStatusTypeDef FIFO_Set_X_BDR(float Bdr); + LSM6DSVStatusTypeDef FIFO_Get_G_Axes(int32_t *AngularVelocity); + LSM6DSVStatusTypeDef FIFO_Set_G_BDR(float Bdr); + LSM6DSVStatusTypeDef FIFO_Set_SFLP_Batch(bool GameRotation, bool Gravity, bool gBias); + LSM6DSVStatusTypeDef FIFO_Get_Status(lsm6dsv_fifo_status_t *Status); + LSM6DSVStatusTypeDef FIFO_Get_Gravity_Vector(float *data); + LSM6DSVStatusTypeDef FIFO_Get_Game_Vector(float *data); + LSM6DSVStatusTypeDef FIFO_Get_GBias_Vector(float *data); + LSM6DSVStatusTypeDef FIFO_Get_Timestamp(uint32_t *timestamp); + LSM6DSVStatusTypeDef FIFO_Set_Timestamp_Decimation(uint8_t decimation); + LSM6DSVStatusTypeDef Enable_Timestamp(); + + LSM6DSVStatusTypeDef Read_Reg(uint8_t Reg, uint8_t *Data); + LSM6DSVStatusTypeDef Write_Reg(uint8_t Reg, uint8_t Data); + + LSM6DSVStatusTypeDef Reset_Set(uint8_t flags); + + LSM6DSVStatusTypeDef Enable_Block_Data_Update(bool enable = true); + LSM6DSVStatusTypeDef Set_Auto_Increment(bool enable); + + LSM6DSVStatusTypeDef Get_Temperature_Raw(int16_t *value); + LSM6DSVStatusTypeDef Is_New_Temperature_Data(bool *available); + + uint32_t Half_Bits_To_Float_Bits(uint16_t half_bits); + + /** + * @brief Utility function to read data. + * @param pBuffer: pointer to data to be read. + * @param RegisterAddr: specifies internal address register to be read. + * @param NumByteToRead: number of bytes to be read. + * @retval 0 if ok, an error code otherwise. + */ + uint8_t IO_Read(uint8_t *pBuffer, uint8_t RegisterAddr, uint16_t NumByteToRead) + { +#ifdef ENABLE_SPI + if (dev_spi) { +//#ifdef esp32 + dev_spi->beginTransaction(SPISettings(spi_speed, SPI_MSBFIRST, SPI_MODE3)); +//#else + dev_spi->beginTransaction(SPISettings(spi_speed, MSBFIRST, SPI_MODE3)); +//#endif + digitalWrite(cs_pin, LOW); + + /* Write Reg Address */ + dev_spi->transfer(RegisterAddr | 0x80); + /* Read the data */ + for (uint16_t i = 0; i < NumByteToRead; i++) { + *(pBuffer + i) = dev_spi->transfer(0x00); + } + + digitalWrite(cs_pin, HIGH); + + dev_spi->endTransaction(); + + return 0; + } +#endif + + if (dev_i2c) { +#ifdef I2C_LIB_DEBUG + printf("\n\n[LSM LIB] Read register: 0x%02x, Byte Count: %d bytes", RegisterAddr, NumByteToRead); +#endif + dev_i2c->beginTransmission(((uint8_t)(((address) >> 1) & 0x7F))); + dev_i2c->write(RegisterAddr); + dev_i2c->endTransmission(false); + + dev_i2c->requestFrom(((uint8_t)(((address) >> 1) & 0x7F)), (uint8_t) NumByteToRead); + + int i = 0; + while (dev_i2c->available()) { + pBuffer[i] = dev_i2c->read(); +#ifdef I2C_LIB_DEBUG + printf("\n[LSM LIB] Register Read: 0x%02x, Data: 0x%02x", RegisterAddr + i, pBuffer[i]); +#endif + i++; + } + + return 0; + } + + return 1; + } + + /** + * @brief Utility function to write data. + * @param pBuffer: pointer to data to be written. + * @param RegisterAddr: specifies internal address register to be written. + * @param NumByteToWrite: number of bytes to write. + * @retval 0 if ok, an error code otherwise. + */ + uint8_t IO_Write(const uint8_t *pBuffer, uint8_t RegisterAddr, uint16_t NumByteToWrite) + { +#ifdef ENABLE_SPI + if (dev_spi) { +//#ifdef esp32 + dev_spi->beginTransaction(SPISettings(spi_speed, SPI_MSBFIRST, SPI_MODE3)); +//#else + dev_spi->beginTransaction(SPISettings(spi_speed, MSBFIRST, SPI_MODE3)); +//#endif + digitalWrite(cs_pin, LOW); + + /* Write Reg Address */ + dev_spi->transfer(RegisterAddr); + /* Write the data */ + for (uint16_t i = 0; i < NumByteToWrite; i++) { + dev_spi->transfer(pBuffer[i]); + } + + digitalWrite(cs_pin, HIGH); + + dev_spi->endTransaction(); + + return 0; + } +#endif + + if (dev_i2c) { +#ifdef I2C_LIB_DEBUG + printf("\n\n[LSM LIB] Write register: 0x%02x, Byte Count: %d bytes", RegisterAddr, NumByteToWrite); +#endif + dev_i2c->beginTransmission(((uint8_t)(((address) >> 1) & 0x7F))); + + dev_i2c->write(RegisterAddr); + for (uint16_t i = 0 ; i < NumByteToWrite ; i++) { +#ifdef I2C_LIB_DEBUG + printf("\n[LSM LIB] Register Wrote: 0x%02x, Data: 0x%02x", RegisterAddr + i, pBuffer[i]); +#endif + dev_i2c->write(pBuffer[i]); + } + + dev_i2c->endTransmission(true); + + return 0; + } + + return 1; + } + + private: + LSM6DSVStatusTypeDef Set_X_ODR_When_Enabled(float Odr); + LSM6DSVStatusTypeDef Set_X_ODR_When_Disabled(float Odr); + LSM6DSVStatusTypeDef Set_G_ODR_When_Enabled(float Odr); + LSM6DSVStatusTypeDef Set_G_ODR_When_Disabled(float Odr); + + float Convert_X_Sensitivity(lsm6dsv_xl_full_scale_t full_scale); + float Convert_G_Sensitivity(lsm6dsv_gy_full_scale_t full_scale); + + /* Helper classes. */ + TwoWire *dev_i2c; + SPIClass *dev_spi; + + /* Configuration */ + uint8_t address; + int cs_pin; + uint32_t spi_speed; + + lsm6dsv_data_rate_t acc_odr; + lsm6dsv_data_rate_t gyro_odr; + lsm6dsv_xl_full_scale_t acc_fs; + lsm6dsv_gy_full_scale_t gyro_fs; + lsm6dsv_fifo_mode_t fifo_mode; + + uint8_t acc_is_enabled; + uint8_t gyro_is_enabled; + uint8_t initialized; + stmdev_ctx_t reg_ctx; +}; + +#ifdef __cplusplus +extern "C" { +#endif +int32_t LSM6DSV_io_write(void *handle, uint8_t WriteAddr, const uint8_t *pBuffer, uint16_t nBytesToWrite); +int32_t LSM6DSV_io_read(void *handle, uint8_t ReadAddr, uint8_t *pBuffer, uint16_t nBytesToRead); +void LSM6DSV_sleep(uint32_t ms); +#ifdef __cplusplus +} +#endif + +#endif /* __LSM6DSV_H__ */ diff --git a/lib/LSM6DSV16X/LSM6DSV16X.cpp b/lib/LSM6DSV16X/LSM6DSV16X.cpp deleted file mode 100644 index f5801271b..000000000 --- a/lib/LSM6DSV16X/LSM6DSV16X.cpp +++ /dev/null @@ -1,3900 +0,0 @@ -/** - ****************************************************************************** - * @file LSM6DSV16X.cpp - * @author SRA - * @version V1.5.1 - * @date July 2022 - * @brief Implementation of a LSM6DSV16X inertial measurement sensor. - ****************************************************************************** - * @attention - * - *

© COPYRIGHT(c) 2022 STMicroelectronics

- * - * Redistribution and use in source and binary forms, with or without modification, - * are permitted provided that the following conditions are met: - * 1. Redistributions of source code must retain the above copyright notice, - * this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * 3. Neither the name of STMicroelectronics nor the names of its contributors - * may be used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************** - */ - - -/* Includes ------------------------------------------------------------------*/ - -#include "LSM6DSV16X.h" - - -/* Class Implementation ------------------------------------------------------*/ - -/** Constructor - * @param i2c object of an helper class which handles the I2C peripheral - * @param address the address of the component's instance - */ -LSM6DSV16X::LSM6DSV16X(TwoWire *i2c, uint8_t address) : dev_i2c(i2c), address(address) -{ - reg_ctx.write_reg = LSM6DSV16X_io_write; - reg_ctx.read_reg = LSM6DSV16X_io_read; - reg_ctx.mdelay = LSM6DSV16X_sleep; - reg_ctx.handle = (void *)this; - dev_spi = NULL; - acc_is_enabled = 0L; - gyro_is_enabled = 0L; -} - -/** Constructor - * @param spi object of an helper class which handles the SPI peripheral - * @param cs_pin the chip select pin - * @param spi_speed the SPI speed - */ -LSM6DSV16X::LSM6DSV16X(SPIClass *spi, int cs_pin, uint32_t spi_speed) : dev_spi(spi), cs_pin(cs_pin), spi_speed(spi_speed) -{ - reg_ctx.write_reg = LSM6DSV16X_io_write; - reg_ctx.read_reg = LSM6DSV16X_io_read; - reg_ctx.handle = (void *)this; - dev_i2c = NULL; - acc_is_enabled = 0L; - gyro_is_enabled = 0L; -} - -/** - * @brief Initialize the LSM6DSV16X sensor - * @retval 0 in case of success, an error code otherwise - */ -LSM6DSV16XStatusTypeDef LSM6DSV16X::begin() -{ - if (dev_spi) { - // Configure CS pin - pinMode(cs_pin, OUTPUT); - digitalWrite(cs_pin, HIGH); - } - - /* Enable register address automatically incremented during a multiple byte - access with a serial interface. */ - if (lsm6dsv16x_auto_increment_set(®_ctx, PROPERTY_ENABLE) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - /* Enable BDU */ - if (Enable_Block_Data_Update() != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - /* FIFO mode selection */ - fifo_mode = LSM6DSV16X_BYPASS_MODE; - if (lsm6dsv16x_fifo_mode_set(®_ctx, fifo_mode) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - /* Select default output data rate. */ - acc_odr = LSM6DSV16X_ODR_AT_120Hz; - - /* Output data rate selection - power down. */ - if (lsm6dsv16x_xl_data_rate_set(®_ctx, LSM6DSV16X_ODR_OFF) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - /* Full scale selection. */ - acc_fs = LSM6DSV16X_2g; - if (lsm6dsv16x_xl_full_scale_set(®_ctx, acc_fs) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - /* Select default output data rate. */ - gyro_odr = LSM6DSV16X_ODR_AT_120Hz; - - /* Output data rate selection - power down. */ - if (lsm6dsv16x_gy_data_rate_set(®_ctx, LSM6DSV16X_ODR_OFF) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - /* Full scale selection. */ - gyro_fs = LSM6DSV16X_2000dps; - if (lsm6dsv16x_gy_full_scale_set(®_ctx, gyro_fs) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - initialized = 1U; - - return LSM6DSV16X_OK; -} - -/** - * @brief Initialize the LSM6DSV16X sensor when it has already been configured - * @retval 0 in case of success, an error code otherwise - */ -LSM6DSV16XStatusTypeDef LSM6DSV16X::beginPreconfigured() -{ - if (dev_spi) { - // Configure CS pin - pinMode(cs_pin, OUTPUT); - digitalWrite(cs_pin, HIGH); - } - - initialized = 1U; - - return LSM6DSV16X_OK; -} - -/** - * @brief Deinitialize the LSM6DSV16X sensor - * @retval 0 in case of success, an error code otherwise - */ -LSM6DSV16XStatusTypeDef LSM6DSV16X::end() -{ - /* Disable the component */ - if (Disable_X() != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - if (Disable_G() != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - /* Reset output data rate. */ - acc_odr = LSM6DSV16X_ODR_OFF; - gyro_odr = LSM6DSV16X_ODR_OFF; - - initialized = 0U; - - return LSM6DSV16X_OK; -} - -/** - * @brief Read component ID - * @param Id the WHO_AM_I value - * @retval 0 in case of success, an error code otherwise - */ -LSM6DSV16XStatusTypeDef LSM6DSV16X::ReadID(uint8_t *Id) -{ - if (lsm6dsv16x_device_id_get(®_ctx, Id) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - return LSM6DSV16X_OK; -} - -/** - * @brief Enable the LSM6DSV16X accelerometer sensor - * @retval 0 in case of success, an error code otherwise - */ -LSM6DSV16XStatusTypeDef LSM6DSV16X::Enable_X() -{ - /* Check if the component is already enabled */ - if (acc_is_enabled == 1U) { - return LSM6DSV16X_OK; - } - - /* Output data rate selection. */ - if (lsm6dsv16x_xl_data_rate_set(®_ctx, acc_odr) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - acc_is_enabled = 1U; - - return LSM6DSV16X_OK; -} - -/** - * @brief Disable the LSM6DSV16X accelerometer sensor - * @retval 0 in case of success, an error code otherwise - */ -LSM6DSV16XStatusTypeDef LSM6DSV16X::Disable_X() -{ - /* Check if the component is already disabled */ - if (acc_is_enabled == 0U) { - return LSM6DSV16X_OK; - } - - /* Get current output data rate. */ - if (lsm6dsv16x_xl_data_rate_get(®_ctx, &acc_odr) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - /* Output data rate selection - power down. */ - if (lsm6dsv16x_xl_data_rate_set(®_ctx, LSM6DSV16X_ODR_OFF) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - acc_is_enabled = 0U; - - return LSM6DSV16X_OK; -} - -/** - * @brief Get the LSM6DSV16X accelerometer sensor sensitivity - * @param Sensitivity pointer - * @retval 0 in case of success, an error code otherwise - */ -LSM6DSV16XStatusTypeDef LSM6DSV16X::Get_X_Sensitivity(float *Sensitivity) -{ - lsm6dsv16x_xl_full_scale_t full_scale; - - /* Read actual full scale selection from sensor. */ - if (lsm6dsv16x_xl_full_scale_get(®_ctx, &full_scale) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - *Sensitivity = Convert_X_Sensitivity(full_scale); - if (*Sensitivity == 0.0f) { - return LSM6DSV16X_ERROR; - } - return LSM6DSV16X_OK; -} - -float LSM6DSV16X::Convert_X_Sensitivity(lsm6dsv16x_xl_full_scale_t full_scale) { - float Sensitivity = 0.0f; - /* Store the Sensitivity based on actual full scale. */ - switch (full_scale) { - case LSM6DSV16X_2g: - Sensitivity = LSM6DSV16X_ACC_SENSITIVITY_FS_2G; - break; - - case LSM6DSV16X_4g: - Sensitivity = LSM6DSV16X_ACC_SENSITIVITY_FS_4G; - break; - - case LSM6DSV16X_8g: - Sensitivity = LSM6DSV16X_ACC_SENSITIVITY_FS_8G; - break; - - case LSM6DSV16X_16g: - Sensitivity = LSM6DSV16X_ACC_SENSITIVITY_FS_16G; - break; - } - return Sensitivity; -} - -/** - * @brief Get the LSM6DSV16X accelerometer sensor output data rate - * @param Odr pointer where the output data rate is written - * @retval 0 in case of success, an error code otherwise - */ -LSM6DSV16XStatusTypeDef LSM6DSV16X::Get_X_ODR(float *Odr) -{ - LSM6DSV16XStatusTypeDef ret = LSM6DSV16X_OK; - lsm6dsv16x_data_rate_t odr_low_level; - - /* Get current output data rate. */ - if (lsm6dsv16x_xl_data_rate_get(®_ctx, &odr_low_level) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - switch (odr_low_level) { - case LSM6DSV16X_ODR_OFF: - *Odr = 0.0f; - break; - - case LSM6DSV16X_ODR_AT_1Hz875: - *Odr = 1.875f; - break; - - case LSM6DSV16X_ODR_AT_7Hz5: - *Odr = 7.5f; - break; - - case LSM6DSV16X_ODR_AT_15Hz: - *Odr = 15.0f; - break; - - case LSM6DSV16X_ODR_AT_30Hz: - *Odr = 30.0f; - break; - - case LSM6DSV16X_ODR_AT_60Hz: - *Odr = 60.0f; - break; - - case LSM6DSV16X_ODR_AT_120Hz: - *Odr = 120.0f; - break; - - case LSM6DSV16X_ODR_AT_240Hz: - *Odr = 240.0f; - break; - - case LSM6DSV16X_ODR_AT_480Hz: - *Odr = 480.0f; - break; - - case LSM6DSV16X_ODR_AT_960Hz: - *Odr = 960.0f; - break; - - case LSM6DSV16X_ODR_AT_1920Hz: - *Odr = 1920.0f; - break; - - case LSM6DSV16X_ODR_AT_3840Hz: - *Odr = 3840.0f; - break; - - case LSM6DSV16X_ODR_AT_7680Hz: - *Odr = 7680.0f; - break; - - default: - ret = LSM6DSV16X_ERROR; - break; - } - - return ret; -} - -/** - * @brief Get the LSM6DSV16X accelerometer sensor axes - * @param Acceleration pointer where the values of the axes are written - * @retval 0 in case of success, an error code otherwise - */ -LSM6DSV16XStatusTypeDef LSM6DSV16X::Get_X_Axes(int32_t *Acceleration) -{ - lsm6dsv16x_axis3bit16_t data_raw; - float sensitivity = Convert_X_Sensitivity(acc_fs); - - /* Read raw data values. */ - if (lsm6dsv16x_acceleration_raw_get(®_ctx, data_raw.i16bit) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - /* Get LSM6DSV16X actual sensitivity. */ - if (sensitivity == 0.0f) { - return LSM6DSV16X_ERROR; - } - - /* Calculate the data. */ - Acceleration[0] = (int32_t)((float)((float)data_raw.i16bit[0] * sensitivity)); - Acceleration[1] = (int32_t)((float)((float)data_raw.i16bit[1] * sensitivity)); - Acceleration[2] = (int32_t)((float)((float)data_raw.i16bit[2] * sensitivity)); - - return LSM6DSV16X_OK; -} - -/** - * @brief Set the LSM6DSV16X accelerometer sensor output data rate - * @param Odr the output data rate value to be set - * @retval 0 in case of success, an error code otherwise - */ -LSM6DSV16XStatusTypeDef LSM6DSV16X::Set_X_ODR(float Odr, LSM6DSV16X_ACC_Operating_Mode_t Mode) -{ - switch (Mode) { - case LSM6DSV16X_ACC_HIGH_PERFORMANCE_MODE: { - if (lsm6dsv16x_xl_mode_set(®_ctx, LSM6DSV16X_XL_HIGH_PERFORMANCE_MD) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - /* Valid ODR: 7.5Hz <= Odr <= 7.68kHz */ - Odr = (Odr < 7.5f) ? 7.5f - : (Odr > 7680.0f) ? 7680.0f - : Odr; - break; - } - - case LSM6DSV16X_ACC_HIGH_ACCURACY_MODE: - // TODO: Not implemented. - // NOTE: According to datasheet, section `6.5 High-accuracy ODR mode`: - // "... the other sensor also has to be configured in high-accuracy ODR (HAODR) mode." - return LSM6DSV16X_ERROR; - - case LSM6DSV16X_ACC_NORMAL_MODE: { - if (lsm6dsv16x_xl_mode_set(®_ctx, LSM6DSV16X_XL_NORMAL_MD) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - /* Valid ODR: 7.5Hz <= Odr <= 1.92kHz */ - Odr = (Odr < 7.5f) ? 7.5f - : (Odr > 1920.0f) ? 1920.0f - : Odr; - break; - } - - case LSM6DSV16X_ACC_LOW_POWER_MODE1: { - if (lsm6dsv16x_xl_mode_set(®_ctx, LSM6DSV16X_XL_LOW_POWER_2_AVG_MD) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - /* Valid ODR: 1.875Hz; 15Hz <= Odr <= 240kHz */ - Odr = (Odr == 1.875f) ? Odr - : (Odr < 15.000f) ? 15.0f - : (Odr > 240.000f) ? 240.0f - : Odr; - break; - } - - case LSM6DSV16X_ACC_LOW_POWER_MODE2: { - if (lsm6dsv16x_xl_mode_set(®_ctx, LSM6DSV16X_XL_LOW_POWER_4_AVG_MD) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - /* Valid ODR: 1.875Hz; 15Hz <= Odr <= 240kHz */ - Odr = (Odr == 1.875f) ? Odr - : (Odr < 15.000f) ? 15.0f - : (Odr > 240.000f) ? 240.0f - : Odr; - break; - } - - case LSM6DSV16X_ACC_LOW_POWER_MODE3: { - if (lsm6dsv16x_xl_mode_set(®_ctx, LSM6DSV16X_XL_LOW_POWER_8_AVG_MD) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - /* Valid ODR: 1.875Hz; 15Hz <= Odr <= 240kHz */ - Odr = (Odr == 1.875f) ? Odr - : (Odr < 15.000f) ? 15.0f - : (Odr > 240.000f) ? 240.0f - : Odr; - break; - } - - default: - return LSM6DSV16X_ERROR; - } - - if (acc_is_enabled == 1U) { - return Set_X_ODR_When_Enabled(Odr); - } else { - return Set_X_ODR_When_Disabled(Odr); - } -} - -/** - * @brief Set the LSM6DSV16X accelerometer sensor output data rate when enabled - * @param Odr the functional output data rate to be set - * @retval 0 in case of success, an error code otherwise - */ -LSM6DSV16XStatusTypeDef LSM6DSV16X::Set_X_ODR_When_Enabled(float Odr) -{ - lsm6dsv16x_data_rate_t new_odr; - - new_odr = (Odr <= 1.875f) ? LSM6DSV16X_ODR_AT_1Hz875 - : (Odr <= 7.5f) ? LSM6DSV16X_ODR_AT_7Hz5 - : (Odr <= 15.0f) ? LSM6DSV16X_ODR_AT_15Hz - : (Odr <= 30.0f) ? LSM6DSV16X_ODR_AT_30Hz - : (Odr <= 60.0f) ? LSM6DSV16X_ODR_AT_60Hz - : (Odr <= 120.0f) ? LSM6DSV16X_ODR_AT_120Hz - : (Odr <= 240.0f) ? LSM6DSV16X_ODR_AT_240Hz - : (Odr <= 480.0f) ? LSM6DSV16X_ODR_AT_480Hz - : (Odr <= 960.0f) ? LSM6DSV16X_ODR_AT_960Hz - : (Odr <= 1920.0f) ? LSM6DSV16X_ODR_AT_1920Hz - : (Odr <= 3840.0f) ? LSM6DSV16X_ODR_AT_3840Hz - : LSM6DSV16X_ODR_AT_7680Hz; - - /* Output data rate selection. */ - if (lsm6dsv16x_xl_data_rate_set(®_ctx, new_odr) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - return LSM6DSV16X_OK; -} - -/** - * @brief Set the LSM6DSV16X accelerometer sensor output data rate when disabled - * @param Odr the functional output data rate to be set - * @retval 0 in case of success, an error code otherwise - */ -LSM6DSV16XStatusTypeDef LSM6DSV16X::Set_X_ODR_When_Disabled(float Odr) -{ - acc_odr = (Odr <= 1.875f) ? LSM6DSV16X_ODR_AT_1Hz875 - : (Odr <= 7.5f) ? LSM6DSV16X_ODR_AT_7Hz5 - : (Odr <= 15.0f) ? LSM6DSV16X_ODR_AT_15Hz - : (Odr <= 30.0f) ? LSM6DSV16X_ODR_AT_30Hz - : (Odr <= 60.0f) ? LSM6DSV16X_ODR_AT_60Hz - : (Odr <= 120.0f) ? LSM6DSV16X_ODR_AT_120Hz - : (Odr <= 240.0f) ? LSM6DSV16X_ODR_AT_240Hz - : (Odr <= 480.0f) ? LSM6DSV16X_ODR_AT_480Hz - : (Odr <= 960.0f) ? LSM6DSV16X_ODR_AT_960Hz - : (Odr <= 1920.0f) ? LSM6DSV16X_ODR_AT_1920Hz - : (Odr <= 3840.0f) ? LSM6DSV16X_ODR_AT_3840Hz - : LSM6DSV16X_ODR_AT_7680Hz; - - return LSM6DSV16X_OK; -} - -/** - * @brief Get the LSM6DSV16X accelerometer sensor full scale - * @param FullScale pointer where the full scale is written - * @retval 0 in case of success, an error code otherwise - */ -LSM6DSV16XStatusTypeDef LSM6DSV16X::Get_X_FS(int32_t *FullScale) -{ - LSM6DSV16XStatusTypeDef ret = LSM6DSV16X_OK; - lsm6dsv16x_xl_full_scale_t fs_low_level; - - /* Read actual full scale selection from sensor. */ - if (lsm6dsv16x_xl_full_scale_get(®_ctx, &fs_low_level) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - switch (fs_low_level) { - case LSM6DSV16X_2g: - *FullScale = 2; - break; - - case LSM6DSV16X_4g: - *FullScale = 4; - break; - - case LSM6DSV16X_8g: - *FullScale = 8; - break; - - case LSM6DSV16X_16g: - *FullScale = 16; - break; - - default: - ret = LSM6DSV16X_ERROR; - break; - } - - return ret; -} - -/** - * @brief Set the LSM6DSV16X accelerometer sensor full scale - * @param FullScale the functional full scale to be set - * @retval 0 in case of success, an error code otherwise - */ -LSM6DSV16XStatusTypeDef LSM6DSV16X::Set_X_FS(int32_t FullScale) -{ - lsm6dsv16x_xl_full_scale_t new_fs; - - /* Seems like MISRA C-2012 rule 14.3a violation but only from single file statical analysis point of view because - the parameter passed to the function is not known at the moment of analysis */ - new_fs = (FullScale <= 2) ? LSM6DSV16X_2g - : (FullScale <= 4) ? LSM6DSV16X_4g - : (FullScale <= 8) ? LSM6DSV16X_8g - : LSM6DSV16X_16g; - - if (new_fs == acc_fs) { - return LSM6DSV16X_OK; - } - acc_fs = new_fs; - - if (lsm6dsv16x_xl_full_scale_set(®_ctx, acc_fs) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - return LSM6DSV16X_OK; -} - -/** - * @brief Get the LSM6DSV16X accelerometer sensor raw axes - * @param Value pointer where the raw values of the axes are written - * @retval 0 in case of success, an error code otherwise - */ -LSM6DSV16XStatusTypeDef LSM6DSV16X::Get_X_AxesRaw(int16_t *Value) -{ - lsm6dsv16x_axis3bit16_t data_raw; - - /* Read raw data values. */ - if (lsm6dsv16x_acceleration_raw_get(®_ctx, data_raw.i16bit) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - /* Format the data. */ - Value[0] = data_raw.i16bit[0]; - Value[1] = data_raw.i16bit[1]; - Value[2] = data_raw.i16bit[2]; - - return LSM6DSV16X_OK; -} - -/** - * @brief Get the LSM6DSV16X accelerometer sensor raw axes when avaiable (Blocking) - * @param Value pointer where the raw values of the axes are written - * @retval 0 in case of success, an error code otherwise - */ -LSM6DSV16XStatusTypeDef LSM6DSV16X::Get_X_AxesRaw_When_Aval(int16_t *Value) { - lsm6dsv16x_data_ready_t drdy; - do { - if (lsm6dsv16x_flag_data_ready_get(®_ctx, &drdy) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - } while (!drdy.drdy_xl); - - if (Get_X_AxesRaw(Value) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - return LSM6DSV16X_OK; -} - -/** - * @brief Get the LSM6DSV16X ACC data ready bit value - * @param Status the status of data ready bit - * @retval 0 in case of success, an error code otherwise - */ -LSM6DSV16XStatusTypeDef LSM6DSV16X::Get_X_DRDY_Status(uint8_t *Status) -{ - lsm6dsv16x_all_sources_t val; - - if (lsm6dsv16x_all_sources_get(®_ctx, &val) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - *Status = val.drdy_xl; - return LSM6DSV16X_OK; -} - -/** - * @brief Get the status of all hardware events - * @param Status the status of all hardware events - * @retval 0 in case of success, an error code otherwise - */ -LSM6DSV16XStatusTypeDef LSM6DSV16X::Get_X_Event_Status(LSM6DSV16X_Event_Status_t *Status) -{ - lsm6dsv16x_emb_func_status_t emb_func_status; - lsm6dsv16x_wake_up_src_t wake_up_src; - lsm6dsv16x_tap_src_t tap_src; - lsm6dsv16x_d6d_src_t d6d_src; - lsm6dsv16x_emb_func_src_t func_src; - lsm6dsv16x_md1_cfg_t md1_cfg; - lsm6dsv16x_md2_cfg_t md2_cfg; - lsm6dsv16x_emb_func_int1_t int1_ctrl; - lsm6dsv16x_emb_func_int2_t int2_ctrl; - - - (void)memset((void *)Status, 0x0, sizeof(LSM6DSV16X_Event_Status_t)); - - if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_WAKE_UP_SRC, (uint8_t *)&wake_up_src, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_TAP_SRC, (uint8_t *)&tap_src, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_D6D_SRC, (uint8_t *)&d6d_src, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - if (lsm6dsv16x_mem_bank_set(®_ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_EMB_FUNC_SRC, (uint8_t *)&func_src, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_EMB_FUNC_INT1, (uint8_t *)&int1_ctrl, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_EMB_FUNC_INT2, (uint8_t *)&int2_ctrl, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_EMB_FUNC_STATUS, (uint8_t *)&emb_func_status, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - if (lsm6dsv16x_mem_bank_set(®_ctx, LSM6DSV16X_MAIN_MEM_BANK) != 0) { - return LSM6DSV16X_ERROR; - } - - if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_MD1_CFG, (uint8_t *)&md1_cfg, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_MD2_CFG, (uint8_t *)&md2_cfg, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - - if ((md1_cfg.int1_ff == 1U) || (md2_cfg.int2_ff == 1U)) { - if (wake_up_src.ff_ia == 1U) { - Status->FreeFallStatus = 1; - } - } - - if ((md1_cfg.int1_wu == 1U) || (md2_cfg.int2_wu == 1U)) { - if (wake_up_src.wu_ia == 1U) { - Status->WakeUpStatus = 1; - } - } - - if ((md1_cfg.int1_single_tap == 1U) || (md2_cfg.int2_single_tap == 1U)) { - if (tap_src.single_tap == 1U) { - Status->TapStatus = 1; - } - } - - if ((md1_cfg.int1_double_tap == 1U) || (md2_cfg.int2_double_tap == 1U)) { - if (tap_src.double_tap == 1U) { - Status->DoubleTapStatus = 1; - } - } - - if ((md1_cfg.int1_6d == 1U) || (md2_cfg.int2_6d == 1U)) { - if (d6d_src.d6d_ia == 1U) { - Status->D6DOrientationStatus = 1; - } - } - - if (int1_ctrl.int1_step_detector == 1U || int2_ctrl.int2_step_detector == 1U) { - if (func_src.step_detected == 1U) { - Status->StepStatus = 1; - } - } - - if ((int1_ctrl.int1_tilt == 1U) || (int2_ctrl.int2_tilt == 1U)) { - if (emb_func_status.is_tilt == 1U) { - Status->TiltStatus = 1; - } - } - - return LSM6DSV16X_OK; -} - -/** - * @brief Set the LSM6DSV16X accelerometer power mode - * @param PowerMode Value of the powerMode - * @retval 0 in case of success, an error code otherwise - */ -LSM6DSV16XStatusTypeDef LSM6DSV16X::Set_X_Power_Mode(uint8_t PowerMode) -{ - if (lsm6dsv16x_xl_mode_set(®_ctx, (lsm6dsv16x_xl_mode_t)PowerMode) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - return LSM6DSV16X_OK; -} - -/** - * @brief Set the LSM6DSV16X accelerometer filter mode - * @param LowHighPassFlag 0/1 for setting low-pass/high-pass filter mode - * @param FilterMode Value of the filter Mode - * @retval 0 in case of success, an error code otherwise - */ -LSM6DSV16XStatusTypeDef LSM6DSV16X::Set_X_Filter_Mode(uint8_t LowHighPassFlag, uint8_t FilterMode) -{ - if (LowHighPassFlag == 0) { - /*Set accelerometer low_pass filter-mode*/ - - /*Set to 1 LPF2 bit (CTRL8_XL)*/ - if (lsm6dsv16x_filt_xl_lp2_set(®_ctx, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - if (lsm6dsv16x_filt_xl_lp2_bandwidth_set(®_ctx, (lsm6dsv16x_filt_xl_lp2_bandwidth_t)FilterMode) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - } else { - if (lsm6dsv16x_filt_xl_lp2_set(®_ctx, 0) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - /*Set accelerometer high_pass filter-mode*/ - if (lsm6dsv16x_filt_xl_lp2_bandwidth_set(®_ctx, (lsm6dsv16x_filt_xl_lp2_bandwidth_t)FilterMode) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - } - return LSM6DSV16X_OK; -} - -/** - * @brief Runs the ST specified accelerometer and gyroscope self test - * @param XTestType LSM6DSV16X_XL_ST_DISABLE = 0x0, LSM6DSV16X_XL_ST_POSITIVE = 0x1, LSM6DSV16X_XL_ST_NEGATIVE = 0x2 - * @param GTestType LSM6DSV16X_GY_ST_DISABLE = 0x0, LSM6DSV16X_GY_ST_POSITIVE = 0x1, LSM6DSV16X_GY_ST_NEGATIVE = 0x2 - * @retval 0 in case of success, an error code otherwise - */ -LSM6DSV16XStatusTypeDef LSM6DSV16X::Test_IMU(uint8_t XTestType, uint8_t GTestType) -{ - uint8_t whoamI; - - if (ReadID(&whoamI) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - if (whoamI != LSM6DSV16X_ID) { - return LSM6DSV16X_ERROR; - } - - if (Test_X_IMU(XTestType) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - if (Test_G_IMU(GTestType) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - printf("IMU Test OK\n"); - return LSM6DSV16X_OK; -} - -/** - * @brief Runs the ST specified accelerometer self test - * @param TestType LSM6DSV16X_XL_ST_DISABLE = 0x0, LSM6DSV16X_XL_ST_POSITIVE = 0x1, LSM6DSV16X_XL_ST_NEGATIVE = 0x2 - * @retval 0 in case of success, an error code otherwise - */ -LSM6DSV16XStatusTypeDef LSM6DSV16X::Test_X_IMU(uint8_t TestType) -{ - int16_t data_raw[3]; - float val_st_off[3]; - float val_st_on[3]; - float test_val[3]; - - if (Reset_Set(LSM6DSV16X_RESTORE_CTRL_REGS) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - if (lsm6dsv16x_block_data_update_set(®_ctx, PROPERTY_ENABLE) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - /* - * Accelerometer Self Test - */ - if (lsm6dsv16x_xl_data_rate_set(®_ctx, LSM6DSV16X_ODR_AT_60Hz) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - if (lsm6dsv16x_xl_full_scale_set(®_ctx, LSM6DSV16X_4g) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - delay(100); //Wait for Accelerometer to stabalize; - memset(val_st_off, 0x00, 3 * sizeof(float)); - memset(val_st_on, 0x00, 3 * sizeof(float)); - - /*Ignore First Data*/ - if (Get_X_AxesRaw_When_Aval(data_raw) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - for (uint8_t i = 0; i < 5; i++) { - if (Get_X_AxesRaw_When_Aval(data_raw) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - /*Average the data in each axis*/ - for (uint8_t j = 0; j < 3; j++) { - val_st_off[j] += lsm6dsv16x_from_fs4_to_mg(data_raw[j]); - } - } - - /* Calculate the mg average values */ - for (uint8_t i = 0; i < 3; i++) { - val_st_off[i] /= 5.0f; - } - - if (lsm6dsv16x_xl_self_test_set(®_ctx, (lsm6dsv16x_xl_self_test_t)TestType) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - delay(100); - - /*Ignore First Data*/ - if (Get_X_AxesRaw_When_Aval(data_raw) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - for (uint8_t i = 0; i < 5; i++) { - if (Get_X_AxesRaw_When_Aval(data_raw) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - /*Average the data in each axis*/ - for (uint8_t j = 0; j < 3; j++) { - val_st_on[j] += lsm6dsv16x_from_fs4_to_mg(data_raw[j]); - } - } - - /* Calculate the mg average values */ - for (uint8_t i = 0; i < 3; i++) { - val_st_on[i] /= 5.0f; - } - - /* Calculate the mg values for self test */ - for (uint8_t i = 0; i < 3; i++) { - test_val[i] = fabs((val_st_on[i] - val_st_off[i])); - } - - for (uint8_t i = 0; i < 3; i++) { - if (( LSM6DSV16X_MIN_ST_LIMIT_mg > test_val[i] ) || ( test_val[i] > LSM6DSV16X_MAX_ST_LIMIT_mg)) - return LSM6DSV16X_ERROR; - } - - if (lsm6dsv16x_xl_self_test_set(®_ctx, LSM6DSV16X_XL_ST_DISABLE) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - if (lsm6dsv16x_xl_data_rate_set(®_ctx, LSM6DSV16X_ODR_OFF) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - return LSM6DSV16X_OK; -} - -/** - * @brief Runs the ST specified self test on the acceleration axis of the IMU - * @param TestType LSM6DSV16X_GY_ST_DISABLE = 0x0, LSM6DSV16X_GY_ST_POSITIVE = 0x1, LSM6DSV16X_GY_ST_NEGATIVE = 0x2 - * @retval 0 in case of success, an error code otherwise - */ -LSM6DSV16XStatusTypeDef LSM6DSV16X::Test_G_IMU(uint8_t TestType = LSM6DSV16X_GY_ST_POSITIVE) -{ - int16_t data_raw[3]; - float test_val[3]; - - if (Reset_Set(LSM6DSV16X_RESTORE_CTRL_REGS) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - if (lsm6dsv16x_block_data_update_set(®_ctx, PROPERTY_ENABLE) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - /* - * Gyroscope Self Test - */ - - if (lsm6dsv16x_gy_data_rate_set(®_ctx, LSM6DSV16X_ODR_AT_240Hz) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - if (lsm6dsv16x_gy_full_scale_set(®_ctx, LSM6DSV16X_2000dps) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - delay(100); - - /*Ignore First Data*/ - if (Get_G_AxesRaw_When_Aval(data_raw) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - float val_st_off[3] = {0}; - float val_st_on[3] = {0}; - - for (uint8_t i = 0; i < 5; i++) { - if (Get_G_AxesRaw_When_Aval(data_raw) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - /*Average the data in each axis*/ - for (uint8_t j = 0; j < 3; j++) { - val_st_off[j] += lsm6dsv16x_from_fs2000_to_mdps(data_raw[j]); - } - } - - /* Calculate the mg average values */ - for (uint8_t i = 0; i < 3; i++) { - val_st_off[i] /= 5.0f; - } - - if (lsm6dsv16x_gy_self_test_set(®_ctx, (lsm6dsv16x_gy_self_test_t)TestType) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - delay(100); - - /*Ignore First Data*/ - if (Get_G_AxesRaw_When_Aval(data_raw) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - for (uint8_t i = 0; i < 5; i++) { - if (Get_G_AxesRaw_When_Aval(data_raw) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - /*Average the data in each axis*/ - for (uint8_t j = 0; j < 3; j++) { - val_st_on[j] += lsm6dsv16x_from_fs2000_to_mdps(data_raw[j]); - } - } - - /* Calculate the mg average values */ - for (uint8_t i = 0; i < 3; i++) { - val_st_on[i] /= 5.0f; - } - - /* Calculate the mg values for self test */ - for (uint8_t i = 0; i < 3; i++) { - test_val[i] = fabs((val_st_on[i] - val_st_off[i])); - } - - /* Check self test limit */ - for (uint8_t i = 0; i < 3; i++) { - if (( LSM6DSV16X_MIN_ST_LIMIT_mdps > test_val[i] ) || - ( test_val[i] > LSM6DSV16X_MAX_ST_LIMIT_mdps)) { - return LSM6DSV16X_ERROR; - } - } - - - if (lsm6dsv16x_gy_self_test_set(®_ctx, LSM6DSV16X_GY_ST_DISABLE) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - if (lsm6dsv16x_xl_data_rate_set(®_ctx, LSM6DSV16X_ODR_OFF) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - return LSM6DSV16X_OK; -} - - -LSM6DSV16XStatusTypeDef LSM6DSV16X::Get_T_ODR(float *Odr) { - lsm6dsv16x_fifo_ctrl4_t ctrl4; - - if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_FIFO_CTRL4, (uint8_t *)&ctrl4, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - switch (ctrl4.odr_t_batch) - { - case LSM6DSV16X_TEMP_NOT_BATCHED: - *Odr = 0; - break; - case LSM6DSV16X_TEMP_BATCHED_AT_1Hz875: - *Odr = 1.875f; - break; - case LSM6DSV16X_TEMP_BATCHED_AT_15Hz: - *Odr = 15; - break; - case LSM6DSV16X_TEMP_BATCHED_AT_60Hz: - *Odr = 60; - break; - default: - break; - } - - return LSM6DSV16X_OK; -} - - - -LSM6DSV16XStatusTypeDef LSM6DSV16X::Set_T_ODR(float Odr) { - lsm6dsv16x_fifo_ctrl4_t ctrl4; - - if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_FIFO_CTRL4, (uint8_t *)&ctrl4, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - if (Odr == 0.0F) { - ctrl4.odr_t_batch = LSM6DSV16X_TEMP_NOT_BATCHED; - } else if (Odr <= 1.875F) { - ctrl4.odr_t_batch = LSM6DSV16X_TEMP_BATCHED_AT_1Hz875; - } else if (Odr <= 15.0F) { - ctrl4.odr_t_batch = LSM6DSV16X_TEMP_BATCHED_AT_15Hz; - } else { - ctrl4.odr_t_batch = LSM6DSV16X_TEMP_BATCHED_AT_60Hz; - } - - return (LSM6DSV16XStatusTypeDef)lsm6dsv16x_write_reg( - ®_ctx, - LSM6DSV16X_FIFO_CTRL4, - (uint8_t *)&ctrl4, - 1 - ); -} - -/** - * @brief Enable 6D orientation detection - * @param IntPin interrupt pin line to be used - * @retval 0 in case of success, an error code otherwise - */ -LSM6DSV16XStatusTypeDef LSM6DSV16X::Enable_6D_Orientation(LSM6DSV16X_SensorIntPin_t IntPin) -{ - LSM6DSV16XStatusTypeDef ret = LSM6DSV16X_OK; - lsm6dsv16x_md1_cfg_t val1; - lsm6dsv16x_md2_cfg_t val2; - lsm6dsv16x_functions_enable_t functions_enable; - - /* Output Data Rate selection */ - if (Set_X_ODR(480.0f) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - /* Full scale selection */ - if (Set_X_FS(2) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - /* 6D orientation enabled. */ - if (Set_6D_Orientation_Threshold(2) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - /* Enable 6D orientation event on either INT1 or INT2 pin */ - switch (IntPin) { - case LSM6DSV16X_INT1_PIN: - if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_MD1_CFG, (uint8_t *)&val1, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - val1.int1_6d = PROPERTY_ENABLE; - - if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_MD1_CFG, (uint8_t *)&val1, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_FUNCTIONS_ENABLE, (uint8_t *)&functions_enable, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - functions_enable.interrupts_enable = PROPERTY_ENABLE; - - if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_FUNCTIONS_ENABLE, (uint8_t *)&functions_enable, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - break; - - case LSM6DSV16X_INT2_PIN: - if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_MD2_CFG, (uint8_t *)&val2, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - val2.int2_6d = PROPERTY_ENABLE; - - if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_MD2_CFG, (uint8_t *)&val2, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_FUNCTIONS_ENABLE, (uint8_t *)&functions_enable, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - functions_enable.interrupts_enable = PROPERTY_ENABLE; - - if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_FUNCTIONS_ENABLE, (uint8_t *)&functions_enable, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - break; - - default: - ret = LSM6DSV16X_ERROR; - break; - } - - return ret; -} - -/** - * @brief Disable 6D orientation detection - * @retval 0 in case of success, an error code otherwise - */ -LSM6DSV16XStatusTypeDef LSM6DSV16X::Disable_6D_Orientation() -{ - lsm6dsv16x_md1_cfg_t val1; - lsm6dsv16x_md2_cfg_t val2; - - /* Reset threshold */ - if (Set_6D_Orientation_Threshold(0) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - /* Disable 6D orientation event on both INT1 and INT2 pins */ - if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_MD1_CFG, (uint8_t *)&val1, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - val1.int1_6d = PROPERTY_DISABLE; - - if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_MD1_CFG, (uint8_t *)&val1, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_MD2_CFG, (uint8_t *)&val2, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - val2.int2_6d = PROPERTY_DISABLE; - - if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_MD2_CFG, (uint8_t *)&val2, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - return LSM6DSV16X_OK; -} - -/** - * @brief Set 6D orientation threshold - * @param Threshold 6D Orientation detection threshold - * @retval 0 in case of success, an error code otherwise - */ -LSM6DSV16XStatusTypeDef LSM6DSV16X::Set_6D_Orientation_Threshold(uint8_t Threshold) -{ - LSM6DSV16XStatusTypeDef ret = LSM6DSV16X_OK; - lsm6dsv16x_6d_threshold_t newThreshold = LSM6DSV16X_DEG_80; - - switch (Threshold) { - case 0: - newThreshold = LSM6DSV16X_DEG_80; - break; - case 1: - newThreshold = LSM6DSV16X_DEG_70; - break; - case 2: - newThreshold = LSM6DSV16X_DEG_60; - break; - case 3: - newThreshold = LSM6DSV16X_DEG_50; - break; - default: - ret = LSM6DSV16X_ERROR; - break; - } - - if (ret == LSM6DSV16X_ERROR) { - return LSM6DSV16X_ERROR; - } - - if (lsm6dsv16x_6d_threshold_set(®_ctx, newThreshold) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - return LSM6DSV16X_OK; - -} - -/** - * @brief Get the status of XLow orientation - * @param XLow the status of XLow orientation - * @retval 0 in case of success, an error code otherwise - */ -LSM6DSV16XStatusTypeDef LSM6DSV16X::Get_6D_Orientation_XL(uint8_t *XLow) -{ - lsm6dsv16x_d6d_src_t data; - - if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_D6D_SRC, (uint8_t *)&data, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - *XLow = data.xl; - - return LSM6DSV16X_OK; -} - -/** - * @brief Get the status of XHigh orientation - * @param XHigh the status of XHigh orientation - * @retval 0 in case of success, an error code otherwise - */ -LSM6DSV16XStatusTypeDef LSM6DSV16X::Get_6D_Orientation_XH(uint8_t *XHigh) -{ - lsm6dsv16x_d6d_src_t data; - - if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_D6D_SRC, (uint8_t *)&data, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - *XHigh = data.xh; - - return LSM6DSV16X_OK; -} - -/** - * @brief Get the status of YLow orientation - * @param YLow the status of YLow orientation - * @retval 0 in case of success, an error code otherwise - */ -LSM6DSV16XStatusTypeDef LSM6DSV16X::Get_6D_Orientation_YL(uint8_t *YLow) -{ - lsm6dsv16x_d6d_src_t data; - - if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_D6D_SRC, (uint8_t *)&data, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - *YLow = data.yl; - - return LSM6DSV16X_OK; -} - -/** - * @brief Get the status of YHigh orientation - * @param YHigh the status of YHigh orientation - * @retval 0 in case of success, an error code otherwise - */ -LSM6DSV16XStatusTypeDef LSM6DSV16X::Get_6D_Orientation_YH(uint8_t *YHigh) -{ - lsm6dsv16x_d6d_src_t data; - - if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_D6D_SRC, (uint8_t *)&data, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - *YHigh = data.yh; - - return LSM6DSV16X_OK; -} - -/** - * @brief Get the status of ZLow orientation - * @param ZLow the status of ZLow orientation - * @retval 0 in case of success, an error code otherwise - */ -LSM6DSV16XStatusTypeDef LSM6DSV16X::Get_6D_Orientation_ZL(uint8_t *ZLow) -{ - lsm6dsv16x_d6d_src_t data; - - if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_D6D_SRC, (uint8_t *)&data, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - *ZLow = data.zl; - - return LSM6DSV16X_OK; -} - -/** - * @brief Get the status of ZHigh orientation - * @param ZHigh the status of ZHigh orientation - * @retval 0 in case of success, an error code otherwise - */ -LSM6DSV16XStatusTypeDef LSM6DSV16X::Get_6D_Orientation_ZH(uint8_t *ZHigh) -{ - lsm6dsv16x_d6d_src_t data; - - if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_D6D_SRC, (uint8_t *)&data, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - *ZHigh = data.zh; - - return LSM6DSV16X_OK; -} - - -/** - * @brief Enable free fall detection - * @param IntPin interrupt pin line to be used - * @retval 0 in case of success, an error code otherwise - */ -LSM6DSV16XStatusTypeDef LSM6DSV16X::Enable_Free_Fall_Detection(LSM6DSV16X_SensorIntPin_t IntPin) -{ - LSM6DSV16XStatusTypeDef ret = LSM6DSV16X_OK; - lsm6dsv16x_md1_cfg_t val1; - lsm6dsv16x_md2_cfg_t val2; - lsm6dsv16x_functions_enable_t functions_enable; - - /* Output Data Rate selection */ - if (Set_X_ODR(480) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - /* Full scale selection */ - if (Set_X_FS(2) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - /* Set free fall duration.*/ - if (Set_Free_Fall_Duration(3) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - /* Set free fall threshold. */ - if (Set_Free_Fall_Threshold(3) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - /* Enable free fall event on either INT1 or INT2 pin */ - switch (IntPin) { - case LSM6DSV16X_INT1_PIN: - if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_MD1_CFG, (uint8_t *)&val1, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - val1.int1_ff = PROPERTY_ENABLE; - - if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_MD1_CFG, (uint8_t *)&val1, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_FUNCTIONS_ENABLE, (uint8_t *)&functions_enable, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - functions_enable.interrupts_enable = PROPERTY_ENABLE; - - if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_FUNCTIONS_ENABLE, (uint8_t *)&functions_enable, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - break; - - case LSM6DSV16X_INT2_PIN: - if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_MD2_CFG, (uint8_t *)&val2, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - val2.int2_ff = PROPERTY_ENABLE; - - if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_MD2_CFG, (uint8_t *)&val2, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_FUNCTIONS_ENABLE, (uint8_t *)&functions_enable, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - functions_enable.interrupts_enable = PROPERTY_ENABLE; - - if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_FUNCTIONS_ENABLE, (uint8_t *)&functions_enable, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - break; - - default: - ret = LSM6DSV16X_ERROR; - break; - } - - return ret; -} - -/** - * @brief Disable free fall detection - * @retval 0 in case of success, an error code otherwise - */ -LSM6DSV16XStatusTypeDef LSM6DSV16X::Disable_Free_Fall_Detection() -{ - lsm6dsv16x_md1_cfg_t val1; - lsm6dsv16x_md2_cfg_t val2; - - /* Disable free fall event on both INT1 and INT2 pins */ - if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_MD1_CFG, (uint8_t *)&val1, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - val1.int1_ff = PROPERTY_DISABLE; - - if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_MD1_CFG, (uint8_t *)&val1, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_MD2_CFG, (uint8_t *)&val2, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - val2.int2_ff = PROPERTY_DISABLE; - - if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_MD2_CFG, (uint8_t *)&val2, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - /* Reset free fall threshold. */ - if (Set_Free_Fall_Threshold(0) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - /* Reset free fall duration */ - if (Set_Free_Fall_Duration(0) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - return LSM6DSV16X_OK; -} - -/** - * @brief Set free fall threshold - * @param Threshold free fall detection threshold - * @retval 0 in case of success, an error code otherwise - */ -LSM6DSV16XStatusTypeDef LSM6DSV16X::Set_Free_Fall_Threshold(uint8_t Threshold) -{ - lsm6dsv16x_ff_thresholds_t val; - switch (Threshold) { - case LSM6DSV16X_156_mg: - val = LSM6DSV16X_156_mg; - break; - - case LSM6DSV16X_219_mg: - val = LSM6DSV16X_219_mg; - break; - - case LSM6DSV16X_250_mg: - val = LSM6DSV16X_250_mg; - break; - - case LSM6DSV16X_312_mg: - val = LSM6DSV16X_312_mg; - break; - - case LSM6DSV16X_344_mg: - val = LSM6DSV16X_344_mg; - break; - - case LSM6DSV16X_406_mg: - val = LSM6DSV16X_406_mg; - break; - - case LSM6DSV16X_469_mg: - val = LSM6DSV16X_469_mg; - break; - - case LSM6DSV16X_500_mg: - val = LSM6DSV16X_500_mg; - break; - - default: - val = LSM6DSV16X_156_mg; - break; - } - - /* Set free fall threshold. */ - if (lsm6dsv16x_ff_thresholds_set(®_ctx, val) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - return LSM6DSV16X_OK; -} - - -/** - * @brief Set free fall duration - * @param Duration free fall detection duration - * @retval 0 in case of success, an error code otherwise - */ -LSM6DSV16XStatusTypeDef LSM6DSV16X::Set_Free_Fall_Duration(uint8_t Duration) -{ - if (lsm6dsv16x_ff_time_windows_set(®_ctx, Duration) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - return LSM6DSV16X_OK; -} - -/** - * @brief Enable wake up detection - * @param IntPin interrupt pin line to be used - * @retval 0 in case of success, an error code otherwise - */ -LSM6DSV16XStatusTypeDef LSM6DSV16X::Enable_Wake_Up_Detection(LSM6DSV16X_SensorIntPin_t IntPin) -{ - LSM6DSV16XStatusTypeDef ret = LSM6DSV16X_OK; - lsm6dsv16x_md1_cfg_t val1; - lsm6dsv16x_md2_cfg_t val2; - lsm6dsv16x_functions_enable_t functions_enable; - - /* Output Data Rate selection */ - if (Set_X_ODR(480) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - /* Full scale selection */ - if (Set_X_FS(2) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - /* Set wake-up threshold */ - if (Set_Wake_Up_Threshold(63) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - /* Set wake-up durantion */ - if (Set_Wake_Up_Duration(0) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - /* Enable wake up event on either INT1 or INT2 pin */ - switch (IntPin) { - case LSM6DSV16X_INT1_PIN: - if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_MD1_CFG, (uint8_t *)&val1, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - val1.int1_wu = PROPERTY_ENABLE; - - if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_MD1_CFG, (uint8_t *)&val1, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_FUNCTIONS_ENABLE, (uint8_t *)&functions_enable, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - functions_enable.interrupts_enable = PROPERTY_ENABLE; - - if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_FUNCTIONS_ENABLE, (uint8_t *)&functions_enable, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - break; - - case LSM6DSV16X_INT2_PIN: - if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_MD2_CFG, (uint8_t *)&val2, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - val2.int2_wu = PROPERTY_ENABLE; - - if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_MD2_CFG, (uint8_t *)&val2, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_FUNCTIONS_ENABLE, (uint8_t *)&functions_enable, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - functions_enable.interrupts_enable = PROPERTY_ENABLE; - - if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_FUNCTIONS_ENABLE, (uint8_t *)&functions_enable, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - break; - - default: - ret = LSM6DSV16X_ERROR; - break; - } - - return ret; -} - - -/** - * @brief Disable wake up detection - * @retval 0 in case of success, an error code otherwise - */ -LSM6DSV16XStatusTypeDef LSM6DSV16X::Disable_Wake_Up_Detection() -{ - lsm6dsv16x_md1_cfg_t val1; - lsm6dsv16x_md2_cfg_t val2; - - /* Disable wake up event on both INT1 and INT2 pins */ - if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_MD1_CFG, (uint8_t *)&val1, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - val1.int1_wu = PROPERTY_DISABLE; - - if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_MD1_CFG, (uint8_t *)&val1, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_MD2_CFG, (uint8_t *)&val2, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - val2.int2_wu = PROPERTY_DISABLE; - - if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_MD2_CFG, (uint8_t *)&val2, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - /* Reset wake-up threshold */ - if (Set_Wake_Up_Threshold(0) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - /* Reset wake-up durantion */ - if (Set_Wake_Up_Duration(0) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - return LSM6DSV16X_OK; -} - -/** - * @brief Set wake up threshold - * @param Threshold wake up detection threshold - * @retval 0 in case of success, an error code otherwise - */ -LSM6DSV16XStatusTypeDef LSM6DSV16X::Set_Wake_Up_Threshold(uint32_t Threshold) -{ - lsm6dsv16x_act_thresholds_t wake_up_ths; - - if (lsm6dsv16x_act_thresholds_get(®_ctx, &wake_up_ths) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - wake_up_ths.threshold = Threshold; - - if (lsm6dsv16x_act_thresholds_set(®_ctx, &wake_up_ths) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - return LSM6DSV16X_OK; -} - -/** - * @brief Set wake up duration - * @param Duration wake up detection duration - * @retval 0 in case of success, an error code otherwise - */ -LSM6DSV16XStatusTypeDef LSM6DSV16X::Set_Wake_Up_Duration(uint8_t Duration) -{ - lsm6dsv16x_act_wkup_time_windows_t dur_t; - - if (lsm6dsv16x_act_wkup_time_windows_get(®_ctx, &dur_t) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - dur_t.shock = Duration; - - if (lsm6dsv16x_act_wkup_time_windows_set(®_ctx, dur_t) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - return LSM6DSV16X_OK; -} - -/** - * @brief Enable single tap detection - * @param IntPin interrupt pin line to be used - * @retval 0 in case of success, an error code otherwise - */ -LSM6DSV16XStatusTypeDef LSM6DSV16X::Enable_Single_Tap_Detection(LSM6DSV16X_SensorIntPin_t IntPin) -{ - LSM6DSV16XStatusTypeDef ret = LSM6DSV16X_OK; - lsm6dsv16x_md1_cfg_t val1; - lsm6dsv16x_md2_cfg_t val2; - lsm6dsv16x_functions_enable_t functions_enable; - - lsm6dsv16x_tap_dur_t tap_dur; - lsm6dsv16x_tap_cfg0_t tap_cfg0; - lsm6dsv16x_tap_ths_6d_t tap_ths_6d; - - /* Output Data Rate selection */ - if (Set_X_ODR(480) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - /* Full scale selection */ - if (Set_X_FS(8) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - /* Enable tap detection on Z-axis. */ - if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_TAP_CFG0, (uint8_t *)&tap_cfg0, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - tap_cfg0.tap_z_en = 0x01U; - - if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_TAP_CFG0, (uint8_t *)&tap_cfg0, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - /* Set Z-axis threshold. */ - if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_TAP_THS_6D, (uint8_t *)&tap_ths_6d, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - tap_ths_6d.tap_ths_z = 0x2U; - - if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_TAP_THS_6D, (uint8_t *)&tap_ths_6d, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - /* Set quiet and shock time windows. */ - if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_TAP_DUR, (uint8_t *)&tap_dur, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - tap_dur.quiet = (uint8_t)0x02U; - tap_dur.shock = (uint8_t)0x01U; - - if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_TAP_DUR, (uint8_t *)&tap_dur, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - /* Set tap mode. */ - if (lsm6dsv16x_tap_mode_set(®_ctx, LSM6DSV16X_ONLY_SINGLE) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - /* Enable single tap event on either INT1 or INT2 pin */ - switch (IntPin) { - case LSM6DSV16X_INT1_PIN: - if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_MD1_CFG, (uint8_t *)&val1, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - val1.int1_single_tap = PROPERTY_ENABLE; - - if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_MD1_CFG, (uint8_t *)&val1, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_FUNCTIONS_ENABLE, (uint8_t *)&functions_enable, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - functions_enable.interrupts_enable = PROPERTY_ENABLE; - - if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_FUNCTIONS_ENABLE, (uint8_t *)&functions_enable, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - break; - - case LSM6DSV16X_INT2_PIN: - if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_MD2_CFG, (uint8_t *)&val2, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - val2.int2_single_tap = PROPERTY_ENABLE; - - if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_MD2_CFG, (uint8_t *)&val2, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_FUNCTIONS_ENABLE, (uint8_t *)&functions_enable, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - functions_enable.interrupts_enable = PROPERTY_ENABLE; - - if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_FUNCTIONS_ENABLE, (uint8_t *)&functions_enable, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - break; - - default: - ret = LSM6DSV16X_ERROR; - break; - } - - return ret; -} - -/** - * @brief Disable single tap detection - * @retval 0 in case of success, an error code otherwise - */ -LSM6DSV16XStatusTypeDef LSM6DSV16X::Disable_Single_Tap_Detection() -{ - lsm6dsv16x_md1_cfg_t val1; - lsm6dsv16x_md2_cfg_t val2; - lsm6dsv16x_tap_dur_t tap_dur; - lsm6dsv16x_tap_cfg0_t tap_cfg0; - lsm6dsv16x_tap_ths_6d_t tap_ths_6d; - - - /* Disable single tap event on both INT1 and INT2 pins */ - if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_MD1_CFG, (uint8_t *)&val1, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - val1.int1_single_tap = PROPERTY_DISABLE; - - if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_MD1_CFG, (uint8_t *)&val1, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_MD2_CFG, (uint8_t *)&val2, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - val2.int2_single_tap = PROPERTY_DISABLE; - - if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_MD2_CFG, (uint8_t *)&val2, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - /* Disable tap detection on Z-axis. */ - if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_TAP_CFG0, (uint8_t *)&tap_cfg0, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - tap_cfg0.tap_z_en = 0x0U; - - if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_TAP_CFG0, (uint8_t *)&tap_cfg0, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - /* Reset Z-axis threshold. */ - if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_TAP_THS_6D, (uint8_t *)&tap_ths_6d, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - tap_ths_6d.tap_ths_z = 0x0U; - - if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_TAP_THS_6D, (uint8_t *)&tap_ths_6d, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - /* Reset quiet and shock time windows. */ - if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_TAP_DUR, (uint8_t *)&tap_dur, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - tap_dur.quiet = (uint8_t)0x0U; - tap_dur.shock = (uint8_t)0x0U; - - if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_TAP_DUR, (uint8_t *)&tap_dur, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - return LSM6DSV16X_OK; -} - -/** - * @brief Enable double tap detection - * @param IntPin interrupt pin line to be used - * @retval 0 in case of success, an error code otherwise - */ -LSM6DSV16XStatusTypeDef LSM6DSV16X::Enable_Double_Tap_Detection(LSM6DSV16X_SensorIntPin_t IntPin) -{ - LSM6DSV16XStatusTypeDef ret = LSM6DSV16X_OK; - lsm6dsv16x_md1_cfg_t val1; - lsm6dsv16x_md2_cfg_t val2; - lsm6dsv16x_functions_enable_t functions_enable; - - lsm6dsv16x_tap_dur_t tap_dur; - lsm6dsv16x_tap_cfg0_t tap_cfg0; - lsm6dsv16x_tap_ths_6d_t tap_ths_6d; - - - /* Enable tap detection on Z-axis. */ - if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_TAP_CFG0, (uint8_t *)&tap_cfg0, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - tap_cfg0.tap_z_en = 0x01U; - - if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_TAP_CFG0, (uint8_t *)&tap_cfg0, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - /* Set Z-axis threshold. */ - if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_TAP_THS_6D, (uint8_t *)&tap_ths_6d, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - tap_ths_6d.tap_ths_z = 0x03U; - - if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_TAP_THS_6D, (uint8_t *)&tap_ths_6d, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - /* Set quiet shock and duration. */ - if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_TAP_DUR, (uint8_t *)&tap_dur, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - tap_dur.dur = (uint8_t)0x03U; - tap_dur.quiet = (uint8_t)0x02U; - tap_dur.shock = (uint8_t)0x02U; - - if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_TAP_DUR, (uint8_t *)&tap_dur, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - /* Set tap mode. */ - if (lsm6dsv16x_tap_mode_set(®_ctx, LSM6DSV16X_BOTH_SINGLE_DOUBLE) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - /* Output Data Rate selection */ - if (Set_X_ODR(480) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - /* Full scale selection */ - if (Set_X_FS(8) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - /* Enable double tap event on either INT1 or INT2 pin */ - switch (IntPin) { - case LSM6DSV16X_INT1_PIN: - if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_MD1_CFG, (uint8_t *)&val1, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - val1.int1_double_tap = PROPERTY_ENABLE; - - if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_MD1_CFG, (uint8_t *)&val1, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_FUNCTIONS_ENABLE, (uint8_t *)&functions_enable, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - functions_enable.interrupts_enable = PROPERTY_ENABLE; - - if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_FUNCTIONS_ENABLE, (uint8_t *)&functions_enable, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - break; - - case LSM6DSV16X_INT2_PIN: - if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_MD2_CFG, (uint8_t *)&val2, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - val2.int2_double_tap = PROPERTY_ENABLE; - - if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_MD2_CFG, (uint8_t *)&val2, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_FUNCTIONS_ENABLE, (uint8_t *)&functions_enable, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - functions_enable.interrupts_enable = PROPERTY_ENABLE; - - if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_FUNCTIONS_ENABLE, (uint8_t *)&functions_enable, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - break; - - default: - ret = LSM6DSV16X_ERROR; - break; - } - - return ret; -} - -/** - * @brief Disable double tap detection - * @retval 0 in case of success, an error code otherwise - */ -LSM6DSV16XStatusTypeDef LSM6DSV16X::Disable_Double_Tap_Detection() -{ - lsm6dsv16x_md1_cfg_t val1; - lsm6dsv16x_md2_cfg_t val2; - - lsm6dsv16x_tap_dur_t tap_dur; - lsm6dsv16x_tap_cfg0_t tap_cfg0; - lsm6dsv16x_tap_ths_6d_t tap_ths_6d; - - /* Disable double tap event on both INT1 and INT2 pins */ - if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_MD1_CFG, (uint8_t *)&val1, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - val1.int1_ff = PROPERTY_DISABLE; - - if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_MD1_CFG, (uint8_t *)&val1, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_MD2_CFG, (uint8_t *)&val2, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - val2.int2_ff = PROPERTY_DISABLE; - - if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_MD2_CFG, (uint8_t *)&val2, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - /* Disable tap detection on Z-axis. */ - if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_TAP_CFG0, (uint8_t *)&tap_cfg0, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - tap_cfg0.tap_z_en = 0x0U; - - if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_TAP_CFG0, (uint8_t *)&tap_cfg0, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - /* Reset Z-axis threshold. */ - if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_TAP_THS_6D, (uint8_t *)&tap_ths_6d, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - tap_ths_6d.tap_ths_z = 0x0U; - - if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_TAP_THS_6D, (uint8_t *)&tap_ths_6d, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - /* Reset quiet shock and duratio. */ - if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_TAP_DUR, (uint8_t *)&tap_dur, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - tap_dur.dur = (uint8_t)0x0U; - tap_dur.quiet = (uint8_t)0x0U; - tap_dur.shock = (uint8_t)0x0U; - - if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_TAP_DUR, (uint8_t *)&tap_dur, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - /* Set tap mode. */ - if (lsm6dsv16x_tap_mode_set(®_ctx, LSM6DSV16X_ONLY_SINGLE) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - - return LSM6DSV16X_OK; -} - -/** - * @brief Set tap threshold - * @param Threshold tap threshold - * @retval 0 in case of success, an error code otherwise - */ -LSM6DSV16XStatusTypeDef LSM6DSV16X::Set_Tap_Threshold(uint8_t Threshold) -{ - lsm6dsv16x_tap_ths_6d_t tap_ths_6d; - - /* Set Z-axis threshold */ - if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_TAP_THS_6D, (uint8_t *)&tap_ths_6d, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - tap_ths_6d.tap_ths_z = Threshold; - - if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_TAP_THS_6D, (uint8_t *)&tap_ths_6d, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - return LSM6DSV16X_OK; -} - -/** - * @brief Set tap shock time - * @param Time tap shock time - * @retval 0 in case of success, an error code otherwise - */ -LSM6DSV16XStatusTypeDef LSM6DSV16X::Set_Tap_Shock_Time(uint8_t Time) -{ - lsm6dsv16x_tap_dur_t tap_dur; - - /* Set shock time */ - if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_TAP_DUR, (uint8_t *)&tap_dur, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - tap_dur.shock = Time; - - if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_TAP_DUR, (uint8_t *)&tap_dur, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - return LSM6DSV16X_OK; -} - -/** - * @brief Set tap quiet time - * @param Time tap quiet time - * @retval 0 in case of success, an error code otherwise - */ -LSM6DSV16XStatusTypeDef LSM6DSV16X::Set_Tap_Quiet_Time(uint8_t Time) -{ - lsm6dsv16x_tap_dur_t tap_dur; - - /* Set quiet time */ - if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_TAP_DUR, (uint8_t *)&tap_dur, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - tap_dur.quiet = Time; - - if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_TAP_DUR, (uint8_t *)&tap_dur, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - return LSM6DSV16X_OK; -} - -/** - * @brief Set tap duration time - * @param Time tap duration time - * @retval 0 in case of success, an error code otherwise - */ -LSM6DSV16XStatusTypeDef LSM6DSV16X::Set_Tap_Duration_Time(uint8_t Time) -{ - lsm6dsv16x_tap_dur_t tap_dur; - - /* Set duration time */ - if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_TAP_DUR, (uint8_t *)&tap_dur, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - tap_dur.dur = Time; - - if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_TAP_DUR, (uint8_t *)&tap_dur, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - return LSM6DSV16X_OK; -} - -/** - * @brief Enable pedometer - * @retval 0 in case of success, an error code otherwise - */ -LSM6DSV16XStatusTypeDef LSM6DSV16X::Enable_Pedometer(LSM6DSV16X_SensorIntPin_t IntPin) -{ - lsm6dsv16x_stpcnt_mode_t mode; - lsm6dsv16x_md1_cfg_t val1; - lsm6dsv16x_md2_cfg_t val2; - lsm6dsv16x_emb_func_int1_t emb_func_int1; - lsm6dsv16x_emb_func_int2_t emb_func_int2; - - /* Output Data Rate selection */ - if (Set_X_ODR(30) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - /* Full scale selection */ - if (Set_X_FS(2) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - if (lsm6dsv16x_stpcnt_mode_get(®_ctx, &mode) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - /* Enable pedometer algorithm. */ - mode.step_counter_enable = PROPERTY_ENABLE; - mode.false_step_rej = PROPERTY_DISABLE; - - /* Turn on embedded features */ - if (lsm6dsv16x_stpcnt_mode_set(®_ctx, mode) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - /* Enable free fall event on either INT1 or INT2 pin */ - switch (IntPin) { - case LSM6DSV16X_INT1_PIN: - /* Enable access to embedded functions registers. */ - if (lsm6dsv16x_mem_bank_set(®_ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - /* Step detector interrupt driven to INT1 pin */ - if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_EMB_FUNC_INT1, (uint8_t *)&emb_func_int1, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - emb_func_int1.int1_step_detector = PROPERTY_ENABLE; - - if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_EMB_FUNC_INT1, (uint8_t *)&emb_func_int1, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - /* Disable access to embedded functions registers */ - if (lsm6dsv16x_mem_bank_set(®_ctx, LSM6DSV16X_MAIN_MEM_BANK) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_MD1_CFG, (uint8_t *)&val1, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - val1.int1_emb_func = PROPERTY_ENABLE; - - if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_MD1_CFG, (uint8_t *)&val1, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - break; - - case LSM6DSV16X_INT2_PIN: - /* Enable access to embedded functions registers. */ - if (lsm6dsv16x_mem_bank_set(®_ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - /* Step detector interrupt driven to INT1 pin */ - if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_EMB_FUNC_INT2, (uint8_t *)&emb_func_int2, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - emb_func_int2.int2_step_detector = PROPERTY_ENABLE; - - if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_EMB_FUNC_INT2, (uint8_t *)&emb_func_int2, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - /* Disable access to embedded functions registers */ - if (lsm6dsv16x_mem_bank_set(®_ctx, LSM6DSV16X_MAIN_MEM_BANK) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_MD2_CFG, (uint8_t *)&val2, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - val2.int2_emb_func = PROPERTY_ENABLE; - - if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_MD2_CFG, (uint8_t *)&val2, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - break; - - default: - return LSM6DSV16X_ERROR; - break; - } - - return LSM6DSV16X_OK; -} - - -/** - * @brief Disable pedometer - * @retval 0 in case of success, an error code otherwise - */ -LSM6DSV16XStatusTypeDef LSM6DSV16X::Disable_Pedometer() -{ - lsm6dsv16x_md1_cfg_t val1; - lsm6dsv16x_md2_cfg_t val2; - - lsm6dsv16x_emb_func_int1_t emb_func_int1; - lsm6dsv16x_emb_func_int2_t emb_func_int2; - - lsm6dsv16x_stpcnt_mode_t mode; - - - if (lsm6dsv16x_stpcnt_mode_get(®_ctx, &mode) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - /* Enable pedometer algorithm. */ - mode.step_counter_enable = PROPERTY_DISABLE; - mode.false_step_rej = PROPERTY_DISABLE; - - /* Turn off embedded features */ - if (lsm6dsv16x_stpcnt_mode_set(®_ctx, mode) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - /* Disable emb func event on either INT1 or INT2 pin */ - if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_MD1_CFG, (uint8_t *)&val1, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - val1.int1_emb_func = PROPERTY_DISABLE; - - if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_MD1_CFG, (uint8_t *)&val1, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_MD2_CFG, (uint8_t *)&val2, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - val2.int2_emb_func = PROPERTY_DISABLE; - - if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_MD2_CFG, (uint8_t *)&val2, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - /* Enable access to embedded functions registers. */ - if (lsm6dsv16x_mem_bank_set(®_ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - /* Reset interrupt driven to INT1 pin. */ - if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_EMB_FUNC_INT1, (uint8_t *)&emb_func_int1, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - emb_func_int1.int1_step_detector = PROPERTY_DISABLE; - - if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_EMB_FUNC_INT1, (uint8_t *)&emb_func_int1, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - /* Reset interrupt driven to INT2 pin. */ - if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_EMB_FUNC_INT2, (uint8_t *)&emb_func_int2, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - emb_func_int2.int2_step_detector = PROPERTY_DISABLE; - - if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_EMB_FUNC_INT2, (uint8_t *)&emb_func_int2, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - /* Disable access to embedded functions registers. */ - if (lsm6dsv16x_mem_bank_set(®_ctx, LSM6DSV16X_MAIN_MEM_BANK) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - - return LSM6DSV16X_OK; -} - -/** - * @brief Get step count - * @param StepCount step counter - * @retval 0 in case of success, an error code otherwise - */ -LSM6DSV16XStatusTypeDef LSM6DSV16X::Get_Step_Count(uint16_t *StepCount) -{ - if (lsm6dsv16x_stpcnt_steps_get(®_ctx, StepCount) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - return LSM6DSV16X_OK; -} - -/** - * @brief Enable step counter reset - * @retval 0 in case of success, an error code otherwise - */ -LSM6DSV16XStatusTypeDef LSM6DSV16X::Step_Counter_Reset() -{ - if (lsm6dsv16x_stpcnt_rst_step_set(®_ctx, PROPERTY_ENABLE) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - return LSM6DSV16X_OK; -} - -/** - * @brief Enable tilt detection - * @param IntPin interrupt pin line to be used - * @retval 0 in case of success, an error code otherwise - */ -LSM6DSV16XStatusTypeDef LSM6DSV16X::Enable_Tilt_Detection(LSM6DSV16X_SensorIntPin_t IntPin) -{ - LSM6DSV16XStatusTypeDef ret = LSM6DSV16X_OK; - lsm6dsv16x_md1_cfg_t val1; - lsm6dsv16x_md2_cfg_t val2; - lsm6dsv16x_emb_func_en_a_t emb_func_en_a; - lsm6dsv16x_emb_func_int1_t emb_func_int1; - lsm6dsv16x_emb_func_int2_t emb_func_int2; - - /* Output Data Rate selection */ - if (Set_X_ODR(30) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - /* Full scale selection */ - if (Set_X_FS(2) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - switch (IntPin) { - case LSM6DSV16X_INT1_PIN: - /* Enable access to embedded functions registers */ - if (lsm6dsv16x_mem_bank_set(®_ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - /* Enable tilt detection */ - if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_EMB_FUNC_EN_A, (uint8_t *)&emb_func_en_a, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - emb_func_en_a.tilt_en = PROPERTY_ENABLE; - - if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_EMB_FUNC_EN_A, (uint8_t *)&emb_func_en_a, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - /* Tilt interrupt driven to INT1 pin */ - if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_EMB_FUNC_INT1, (uint8_t *)&emb_func_int1, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - emb_func_int1.int1_tilt = PROPERTY_ENABLE; - - if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_EMB_FUNC_INT1, (uint8_t *)&emb_func_int1, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - /* Disable access to embedded functions registers */ - if (lsm6dsv16x_mem_bank_set(®_ctx, LSM6DSV16X_MAIN_MEM_BANK) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - /* Enable routing the embedded functions interrupt */ - if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_MD1_CFG, (uint8_t *)&val1, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - val1.int1_emb_func = PROPERTY_ENABLE; - - if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_MD1_CFG, (uint8_t *)&val1, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - break; - - - case LSM6DSV16X_INT2_PIN: - /* Enable access to embedded functions registers */ - if (lsm6dsv16x_mem_bank_set(®_ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - /* Enable tilt detection */ - if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_EMB_FUNC_EN_A, (uint8_t *)&emb_func_en_a, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - emb_func_en_a.tilt_en = PROPERTY_ENABLE; - - if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_EMB_FUNC_EN_A, (uint8_t *)&emb_func_en_a, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - /* Tilt interrupt driven to INT2 pin */ - if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_EMB_FUNC_INT2, (uint8_t *)&emb_func_int2, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - emb_func_int2.int2_tilt = PROPERTY_ENABLE; - - if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_EMB_FUNC_INT2, (uint8_t *)&emb_func_int2, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - /* Disable access to embedded functions registers */ - if (lsm6dsv16x_mem_bank_set(®_ctx, LSM6DSV16X_MAIN_MEM_BANK) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - /* Enable routing the embedded functions interrupt */ - if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_MD2_CFG, (uint8_t *)&val2, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - val2.int2_emb_func = PROPERTY_ENABLE; - - if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_MD2_CFG, (uint8_t *)&val2, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - break; - - default: - ret = LSM6DSV16X_ERROR; - break; - } - - return ret; -} - -/** - * @brief Disable tilt detection - * @retval 0 in case of success, an error code otherwise - */ -LSM6DSV16XStatusTypeDef LSM6DSV16X::Disable_Tilt_Detection() -{ - LSM6DSV16XStatusTypeDef ret = LSM6DSV16X_OK; - lsm6dsv16x_md1_cfg_t val1; - lsm6dsv16x_md2_cfg_t val2; - - lsm6dsv16x_emb_func_en_a_t emb_func_en_a; - lsm6dsv16x_emb_func_int1_t emb_func_int1; - lsm6dsv16x_emb_func_int2_t emb_func_int2; - - /* Disable emb func event on either INT1 or INT2 pin */ - if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_MD1_CFG, (uint8_t *)&val1, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - val1.int1_emb_func = PROPERTY_DISABLE; - - if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_MD1_CFG, (uint8_t *)&val1, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_MD2_CFG, (uint8_t *)&val2, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - val2.int2_emb_func = PROPERTY_DISABLE; - - if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_MD2_CFG, (uint8_t *)&val2, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - /* Enable access to embedded functions registers. */ - if (lsm6dsv16x_mem_bank_set(®_ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - /* Disable tilt detection. */ - if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_EMB_FUNC_EN_A, (uint8_t *)&emb_func_en_a, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - emb_func_en_a.tilt_en = PROPERTY_DISABLE; - - if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_EMB_FUNC_EN_A, (uint8_t *)&emb_func_en_a, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - /* Reset interrupt driven to INT1 pin. */ - if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_EMB_FUNC_INT1, (uint8_t *)&emb_func_int1, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - emb_func_int1.int1_tilt = PROPERTY_DISABLE; - - if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_EMB_FUNC_INT1, (uint8_t *)&emb_func_int1, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - /* Reset interrupt driven to INT2 pin. */ - if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_EMB_FUNC_INT2, (uint8_t *)&emb_func_int2, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - emb_func_int2.int2_tilt = PROPERTY_DISABLE; - - if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_EMB_FUNC_INT2, (uint8_t *)&emb_func_int2, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - /* Disable access to embedded functions registers. */ - if (lsm6dsv16x_mem_bank_set(®_ctx, LSM6DSV16X_MAIN_MEM_BANK) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - return ret; -} - - -LSM6DSV16XStatusTypeDef LSM6DSV16X::FIFO_Reset() { - if (lsm6dsv16x_fifo_mode_set(®_ctx, LSM6DSV16X_BYPASS_MODE) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - if (lsm6dsv16x_fifo_mode_set(®_ctx, fifo_mode) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - return LSM6DSV16X_OK; -} - -/** - * @brief Get the LSM6DSV16X FIFO number of samples - - * @param NumSamples number of samples - * @retval 0 in case of success, an error code otherwise - */ -LSM6DSV16XStatusTypeDef LSM6DSV16X::FIFO_Get_Num_Samples(uint16_t *NumSamples) -{ - lsm6dsv16x_fifo_status_t fifo_status; - LSM6DSV16XStatusTypeDef result = - (LSM6DSV16XStatusTypeDef) lsm6dsv16x_fifo_status_get(®_ctx, &fifo_status); - *NumSamples = fifo_status.fifo_level; - return result; -} - -/** - * @brief Get the LSM6DSV16X FIFO full status - - * @param Status FIFO full status - * @retval 0 in case of success, an error code otherwise - */ -LSM6DSV16XStatusTypeDef LSM6DSV16X::FIFO_Get_Full_Status(uint8_t *Status) -{ - lsm6dsv16x_fifo_status2_t val; - - if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_FIFO_STATUS2, (uint8_t *)&val, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - *Status = val.fifo_full_ia; - - return LSM6DSV16X_OK; -} - -/** - * @brief Set the LSM6DSV16X FIFO full interrupt on INT1 pin - - * @param Status FIFO full interrupt on INT1 pin status - * @retval 0 in case of success, an error code otherwise - */ -LSM6DSV16XStatusTypeDef LSM6DSV16X::FIFO_Set_INT1_FIFO_Full(uint8_t Status) -{ - lsm6dsv16x_int1_ctrl_t reg; - - if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_INT1_CTRL, (uint8_t *)®, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - reg.int1_fifo_full = Status; - - if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_INT1_CTRL, (uint8_t *)®, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - return LSM6DSV16X_OK; -} - -/** - * @brief Set the LSM6DSV16X FIFO full interrupt on INT2 pin - - * @param Status FIFO full interrupt on INT1 pin status - * @retval 0 in case of success, an error code otherwise - */ -LSM6DSV16XStatusTypeDef LSM6DSV16X::FIFO_Set_INT2_FIFO_Full(uint8_t Status) -{ - lsm6dsv16x_int2_ctrl_t reg; - - if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_INT2_CTRL, (uint8_t *)®, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - reg.int2_fifo_full = Status; - - if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_INT2_CTRL, (uint8_t *)®, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - return LSM6DSV16X_OK; -} - -/** - * @brief Set the LSM6DSV16X FIFO watermark level - - * @param Watermark FIFO watermark level - * @retval 0 in case of success, an error code otherwise - */ -LSM6DSV16XStatusTypeDef LSM6DSV16X::FIFO_Set_Watermark_Level(uint8_t Watermark) -{ - return (LSM6DSV16XStatusTypeDef) lsm6dsv16x_fifo_watermark_set(®_ctx, Watermark); -} - -/** - * @brief Set the LSM6DSV16X FIFO stop on watermark - - * @param Status FIFO stop on watermark status - * @retval 0 in case of success, an error code otherwise - */ -LSM6DSV16XStatusTypeDef LSM6DSV16X::FIFO_Set_Stop_On_Fth(uint8_t Status) -{ - return (LSM6DSV16XStatusTypeDef) lsm6dsv16x_fifo_stop_on_wtm_set(®_ctx, Status); -} - -/** - * @brief Set the LSM6DSV16X FIFO mode - - * @param Mode FIFO mode - * @retval 0 in case of success, an error code otherwise - */ -LSM6DSV16XStatusTypeDef LSM6DSV16X::FIFO_Set_Mode(uint8_t Mode) -{ - lsm6dsv16x_fifo_mode_t newMode = LSM6DSV16X_BYPASS_MODE; - - switch (Mode) { - case 0: - newMode = LSM6DSV16X_BYPASS_MODE; - break; - case 1: - newMode = LSM6DSV16X_FIFO_MODE; - break; - case 3: - newMode = LSM6DSV16X_STREAM_TO_FIFO_MODE; - break; - case 4: - newMode = LSM6DSV16X_BYPASS_TO_STREAM_MODE; - break; - case 6: - newMode = LSM6DSV16X_STREAM_MODE; - break; - case 7: - newMode = LSM6DSV16X_BYPASS_TO_FIFO_MODE; - break; - default: - return LSM6DSV16X_ERROR; - } - fifo_mode = newMode; - if (lsm6dsv16x_fifo_mode_set(®_ctx, fifo_mode) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - return LSM6DSV16X_OK; -} - -/** - * @brief Get the LSM6DSV16X FIFO tag - - * @param Tag FIFO tag - * @retval 0 in case of success, an error code otherwise - */ -LSM6DSV16XStatusTypeDef LSM6DSV16X::FIFO_Get_Tag(uint8_t *Tag) -{ - lsm6dsv16x_fifo_data_out_tag_t tag_local; - - if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_FIFO_DATA_OUT_TAG, (uint8_t *)&tag_local, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - *Tag = (uint8_t)tag_local.tag_sensor; - - return LSM6DSV16X_OK; -} - -/** - * @brief Get the LSM6DSV16X FIFO raw data - - * @param Data FIFO raw data array [6] - * @retval 0 in case of success, an error code otherwise - */ -LSM6DSV16XStatusTypeDef LSM6DSV16X::FIFO_Get_Data(uint8_t *Data) -{ - return (LSM6DSV16XStatusTypeDef) lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_FIFO_DATA_OUT_X_L, Data, 6); -} - -/** - * @brief Get the LSM6DSV16X FIFO accelero single sample (16-bit data per 3 axes) and calculate acceleration [mg] - * @param Acceleration FIFO accelero axes [mg] - * @retval 0 in case of success, an error code otherwise - */ -LSM6DSV16XStatusTypeDef LSM6DSV16X::FIFO_Get_X_Axes(int32_t *Acceleration) -{ - lsm6dsv16x_axis3bit16_t data_raw; - float sensitivity = Convert_X_Sensitivity(acc_fs); - float acceleration_float[3]; - - if (FIFO_Get_Data(data_raw.u8bit) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - if (sensitivity == 0.0f) { - return LSM6DSV16X_ERROR; - } - acceleration_float[0] = (float)data_raw.i16bit[0] * sensitivity; - acceleration_float[1] = (float)data_raw.i16bit[1] * sensitivity; - acceleration_float[2] = (float)data_raw.i16bit[2] * sensitivity; - - Acceleration[0] = (int32_t)acceleration_float[0]; - Acceleration[1] = (int32_t)acceleration_float[1]; - Acceleration[2] = (int32_t)acceleration_float[2]; - - return LSM6DSV16X_OK; - -} - -/** - * @brief Set the LSM6DSV16X FIFO accelero BDR value - - * @param Bdr FIFO accelero BDR value - * @retval 0 in case of success, an error code otherwise - */ -LSM6DSV16XStatusTypeDef LSM6DSV16X::FIFO_Set_X_BDR(float Bdr) -{ - lsm6dsv16x_fifo_xl_batch_t new_bdr; - - new_bdr = (Bdr <= 0.0f) ? LSM6DSV16X_XL_NOT_BATCHED - : (Bdr <= 1.8f) ? LSM6DSV16X_XL_BATCHED_AT_1Hz875 - : (Bdr <= 7.5f) ? LSM6DSV16X_XL_BATCHED_AT_7Hz5 - : (Bdr <= 15.0f) ? LSM6DSV16X_XL_BATCHED_AT_15Hz - : (Bdr <= 30.0f) ? LSM6DSV16X_XL_BATCHED_AT_30Hz - : (Bdr <= 60.0f) ? LSM6DSV16X_XL_BATCHED_AT_60Hz - : (Bdr <= 120.0f) ? LSM6DSV16X_XL_BATCHED_AT_120Hz - : (Bdr <= 240.0f) ? LSM6DSV16X_XL_BATCHED_AT_240Hz - : (Bdr <= 480.0f) ? LSM6DSV16X_XL_BATCHED_AT_480Hz - : (Bdr <= 960.0f) ? LSM6DSV16X_XL_BATCHED_AT_960Hz - : (Bdr <= 1920.0f) ? LSM6DSV16X_XL_BATCHED_AT_1920Hz - : (Bdr <= 3840.0f) ? LSM6DSV16X_XL_BATCHED_AT_3840Hz - : LSM6DSV16X_XL_BATCHED_AT_7680Hz; - - return (LSM6DSV16XStatusTypeDef) lsm6dsv16x_fifo_xl_batch_set(®_ctx, new_bdr); -} - -/** - * @brief Get the LSM6DSV16X FIFO gyro single sample (16-bit data per 3 axes) and calculate angular velocity [mDPS] - - * @param AngularVelocity FIFO gyro axes [mDPS] - * @retval 0 in case of success, an error code otherwise - */ -LSM6DSV16XStatusTypeDef LSM6DSV16X::FIFO_Get_G_Axes(int32_t *AngularVelocity) -{ - lsm6dsv16x_axis3bit16_t data_raw; - float sensitivity = Convert_G_Sensitivity(gyro_fs); - float angular_velocity_float[3]; - - if (FIFO_Get_Data(data_raw.u8bit) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - if (sensitivity == 0.0f) { - return LSM6DSV16X_ERROR; - } - - angular_velocity_float[0] = (float)data_raw.i16bit[0] * sensitivity; - angular_velocity_float[1] = (float)data_raw.i16bit[1] * sensitivity; - angular_velocity_float[2] = (float)data_raw.i16bit[2] * sensitivity; - - AngularVelocity[0] = (int32_t)angular_velocity_float[0]; - AngularVelocity[1] = (int32_t)angular_velocity_float[1]; - AngularVelocity[2] = (int32_t)angular_velocity_float[2]; - - return LSM6DSV16X_OK; -} - -/** - * @brief Set the LSM6DSV16X FIFO gyro BDR value - - * @param Bdr FIFO gyro BDR value - * @retval 0 in case of success, an error code otherwise - */ -LSM6DSV16XStatusTypeDef LSM6DSV16X::FIFO_Set_G_BDR(float Bdr) -{ - lsm6dsv16x_fifo_gy_batch_t new_bdr; - - new_bdr = (Bdr <= 0.0f) ? LSM6DSV16X_GY_NOT_BATCHED - : (Bdr <= 1.8f) ? LSM6DSV16X_GY_BATCHED_AT_1Hz875 - : (Bdr <= 7.5f) ? LSM6DSV16X_GY_BATCHED_AT_7Hz5 - : (Bdr <= 15.0f) ? LSM6DSV16X_GY_BATCHED_AT_15Hz - : (Bdr <= 30.0f) ? LSM6DSV16X_GY_BATCHED_AT_30Hz - : (Bdr <= 60.0f) ? LSM6DSV16X_GY_BATCHED_AT_60Hz - : (Bdr <= 120.0f) ? LSM6DSV16X_GY_BATCHED_AT_120Hz - : (Bdr <= 240.0f) ? LSM6DSV16X_GY_BATCHED_AT_240Hz - : (Bdr <= 480.0f) ? LSM6DSV16X_GY_BATCHED_AT_480Hz - : (Bdr <= 960.0f) ? LSM6DSV16X_GY_BATCHED_AT_960Hz - : (Bdr <= 1920.0f) ? LSM6DSV16X_GY_BATCHED_AT_1920Hz - : (Bdr <= 3840.0f) ? LSM6DSV16X_GY_BATCHED_AT_3840Hz - : LSM6DSV16X_GY_BATCHED_AT_7680Hz; - - return (LSM6DSV16XStatusTypeDef) lsm6dsv16x_fifo_gy_batch_set(®_ctx, new_bdr); -} - -LSM6DSV16XStatusTypeDef LSM6DSV16X::FIFO_Set_SFLP_Batch(bool GameRotation, bool Gravity, bool gBias) -{ - lsm6dsv16x_fifo_sflp_raw_t fifo_sflp; - fifo_sflp.game_rotation = GameRotation; - fifo_sflp.gravity = Gravity; - fifo_sflp.gbias = gBias; - return (LSM6DSV16XStatusTypeDef)lsm6dsv16x_fifo_sflp_batch_set(®_ctx, fifo_sflp); -} - -LSM6DSV16XStatusTypeDef LSM6DSV16X::FIFO_Get_Status(lsm6dsv16x_fifo_status_t * Status) -{ - return (LSM6DSV16XStatusTypeDef) lsm6dsv16x_fifo_status_get(®_ctx, Status); -} - -LSM6DSV16XStatusTypeDef LSM6DSV16X::FIFO_Get_Gravity_Vector(float * data) -{ - uint16_t raw_data[6]; - if (FIFO_Get_Data((uint8_t *)raw_data) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - data[0] = lsm6dsv16x_from_sflp_to_mg(raw_data[0]); - data[1] = lsm6dsv16x_from_sflp_to_mg(raw_data[1]); - data[2] = lsm6dsv16x_from_sflp_to_mg(raw_data[2]); - return LSM6DSV16X_OK; -} - -LSM6DSV16XStatusTypeDef LSM6DSV16X::FIFO_Get_Game_Vector(float * data) -{ - uint16_t raw_data[3]; - if (FIFO_Get_Data((uint8_t *)raw_data) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - union bits_to_float { - uint32_t bits; - float value; - }; - - bits_to_float float_bits[3]; - float_bits[0].bits = Half_Bits_To_Float_Bits(raw_data[0]); - float_bits[1].bits = Half_Bits_To_Float_Bits(raw_data[1]); - float_bits[2].bits = Half_Bits_To_Float_Bits(raw_data[2]); - data[0] = float_bits[0].value; - data[1] = float_bits[1].value; - data[2] = float_bits[2].value; - return LSM6DSV16X_OK; -} - -LSM6DSV16XStatusTypeDef LSM6DSV16X::FIFO_Get_GBias_Vector(float * data) -{ - uint16_t raw_data[3]; - if (FIFO_Get_Data((uint8_t *)raw_data) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - data[0] = lsm6dsv16x_from_fs125_to_mdps(raw_data[0]); - data[1] = lsm6dsv16x_from_fs125_to_mdps(raw_data[1]); - data[2] = lsm6dsv16x_from_fs125_to_mdps(raw_data[2]); - return LSM6DSV16X_OK; -} - -LSM6DSV16XStatusTypeDef LSM6DSV16X::FIFO_Get_Timestamp(uint32_t *timestamp) -{ - uint32_t raw_data[2]; //first is timestamp second is half full of meta data - if (FIFO_Get_Data((uint8_t *)raw_data) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - *timestamp = raw_data[0]; - return LSM6DSV16X_OK; -} - -LSM6DSV16XStatusTypeDef LSM6DSV16X::FIFO_Set_Timestamp_Decimation(uint8_t decimation) -{ - return (LSM6DSV16XStatusTypeDef)lsm6dsv16x_fifo_timestamp_batch_set(®_ctx, (lsm6dsv16x_fifo_timestamp_batch_t)decimation); - -} - -LSM6DSV16XStatusTypeDef LSM6DSV16X::Enable_Timestamp() -{ - return (LSM6DSV16XStatusTypeDef)lsm6dsv16x_timestamp_set(®_ctx, 1); -} - - -/** - * @brief Enable the LSM6DSV16X gyroscope sensor - * @retval 0 in case of success, an error code otherwise - */ -LSM6DSV16XStatusTypeDef LSM6DSV16X::Enable_G() -{ - /* Check if the component is already enabled */ - if (gyro_is_enabled == 1U) { - return LSM6DSV16X_OK; - } - - /* Output data rate selection. */ - if (lsm6dsv16x_gy_data_rate_set(®_ctx, gyro_odr) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - gyro_is_enabled = 1U; - - return LSM6DSV16X_OK; -} - -/** - * @brief Disable the LSM6DSV16X gyroscope sensor - * @retval 0 in case of success, an error code otherwise - */ -LSM6DSV16XStatusTypeDef LSM6DSV16X::Disable_G() -{ - /* Check if the component is already disabled */ - if (gyro_is_enabled == 0U) { - return LSM6DSV16X_OK; - } - - /* Get current output data rate. */ - if (lsm6dsv16x_gy_data_rate_get(®_ctx, &gyro_odr) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - /* Output data rate selection - power down. */ - if (lsm6dsv16x_gy_data_rate_set(®_ctx, LSM6DSV16X_ODR_OFF) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - gyro_is_enabled = 0U; - - return LSM6DSV16X_OK; -} - -/** - * @brief Get the LSM6DSV16X gyroscope sensor sensitivity - * @param Sensitivity pointer - * @retval 0 in case of success, an error code otherwise - */ -LSM6DSV16XStatusTypeDef LSM6DSV16X::Get_G_Sensitivity(float *Sensitivity) -{ - lsm6dsv16x_gy_full_scale_t full_scale; - - /* Read actual full scale selection from sensor. */ - if (lsm6dsv16x_gy_full_scale_get(®_ctx, &full_scale) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - *Sensitivity = Convert_G_Sensitivity(full_scale); - if (*Sensitivity == 0.0f) { - return LSM6DSV16X_ERROR; - } - - return LSM6DSV16X_OK; -} - - -float LSM6DSV16X::Convert_G_Sensitivity(lsm6dsv16x_gy_full_scale_t full_scale) { - float Sensitivity = 0.0f; - /* Store the sensitivity based on actual full scale. */ - switch (full_scale) { - case LSM6DSV16X_125dps: - Sensitivity = LSM6DSV16X_GYRO_SENSITIVITY_FS_125DPS; - break; - - case LSM6DSV16X_250dps: - Sensitivity = LSM6DSV16X_GYRO_SENSITIVITY_FS_250DPS; - break; - - case LSM6DSV16X_500dps: - Sensitivity = LSM6DSV16X_GYRO_SENSITIVITY_FS_500DPS; - break; - - case LSM6DSV16X_1000dps: - Sensitivity = LSM6DSV16X_GYRO_SENSITIVITY_FS_1000DPS; - break; - - case LSM6DSV16X_2000dps: - Sensitivity = LSM6DSV16X_GYRO_SENSITIVITY_FS_2000DPS; - break; - - case LSM6DSV16X_4000dps: - Sensitivity = LSM6DSV16X_GYRO_SENSITIVITY_FS_4000DPS; - break; - } - return Sensitivity; -} - -/** - * @brief Get the LSM6DSV16X gyroscope sensor output data rate - * @param Odr pointer where the output data rate is written - * @retval 0 in case of success, an error code otherwise - */ -LSM6DSV16XStatusTypeDef LSM6DSV16X::Get_G_ODR(float *Odr) -{ - LSM6DSV16XStatusTypeDef ret = LSM6DSV16X_OK; - lsm6dsv16x_data_rate_t odr_low_level; - - /* Get current output data rate. */ - if (lsm6dsv16x_gy_data_rate_get(®_ctx, &odr_low_level) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - switch (odr_low_level) { - case LSM6DSV16X_ODR_OFF: - *Odr = 0.0f; - break; - - case LSM6DSV16X_ODR_AT_7Hz5: - *Odr = 7.5f; - break; - - case LSM6DSV16X_ODR_AT_15Hz: - *Odr = 15.0f; - break; - - case LSM6DSV16X_ODR_AT_30Hz: - *Odr = 30.0f; - break; - - case LSM6DSV16X_ODR_AT_60Hz: - *Odr = 60.0f; - break; - - case LSM6DSV16X_ODR_AT_120Hz: - *Odr = 120.0f; - break; - - case LSM6DSV16X_ODR_AT_240Hz: - *Odr = 240.0f; - break; - - case LSM6DSV16X_ODR_AT_480Hz: - *Odr = 480.0f; - break; - - case LSM6DSV16X_ODR_AT_960Hz: - *Odr = 960.0f; - break; - - case LSM6DSV16X_ODR_AT_1920Hz: - *Odr = 1920.0f; - break; - - case LSM6DSV16X_ODR_AT_3840Hz: - *Odr = 3840.0f; - break; - - case LSM6DSV16X_ODR_AT_7680Hz: - *Odr = 7680.0f; - break; - - default: - ret = LSM6DSV16X_ERROR; - break; - } - - return ret; -} - -/** - * @brief Set the LSM6DSV16X gyroscope sensor output data rate with operating mode - * @param Odr the output data rate value to be set - * @param Mode the gyroscope operating mode - * @retval 0 in case of success, an error code otherwise - */ -LSM6DSV16XStatusTypeDef LSM6DSV16X::Set_G_ODR(float Odr, LSM6DSV16X_GYRO_Operating_Mode_t Mode) -{ - switch (Mode) { - case LSM6DSV16X_GYRO_HIGH_PERFORMANCE_MODE: { - if (lsm6dsv16x_gy_mode_set(®_ctx, LSM6DSV16X_GY_HIGH_PERFORMANCE_MD) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - /* Valid ODR: 7.5Hz <= Odr <= 7.68kHz */ - Odr = (Odr < 7.5f) ? 7.5f - : (Odr > 7680.0f) ? 7680.0f - : Odr; - break; - } - - case LSM6DSV16X_GYRO_HIGH_ACCURACY_MODE: - // TODO: Not implemented. - // NOTE: According to datasheet, section `6.5 High-accuracy ODR mode`: - // "... the other sensor also has to be configured in high-accuracy ODR (HAODR) mode." - return LSM6DSV16X_ERROR; - - case LSM6DSV16X_GYRO_SLEEP_MODE: - // TODO: Not implemented. - // NOTE: Unknown ODR validity for this mode - return LSM6DSV16X_ERROR; - - case LSM6DSV16X_GYRO_LOW_POWER_MODE: { - if (lsm6dsv16x_gy_mode_set(®_ctx, LSM6DSV16X_GY_LOW_POWER_MD) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - /* Valid ODR: 7.5Hz <= Odr <= 240kHz */ - Odr = (Odr < 7.5f) ? 7.5f - : (Odr > 240.0f) ? 240.0f - : Odr; - break; - } - - default: - return LSM6DSV16X_ERROR; - } - - if (gyro_is_enabled == 1U) { - return Set_G_ODR_When_Enabled(Odr); - } else { - return Set_G_ODR_When_Disabled(Odr); - } -} - -/** - * @brief Set the LSM6DSV16X gyroscope sensor output data rate when enabled - * @param Odr the functional output data rate to be set - * @retval 0 in case of success, an error code otherwise - */ -LSM6DSV16XStatusTypeDef LSM6DSV16X::Set_G_ODR_When_Enabled(float Odr) -{ - lsm6dsv16x_data_rate_t new_odr; - - new_odr = (Odr <= 7.5f) ? LSM6DSV16X_ODR_AT_7Hz5 - : (Odr <= 15.0f) ? LSM6DSV16X_ODR_AT_15Hz - : (Odr <= 30.0f) ? LSM6DSV16X_ODR_AT_30Hz - : (Odr <= 60.0f) ? LSM6DSV16X_ODR_AT_60Hz - : (Odr <= 120.0f) ? LSM6DSV16X_ODR_AT_120Hz - : (Odr <= 240.0f) ? LSM6DSV16X_ODR_AT_240Hz - : (Odr <= 480.0f) ? LSM6DSV16X_ODR_AT_480Hz - : (Odr <= 960.0f) ? LSM6DSV16X_ODR_AT_960Hz - : (Odr <= 1920.0f) ? LSM6DSV16X_ODR_AT_1920Hz - : (Odr <= 3840.0f) ? LSM6DSV16X_ODR_AT_3840Hz - : LSM6DSV16X_ODR_AT_7680Hz; - - return (LSM6DSV16XStatusTypeDef) lsm6dsv16x_gy_data_rate_set(®_ctx, new_odr); -} - -/** - * @brief Set the LSM6DSV16X gyroscope sensor output data rate when disabled - * @param Odr the functional output data rate to be set - * @retval 0 in case of success, an error code otherwise - */ -LSM6DSV16XStatusTypeDef LSM6DSV16X::Set_G_ODR_When_Disabled(float Odr) -{ - gyro_odr = (Odr <= 7.5f) ? LSM6DSV16X_ODR_AT_7Hz5 - : (Odr <= 15.0f) ? LSM6DSV16X_ODR_AT_15Hz - : (Odr <= 30.0f) ? LSM6DSV16X_ODR_AT_30Hz - : (Odr <= 60.0f) ? LSM6DSV16X_ODR_AT_60Hz - : (Odr <= 120.0f) ? LSM6DSV16X_ODR_AT_120Hz - : (Odr <= 240.0f) ? LSM6DSV16X_ODR_AT_240Hz - : (Odr <= 480.0f) ? LSM6DSV16X_ODR_AT_480Hz - : (Odr <= 960.0f) ? LSM6DSV16X_ODR_AT_960Hz - : (Odr <= 1920.0f) ? LSM6DSV16X_ODR_AT_1920Hz - : (Odr <= 3840.0f) ? LSM6DSV16X_ODR_AT_3840Hz - : LSM6DSV16X_ODR_AT_7680Hz; - - return LSM6DSV16X_OK; -} - -/** - * @brief Get the LSM6DSV16X gyroscope sensor full scale - * @param FullScale pointer where the full scale is written - * @retval 0 in case of success, an error code otherwise - */ -LSM6DSV16XStatusTypeDef LSM6DSV16X::Get_G_FS(int32_t *FullScale) -{ - LSM6DSV16XStatusTypeDef ret = LSM6DSV16X_OK; - lsm6dsv16x_gy_full_scale_t fs_low_level; - - /* Read actual full scale selection from sensor. */ - if (lsm6dsv16x_gy_full_scale_get(®_ctx, &fs_low_level) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - switch (fs_low_level) { - case LSM6DSV16X_125dps: - *FullScale = 125; - break; - - case LSM6DSV16X_250dps: - *FullScale = 250; - break; - - case LSM6DSV16X_500dps: - *FullScale = 500; - break; - - case LSM6DSV16X_1000dps: - *FullScale = 1000; - break; - - case LSM6DSV16X_2000dps: - *FullScale = 2000; - break; - - case LSM6DSV16X_4000dps: - *FullScale = 4000; - break; - - default: - ret = LSM6DSV16X_ERROR; - break; - } - - return ret; -} - -/** - * @brief Set the LSM6DSV16X gyroscope sensor full scale - * @param FullScale the functional full scale to be set - * @retval 0 in case of success, an error code otherwise - */ -LSM6DSV16XStatusTypeDef LSM6DSV16X::Set_G_FS(int32_t FullScale) -{ - lsm6dsv16x_gy_full_scale_t new_fs; - - new_fs = (FullScale <= 125) ? LSM6DSV16X_125dps - : (FullScale <= 250) ? LSM6DSV16X_250dps - : (FullScale <= 500) ? LSM6DSV16X_500dps - : (FullScale <= 1000) ? LSM6DSV16X_1000dps - : (FullScale <= 2000) ? LSM6DSV16X_2000dps - : LSM6DSV16X_4000dps; - - if (new_fs == gyro_fs) { - return LSM6DSV16X_OK; - } - gyro_fs = new_fs; - - return (LSM6DSV16XStatusTypeDef) lsm6dsv16x_gy_full_scale_set(®_ctx, gyro_fs); -} - -/** - * @brief Get the LSM6DSV16X gyroscope sensor raw axes - * @param Value pointer where the raw values of the axes are written - * @retval 0 in case of success, an error code otherwise - */ -LSM6DSV16XStatusTypeDef LSM6DSV16X::Get_G_AxesRaw(int16_t *Value) -{ - lsm6dsv16x_axis3bit16_t data_raw; - - /* Read raw data values. */ - if (lsm6dsv16x_angular_rate_raw_get(®_ctx, data_raw.i16bit) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - /* Format the data. */ - Value[0] = data_raw.i16bit[0]; - Value[1] = data_raw.i16bit[1]; - Value[2] = data_raw.i16bit[2]; - - return LSM6DSV16X_OK; -} - -/** - * @brief Get the LSM6DSV16X gyroscope sensor raw axes when data available (Blocking) - * @param Value pointer where the raw values of the axes are written - * @retval 0 in case of success, an error code otherwise - */ -LSM6DSV16XStatusTypeDef LSM6DSV16X::Get_G_AxesRaw_When_Aval(int16_t *Value) { - lsm6dsv16x_data_ready_t drdy; - do { - if (lsm6dsv16x_flag_data_ready_get(®_ctx, &drdy) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - } while (!drdy.drdy_gy); - - if (Get_G_AxesRaw(Value) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - return LSM6DSV16X_OK; -} - -/** - * @brief Get the LSM6DSV16X gyroscope sensor axes - * @param AngularRate pointer where the values of the axes are written - * @retval 0 in case of success, an error code otherwise - */ -LSM6DSV16XStatusTypeDef LSM6DSV16X::Get_G_Axes(int32_t *AngularRate) -{ - lsm6dsv16x_axis3bit16_t data_raw; - float sensitivity = Convert_G_Sensitivity(gyro_fs); - - /* Read raw data values. */ - if (lsm6dsv16x_angular_rate_raw_get(®_ctx, data_raw.i16bit) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - /* Get LSM6DSV16X actual sensitivity. */ - if (sensitivity == 0.0f) { - return LSM6DSV16X_ERROR; - } - - /* Calculate the data. */ - AngularRate[0] = (int32_t)((float)((float)data_raw.i16bit[0] * sensitivity)); - AngularRate[1] = (int32_t)((float)((float)data_raw.i16bit[1] * sensitivity)); - AngularRate[2] = (int32_t)((float)((float)data_raw.i16bit[2] * sensitivity)); - - return LSM6DSV16X_OK; -} - -/** - * @brief Get the LSM6DSV16X GYRO data ready bit value - * @param Status the status of data ready bit - * @retval 0 in case of success, an error code otherwise - */ -LSM6DSV16XStatusTypeDef LSM6DSV16X::Get_G_DRDY_Status(uint8_t *Status) -{ - lsm6dsv16x_all_sources_t val; - - if (lsm6dsv16x_all_sources_get(®_ctx, &val) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - *Status = val.drdy_gy; - return LSM6DSV16X_OK; -} - -/** - * @brief Set the LSM6DSV16X gyroscope power mode - * @param PowerMode Value of the powerMode - * @retval 0 in case of success, an error code otherwise - */ -LSM6DSV16XStatusTypeDef LSM6DSV16X::Set_G_Power_Mode(uint8_t PowerMode) -{ - return (LSM6DSV16XStatusTypeDef)lsm6dsv16x_gy_mode_set( - ®_ctx, - (lsm6dsv16x_gy_mode_t)PowerMode - ); -} - -/** - * @brief Set the LSM6DSV16X gyroscope filter mode - * @param LowHighPassFlag 0/1 for setting low-pass/high-pass filter mode - * @param FilterMode Value of the filter Mode - * @retval 0 in case of success, an error code otherwise - */ -LSM6DSV16XStatusTypeDef LSM6DSV16X::Set_G_Filter_Mode(uint8_t LowHighPassFlag, uint8_t FilterMode) -{ - if (LowHighPassFlag == 0) { - /*Set gyroscope low_pass 1 filter-mode*/ - /* Enable low-pass filter */ - if (lsm6dsv16x_filt_gy_lp1_set(®_ctx, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - if (lsm6dsv16x_filt_gy_lp1_bandwidth_set(®_ctx, (lsm6dsv16x_filt_gy_lp1_bandwidth_t)FilterMode) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - } else { - /*Set gyroscope high_pass filter-mode*/ - /* Enable high-pass filter */ - if (lsm6dsv16x_filt_gy_lp1_set(®_ctx, 0) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - if (lsm6dsv16x_filt_gy_lp1_bandwidth_set(®_ctx, (lsm6dsv16x_filt_gy_lp1_bandwidth_t)FilterMode) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - } - return LSM6DSV16X_OK; -} - -LSM6DSV16XStatusTypeDef LSM6DSV16X::Set_G_Bias(float x, float y, float z) -{ - lsm6dsv16x_sflp_gbias_t gbias; - gbias.gbias_x = x; - gbias.gbias_y = y; - gbias.gbias_z = z; - return (LSM6DSV16XStatusTypeDef)lsm6dsv16x_sflp_game_gbias_set( - ®_ctx, - &gbias - ); -} - -LSM6DSV16XStatusTypeDef LSM6DSV16X::Set_SFLP_ODR(float odr) { - lsm6dsv16x_sflp_data_rate_t rate = odr <= 15 ? LSM6DSV16X_SFLP_15Hz - : odr <= 30 ? LSM6DSV16X_SFLP_30Hz - : odr <= 60 ? LSM6DSV16X_SFLP_60Hz - : odr <= 120 ? LSM6DSV16X_SFLP_120Hz - : odr <= 240 ? LSM6DSV16X_SFLP_240Hz - : LSM6DSV16X_SFLP_480Hz; - - return (LSM6DSV16XStatusTypeDef)lsm6dsv16x_sflp_data_rate_set( - ®_ctx, - rate - ); -} - - -/** - * @brief Enable the LSM6DSV16X QVAR sensor - * @retval 0 in case of success, an error code otherwise - */ -LSM6DSV16XStatusTypeDef LSM6DSV16X::QVAR_Enable() -{ - lsm6dsv16x_ctrl7_t ctrl7; - - if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_CTRL7, (uint8_t *)&ctrl7, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - ctrl7.ah_qvar_en = 1; - ctrl7.int2_drdy_ah_qvar = 1; - - if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_CTRL7, (uint8_t *)&ctrl7, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - return LSM6DSV16X_OK; -} - - -/** - * @brief Read LSM6DSV16X QVAR output data - * @param Data pointer where the value is written - * @retval 0 in case of success, an error code otherwise - */ -LSM6DSV16XStatusTypeDef LSM6DSV16X::QVAR_GetData(float *Data) -{ - lsm6dsv16x_axis1bit16_t data_raw; - (void)memset(data_raw.u8bit, 0x00, sizeof(int16_t)); - - if (lsm6dsv16x_ah_qvar_raw_get(®_ctx, &data_raw.i16bit) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - *Data = ((float)data_raw.i16bit) / LSM6DSV16X_QVAR_GAIN; - return LSM6DSV16X_OK; -} - - -/** - * @brief Set LSM6DSV16X QVAR equivalent input impedance - * @param val impedance in MOhm (2400MOhm, 730MOhm, 300MOhm, 255MOhm) - * @retval 0 in case of success, an error code otherwise - */ -LSM6DSV16XStatusTypeDef LSM6DSV16X::QVAR_SetImpedance(uint16_t val) -{ - LSM6DSV16XStatusTypeDef ret = LSM6DSV16X_OK; - lsm6dsv16x_ah_qvar_zin_t imp; - switch (val) { - case 2400: - imp = LSM6DSV16X_2400MOhm; - break; - case 730: - imp = LSM6DSV16X_730MOhm; - break; - case 300: - imp = LSM6DSV16X_300MOhm; - break; - case 255: - imp = LSM6DSV16X_255MOhm; - break; - default: - ret = LSM6DSV16X_ERROR; - break; - } - if (ret != LSM6DSV16X_ERROR) { - if (lsm6dsv16x_ah_qvar_zin_set(®_ctx, imp) != LSM6DSV16X_OK) { - ret = LSM6DSV16X_ERROR; - } - } - return ret; -} - -/** - * @brief Read LSM6DSV16X QVAR status - * @param val pointer where the value is written - * @retval 0 in case of success, an error code otherwise - */ - -LSM6DSV16XStatusTypeDef LSM6DSV16X::QVAR_GetStatus(uint8_t *val) -{ - lsm6dsv16x_status_reg_t status; - - if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_STATUS_REG, (uint8_t *)&status, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - *val = status.ah_qvarda; - - return LSM6DSV16X_OK; -} - -/** - * @brief Get MLC status - * @param status pointer where the MLC status is written - * @retval 0 in case of success, an error code otherwise - */ -LSM6DSV16XStatusTypeDef LSM6DSV16X::Get_MLC_Status(lsm6dsv16x_mlc_status_mainpage_t *status) -{ - if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_MLC_STATUS_MAINPAGE, (uint8_t *)status, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - return LSM6DSV16X_OK; -} - -/** - * @brief Get MLC output - * @param output pointer where the MLC output is written - * @retval 0 in case of success, an error code otherwise - */ -LSM6DSV16XStatusTypeDef LSM6DSV16X::Get_MLC_Output(lsm6dsv16x_mlc_out_t *output) -{ - if (lsm6dsv16x_mlc_out_get(®_ctx, output) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - return LSM6DSV16X_OK; -} - -/** - * @brief Get the LSM6DSV16X register value - * @param Reg address to be read - * @param Data pointer where the value is written - * @retval 0 in case of success, an error code otherwise - */ -LSM6DSV16XStatusTypeDef LSM6DSV16X::Read_Reg(uint8_t Reg, uint8_t *Data) -{ - if (lsm6dsv16x_read_reg(®_ctx, Reg, Data, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - return LSM6DSV16X_OK; -} - -/** - * @brief Set the LSM6DSV16X register value - * @param Reg address to be written - * @param Data value to be written - * @retval 0 in case of success, an error code otherwise - */ -LSM6DSV16XStatusTypeDef LSM6DSV16X::Write_Reg(uint8_t Reg, uint8_t Data) -{ - if (lsm6dsv16x_write_reg(®_ctx, Reg, &Data, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - return LSM6DSV16X_OK; -} - -int32_t LSM6DSV16X_io_write(void *handle, uint8_t WriteAddr, const uint8_t *pBuffer, uint16_t nBytesToWrite) -{ - return ((LSM6DSV16X *)handle)->IO_Write(pBuffer, WriteAddr, nBytesToWrite); -} - -int32_t LSM6DSV16X_io_read(void *handle, uint8_t ReadAddr, uint8_t *pBuffer, uint16_t nBytesToRead) -{ - return ((LSM6DSV16X *)handle)->IO_Read(pBuffer, ReadAddr, nBytesToRead); -} - -void LSM6DSV16X_sleep(uint32_t ms) { - delay(ms); -} - -LSM6DSV16XStatusTypeDef LSM6DSV16X::Reset_Set(uint8_t flags) -{ - if (lsm6dsv16x_reset_set(®_ctx, (lsm6dsv16x_reset_t)flags) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - lsm6dsv16x_reset_t rst; - do { - if (lsm6dsv16x_reset_get(®_ctx, &rst) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - } while (rst != LSM6DSV16X_READY); - return LSM6DSV16X_OK; -} - -LSM6DSV16XStatusTypeDef LSM6DSV16X::Enable_SFLP_Block(bool enable) -{ - return (LSM6DSV16XStatusTypeDef)lsm6dsv16x_sflp_game_rotation_set( - ®_ctx, - enable ? PROPERTY_ENABLE : PROPERTY_DISABLE - ); -} - -LSM6DSV16XStatusTypeDef LSM6DSV16X::Enable_Block_Data_Update(bool enable) -{ - return (LSM6DSV16XStatusTypeDef)lsm6dsv16x_block_data_update_set( - ®_ctx, - enable ? PROPERTY_ENABLE : PROPERTY_DISABLE - ); -} - -/** - * @brief Enable register address automatically incremented during a multiple byte - access with a serial interface. - * @retval 0 in case of success, an error code otherwise - */ -LSM6DSV16XStatusTypeDef LSM6DSV16X::Set_Auto_Increment(bool enable) -{ - return (LSM6DSV16XStatusTypeDef)lsm6dsv16x_auto_increment_set( - ®_ctx, - enable ? PROPERTY_ENABLE : PROPERTY_DISABLE - ); -} - -LSM6DSV16XStatusTypeDef LSM6DSV16X::Get_Temperature_Raw(int16_t * value) -{ - return (LSM6DSV16XStatusTypeDef)lsm6dsv16x_temperature_raw_get( - ®_ctx, - value - ); -} - -LSM6DSV16XStatusTypeDef LSM6DSV16X::Is_New_Temperature_Data(bool * available) -{ - lsm6dsv16x_status_reg_t status; - if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_STATUS_REG, (uint8_t *)&status, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - *available = status.tda; - return LSM6DSV16X_OK; -} - -LSM6DSV16XStatusTypeDef LSM6DSV16X::Set_SFLP_GBIAS(float x, float y, float z) { - lsm6dsv16x_sflp_gbias_t val = {0, 0, 0}; - val.gbias_x = x; - val.gbias_y = y; - val.gbias_z = z; - if(lsm6dsv16x_sflp_game_gbias_set(®_ctx, &val) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - return LSM6DSV16X_OK; -} - -LSM6DSV16XStatusTypeDef LSM6DSV16X::Enable_X_User_Offset() { - lsm6dsv16x_ctrl9_t ctrl9; - if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_CTRL9, (uint8_t *)&ctrl9, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - ctrl9.usr_off_on_out = true; //1 On, 0 Off - - if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_CTRL9, (uint8_t *)&ctrl9, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - // lsm6dsv16x_wake_up_ths_t wake_up_ths; - // if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_CTRL9, (uint8_t *)&wake_up_ths, 1) != LSM6DSV16X_OK) { - // return LSM6DSV16X_ERROR; - //} - //wake_up_ths.usr_off_on_wu = true; //1 On, 0 Off - - //if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_CTRL9, (uint8_t *)&wake_up_ths, 1) != LSM6DSV16X_OK) { - // return LSM6DSV16X_ERROR; - //} - - return LSM6DSV16X_OK; -} - -LSM6DSV16XStatusTypeDef LSM6DSV16X::Disable_X_User_Offset() { - lsm6dsv16x_ctrl9_t ctrl9; - if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_CTRL9, (uint8_t *)&ctrl9, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - ctrl9.usr_off_on_out = false; //1 On, 0 Off - - if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_CTRL9, (uint8_t *)&ctrl9, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - // lsm6dsv16x_wake_up_ths_t wake_up_ths; - // if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_CTRL9, (uint8_t *)&wake_up_ths, 1) != LSM6DSV16X_OK) { - // return LSM6DSV16X_ERROR; - //} - //wake_up_ths.usr_off_on_wu = false; //1 On, 0 Off - - //if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_CTRL9, (uint8_t *)&wake_up_ths, 1) != LSM6DSV16X_OK) { - // return LSM6DSV16X_ERROR; - //} - - return LSM6DSV16X_OK; -} - -LSM6DSV16XStatusTypeDef LSM6DSV16X::Set_X_User_Offset(float x, float y, float z) { - lsm6dsv16x_ctrl9_t ctrl9; - if (lsm6dsv16x_read_reg(®_ctx, LSM6DSV16X_CTRL9, (uint8_t *)&ctrl9, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - - - - int8_t xyz[3]; - - if ( // about +- 2 G's for high and +- 0.124 G's for low - (x <= LSM6DSV16X_ACC_USR_OFF_W_LOW_MAX && x >= -LSM6DSV16X_ACC_USR_OFF_W_LOW_MAX) && - (y <= LSM6DSV16X_ACC_USR_OFF_W_LOW_MAX && y >= -LSM6DSV16X_ACC_USR_OFF_W_LOW_MAX) && - (z <= LSM6DSV16X_ACC_USR_OFF_W_LOW_MAX && z >= -LSM6DSV16X_ACC_USR_OFF_W_LOW_MAX) - ) { //Then we are under the low requirements - xyz[0] = (int8_t)(x / LSM6DSV16X_ACC_USR_OFF_W_LOW_LSB); - xyz[1] = (int8_t)(y / LSM6DSV16X_ACC_USR_OFF_W_LOW_LSB); - xyz[2] = (int8_t)(z / LSM6DSV16X_ACC_USR_OFF_W_LOW_LSB); - ctrl9.usr_off_w = false; //(0: 2^-10 g/LSB; 1: 2^-6 g/LSB) - } - - else if ( // about +- 2 G's for high and +- 0.124 G's for low - (x <= LSM6DSV16X_ACC_USR_OFF_W_HIGH_MAX && x >= -LSM6DSV16X_ACC_USR_OFF_W_HIGH_MAX) && - (y <= LSM6DSV16X_ACC_USR_OFF_W_HIGH_MAX && y >= -LSM6DSV16X_ACC_USR_OFF_W_HIGH_MAX) && - (z <= LSM6DSV16X_ACC_USR_OFF_W_HIGH_MAX && z >= -LSM6DSV16X_ACC_USR_OFF_W_HIGH_MAX) - ) { //Then we are under the high requirements - xyz[0] = (int8_t)(x / LSM6DSV16X_ACC_USR_OFF_W_HIGH_LSB); - xyz[1] = (int8_t)(y / LSM6DSV16X_ACC_USR_OFF_W_HIGH_LSB); - xyz[2] = (int8_t)(z / LSM6DSV16X_ACC_USR_OFF_W_HIGH_LSB); - ctrl9.usr_off_w = true; //(0: 2^-10 g/LSB; 1: 2^-6 g/LSB) - } else { - return LSM6DSV16X_ERROR; //Value too big - } - - - if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_CTRL9, (uint8_t *)&ctrl9, 1) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - - - //convert from float in G's to what it wants. Signed byte - printf("x: %d, y: %d, z: %d", xyz[0], xyz[1], xyz[2]); - if (lsm6dsv16x_write_reg(®_ctx, LSM6DSV16X_X_OFS_USR, (uint8_t*)&xyz, 3) != LSM6DSV16X_OK) { - return LSM6DSV16X_ERROR; - } - return LSM6DSV16X_OK; -} - -uint32_t LSM6DSV16X::Half_Bits_To_Float_Bits(uint16_t h) -{ - uint16_t h_exp, h_sig; - uint32_t f_sgn, f_exp, f_sig; - - h_exp = (h&0x7c00u); - f_sgn = ((uint32_t)h&0x8000u) << 16; - switch (h_exp) { - case 0x0000u: /* 0 or subnormal */ - h_sig = (h&0x03ffu); - /* Signed zero */ - if (h_sig == 0) { - return f_sgn; - } - /* Subnormal */ - h_sig <<= 1; - while ((h_sig&0x0400u) == 0) { - h_sig <<= 1; - h_exp++; - } - f_exp = ((uint32_t)(127 - 15 - h_exp)) << 23; - f_sig = ((uint32_t)(h_sig&0x03ffu)) << 13; - return f_sgn + f_exp + f_sig; - case 0x7c00u: /* inf or NaN */ - /* All-ones exponent and a copy of the significand */ - return f_sgn + 0x7f800000u + (((uint32_t)(h&0x03ffu)) << 13); - default: /* normalized */ - /* Just need to adjust the exponent and shift */ - return f_sgn + (((uint32_t)(h&0x7fffu) + 0x1c000u) << 13); - } -} diff --git a/lib/LSM6DSV16X/LSM6DSV16X.h b/lib/LSM6DSV16X/LSM6DSV16X.h deleted file mode 100644 index 232f50690..000000000 --- a/lib/LSM6DSV16X/LSM6DSV16X.h +++ /dev/null @@ -1,433 +0,0 @@ -/** - ****************************************************************************** - * @file LSM6DSV16X.h - * @author SRA - * @version V1.5.1 - * @date July 2022 - * @brief Abstract Class of a LSM6DSV16X inertial measurement sensor. - ****************************************************************************** - * @attention - * - *

© COPYRIGHT(c) 2022 STMicroelectronics

- * - * Redistribution and use in source and binary forms, with or without modification, - * are permitted provided that the following conditions are met: - * 1. Redistributions of source code must retain the above copyright notice, - * this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * 3. Neither the name of STMicroelectronics nor the names of its contributors - * may be used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************** - */ - - -/* Prevent recursive inclusion -----------------------------------------------*/ - -#ifndef __LSM6DSV16X_H__ -#define __LSM6DSV16X_H__ - - -/* Includes ------------------------------------------------------------------*/ - -#include "Wire.h" -#include "SPI.h" -#include "lsm6dsv16x_reg.h" - - -/* Defines -------------------------------------------------------------------*/ - -#define LSM6DSV16X_ACC_SENSITIVITY_FS_2G 0.061f -#define LSM6DSV16X_ACC_SENSITIVITY_FS_4G 0.122f -#define LSM6DSV16X_ACC_SENSITIVITY_FS_8G 0.244f -#define LSM6DSV16X_ACC_SENSITIVITY_FS_16G 0.488f - -#define LSM6DSV16X_GYRO_SENSITIVITY_FS_125DPS 4.375f -#define LSM6DSV16X_GYRO_SENSITIVITY_FS_250DPS 8.750f -#define LSM6DSV16X_GYRO_SENSITIVITY_FS_500DPS 17.500f -#define LSM6DSV16X_GYRO_SENSITIVITY_FS_1000DPS 35.000f -#define LSM6DSV16X_GYRO_SENSITIVITY_FS_2000DPS 70.000f -#define LSM6DSV16X_GYRO_SENSITIVITY_FS_4000DPS 140.000f - -#define LSM6DSV16X_MIN_ST_LIMIT_mg 50.0f -#define LSM6DSV16X_MAX_ST_LIMIT_mg 1700.0f -#define LSM6DSV16X_MIN_ST_LIMIT_mdps 150000.0f -#define LSM6DSV16X_MAX_ST_LIMIT_mdps 700000.0f - -#define LSM6DSV16X_QVAR_GAIN 78.000f - -#define LSM6DSV16X_ACC_USR_OFF_W_HIGH_LSB (float)(pow(2, -6)) -#define LSM6DSV16X_ACC_USR_OFF_W_LOW_LSB (float)(pow(2, -10)) -#define LSM6DSV16X_ACC_USR_OFF_W_HIGH_MAX LSM6DSV16X_ACC_USR_OFF_W_HIGH_LSB * INT8_MAX -#define LSM6DSV16X_ACC_USR_OFF_W_LOW_MAX LSM6DSV16X_ACC_USR_OFF_W_LOW_LSB * INT8_MAX - - - - -//#define ENABLE_SPI -//#define I2C_LIB_DEBUG - -/* Typedefs ------------------------------------------------------------------*/ - -typedef enum { - LSM6DSV16X_OK = 0, - LSM6DSV16X_ERROR = -1 -} LSM6DSV16XStatusTypeDef; - -typedef enum { - LSM6DSV16X_INT1_PIN, - LSM6DSV16X_INT2_PIN, -} LSM6DSV16X_SensorIntPin_t; - -typedef struct { - unsigned int FreeFallStatus : 1; - unsigned int TapStatus : 1; - unsigned int DoubleTapStatus : 1; - unsigned int WakeUpStatus : 1; - unsigned int StepStatus : 1; - unsigned int TiltStatus : 1; - unsigned int D6DOrientationStatus : 1; - unsigned int SleepStatus : 1; -} LSM6DSV16X_Event_Status_t; - -typedef union { - int16_t i16bit[3]; - uint8_t u8bit[6]; -} lsm6dsv16x_axis3bit16_t; - -typedef union { - int16_t i16bit; - uint8_t u8bit[2]; -} lsm6dsv16x_axis1bit16_t; - -enum LSM6DSV16X_Reset { - LSM6DSV16X_RESET_GLOBAL = 0x1, - LSM6DSV16X_RESET_CAL_PARAM = 0x2, - LSM6DSV16X_RESET_CTRL_REGS = 0x4, -}; - -typedef enum { - LSM6DSV16X_ACC_HIGH_PERFORMANCE_MODE, - LSM6DSV16X_ACC_HIGH_ACCURACY_MODE, - LSM6DSV16X_ACC_NORMAL_MODE, - LSM6DSV16X_ACC_LOW_POWER_MODE1, - LSM6DSV16X_ACC_LOW_POWER_MODE2, - LSM6DSV16X_ACC_LOW_POWER_MODE3 -} LSM6DSV16X_ACC_Operating_Mode_t; - -typedef enum { - LSM6DSV16X_GYRO_HIGH_PERFORMANCE_MODE, - LSM6DSV16X_GYRO_HIGH_ACCURACY_MODE, - LSM6DSV16X_GYRO_SLEEP_MODE, - LSM6DSV16X_GYRO_LOW_POWER_MODE -} LSM6DSV16X_GYRO_Operating_Mode_t; - - -/* Class Declaration ---------------------------------------------------------*/ - -/** - * Abstract class of a LSM6DSV16X sensor. - */ -class LSM6DSV16X { - public: - LSM6DSV16X(TwoWire *i2c, uint8_t address = LSM6DSV16X_I2C_ADD_H); - LSM6DSV16X(SPIClass *spi, int cs_pin, uint32_t spi_speed = 2000000); - - LSM6DSV16XStatusTypeDef begin(); - LSM6DSV16XStatusTypeDef beginPreconfigured(); - LSM6DSV16XStatusTypeDef end(); - LSM6DSV16XStatusTypeDef ReadID(uint8_t *Id); - - LSM6DSV16XStatusTypeDef Enable_X(); - LSM6DSV16XStatusTypeDef Disable_X(); - LSM6DSV16XStatusTypeDef Enable_X_User_Offset(); - LSM6DSV16XStatusTypeDef Disable_X_User_Offset(); - LSM6DSV16XStatusTypeDef Get_X_Sensitivity(float *Sensitivity); - LSM6DSV16XStatusTypeDef Get_X_ODR(float *Odr); - LSM6DSV16XStatusTypeDef Set_X_ODR(float Odr, LSM6DSV16X_ACC_Operating_Mode_t Mode = LSM6DSV16X_ACC_HIGH_PERFORMANCE_MODE); - LSM6DSV16XStatusTypeDef Get_X_FS(int32_t *FullScale); - LSM6DSV16XStatusTypeDef Set_X_FS(int32_t FullScale); - LSM6DSV16XStatusTypeDef Get_X_AxesRaw(int16_t *Value); - LSM6DSV16XStatusTypeDef Get_X_AxesRaw_When_Aval(int16_t *Value); - LSM6DSV16XStatusTypeDef Get_X_Axes(int32_t *Acceleration); - LSM6DSV16XStatusTypeDef Get_X_DRDY_Status(uint8_t *Status); - LSM6DSV16XStatusTypeDef Get_X_Event_Status(LSM6DSV16X_Event_Status_t *Status); - LSM6DSV16XStatusTypeDef Set_X_Power_Mode(uint8_t PowerMode); - LSM6DSV16XStatusTypeDef Set_X_Filter_Mode(uint8_t LowHighPassFlag, uint8_t FilterMode); - LSM6DSV16XStatusTypeDef Set_X_User_Offset(float x, float y, float z); - - LSM6DSV16XStatusTypeDef Enable_G(); - LSM6DSV16XStatusTypeDef Disable_G(); - LSM6DSV16XStatusTypeDef Get_G_Sensitivity(float *Sensitivity); - LSM6DSV16XStatusTypeDef Get_G_ODR(float *Odr); - LSM6DSV16XStatusTypeDef Set_G_ODR(float Odr, LSM6DSV16X_GYRO_Operating_Mode_t Mode = LSM6DSV16X_GYRO_HIGH_PERFORMANCE_MODE); - LSM6DSV16XStatusTypeDef Get_G_FS(int32_t *FullScale); - LSM6DSV16XStatusTypeDef Set_G_FS(int32_t FullScale); - LSM6DSV16XStatusTypeDef Get_G_AxesRaw(int16_t *Value); - LSM6DSV16XStatusTypeDef Get_G_AxesRaw_When_Aval(int16_t *Value); - LSM6DSV16XStatusTypeDef Get_G_Axes(int32_t *AngularRate); - LSM6DSV16XStatusTypeDef Get_G_DRDY_Status(uint8_t *Status); - LSM6DSV16XStatusTypeDef Set_G_Power_Mode(uint8_t PowerMode); - LSM6DSV16XStatusTypeDef Set_G_Filter_Mode(uint8_t LowHighPassFlag, uint8_t FilterMode); - LSM6DSV16XStatusTypeDef Set_G_Bias(float x, float y, float z); - - LSM6DSV16XStatusTypeDef Test_IMU(uint8_t XTestType, uint8_t GTestType); - LSM6DSV16XStatusTypeDef Test_X_IMU(uint8_t TestType); - LSM6DSV16XStatusTypeDef Test_G_IMU(uint8_t TestType); - - LSM6DSV16XStatusTypeDef Get_T_ODR(float *Odr); - LSM6DSV16XStatusTypeDef Set_T_ODR(float Odr); - - LSM6DSV16XStatusTypeDef Enable_SFLP_Block(bool enable = true); - LSM6DSV16XStatusTypeDef Set_SFLP_ODR(float Odr); - LSM6DSV16XStatusTypeDef Set_SFLP_GBIAS(float x, float y, float z); - - LSM6DSV16XStatusTypeDef Enable_6D_Orientation(LSM6DSV16X_SensorIntPin_t IntPin); - LSM6DSV16XStatusTypeDef Disable_6D_Orientation(); - LSM6DSV16XStatusTypeDef Set_6D_Orientation_Threshold(uint8_t Threshold); - LSM6DSV16XStatusTypeDef Get_6D_Orientation_XL(uint8_t *XLow); - LSM6DSV16XStatusTypeDef Get_6D_Orientation_XH(uint8_t *XHigh); - LSM6DSV16XStatusTypeDef Get_6D_Orientation_YL(uint8_t *YLow); - LSM6DSV16XStatusTypeDef Get_6D_Orientation_YH(uint8_t *YHigh); - LSM6DSV16XStatusTypeDef Get_6D_Orientation_ZL(uint8_t *ZLow); - LSM6DSV16XStatusTypeDef Get_6D_Orientation_ZH(uint8_t *ZHigh); - - LSM6DSV16XStatusTypeDef Enable_Free_Fall_Detection(LSM6DSV16X_SensorIntPin_t IntPin); - LSM6DSV16XStatusTypeDef Disable_Free_Fall_Detection(); - LSM6DSV16XStatusTypeDef Set_Free_Fall_Threshold(uint8_t Threshold); - LSM6DSV16XStatusTypeDef Set_Free_Fall_Duration(uint8_t Duration); - - LSM6DSV16XStatusTypeDef Enable_Wake_Up_Detection(LSM6DSV16X_SensorIntPin_t IntPin); - LSM6DSV16XStatusTypeDef Disable_Wake_Up_Detection(); - LSM6DSV16XStatusTypeDef Set_Wake_Up_Threshold(uint32_t Threshold); - LSM6DSV16XStatusTypeDef Set_Wake_Up_Duration(uint8_t Duration); - - LSM6DSV16XStatusTypeDef Enable_Single_Tap_Detection(LSM6DSV16X_SensorIntPin_t IntPin); - LSM6DSV16XStatusTypeDef Disable_Single_Tap_Detection(); - LSM6DSV16XStatusTypeDef Enable_Double_Tap_Detection(LSM6DSV16X_SensorIntPin_t IntPin); - LSM6DSV16XStatusTypeDef Disable_Double_Tap_Detection(); - LSM6DSV16XStatusTypeDef Set_Tap_Threshold(uint8_t Threshold); - LSM6DSV16XStatusTypeDef Set_Tap_Shock_Time(uint8_t Time); - LSM6DSV16XStatusTypeDef Set_Tap_Quiet_Time(uint8_t Time); - LSM6DSV16XStatusTypeDef Set_Tap_Duration_Time(uint8_t Time); - - LSM6DSV16XStatusTypeDef Enable_Pedometer(LSM6DSV16X_SensorIntPin_t IntPin); - LSM6DSV16XStatusTypeDef Disable_Pedometer(); - LSM6DSV16XStatusTypeDef Get_Step_Count(uint16_t *StepCount); - LSM6DSV16XStatusTypeDef Step_Counter_Reset(); - - LSM6DSV16XStatusTypeDef Enable_Tilt_Detection(LSM6DSV16X_SensorIntPin_t IntPin); - LSM6DSV16XStatusTypeDef Disable_Tilt_Detection(); - - LSM6DSV16XStatusTypeDef FIFO_Reset(); - LSM6DSV16XStatusTypeDef FIFO_Get_Num_Samples(uint16_t *NumSamples); - LSM6DSV16XStatusTypeDef FIFO_Get_Full_Status(uint8_t *Status); - LSM6DSV16XStatusTypeDef FIFO_Set_INT1_FIFO_Full(uint8_t Status); - LSM6DSV16XStatusTypeDef FIFO_Set_INT2_FIFO_Full(uint8_t Status); - LSM6DSV16XStatusTypeDef FIFO_Set_Watermark_Level(uint8_t Watermark); - LSM6DSV16XStatusTypeDef FIFO_Set_Stop_On_Fth(uint8_t Status); - LSM6DSV16XStatusTypeDef FIFO_Set_Mode(uint8_t Mode); - LSM6DSV16XStatusTypeDef FIFO_Get_Tag(uint8_t *Tag); - LSM6DSV16XStatusTypeDef FIFO_Get_Data(uint8_t *Data); - LSM6DSV16XStatusTypeDef FIFO_Get_X_Axes(int32_t *Acceleration); - LSM6DSV16XStatusTypeDef FIFO_Set_X_BDR(float Bdr); - LSM6DSV16XStatusTypeDef FIFO_Get_G_Axes(int32_t *AngularVelocity); - LSM6DSV16XStatusTypeDef FIFO_Set_G_BDR(float Bdr); - LSM6DSV16XStatusTypeDef FIFO_Set_SFLP_Batch(bool GameRotation, bool Gravity, bool gBias); - LSM6DSV16XStatusTypeDef FIFO_Get_Status(lsm6dsv16x_fifo_status_t *Status); - LSM6DSV16XStatusTypeDef FIFO_Get_Gravity_Vector(float *data); - LSM6DSV16XStatusTypeDef FIFO_Get_Game_Vector(float *data); - LSM6DSV16XStatusTypeDef FIFO_Get_GBias_Vector(float *data); - LSM6DSV16XStatusTypeDef FIFO_Get_Timestamp(uint32_t *timestamp); - LSM6DSV16XStatusTypeDef FIFO_Set_Timestamp_Decimation(uint8_t decimation); - LSM6DSV16XStatusTypeDef Enable_Timestamp(); - - LSM6DSV16XStatusTypeDef QVAR_Enable(); - LSM6DSV16XStatusTypeDef QVAR_GetStatus(uint8_t *val); - LSM6DSV16XStatusTypeDef QVAR_SetImpedance(uint16_t val); - LSM6DSV16XStatusTypeDef QVAR_GetData(float *Data); - - LSM6DSV16XStatusTypeDef Get_MLC_Status(lsm6dsv16x_mlc_status_mainpage_t *status); - LSM6DSV16XStatusTypeDef Get_MLC_Output(lsm6dsv16x_mlc_out_t *output); - - LSM6DSV16XStatusTypeDef Read_Reg(uint8_t Reg, uint8_t *Data); - LSM6DSV16XStatusTypeDef Write_Reg(uint8_t Reg, uint8_t Data); - - LSM6DSV16XStatusTypeDef Reset_Set(uint8_t flags); - - LSM6DSV16XStatusTypeDef Enable_Block_Data_Update(bool enable = true); - LSM6DSV16XStatusTypeDef Set_Auto_Increment(bool enable); - - LSM6DSV16XStatusTypeDef Get_Temperature_Raw(int16_t *value); - LSM6DSV16XStatusTypeDef Is_New_Temperature_Data(bool *available); - - uint32_t Half_Bits_To_Float_Bits(uint16_t half_bits); - - /** - * @brief Utility function to read data. - * @param pBuffer: pointer to data to be read. - * @param RegisterAddr: specifies internal address register to be read. - * @param NumByteToRead: number of bytes to be read. - * @retval 0 if ok, an error code otherwise. - */ - uint8_t IO_Read(uint8_t *pBuffer, uint8_t RegisterAddr, uint16_t NumByteToRead) - { -#ifdef ENABLE_SPI - if (dev_spi) { -//#ifdef esp32 - dev_spi->beginTransaction(SPISettings(spi_speed, SPI_MSBFIRST, SPI_MODE3)); -//#else - dev_spi->beginTransaction(SPISettings(spi_speed, MSBFIRST, SPI_MODE3)); -//#endif - digitalWrite(cs_pin, LOW); - - /* Write Reg Address */ - dev_spi->transfer(RegisterAddr | 0x80); - /* Read the data */ - for (uint16_t i = 0; i < NumByteToRead; i++) { - *(pBuffer + i) = dev_spi->transfer(0x00); - } - - digitalWrite(cs_pin, HIGH); - - dev_spi->endTransaction(); - - return 0; - } -#endif - - if (dev_i2c) { -#ifdef I2C_LIB_DEBUG - printf("\n\n[LSM LIB] Read register: 0x%02x, Byte Count: %d bytes", RegisterAddr, NumByteToRead); -#endif - dev_i2c->beginTransmission(((uint8_t)(((address) >> 1) & 0x7F))); - dev_i2c->write(RegisterAddr); - dev_i2c->endTransmission(false); - - dev_i2c->requestFrom(((uint8_t)(((address) >> 1) & 0x7F)), (uint8_t) NumByteToRead); - - int i = 0; - while (dev_i2c->available()) { - pBuffer[i] = dev_i2c->read(); -#ifdef I2C_LIB_DEBUG - printf("\n[LSM LIB] Register Read: 0x%02x, Data: 0x%02x", RegisterAddr + i, pBuffer[i]); -#endif - i++; - } - - return 0; - } - - return 1; - } - - /** - * @brief Utility function to write data. - * @param pBuffer: pointer to data to be written. - * @param RegisterAddr: specifies internal address register to be written. - * @param NumByteToWrite: number of bytes to write. - * @retval 0 if ok, an error code otherwise. - */ - uint8_t IO_Write(const uint8_t *pBuffer, uint8_t RegisterAddr, uint16_t NumByteToWrite) - { -#ifdef ENABLE_SPI - if (dev_spi) { -//#ifdef esp32 - dev_spi->beginTransaction(SPISettings(spi_speed, SPI_MSBFIRST, SPI_MODE3)); -//#else - dev_spi->beginTransaction(SPISettings(spi_speed, MSBFIRST, SPI_MODE3)); -//#endif - digitalWrite(cs_pin, LOW); - - /* Write Reg Address */ - dev_spi->transfer(RegisterAddr); - /* Write the data */ - for (uint16_t i = 0; i < NumByteToWrite; i++) { - dev_spi->transfer(pBuffer[i]); - } - - digitalWrite(cs_pin, HIGH); - - dev_spi->endTransaction(); - - return 0; - } -#endif - - if (dev_i2c) { -#ifdef I2C_LIB_DEBUG - printf("\n\n[LSM LIB] Write register: 0x%02x, Byte Count: %d bytes", RegisterAddr, NumByteToWrite); -#endif - dev_i2c->beginTransmission(((uint8_t)(((address) >> 1) & 0x7F))); - - dev_i2c->write(RegisterAddr); - for (uint16_t i = 0 ; i < NumByteToWrite ; i++) { -#ifdef I2C_LIB_DEBUG - printf("\n[LSM LIB] Register Wrote: 0x%02x, Data: 0x%02x", RegisterAddr + i, pBuffer[i]); -#endif - dev_i2c->write(pBuffer[i]); - } - - dev_i2c->endTransmission(true); - - return 0; - } - - return 1; - } - - private: - LSM6DSV16XStatusTypeDef Set_X_ODR_When_Enabled(float Odr); - LSM6DSV16XStatusTypeDef Set_X_ODR_When_Disabled(float Odr); - LSM6DSV16XStatusTypeDef Set_G_ODR_When_Enabled(float Odr); - LSM6DSV16XStatusTypeDef Set_G_ODR_When_Disabled(float Odr); - - float Convert_X_Sensitivity(lsm6dsv16x_xl_full_scale_t full_scale); - float Convert_G_Sensitivity(lsm6dsv16x_gy_full_scale_t full_scale); - - /* Helper classes. */ - TwoWire *dev_i2c; - SPIClass *dev_spi; - - /* Configuration */ - uint8_t address; - int cs_pin; - uint32_t spi_speed; - - lsm6dsv16x_data_rate_t acc_odr; - lsm6dsv16x_data_rate_t gyro_odr; - lsm6dsv16x_xl_full_scale_t acc_fs; - lsm6dsv16x_gy_full_scale_t gyro_fs; - lsm6dsv16x_fifo_mode_t fifo_mode; - - uint8_t acc_is_enabled; - uint8_t gyro_is_enabled; - uint8_t initialized; - stmdev_ctx_t reg_ctx; -}; - -#ifdef __cplusplus -extern "C" { -#endif -int32_t LSM6DSV16X_io_write(void *handle, uint8_t WriteAddr, const uint8_t *pBuffer, uint16_t nBytesToWrite); -int32_t LSM6DSV16X_io_read(void *handle, uint8_t ReadAddr, uint8_t *pBuffer, uint16_t nBytesToRead); -void LSM6DSV16X_sleep(uint32_t ms); -#ifdef __cplusplus -} -#endif - -#endif /* __LSM6DSV16X_H__ */ diff --git a/lib/LSM6DSV16X/lsm6dsv16x_reg.c b/lib/LSM6DSV16X/lsm6dsv_reg.c similarity index 54% rename from lib/LSM6DSV16X/lsm6dsv16x_reg.c rename to lib/LSM6DSV16X/lsm6dsv_reg.c index 36c00ff84..1b1f6b9cb 100644 --- a/lib/LSM6DSV16X/lsm6dsv16x_reg.c +++ b/lib/LSM6DSV16X/lsm6dsv_reg.c @@ -1,8 +1,8 @@ /** ****************************************************************************** - * @file lsm6dsv16x_reg.c + * @file lsm6dsv_reg.c * @author Sensors Software Solution Team - * @brief LSM6DSV16X driver file + * @brief LSM6DSV driver file ****************************************************************************** * @attention * @@ -17,12 +17,12 @@ ****************************************************************************** */ -#include "lsm6dsv16x_reg.h" +#include "lsm6dsv_reg.h" /** - * @defgroup LSM6DSV16X + * @defgroup LSM6DSV * @brief This file provides a set of functions needed to drive the - * lsm6dsv16x enhanced inertial module. + * lsm6dsv enhanced inertial module. * @{ * */ @@ -46,9 +46,9 @@ * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t __weak lsm6dsv16x_read_reg(stmdev_ctx_t *ctx, uint8_t reg, - uint8_t *data, - uint16_t len) +int32_t __weak lsm6dsv_read_reg(stmdev_ctx_t *ctx, uint8_t reg, + uint8_t *data, + uint16_t len) { int32_t ret; @@ -67,9 +67,9 @@ int32_t __weak lsm6dsv16x_read_reg(stmdev_ctx_t *ctx, uint8_t reg, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t __weak lsm6dsv16x_write_reg(stmdev_ctx_t *ctx, uint8_t reg, - uint8_t *data, - uint16_t len) +int32_t __weak lsm6dsv_write_reg(stmdev_ctx_t *ctx, uint8_t reg, + uint8_t *data, + uint16_t len) { int32_t ret; @@ -109,76 +109,71 @@ static void bytecpy(uint8_t *target, uint8_t *source) * @{ * */ -float_t lsm6dsv16x_from_sflp_to_mg(int16_t lsb) +float_t lsm6dsv_from_sflp_to_mg(int16_t lsb) { return ((float_t)lsb) * 0.061f; } -float_t lsm6dsv16x_from_fs2_to_mg(int16_t lsb) +float_t lsm6dsv_from_fs2_to_mg(int16_t lsb) { return ((float_t)lsb) * 0.061f; } -float_t lsm6dsv16x_from_fs4_to_mg(int16_t lsb) +float_t lsm6dsv_from_fs4_to_mg(int16_t lsb) { return ((float_t)lsb) * 0.122f; } -float_t lsm6dsv16x_from_fs8_to_mg(int16_t lsb) +float_t lsm6dsv_from_fs8_to_mg(int16_t lsb) { return ((float_t)lsb) * 0.244f; } -float_t lsm6dsv16x_from_fs16_to_mg(int16_t lsb) +float_t lsm6dsv_from_fs16_to_mg(int16_t lsb) { return ((float_t)lsb) * 0.488f; } -float_t lsm6dsv16x_from_fs125_to_mdps(int16_t lsb) +float_t lsm6dsv_from_fs125_to_mdps(int16_t lsb) { return ((float_t)lsb) * 4.375f; } -float_t lsm6dsv16x_from_fs250_to_mdps(int16_t lsb) +float_t lsm6dsv_from_fs250_to_mdps(int16_t lsb) { return ((float_t)lsb) * 8.750f; } -float_t lsm6dsv16x_from_fs500_to_mdps(int16_t lsb) +float_t lsm6dsv_from_fs500_to_mdps(int16_t lsb) { return ((float_t)lsb) * 17.50f; } -float_t lsm6dsv16x_from_fs1000_to_mdps(int16_t lsb) +float_t lsm6dsv_from_fs1000_to_mdps(int16_t lsb) { return ((float_t)lsb) * 35.0f; } -float_t lsm6dsv16x_from_fs2000_to_mdps(int16_t lsb) +float_t lsm6dsv_from_fs2000_to_mdps(int16_t lsb) { return ((float_t)lsb) * 70.0f; } -float_t lsm6dsv16x_from_fs4000_to_mdps(int16_t lsb) +float_t lsm6dsv_from_fs4000_to_mdps(int16_t lsb) { return ((float_t)lsb) * 140.0f; } -float_t lsm6dsv16x_from_lsb_to_celsius(int16_t lsb) +float_t lsm6dsv_from_lsb_to_celsius(int16_t lsb) { return (((float_t)lsb / 256.0f) + 25.0f); } -float_t lsm6dsv16x_from_lsb_to_nsec(uint32_t lsb) +float_t lsm6dsv_from_lsb_to_nsec(uint32_t lsb) { return ((float_t)lsb * 21750.0f); } -float_t lsm6dsv16x_from_lsb_to_mv(int16_t lsb) -{ - return ((float_t)lsb) / 78.0f; -} - /** * @} * @@ -200,16 +195,16 @@ float_t lsm6dsv16x_from_lsb_to_mv(int16_t lsb) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_xl_offset_on_out_set(stmdev_ctx_t *ctx, uint8_t val) +int32_t lsm6dsv_xl_offset_on_out_set(stmdev_ctx_t *ctx, uint8_t val) { - lsm6dsv16x_ctrl9_t ctrl9; + lsm6dsv_ctrl9_t ctrl9; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL9, (uint8_t *)&ctrl9, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_CTRL9, (uint8_t *)&ctrl9, 1); if (ret == 0) { ctrl9.usr_off_on_out = val; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_CTRL9, (uint8_t *)&ctrl9, 1); + ret = lsm6dsv_write_reg(ctx, LSM6DSV_CTRL9, (uint8_t *)&ctrl9, 1); } return ret; @@ -223,12 +218,12 @@ int32_t lsm6dsv16x_xl_offset_on_out_set(stmdev_ctx_t *ctx, uint8_t val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_xl_offset_on_out_get(stmdev_ctx_t *ctx, uint8_t *val) +int32_t lsm6dsv_xl_offset_on_out_get(stmdev_ctx_t *ctx, uint8_t *val) { - lsm6dsv16x_ctrl9_t ctrl9; + lsm6dsv_ctrl9_t ctrl9; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL9, (uint8_t *)&ctrl9, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_CTRL9, (uint8_t *)&ctrl9, 1); *val = ctrl9.usr_off_on_out; return ret; @@ -242,19 +237,19 @@ int32_t lsm6dsv16x_xl_offset_on_out_get(stmdev_ctx_t *ctx, uint8_t *val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_xl_offset_mg_set(stmdev_ctx_t *ctx, - lsm6dsv16x_xl_offset_mg_t val) +int32_t lsm6dsv_xl_offset_mg_set(stmdev_ctx_t *ctx, + lsm6dsv_xl_offset_mg_t val) { - lsm6dsv16x_z_ofs_usr_t z_ofs_usr; - lsm6dsv16x_y_ofs_usr_t y_ofs_usr; - lsm6dsv16x_x_ofs_usr_t x_ofs_usr; - lsm6dsv16x_ctrl9_t ctrl9; + lsm6dsv_z_ofs_usr_t z_ofs_usr; + lsm6dsv_y_ofs_usr_t y_ofs_usr; + lsm6dsv_x_ofs_usr_t x_ofs_usr; + lsm6dsv_ctrl9_t ctrl9; int32_t ret; float_t tmp; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_Z_OFS_USR, (uint8_t *)&z_ofs_usr, 1); - ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_Y_OFS_USR, (uint8_t *)&y_ofs_usr, 1); - ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_X_OFS_USR, (uint8_t *)&x_ofs_usr, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_Z_OFS_USR, (uint8_t *)&z_ofs_usr, 1); + ret += lsm6dsv_read_reg(ctx, LSM6DSV_Y_OFS_USR, (uint8_t *)&y_ofs_usr, 1); + ret += lsm6dsv_read_reg(ctx, LSM6DSV_X_OFS_USR, (uint8_t *)&x_ofs_usr, 1); if (ret != 0) { return ret; } @@ -296,10 +291,10 @@ int32_t lsm6dsv16x_xl_offset_mg_set(stmdev_ctx_t *ctx, x_ofs_usr.x_ofs_usr = 0xFFU; } - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_Z_OFS_USR, (uint8_t *)&z_ofs_usr, 1); - ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_Y_OFS_USR, (uint8_t *)&y_ofs_usr, 1); - ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_X_OFS_USR, (uint8_t *)&x_ofs_usr, 1); - ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_CTRL9, (uint8_t *)&ctrl9, 1); + ret = lsm6dsv_write_reg(ctx, LSM6DSV_Z_OFS_USR, (uint8_t *)&z_ofs_usr, 1); + ret += lsm6dsv_write_reg(ctx, LSM6DSV_Y_OFS_USR, (uint8_t *)&y_ofs_usr, 1); + ret += lsm6dsv_write_reg(ctx, LSM6DSV_X_OFS_USR, (uint8_t *)&x_ofs_usr, 1); + ret += lsm6dsv_write_reg(ctx, LSM6DSV_CTRL9, (uint8_t *)&ctrl9, 1); return ret; } @@ -312,19 +307,19 @@ int32_t lsm6dsv16x_xl_offset_mg_set(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_xl_offset_mg_get(stmdev_ctx_t *ctx, - lsm6dsv16x_xl_offset_mg_t *val) +int32_t lsm6dsv_xl_offset_mg_get(stmdev_ctx_t *ctx, + lsm6dsv_xl_offset_mg_t *val) { - lsm6dsv16x_z_ofs_usr_t z_ofs_usr; - lsm6dsv16x_y_ofs_usr_t y_ofs_usr; - lsm6dsv16x_x_ofs_usr_t x_ofs_usr; - lsm6dsv16x_ctrl9_t ctrl9; + lsm6dsv_z_ofs_usr_t z_ofs_usr; + lsm6dsv_y_ofs_usr_t y_ofs_usr; + lsm6dsv_x_ofs_usr_t x_ofs_usr; + lsm6dsv_ctrl9_t ctrl9; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL9, (uint8_t *)&ctrl9, 1); - ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_Z_OFS_USR, (uint8_t *)&z_ofs_usr, 1); - ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_Y_OFS_USR, (uint8_t *)&y_ofs_usr, 1); - ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_X_OFS_USR, (uint8_t *)&x_ofs_usr, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_CTRL9, (uint8_t *)&ctrl9, 1); + ret += lsm6dsv_read_reg(ctx, LSM6DSV_Z_OFS_USR, (uint8_t *)&z_ofs_usr, 1); + ret += lsm6dsv_read_reg(ctx, LSM6DSV_Y_OFS_USR, (uint8_t *)&y_ofs_usr, 1); + ret += lsm6dsv_read_reg(ctx, LSM6DSV_X_OFS_USR, (uint8_t *)&x_ofs_usr, 1); if (ret != 0) { return ret; } if (ctrl9.usr_off_w == PROPERTY_DISABLE) @@ -356,22 +351,22 @@ int32_t lsm6dsv16x_xl_offset_mg_get(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_reset_set(stmdev_ctx_t *ctx, lsm6dsv16x_reset_t val) +int32_t lsm6dsv_reset_set(stmdev_ctx_t *ctx, lsm6dsv_reset_t val) { - lsm6dsv16x_func_cfg_access_t func_cfg_access; - lsm6dsv16x_ctrl3_t ctrl3; + lsm6dsv_func_cfg_access_t func_cfg_access; + lsm6dsv_ctrl3_t ctrl3; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL3, (uint8_t *)&ctrl3, 1); - ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FUNC_CFG_ACCESS, (uint8_t *)&func_cfg_access, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_CTRL3, (uint8_t *)&ctrl3, 1); + ret += lsm6dsv_read_reg(ctx, LSM6DSV_FUNC_CFG_ACCESS, (uint8_t *)&func_cfg_access, 1); if (ret != 0) { return ret; } ctrl3.boot = ((uint8_t)val & 0x04U) >> 2; ctrl3.sw_reset = ((uint8_t)val & 0x02U) >> 1; func_cfg_access.sw_por = (uint8_t)val & 0x01U; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_CTRL3, (uint8_t *)&ctrl3, 1); - ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_FUNC_CFG_ACCESS, (uint8_t *)&func_cfg_access, 1); + ret = lsm6dsv_write_reg(ctx, LSM6DSV_CTRL3, (uint8_t *)&ctrl3, 1); + ret += lsm6dsv_write_reg(ctx, LSM6DSV_FUNC_CFG_ACCESS, (uint8_t *)&func_cfg_access, 1); return ret; } @@ -384,36 +379,36 @@ int32_t lsm6dsv16x_reset_set(stmdev_ctx_t *ctx, lsm6dsv16x_reset_t val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_reset_get(stmdev_ctx_t *ctx, lsm6dsv16x_reset_t *val) +int32_t lsm6dsv_reset_get(stmdev_ctx_t *ctx, lsm6dsv_reset_t *val) { - lsm6dsv16x_func_cfg_access_t func_cfg_access; - lsm6dsv16x_ctrl3_t ctrl3; + lsm6dsv_func_cfg_access_t func_cfg_access; + lsm6dsv_ctrl3_t ctrl3; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL3, (uint8_t *)&ctrl3, 1); - ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FUNC_CFG_ACCESS, (uint8_t *)&func_cfg_access, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_CTRL3, (uint8_t *)&ctrl3, 1); + ret += lsm6dsv_read_reg(ctx, LSM6DSV_FUNC_CFG_ACCESS, (uint8_t *)&func_cfg_access, 1); if (ret != 0) { return ret; } switch ((ctrl3.sw_reset << 2) + (ctrl3.boot << 1) + func_cfg_access.sw_por) { - case LSM6DSV16X_READY: - *val = LSM6DSV16X_READY; + case LSM6DSV_READY: + *val = LSM6DSV_READY; break; - case LSM6DSV16X_GLOBAL_RST: - *val = LSM6DSV16X_GLOBAL_RST; + case LSM6DSV_GLOBAL_RST: + *val = LSM6DSV_GLOBAL_RST; break; - case LSM6DSV16X_RESTORE_CAL_PARAM: - *val = LSM6DSV16X_RESTORE_CAL_PARAM; + case LSM6DSV_RESTORE_CAL_PARAM: + *val = LSM6DSV_RESTORE_CAL_PARAM; break; - case LSM6DSV16X_RESTORE_CTRL_REGS: - *val = LSM6DSV16X_RESTORE_CTRL_REGS; + case LSM6DSV_RESTORE_CTRL_REGS: + *val = LSM6DSV_RESTORE_CTRL_REGS; break; default: - *val = LSM6DSV16X_GLOBAL_RST; + *val = LSM6DSV_GLOBAL_RST; break; } @@ -428,17 +423,17 @@ int32_t lsm6dsv16x_reset_get(stmdev_ctx_t *ctx, lsm6dsv16x_reset_t *val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_mem_bank_set(stmdev_ctx_t *ctx, lsm6dsv16x_mem_bank_t val) +int32_t lsm6dsv_mem_bank_set(stmdev_ctx_t *ctx, lsm6dsv_mem_bank_t val) { - lsm6dsv16x_func_cfg_access_t func_cfg_access; + lsm6dsv_func_cfg_access_t func_cfg_access; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FUNC_CFG_ACCESS, (uint8_t *)&func_cfg_access, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_FUNC_CFG_ACCESS, (uint8_t *)&func_cfg_access, 1); if (ret != 0) { return ret; } func_cfg_access.shub_reg_access = ((uint8_t)val & 0x02U) >> 1; func_cfg_access.emb_func_reg_access = (uint8_t)val & 0x01U; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_FUNC_CFG_ACCESS, (uint8_t *)&func_cfg_access, 1); + ret = lsm6dsv_write_reg(ctx, LSM6DSV_FUNC_CFG_ACCESS, (uint8_t *)&func_cfg_access, 1); return ret; } @@ -451,30 +446,30 @@ int32_t lsm6dsv16x_mem_bank_set(stmdev_ctx_t *ctx, lsm6dsv16x_mem_bank_t val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_mem_bank_get(stmdev_ctx_t *ctx, lsm6dsv16x_mem_bank_t *val) +int32_t lsm6dsv_mem_bank_get(stmdev_ctx_t *ctx, lsm6dsv_mem_bank_t *val) { - lsm6dsv16x_func_cfg_access_t func_cfg_access; + lsm6dsv_func_cfg_access_t func_cfg_access; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FUNC_CFG_ACCESS, (uint8_t *)&func_cfg_access, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_FUNC_CFG_ACCESS, (uint8_t *)&func_cfg_access, 1); if (ret != 0) { return ret; } switch ((func_cfg_access.shub_reg_access << 1) + func_cfg_access.emb_func_reg_access) { - case LSM6DSV16X_MAIN_MEM_BANK: - *val = LSM6DSV16X_MAIN_MEM_BANK; + case LSM6DSV_MAIN_MEM_BANK: + *val = LSM6DSV_MAIN_MEM_BANK; break; - case LSM6DSV16X_EMBED_FUNC_MEM_BANK: - *val = LSM6DSV16X_EMBED_FUNC_MEM_BANK; + case LSM6DSV_EMBED_FUNC_MEM_BANK: + *val = LSM6DSV_EMBED_FUNC_MEM_BANK; break; - case LSM6DSV16X_SENSOR_HUB_MEM_BANK: - *val = LSM6DSV16X_SENSOR_HUB_MEM_BANK; + case LSM6DSV_SENSOR_HUB_MEM_BANK: + *val = LSM6DSV_SENSOR_HUB_MEM_BANK; break; default: - *val = LSM6DSV16X_MAIN_MEM_BANK; + *val = LSM6DSV_MAIN_MEM_BANK; break; } @@ -490,11 +485,11 @@ int32_t lsm6dsv16x_mem_bank_get(stmdev_ctx_t *ctx, lsm6dsv16x_mem_bank_t *val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_device_id_get(stmdev_ctx_t *ctx, uint8_t *val) +int32_t lsm6dsv_device_id_get(stmdev_ctx_t *ctx, uint8_t *val) { int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_WHO_AM_I, val, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_WHO_AM_I, val, 1); return ret; } @@ -503,31 +498,31 @@ int32_t lsm6dsv16x_device_id_get(stmdev_ctx_t *ctx, uint8_t *val) * @brief Accelerometer output data rate (ODR) selection.[set] * * @param ctx read / write interface definitions - * @param val lsm6dsv16x_data_rate_t enum + * @param val lsm6dsv_data_rate_t enum * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_xl_data_rate_set(stmdev_ctx_t *ctx, - lsm6dsv16x_data_rate_t val) +int32_t lsm6dsv_xl_data_rate_set(stmdev_ctx_t *ctx, + lsm6dsv_data_rate_t val) { - lsm6dsv16x_ctrl1_t ctrl1; - lsm6dsv16x_haodr_cfg_t haodr; + lsm6dsv_ctrl1_t ctrl1; + lsm6dsv_haodr_cfg_t haodr; uint8_t sel; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL1, (uint8_t *)&ctrl1, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_CTRL1, (uint8_t *)&ctrl1, 1); if (ret != 0) { return ret; } ctrl1.odr_xl = (uint8_t)val & 0x0Fu; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_CTRL1, (uint8_t *)&ctrl1, 1); + ret = lsm6dsv_write_reg(ctx, LSM6DSV_CTRL1, (uint8_t *)&ctrl1, 1); if (ret != 0) { return ret; } sel = ((uint8_t)val >> 4) & 0xFU; if (sel != 0U) { - ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_HAODR_CFG, (uint8_t *)&haodr, 1); + ret += lsm6dsv_read_reg(ctx, LSM6DSV_HAODR_CFG, (uint8_t *)&haodr, 1); haodr.haodr_sel = sel; - ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_HAODR_CFG, (uint8_t *)&haodr, 1); + ret += lsm6dsv_write_reg(ctx, LSM6DSV_HAODR_CFG, (uint8_t *)&haodr, 1); } return ret; @@ -537,190 +532,190 @@ int32_t lsm6dsv16x_xl_data_rate_set(stmdev_ctx_t *ctx, * @brief Accelerometer output data rate (ODR) selection.[get] * * @param ctx read / write interface definitions - * @param val lsm6dsv16x_data_rate_t enum + * @param val lsm6dsv_data_rate_t enum * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_xl_data_rate_get(stmdev_ctx_t *ctx, - lsm6dsv16x_data_rate_t *val) +int32_t lsm6dsv_xl_data_rate_get(stmdev_ctx_t *ctx, + lsm6dsv_data_rate_t *val) { - lsm6dsv16x_ctrl1_t ctrl1; - lsm6dsv16x_haodr_cfg_t haodr; + lsm6dsv_ctrl1_t ctrl1; + lsm6dsv_haodr_cfg_t haodr; uint8_t sel; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL1, (uint8_t *)&ctrl1, 1); - ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_HAODR_CFG, (uint8_t *)&haodr, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_CTRL1, (uint8_t *)&ctrl1, 1); + ret += lsm6dsv_read_reg(ctx, LSM6DSV_HAODR_CFG, (uint8_t *)&haodr, 1); if (ret != 0) { return ret; } sel = haodr.haodr_sel; switch (ctrl1.odr_xl) { - case LSM6DSV16X_ODR_OFF: - *val = LSM6DSV16X_ODR_OFF; + case LSM6DSV_ODR_OFF: + *val = LSM6DSV_ODR_OFF; break; - case LSM6DSV16X_ODR_AT_1Hz875: - *val = LSM6DSV16X_ODR_AT_1Hz875; + case LSM6DSV_ODR_AT_1Hz875: + *val = LSM6DSV_ODR_AT_1Hz875; break; - case LSM6DSV16X_ODR_AT_7Hz5: - *val = LSM6DSV16X_ODR_AT_7Hz5; + case LSM6DSV_ODR_AT_7Hz5: + *val = LSM6DSV_ODR_AT_7Hz5; break; - case LSM6DSV16X_ODR_AT_15Hz: + case LSM6DSV_ODR_AT_15Hz: switch (sel) { default: case 0: - *val = LSM6DSV16X_ODR_AT_15Hz; + *val = LSM6DSV_ODR_AT_15Hz; break; case 1: - *val = LSM6DSV16X_ODR_HA01_AT_15Hz625; + *val = LSM6DSV_ODR_HA01_AT_15Hz625; break; case 2: - *val = LSM6DSV16X_ODR_HA02_AT_12Hz5; + *val = LSM6DSV_ODR_HA02_AT_12Hz5; break; } break; - case LSM6DSV16X_ODR_AT_30Hz: + case LSM6DSV_ODR_AT_30Hz: switch (sel) { default: case 0: - *val = LSM6DSV16X_ODR_AT_30Hz; + *val = LSM6DSV_ODR_AT_30Hz; break; case 1: - *val = LSM6DSV16X_ODR_HA01_AT_31Hz25; + *val = LSM6DSV_ODR_HA01_AT_31Hz25; break; case 2: - *val = LSM6DSV16X_ODR_HA02_AT_25Hz; + *val = LSM6DSV_ODR_HA02_AT_25Hz; break; } break; - case LSM6DSV16X_ODR_AT_60Hz: + case LSM6DSV_ODR_AT_60Hz: switch (sel) { default: case 0: - *val = LSM6DSV16X_ODR_AT_60Hz; + *val = LSM6DSV_ODR_AT_60Hz; break; case 1: - *val = LSM6DSV16X_ODR_HA01_AT_62Hz5; + *val = LSM6DSV_ODR_HA01_AT_62Hz5; break; case 2: - *val = LSM6DSV16X_ODR_HA02_AT_50Hz; + *val = LSM6DSV_ODR_HA02_AT_50Hz; break; } break; - case LSM6DSV16X_ODR_AT_120Hz: + case LSM6DSV_ODR_AT_120Hz: switch (sel) { default: case 0: - *val = LSM6DSV16X_ODR_AT_120Hz; + *val = LSM6DSV_ODR_AT_120Hz; break; case 1: - *val = LSM6DSV16X_ODR_HA01_AT_125Hz; + *val = LSM6DSV_ODR_HA01_AT_125Hz; break; case 2: - *val = LSM6DSV16X_ODR_HA02_AT_100Hz; + *val = LSM6DSV_ODR_HA02_AT_100Hz; break; } break; - case LSM6DSV16X_ODR_AT_240Hz: + case LSM6DSV_ODR_AT_240Hz: switch (sel) { default: case 0: - *val = LSM6DSV16X_ODR_AT_240Hz; + *val = LSM6DSV_ODR_AT_240Hz; break; case 1: - *val = LSM6DSV16X_ODR_HA01_AT_250Hz; + *val = LSM6DSV_ODR_HA01_AT_250Hz; break; case 2: - *val = LSM6DSV16X_ODR_HA02_AT_200Hz; + *val = LSM6DSV_ODR_HA02_AT_200Hz; break; } break; - case LSM6DSV16X_ODR_AT_480Hz: + case LSM6DSV_ODR_AT_480Hz: switch (sel) { default: case 0: - *val = LSM6DSV16X_ODR_AT_480Hz; + *val = LSM6DSV_ODR_AT_480Hz; break; case 1: - *val = LSM6DSV16X_ODR_HA01_AT_500Hz; + *val = LSM6DSV_ODR_HA01_AT_500Hz; break; case 2: - *val = LSM6DSV16X_ODR_HA02_AT_400Hz; + *val = LSM6DSV_ODR_HA02_AT_400Hz; break; } break; - case LSM6DSV16X_ODR_AT_960Hz: + case LSM6DSV_ODR_AT_960Hz: switch (sel) { default: case 0: - *val = LSM6DSV16X_ODR_AT_960Hz; + *val = LSM6DSV_ODR_AT_960Hz; break; case 1: - *val = LSM6DSV16X_ODR_HA01_AT_1000Hz; + *val = LSM6DSV_ODR_HA01_AT_1000Hz; break; case 2: - *val = LSM6DSV16X_ODR_HA02_AT_800Hz; + *val = LSM6DSV_ODR_HA02_AT_800Hz; break; } break; - case LSM6DSV16X_ODR_AT_1920Hz: + case LSM6DSV_ODR_AT_1920Hz: switch (sel) { default: case 0: - *val = LSM6DSV16X_ODR_AT_1920Hz; + *val = LSM6DSV_ODR_AT_1920Hz; break; case 1: - *val = LSM6DSV16X_ODR_HA01_AT_2000Hz; + *val = LSM6DSV_ODR_HA01_AT_2000Hz; break; case 2: - *val = LSM6DSV16X_ODR_HA02_AT_1600Hz; + *val = LSM6DSV_ODR_HA02_AT_1600Hz; break; } break; - case LSM6DSV16X_ODR_AT_3840Hz: + case LSM6DSV_ODR_AT_3840Hz: switch (sel) { default: case 0: - *val = LSM6DSV16X_ODR_AT_3840Hz; + *val = LSM6DSV_ODR_AT_3840Hz; break; case 1: - *val = LSM6DSV16X_ODR_HA01_AT_4000Hz; + *val = LSM6DSV_ODR_HA01_AT_4000Hz; break; case 2: - *val = LSM6DSV16X_ODR_HA02_AT_3200Hz; + *val = LSM6DSV_ODR_HA02_AT_3200Hz; break; } break; - case LSM6DSV16X_ODR_AT_7680Hz: + case LSM6DSV_ODR_AT_7680Hz: switch (sel) { default: case 0: - *val = LSM6DSV16X_ODR_AT_7680Hz; + *val = LSM6DSV_ODR_AT_7680Hz; break; case 1: - *val = LSM6DSV16X_ODR_HA01_AT_8000Hz; + *val = LSM6DSV_ODR_HA01_AT_8000Hz; break; case 2: - *val = LSM6DSV16X_ODR_HA02_AT_6400Hz; + *val = LSM6DSV_ODR_HA02_AT_6400Hz; break; } break; default: - *val = LSM6DSV16X_ODR_OFF; + *val = LSM6DSV_ODR_OFF; break; } @@ -735,17 +730,17 @@ int32_t lsm6dsv16x_xl_data_rate_get(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_xl_mode_set(stmdev_ctx_t *ctx, lsm6dsv16x_xl_mode_t val) +int32_t lsm6dsv_xl_mode_set(stmdev_ctx_t *ctx, lsm6dsv_xl_mode_t val) { - lsm6dsv16x_ctrl1_t ctrl1; + lsm6dsv_ctrl1_t ctrl1; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL1, (uint8_t *)&ctrl1, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_CTRL1, (uint8_t *)&ctrl1, 1); if (ret == 0) { ctrl1.op_mode_xl = (uint8_t)val & 0x07U; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_CTRL1, (uint8_t *)&ctrl1, 1); + ret = lsm6dsv_write_reg(ctx, LSM6DSV_CTRL1, (uint8_t *)&ctrl1, 1); } return ret; @@ -759,46 +754,46 @@ int32_t lsm6dsv16x_xl_mode_set(stmdev_ctx_t *ctx, lsm6dsv16x_xl_mode_t val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_xl_mode_get(stmdev_ctx_t *ctx, lsm6dsv16x_xl_mode_t *val) +int32_t lsm6dsv_xl_mode_get(stmdev_ctx_t *ctx, lsm6dsv_xl_mode_t *val) { - lsm6dsv16x_ctrl1_t ctrl1; + lsm6dsv_ctrl1_t ctrl1; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL1, (uint8_t *)&ctrl1, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_CTRL1, (uint8_t *)&ctrl1, 1); if (ret != 0) { return ret; } switch (ctrl1.op_mode_xl) { - case LSM6DSV16X_XL_HIGH_PERFORMANCE_MD: - *val = LSM6DSV16X_XL_HIGH_PERFORMANCE_MD; + case LSM6DSV_XL_HIGH_PERFORMANCE_MD: + *val = LSM6DSV_XL_HIGH_PERFORMANCE_MD; break; - case LSM6DSV16X_XL_HIGH_ACCURACY_ODR_MD: - *val = LSM6DSV16X_XL_HIGH_ACCURACY_ODR_MD; + case LSM6DSV_XL_HIGH_ACCURACY_ODR_MD: + *val = LSM6DSV_XL_HIGH_ACCURACY_ODR_MD; break; - case LSM6DSV16X_XL_ODR_TRIGGERED_MD: - *val = LSM6DSV16X_XL_ODR_TRIGGERED_MD; + case LSM6DSV_XL_ODR_TRIGGERED_MD: + *val = LSM6DSV_XL_ODR_TRIGGERED_MD; break; - case LSM6DSV16X_XL_LOW_POWER_2_AVG_MD: - *val = LSM6DSV16X_XL_LOW_POWER_2_AVG_MD; + case LSM6DSV_XL_LOW_POWER_2_AVG_MD: + *val = LSM6DSV_XL_LOW_POWER_2_AVG_MD; break; - case LSM6DSV16X_XL_LOW_POWER_4_AVG_MD: - *val = LSM6DSV16X_XL_LOW_POWER_4_AVG_MD; + case LSM6DSV_XL_LOW_POWER_4_AVG_MD: + *val = LSM6DSV_XL_LOW_POWER_4_AVG_MD; break; - case LSM6DSV16X_XL_LOW_POWER_8_AVG_MD: - *val = LSM6DSV16X_XL_LOW_POWER_8_AVG_MD; + case LSM6DSV_XL_LOW_POWER_8_AVG_MD: + *val = LSM6DSV_XL_LOW_POWER_8_AVG_MD; break; - case LSM6DSV16X_XL_NORMAL_MD: - *val = LSM6DSV16X_XL_NORMAL_MD; + case LSM6DSV_XL_NORMAL_MD: + *val = LSM6DSV_XL_NORMAL_MD; break; default: - *val = LSM6DSV16X_XL_HIGH_PERFORMANCE_MD; + *val = LSM6DSV_XL_HIGH_PERFORMANCE_MD; break; } @@ -809,29 +804,29 @@ int32_t lsm6dsv16x_xl_mode_get(stmdev_ctx_t *ctx, lsm6dsv16x_xl_mode_t *val) * @brief Gyroscope output data rate (ODR) selection.[set] * * @param ctx read / write interface definitions - * @param val lsm6dsv16x_data_rate_t enum + * @param val lsm6dsv_data_rate_t enum * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_gy_data_rate_set(stmdev_ctx_t *ctx, - lsm6dsv16x_data_rate_t val) +int32_t lsm6dsv_gy_data_rate_set(stmdev_ctx_t *ctx, + lsm6dsv_data_rate_t val) { - lsm6dsv16x_ctrl2_t ctrl2; - lsm6dsv16x_haodr_cfg_t haodr; + lsm6dsv_ctrl2_t ctrl2; + lsm6dsv_haodr_cfg_t haodr; uint8_t sel; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL2, (uint8_t *)&ctrl2, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_CTRL2, (uint8_t *)&ctrl2, 1); ctrl2.odr_g = (uint8_t)val & 0x0Fu; - ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_CTRL2, (uint8_t *)&ctrl2, 1); + ret += lsm6dsv_write_reg(ctx, LSM6DSV_CTRL2, (uint8_t *)&ctrl2, 1); if (ret != 0) { return ret; } sel = ((uint8_t)val >> 4) & 0xFU; if (sel != 0U) { - ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_HAODR_CFG, (uint8_t *)&haodr, 1); + ret += lsm6dsv_read_reg(ctx, LSM6DSV_HAODR_CFG, (uint8_t *)&haodr, 1); haodr.haodr_sel = sel; - ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_HAODR_CFG, (uint8_t *)&haodr, 1); + ret += lsm6dsv_write_reg(ctx, LSM6DSV_HAODR_CFG, (uint8_t *)&haodr, 1); } return ret; @@ -841,190 +836,190 @@ int32_t lsm6dsv16x_gy_data_rate_set(stmdev_ctx_t *ctx, * @brief Gyroscope output data rate (ODR) selection.[get] * * @param ctx read / write interface definitions - * @param val lsm6dsv16x_data_rate_t enum + * @param val lsm6dsv_data_rate_t enum * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_gy_data_rate_get(stmdev_ctx_t *ctx, - lsm6dsv16x_data_rate_t *val) +int32_t lsm6dsv_gy_data_rate_get(stmdev_ctx_t *ctx, + lsm6dsv_data_rate_t *val) { - lsm6dsv16x_ctrl2_t ctrl2; - lsm6dsv16x_haodr_cfg_t haodr; + lsm6dsv_ctrl2_t ctrl2; + lsm6dsv_haodr_cfg_t haodr; uint8_t sel; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL2, (uint8_t *)&ctrl2, 1); - ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_HAODR_CFG, (uint8_t *)&haodr, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_CTRL2, (uint8_t *)&ctrl2, 1); + ret += lsm6dsv_read_reg(ctx, LSM6DSV_HAODR_CFG, (uint8_t *)&haodr, 1); if (ret != 0) { return ret; } sel = haodr.haodr_sel; switch (ctrl2.odr_g) { - case LSM6DSV16X_ODR_OFF: - *val = LSM6DSV16X_ODR_OFF; + case LSM6DSV_ODR_OFF: + *val = LSM6DSV_ODR_OFF; break; - case LSM6DSV16X_ODR_AT_1Hz875: - *val = LSM6DSV16X_ODR_AT_1Hz875; + case LSM6DSV_ODR_AT_1Hz875: + *val = LSM6DSV_ODR_AT_1Hz875; break; - case LSM6DSV16X_ODR_AT_7Hz5: - *val = LSM6DSV16X_ODR_AT_7Hz5; + case LSM6DSV_ODR_AT_7Hz5: + *val = LSM6DSV_ODR_AT_7Hz5; break; - case LSM6DSV16X_ODR_AT_15Hz: + case LSM6DSV_ODR_AT_15Hz: switch (sel) { default: case 0: - *val = LSM6DSV16X_ODR_AT_15Hz; + *val = LSM6DSV_ODR_AT_15Hz; break; case 1: - *val = LSM6DSV16X_ODR_HA01_AT_15Hz625; + *val = LSM6DSV_ODR_HA01_AT_15Hz625; break; case 2: - *val = LSM6DSV16X_ODR_HA02_AT_12Hz5; + *val = LSM6DSV_ODR_HA02_AT_12Hz5; break; } break; - case LSM6DSV16X_ODR_AT_30Hz: + case LSM6DSV_ODR_AT_30Hz: switch (sel) { default: case 0: - *val = LSM6DSV16X_ODR_AT_30Hz; + *val = LSM6DSV_ODR_AT_30Hz; break; case 1: - *val = LSM6DSV16X_ODR_HA01_AT_31Hz25; + *val = LSM6DSV_ODR_HA01_AT_31Hz25; break; case 2: - *val = LSM6DSV16X_ODR_HA02_AT_25Hz; + *val = LSM6DSV_ODR_HA02_AT_25Hz; break; } break; - case LSM6DSV16X_ODR_AT_60Hz: + case LSM6DSV_ODR_AT_60Hz: switch (sel) { default: case 0: - *val = LSM6DSV16X_ODR_AT_60Hz; + *val = LSM6DSV_ODR_AT_60Hz; break; case 1: - *val = LSM6DSV16X_ODR_HA01_AT_62Hz5; + *val = LSM6DSV_ODR_HA01_AT_62Hz5; break; case 2: - *val = LSM6DSV16X_ODR_HA02_AT_50Hz; + *val = LSM6DSV_ODR_HA02_AT_50Hz; break; } break; - case LSM6DSV16X_ODR_AT_120Hz: + case LSM6DSV_ODR_AT_120Hz: switch (sel) { default: case 0: - *val = LSM6DSV16X_ODR_AT_120Hz; + *val = LSM6DSV_ODR_AT_120Hz; break; case 1: - *val = LSM6DSV16X_ODR_HA01_AT_125Hz; + *val = LSM6DSV_ODR_HA01_AT_125Hz; break; case 2: - *val = LSM6DSV16X_ODR_HA02_AT_100Hz; + *val = LSM6DSV_ODR_HA02_AT_100Hz; break; } break; - case LSM6DSV16X_ODR_AT_240Hz: + case LSM6DSV_ODR_AT_240Hz: switch (sel) { default: case 0: - *val = LSM6DSV16X_ODR_AT_240Hz; + *val = LSM6DSV_ODR_AT_240Hz; break; case 1: - *val = LSM6DSV16X_ODR_HA01_AT_250Hz; + *val = LSM6DSV_ODR_HA01_AT_250Hz; break; case 2: - *val = LSM6DSV16X_ODR_HA02_AT_200Hz; + *val = LSM6DSV_ODR_HA02_AT_200Hz; break; } break; - case LSM6DSV16X_ODR_AT_480Hz: + case LSM6DSV_ODR_AT_480Hz: switch (sel) { default: case 0: - *val = LSM6DSV16X_ODR_AT_480Hz; + *val = LSM6DSV_ODR_AT_480Hz; break; case 1: - *val = LSM6DSV16X_ODR_HA01_AT_500Hz; + *val = LSM6DSV_ODR_HA01_AT_500Hz; break; case 2: - *val = LSM6DSV16X_ODR_HA02_AT_400Hz; + *val = LSM6DSV_ODR_HA02_AT_400Hz; break; } break; - case LSM6DSV16X_ODR_AT_960Hz: + case LSM6DSV_ODR_AT_960Hz: switch (sel) { default: case 0: - *val = LSM6DSV16X_ODR_AT_960Hz; + *val = LSM6DSV_ODR_AT_960Hz; break; case 1: - *val = LSM6DSV16X_ODR_HA01_AT_1000Hz; + *val = LSM6DSV_ODR_HA01_AT_1000Hz; break; case 2: - *val = LSM6DSV16X_ODR_HA02_AT_800Hz; + *val = LSM6DSV_ODR_HA02_AT_800Hz; break; } break; - case LSM6DSV16X_ODR_AT_1920Hz: + case LSM6DSV_ODR_AT_1920Hz: switch (sel) { default: case 0: - *val = LSM6DSV16X_ODR_AT_1920Hz; + *val = LSM6DSV_ODR_AT_1920Hz; break; case 1: - *val = LSM6DSV16X_ODR_HA01_AT_2000Hz; + *val = LSM6DSV_ODR_HA01_AT_2000Hz; break; case 2: - *val = LSM6DSV16X_ODR_HA02_AT_1600Hz; + *val = LSM6DSV_ODR_HA02_AT_1600Hz; break; } break; - case LSM6DSV16X_ODR_AT_3840Hz: + case LSM6DSV_ODR_AT_3840Hz: switch (sel) { default: case 0: - *val = LSM6DSV16X_ODR_AT_3840Hz; + *val = LSM6DSV_ODR_AT_3840Hz; break; case 1: - *val = LSM6DSV16X_ODR_HA01_AT_4000Hz; + *val = LSM6DSV_ODR_HA01_AT_4000Hz; break; case 2: - *val = LSM6DSV16X_ODR_HA02_AT_3200Hz; + *val = LSM6DSV_ODR_HA02_AT_3200Hz; break; } break; - case LSM6DSV16X_ODR_AT_7680Hz: + case LSM6DSV_ODR_AT_7680Hz: switch (sel) { default: case 0: - *val = LSM6DSV16X_ODR_AT_7680Hz; + *val = LSM6DSV_ODR_AT_7680Hz; break; case 1: - *val = LSM6DSV16X_ODR_HA01_AT_8000Hz; + *val = LSM6DSV_ODR_HA01_AT_8000Hz; break; case 2: - *val = LSM6DSV16X_ODR_HA02_AT_6400Hz; + *val = LSM6DSV_ODR_HA02_AT_6400Hz; break; } break; default: - *val = LSM6DSV16X_ODR_OFF; + *val = LSM6DSV_ODR_OFF; break; } @@ -1039,16 +1034,16 @@ int32_t lsm6dsv16x_gy_data_rate_get(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_gy_mode_set(stmdev_ctx_t *ctx, lsm6dsv16x_gy_mode_t val) +int32_t lsm6dsv_gy_mode_set(stmdev_ctx_t *ctx, lsm6dsv_gy_mode_t val) { - lsm6dsv16x_ctrl2_t ctrl2; + lsm6dsv_ctrl2_t ctrl2; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL2, (uint8_t *)&ctrl2, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_CTRL2, (uint8_t *)&ctrl2, 1); if (ret == 0) { ctrl2.op_mode_g = (uint8_t)val & 0x07U; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_CTRL2, (uint8_t *)&ctrl2, 1); + ret = lsm6dsv_write_reg(ctx, LSM6DSV_CTRL2, (uint8_t *)&ctrl2, 1); } return ret; @@ -1062,34 +1057,34 @@ int32_t lsm6dsv16x_gy_mode_set(stmdev_ctx_t *ctx, lsm6dsv16x_gy_mode_t val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_gy_mode_get(stmdev_ctx_t *ctx, lsm6dsv16x_gy_mode_t *val) +int32_t lsm6dsv_gy_mode_get(stmdev_ctx_t *ctx, lsm6dsv_gy_mode_t *val) { - lsm6dsv16x_ctrl2_t ctrl2; + lsm6dsv_ctrl2_t ctrl2; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL2, (uint8_t *)&ctrl2, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_CTRL2, (uint8_t *)&ctrl2, 1); if (ret != 0) { return ret; } switch (ctrl2.op_mode_g) { - case LSM6DSV16X_GY_HIGH_PERFORMANCE_MD: - *val = LSM6DSV16X_GY_HIGH_PERFORMANCE_MD; + case LSM6DSV_GY_HIGH_PERFORMANCE_MD: + *val = LSM6DSV_GY_HIGH_PERFORMANCE_MD; break; - case LSM6DSV16X_GY_HIGH_ACCURACY_ODR_MD: - *val = LSM6DSV16X_GY_HIGH_ACCURACY_ODR_MD; + case LSM6DSV_GY_HIGH_ACCURACY_ODR_MD: + *val = LSM6DSV_GY_HIGH_ACCURACY_ODR_MD; break; - case LSM6DSV16X_GY_SLEEP_MD: - *val = LSM6DSV16X_GY_SLEEP_MD; + case LSM6DSV_GY_SLEEP_MD: + *val = LSM6DSV_GY_SLEEP_MD; break; - case LSM6DSV16X_GY_LOW_POWER_MD: - *val = LSM6DSV16X_GY_LOW_POWER_MD; + case LSM6DSV_GY_LOW_POWER_MD: + *val = LSM6DSV_GY_LOW_POWER_MD; break; default: - *val = LSM6DSV16X_GY_HIGH_PERFORMANCE_MD; + *val = LSM6DSV_GY_HIGH_PERFORMANCE_MD; break; } @@ -1104,16 +1099,16 @@ int32_t lsm6dsv16x_gy_mode_get(stmdev_ctx_t *ctx, lsm6dsv16x_gy_mode_t *val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_auto_increment_set(stmdev_ctx_t *ctx, uint8_t val) +int32_t lsm6dsv_auto_increment_set(stmdev_ctx_t *ctx, uint8_t val) { - lsm6dsv16x_ctrl3_t ctrl3; + lsm6dsv_ctrl3_t ctrl3; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL3, (uint8_t *)&ctrl3, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_CTRL3, (uint8_t *)&ctrl3, 1); if (ret == 0) { ctrl3.if_inc = val; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_CTRL3, (uint8_t *)&ctrl3, 1); + ret = lsm6dsv_write_reg(ctx, LSM6DSV_CTRL3, (uint8_t *)&ctrl3, 1); } return ret; @@ -1127,12 +1122,12 @@ int32_t lsm6dsv16x_auto_increment_set(stmdev_ctx_t *ctx, uint8_t val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_auto_increment_get(stmdev_ctx_t *ctx, uint8_t *val) +int32_t lsm6dsv_auto_increment_get(stmdev_ctx_t *ctx, uint8_t *val) { - lsm6dsv16x_ctrl3_t ctrl3; + lsm6dsv_ctrl3_t ctrl3; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL3, (uint8_t *)&ctrl3, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_CTRL3, (uint8_t *)&ctrl3, 1); *val = ctrl3.if_inc; return ret; @@ -1146,17 +1141,17 @@ int32_t lsm6dsv16x_auto_increment_get(stmdev_ctx_t *ctx, uint8_t *val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_block_data_update_set(stmdev_ctx_t *ctx, uint8_t val) +int32_t lsm6dsv_block_data_update_set(stmdev_ctx_t *ctx, uint8_t val) { - lsm6dsv16x_ctrl3_t ctrl3; + lsm6dsv_ctrl3_t ctrl3; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL3, (uint8_t *)&ctrl3, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_CTRL3, (uint8_t *)&ctrl3, 1); if (ret == 0) { ctrl3.bdu = val; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_CTRL3, (uint8_t *)&ctrl3, 1); + ret = lsm6dsv_write_reg(ctx, LSM6DSV_CTRL3, (uint8_t *)&ctrl3, 1); } return ret; @@ -1170,12 +1165,12 @@ int32_t lsm6dsv16x_block_data_update_set(stmdev_ctx_t *ctx, uint8_t val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_block_data_update_get(stmdev_ctx_t *ctx, uint8_t *val) +int32_t lsm6dsv_block_data_update_get(stmdev_ctx_t *ctx, uint8_t *val) { - lsm6dsv16x_ctrl3_t ctrl3; + lsm6dsv_ctrl3_t ctrl3; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL3, (uint8_t *)&ctrl3, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_CTRL3, (uint8_t *)&ctrl3, 1); *val = ctrl3.bdu; return ret; @@ -1189,21 +1184,21 @@ int32_t lsm6dsv16x_block_data_update_get(stmdev_ctx_t *ctx, uint8_t *val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_odr_trig_cfg_set(stmdev_ctx_t *ctx, uint8_t val) +int32_t lsm6dsv_odr_trig_cfg_set(stmdev_ctx_t *ctx, uint8_t val) { - lsm6dsv16x_odr_trig_cfg_t odr_trig; + lsm6dsv_odr_trig_cfg_t odr_trig; int32_t ret; if (val >= 1U && val <= 3U) { return -1; } - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_ODR_TRIG_CFG, (uint8_t *)&odr_trig, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_ODR_TRIG_CFG, (uint8_t *)&odr_trig, 1); if (ret == 0) { odr_trig.odr_trig_nodr = val; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_ODR_TRIG_CFG, (uint8_t *)&odr_trig, 1); + ret = lsm6dsv_write_reg(ctx, LSM6DSV_ODR_TRIG_CFG, (uint8_t *)&odr_trig, 1); } return ret; @@ -1217,12 +1212,12 @@ int32_t lsm6dsv16x_odr_trig_cfg_set(stmdev_ctx_t *ctx, uint8_t val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_odr_trig_cfg_get(stmdev_ctx_t *ctx, uint8_t *val) +int32_t lsm6dsv_odr_trig_cfg_get(stmdev_ctx_t *ctx, uint8_t *val) { - lsm6dsv16x_odr_trig_cfg_t odr_trig; + lsm6dsv_odr_trig_cfg_t odr_trig; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_ODR_TRIG_CFG, (uint8_t *)&odr_trig, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_ODR_TRIG_CFG, (uint8_t *)&odr_trig, 1); *val = odr_trig.odr_trig_nodr; return ret; @@ -1236,18 +1231,18 @@ int32_t lsm6dsv16x_odr_trig_cfg_get(stmdev_ctx_t *ctx, uint8_t *val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_data_ready_mode_set(stmdev_ctx_t *ctx, - lsm6dsv16x_data_ready_mode_t val) +int32_t lsm6dsv_data_ready_mode_set(stmdev_ctx_t *ctx, + lsm6dsv_data_ready_mode_t val) { - lsm6dsv16x_ctrl4_t ctrl4; + lsm6dsv_ctrl4_t ctrl4; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL4, (uint8_t *)&ctrl4, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_CTRL4, (uint8_t *)&ctrl4, 1); if (ret == 0) { ctrl4.drdy_pulsed = (uint8_t)val & 0x1U; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_CTRL4, (uint8_t *)&ctrl4, 1); + ret = lsm6dsv_write_reg(ctx, LSM6DSV_CTRL4, (uint8_t *)&ctrl4, 1); } return ret; @@ -1261,26 +1256,26 @@ int32_t lsm6dsv16x_data_ready_mode_set(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_data_ready_mode_get(stmdev_ctx_t *ctx, - lsm6dsv16x_data_ready_mode_t *val) +int32_t lsm6dsv_data_ready_mode_get(stmdev_ctx_t *ctx, + lsm6dsv_data_ready_mode_t *val) { - lsm6dsv16x_ctrl4_t ctrl4; + lsm6dsv_ctrl4_t ctrl4; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL4, (uint8_t *)&ctrl4, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_CTRL4, (uint8_t *)&ctrl4, 1); switch (ctrl4.drdy_pulsed) { - case LSM6DSV16X_DRDY_LATCHED: - *val = LSM6DSV16X_DRDY_LATCHED; + case LSM6DSV_DRDY_LATCHED: + *val = LSM6DSV_DRDY_LATCHED; break; - case LSM6DSV16X_DRDY_PULSED: - *val = LSM6DSV16X_DRDY_PULSED; + case LSM6DSV_DRDY_PULSED: + *val = LSM6DSV_DRDY_PULSED; break; default: - *val = LSM6DSV16X_DRDY_LATCHED; + *val = LSM6DSV_DRDY_LATCHED; break; } @@ -1295,21 +1290,21 @@ int32_t lsm6dsv16x_data_ready_mode_get(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_interrupt_enable_set(stmdev_ctx_t *ctx, - lsm6dsv16x_interrupt_mode_t val) +int32_t lsm6dsv_interrupt_enable_set(stmdev_ctx_t *ctx, + lsm6dsv_interrupt_mode_t val) { - lsm6dsv16x_tap_cfg0_t cfg; - lsm6dsv16x_functions_enable_t func; + lsm6dsv_tap_cfg0_t cfg; + lsm6dsv_functions_enable_t func; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FUNCTIONS_ENABLE, (uint8_t *)&func, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_FUNCTIONS_ENABLE, (uint8_t *)&func, 1); func.interrupts_enable = val.enable; - ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_FUNCTIONS_ENABLE, (uint8_t *)&func, 1); + ret += lsm6dsv_write_reg(ctx, LSM6DSV_FUNCTIONS_ENABLE, (uint8_t *)&func, 1); if (ret != 0) { return ret; } - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_TAP_CFG0, (uint8_t *)&cfg, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_TAP_CFG0, (uint8_t *)&cfg, 1); cfg.lir = val.lir; - ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_TAP_CFG0, (uint8_t *)&cfg, 1); + ret += lsm6dsv_write_reg(ctx, LSM6DSV_TAP_CFG0, (uint8_t *)&cfg, 1); return ret; } @@ -1322,15 +1317,15 @@ int32_t lsm6dsv16x_interrupt_enable_set(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_interrupt_enable_get(stmdev_ctx_t *ctx, - lsm6dsv16x_interrupt_mode_t *val) +int32_t lsm6dsv_interrupt_enable_get(stmdev_ctx_t *ctx, + lsm6dsv_interrupt_mode_t *val) { - lsm6dsv16x_tap_cfg0_t cfg; - lsm6dsv16x_functions_enable_t func; + lsm6dsv_tap_cfg0_t cfg; + lsm6dsv_functions_enable_t func; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FUNCTIONS_ENABLE, (uint8_t *)&func, 1); - ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_TAP_CFG0, (uint8_t *)&cfg, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_FUNCTIONS_ENABLE, (uint8_t *)&func, 1); + ret += lsm6dsv_read_reg(ctx, LSM6DSV_TAP_CFG0, (uint8_t *)&cfg, 1); if (ret != 0) { return ret; } val->enable = func.interrupts_enable; @@ -1347,18 +1342,18 @@ int32_t lsm6dsv16x_interrupt_enable_get(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_gy_full_scale_set(stmdev_ctx_t *ctx, - lsm6dsv16x_gy_full_scale_t val) +int32_t lsm6dsv_gy_full_scale_set(stmdev_ctx_t *ctx, + lsm6dsv_gy_full_scale_t val) { - lsm6dsv16x_ctrl6_t ctrl6; + lsm6dsv_ctrl6_t ctrl6; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL6, (uint8_t *)&ctrl6, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_CTRL6, (uint8_t *)&ctrl6, 1); if (ret == 0) { ctrl6.fs_g = (uint8_t)val & 0xfu; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_CTRL6, (uint8_t *)&ctrl6, 1); + ret = lsm6dsv_write_reg(ctx, LSM6DSV_CTRL6, (uint8_t *)&ctrl6, 1); } return ret; @@ -1372,43 +1367,43 @@ int32_t lsm6dsv16x_gy_full_scale_set(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_gy_full_scale_get(stmdev_ctx_t *ctx, - lsm6dsv16x_gy_full_scale_t *val) +int32_t lsm6dsv_gy_full_scale_get(stmdev_ctx_t *ctx, + lsm6dsv_gy_full_scale_t *val) { - lsm6dsv16x_ctrl6_t ctrl6; + lsm6dsv_ctrl6_t ctrl6; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL6, (uint8_t *)&ctrl6, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_CTRL6, (uint8_t *)&ctrl6, 1); if (ret != 0) { return ret; } switch (ctrl6.fs_g) { - case LSM6DSV16X_125dps: - *val = LSM6DSV16X_125dps; + case LSM6DSV_125dps: + *val = LSM6DSV_125dps; break; - case LSM6DSV16X_250dps: - *val = LSM6DSV16X_250dps; + case LSM6DSV_250dps: + *val = LSM6DSV_250dps; break; - case LSM6DSV16X_500dps: - *val = LSM6DSV16X_500dps; + case LSM6DSV_500dps: + *val = LSM6DSV_500dps; break; - case LSM6DSV16X_1000dps: - *val = LSM6DSV16X_1000dps; + case LSM6DSV_1000dps: + *val = LSM6DSV_1000dps; break; - case LSM6DSV16X_2000dps: - *val = LSM6DSV16X_2000dps; + case LSM6DSV_2000dps: + *val = LSM6DSV_2000dps; break; - case LSM6DSV16X_4000dps: - *val = LSM6DSV16X_4000dps; + case LSM6DSV_4000dps: + *val = LSM6DSV_4000dps; break; default: - *val = LSM6DSV16X_125dps; + *val = LSM6DSV_125dps; break; } @@ -1423,18 +1418,18 @@ int32_t lsm6dsv16x_gy_full_scale_get(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_xl_full_scale_set(stmdev_ctx_t *ctx, - lsm6dsv16x_xl_full_scale_t val) +int32_t lsm6dsv_xl_full_scale_set(stmdev_ctx_t *ctx, + lsm6dsv_xl_full_scale_t val) { - lsm6dsv16x_ctrl8_t ctrl8; + lsm6dsv_ctrl8_t ctrl8; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL8, (uint8_t *)&ctrl8, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_CTRL8, (uint8_t *)&ctrl8, 1); if (ret == 0) { ctrl8.fs_xl = (uint8_t)val & 0x3U; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_CTRL8, (uint8_t *)&ctrl8, 1); + ret = lsm6dsv_write_reg(ctx, LSM6DSV_CTRL8, (uint8_t *)&ctrl8, 1); } return ret; @@ -1448,35 +1443,35 @@ int32_t lsm6dsv16x_xl_full_scale_set(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_xl_full_scale_get(stmdev_ctx_t *ctx, - lsm6dsv16x_xl_full_scale_t *val) +int32_t lsm6dsv_xl_full_scale_get(stmdev_ctx_t *ctx, + lsm6dsv_xl_full_scale_t *val) { - lsm6dsv16x_ctrl8_t ctrl8; + lsm6dsv_ctrl8_t ctrl8; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL8, (uint8_t *)&ctrl8, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_CTRL8, (uint8_t *)&ctrl8, 1); if (ret != 0) { return ret; } switch (ctrl8.fs_xl) { - case LSM6DSV16X_2g: - *val = LSM6DSV16X_2g; + case LSM6DSV_2g: + *val = LSM6DSV_2g; break; - case LSM6DSV16X_4g: - *val = LSM6DSV16X_4g; + case LSM6DSV_4g: + *val = LSM6DSV_4g; break; - case LSM6DSV16X_8g: - *val = LSM6DSV16X_8g; + case LSM6DSV_8g: + *val = LSM6DSV_8g; break; - case LSM6DSV16X_16g: - *val = LSM6DSV16X_16g; + case LSM6DSV_16g: + *val = LSM6DSV_16g; break; default: - *val = LSM6DSV16X_2g; + *val = LSM6DSV_2g; break; } @@ -1491,17 +1486,17 @@ int32_t lsm6dsv16x_xl_full_scale_get(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_xl_dual_channel_set(stmdev_ctx_t *ctx, uint8_t val) +int32_t lsm6dsv_xl_dual_channel_set(stmdev_ctx_t *ctx, uint8_t val) { - lsm6dsv16x_ctrl8_t ctrl8; + lsm6dsv_ctrl8_t ctrl8; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL8, (uint8_t *)&ctrl8, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_CTRL8, (uint8_t *)&ctrl8, 1); if (ret == 0) { ctrl8.xl_dualc_en = val; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_CTRL8, (uint8_t *)&ctrl8, 1); + ret = lsm6dsv_write_reg(ctx, LSM6DSV_CTRL8, (uint8_t *)&ctrl8, 1); } return ret; @@ -1515,12 +1510,12 @@ int32_t lsm6dsv16x_xl_dual_channel_set(stmdev_ctx_t *ctx, uint8_t val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_xl_dual_channel_get(stmdev_ctx_t *ctx, uint8_t *val) +int32_t lsm6dsv_xl_dual_channel_get(stmdev_ctx_t *ctx, uint8_t *val) { - lsm6dsv16x_ctrl8_t ctrl8; + lsm6dsv_ctrl8_t ctrl8; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL8, (uint8_t *)&ctrl8, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_CTRL8, (uint8_t *)&ctrl8, 1); *val = ctrl8.xl_dualc_en; return ret; @@ -1534,18 +1529,17 @@ int32_t lsm6dsv16x_xl_dual_channel_get(stmdev_ctx_t *ctx, uint8_t *val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_xl_self_test_set(stmdev_ctx_t *ctx, - lsm6dsv16x_xl_self_test_t val) +int32_t lsm6dsv_xl_self_test_set(stmdev_ctx_t *ctx, lsm6dsv_xl_self_test_t val) { - lsm6dsv16x_ctrl10_t ctrl10; + lsm6dsv_ctrl10_t ctrl10; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL10, (uint8_t *)&ctrl10, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_CTRL10, (uint8_t *)&ctrl10, 1); if (ret == 0) { ctrl10.st_xl = (uint8_t)val & 0x3U; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_CTRL10, (uint8_t *)&ctrl10, 1); + ret = lsm6dsv_write_reg(ctx, LSM6DSV_CTRL10, (uint8_t *)&ctrl10, 1); } return ret; @@ -1559,31 +1553,30 @@ int32_t lsm6dsv16x_xl_self_test_set(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_xl_self_test_get(stmdev_ctx_t *ctx, - lsm6dsv16x_xl_self_test_t *val) +int32_t lsm6dsv_xl_self_test_get(stmdev_ctx_t *ctx, lsm6dsv_xl_self_test_t *val) { - lsm6dsv16x_ctrl10_t ctrl10; + lsm6dsv_ctrl10_t ctrl10; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL10, (uint8_t *)&ctrl10, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_CTRL10, (uint8_t *)&ctrl10, 1); if (ret != 0) { return ret; } switch (ctrl10.st_xl) { - case LSM6DSV16X_XL_ST_DISABLE: - *val = LSM6DSV16X_XL_ST_DISABLE; + case LSM6DSV_XL_ST_DISABLE: + *val = LSM6DSV_XL_ST_DISABLE; break; - case LSM6DSV16X_XL_ST_POSITIVE: - *val = LSM6DSV16X_XL_ST_POSITIVE; + case LSM6DSV_XL_ST_POSITIVE: + *val = LSM6DSV_XL_ST_POSITIVE; break; - case LSM6DSV16X_XL_ST_NEGATIVE: - *val = LSM6DSV16X_XL_ST_NEGATIVE; + case LSM6DSV_XL_ST_NEGATIVE: + *val = LSM6DSV_XL_ST_NEGATIVE; break; default: - *val = LSM6DSV16X_XL_ST_DISABLE; + *val = LSM6DSV_XL_ST_DISABLE; break; } @@ -1598,18 +1591,17 @@ int32_t lsm6dsv16x_xl_self_test_get(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_gy_self_test_set(stmdev_ctx_t *ctx, - lsm6dsv16x_gy_self_test_t val) +int32_t lsm6dsv_gy_self_test_set(stmdev_ctx_t *ctx, lsm6dsv_gy_self_test_t val) { - lsm6dsv16x_ctrl10_t ctrl10; + lsm6dsv_ctrl10_t ctrl10; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL10, (uint8_t *)&ctrl10, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_CTRL10, (uint8_t *)&ctrl10, 1); if (ret == 0) { ctrl10.st_g = (uint8_t)val & 0x3U; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_CTRL10, (uint8_t *)&ctrl10, 1); + ret = lsm6dsv_write_reg(ctx, LSM6DSV_CTRL10, (uint8_t *)&ctrl10, 1); } return ret; @@ -1623,31 +1615,30 @@ int32_t lsm6dsv16x_gy_self_test_set(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_gy_self_test_get(stmdev_ctx_t *ctx, - lsm6dsv16x_gy_self_test_t *val) +int32_t lsm6dsv_gy_self_test_get(stmdev_ctx_t *ctx, lsm6dsv_gy_self_test_t *val) { - lsm6dsv16x_ctrl10_t ctrl10; + lsm6dsv_ctrl10_t ctrl10; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL10, (uint8_t *)&ctrl10, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_CTRL10, (uint8_t *)&ctrl10, 1); if (ret != 0) { return ret; } switch (ctrl10.st_g) { - case LSM6DSV16X_GY_ST_DISABLE: - *val = LSM6DSV16X_GY_ST_DISABLE; + case LSM6DSV_GY_ST_DISABLE: + *val = LSM6DSV_GY_ST_DISABLE; break; - case LSM6DSV16X_GY_ST_POSITIVE: - *val = LSM6DSV16X_GY_ST_POSITIVE; + case LSM6DSV_GY_ST_POSITIVE: + *val = LSM6DSV_GY_ST_POSITIVE; break; - case LSM6DSV16X_GY_ST_NEGATIVE: - *val = LSM6DSV16X_GY_ST_NEGATIVE; + case LSM6DSV_GY_ST_NEGATIVE: + *val = LSM6DSV_GY_ST_NEGATIVE; break; default: - *val = LSM6DSV16X_GY_ST_DISABLE; + *val = LSM6DSV_GY_ST_DISABLE; break; } @@ -1662,18 +1653,18 @@ int32_t lsm6dsv16x_gy_self_test_get(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_ois_xl_self_test_set(stmdev_ctx_t *ctx, - lsm6dsv16x_ois_xl_self_test_t val) +int32_t lsm6dsv_ois_xl_self_test_set(stmdev_ctx_t *ctx, + lsm6dsv_ois_xl_self_test_t val) { - lsm6dsv16x_spi2_int_ois_t spi2_int_ois; + lsm6dsv_spi2_int_ois_t spi2_int_ois; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_SPI2_INT_OIS, (uint8_t *)&spi2_int_ois, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_SPI2_INT_OIS, (uint8_t *)&spi2_int_ois, 1); if (ret == 0) { spi2_int_ois.st_xl_ois = ((uint8_t)val & 0x3U); - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_SPI2_INT_OIS, (uint8_t *)&spi2_int_ois, 1); + ret = lsm6dsv_write_reg(ctx, LSM6DSV_SPI2_INT_OIS, (uint8_t *)&spi2_int_ois, 1); } return ret; @@ -1687,31 +1678,31 @@ int32_t lsm6dsv16x_ois_xl_self_test_set(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_ois_xl_self_test_get(stmdev_ctx_t *ctx, - lsm6dsv16x_ois_xl_self_test_t *val) +int32_t lsm6dsv_ois_xl_self_test_get(stmdev_ctx_t *ctx, + lsm6dsv_ois_xl_self_test_t *val) { - lsm6dsv16x_spi2_int_ois_t spi2_int_ois; + lsm6dsv_spi2_int_ois_t spi2_int_ois; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_SPI2_INT_OIS, (uint8_t *)&spi2_int_ois, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_SPI2_INT_OIS, (uint8_t *)&spi2_int_ois, 1); if (ret != 0) { return ret; } switch (spi2_int_ois.st_xl_ois) { - case LSM6DSV16X_OIS_XL_ST_DISABLE: - *val = LSM6DSV16X_OIS_XL_ST_DISABLE; + case LSM6DSV_OIS_XL_ST_DISABLE: + *val = LSM6DSV_OIS_XL_ST_DISABLE; break; - case LSM6DSV16X_OIS_XL_ST_POSITIVE: - *val = LSM6DSV16X_OIS_XL_ST_POSITIVE; + case LSM6DSV_OIS_XL_ST_POSITIVE: + *val = LSM6DSV_OIS_XL_ST_POSITIVE; break; - case LSM6DSV16X_OIS_XL_ST_NEGATIVE: - *val = LSM6DSV16X_OIS_XL_ST_NEGATIVE; + case LSM6DSV_OIS_XL_ST_NEGATIVE: + *val = LSM6DSV_OIS_XL_ST_NEGATIVE; break; default: - *val = LSM6DSV16X_OIS_XL_ST_DISABLE; + *val = LSM6DSV_OIS_XL_ST_DISABLE; break; } @@ -1722,23 +1713,23 @@ int32_t lsm6dsv16x_ois_xl_self_test_get(stmdev_ctx_t *ctx, * @brief SPI2 Accelerometer self-test selection.[set] * * @param ctx read / write interface definitions - * @param val GY_ST_DISABLE, GY_ST_POSITIVE, GY_ST_NEGATIVE, LSM6DSV16X_OIS_GY_ST_CLAMP_POS, LSM6DSV16X_OIS_GY_ST_CLAMP_NEG + * @param val GY_ST_DISABLE, GY_ST_POSITIVE, GY_ST_NEGATIVE, LSM6DSV_OIS_GY_ST_CLAMP_POS, LSM6DSV_OIS_GY_ST_CLAMP_NEG * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_ois_gy_self_test_set(stmdev_ctx_t *ctx, - lsm6dsv16x_ois_gy_self_test_t val) +int32_t lsm6dsv_ois_gy_self_test_set(stmdev_ctx_t *ctx, + lsm6dsv_ois_gy_self_test_t val) { - lsm6dsv16x_spi2_int_ois_t spi2_int_ois; + lsm6dsv_spi2_int_ois_t spi2_int_ois; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_SPI2_INT_OIS, (uint8_t *)&spi2_int_ois, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_SPI2_INT_OIS, (uint8_t *)&spi2_int_ois, 1); if (ret == 0) { spi2_int_ois.st_g_ois = ((uint8_t)val & 0x3U); spi2_int_ois.st_ois_clampdis = ((uint8_t)val & 0x04U) >> 2; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_SPI2_INT_OIS, (uint8_t *)&spi2_int_ois, 1); + ret = lsm6dsv_write_reg(ctx, LSM6DSV_SPI2_INT_OIS, (uint8_t *)&spi2_int_ois, 1); } return ret; @@ -1748,35 +1739,35 @@ int32_t lsm6dsv16x_ois_gy_self_test_set(stmdev_ctx_t *ctx, * @brief SPI2 Accelerometer self-test selection.[get] * * @param ctx read / write interface definitions - * @param val GY_ST_DISABLE, GY_ST_POSITIVE, GY_ST_NEGATIVE, LSM6DSV16X_OIS_GY_ST_CLAMP_POS, LSM6DSV16X_OIS_GY_ST_CLAMP_NEG + * @param val GY_ST_DISABLE, GY_ST_POSITIVE, GY_ST_NEGATIVE, LSM6DSV_OIS_GY_ST_CLAMP_POS, LSM6DSV_OIS_GY_ST_CLAMP_NEG * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_ois_gy_self_test_get(stmdev_ctx_t *ctx, - lsm6dsv16x_ois_gy_self_test_t *val) +int32_t lsm6dsv_ois_gy_self_test_get(stmdev_ctx_t *ctx, + lsm6dsv_ois_gy_self_test_t *val) { - lsm6dsv16x_spi2_int_ois_t spi2_int_ois; + lsm6dsv_spi2_int_ois_t spi2_int_ois; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_SPI2_INT_OIS, (uint8_t *)&spi2_int_ois, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_SPI2_INT_OIS, (uint8_t *)&spi2_int_ois, 1); if (ret != 0) { return ret; } switch (spi2_int_ois.st_g_ois) { - case LSM6DSV16X_OIS_GY_ST_DISABLE: - *val = LSM6DSV16X_OIS_GY_ST_DISABLE; + case LSM6DSV_OIS_GY_ST_DISABLE: + *val = LSM6DSV_OIS_GY_ST_DISABLE; break; - case LSM6DSV16X_OIS_GY_ST_POSITIVE: - *val = (spi2_int_ois.st_ois_clampdis == 1U) ? LSM6DSV16X_OIS_GY_ST_CLAMP_POS : LSM6DSV16X_OIS_GY_ST_POSITIVE; + case LSM6DSV_OIS_GY_ST_POSITIVE: + *val = (spi2_int_ois.st_ois_clampdis == 1U) ? LSM6DSV_OIS_GY_ST_CLAMP_POS : LSM6DSV_OIS_GY_ST_POSITIVE; break; - case LSM6DSV16X_OIS_GY_ST_NEGATIVE: - *val = (spi2_int_ois.st_ois_clampdis == 1U) ? LSM6DSV16X_OIS_GY_ST_CLAMP_NEG : LSM6DSV16X_OIS_GY_ST_NEGATIVE; + case LSM6DSV_OIS_GY_ST_NEGATIVE: + *val = (spi2_int_ois.st_ois_clampdis == 1U) ? LSM6DSV_OIS_GY_ST_CLAMP_NEG : LSM6DSV_OIS_GY_ST_NEGATIVE; break; default: - *val = LSM6DSV16X_OIS_GY_ST_DISABLE; + *val = LSM6DSV_OIS_GY_ST_DISABLE; break; } @@ -1799,14 +1790,14 @@ int32_t lsm6dsv16x_ois_gy_self_test_get(stmdev_ctx_t *ctx, * @retval Interface status (MANDATORY: return 0 -> no Error). * */ -int32_t lsm6dsv16x_pin_int1_route_set(stmdev_ctx_t *ctx, - lsm6dsv16x_pin_int_route_t *val) +int32_t lsm6dsv_pin_int1_route_set(stmdev_ctx_t *ctx, + lsm6dsv_pin_int_route_t *val) { - lsm6dsv16x_int1_ctrl_t int1_ctrl; - lsm6dsv16x_md1_cfg_t md1_cfg; + lsm6dsv_int1_ctrl_t int1_ctrl; + lsm6dsv_md1_cfg_t md1_cfg; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_INT1_CTRL, (uint8_t *)&int1_ctrl, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_INT1_CTRL, (uint8_t *)&int1_ctrl, 1); if (ret != 0) { return ret; } int1_ctrl.int1_drdy_xl = val->drdy_xl; @@ -1816,10 +1807,10 @@ int32_t lsm6dsv16x_pin_int1_route_set(stmdev_ctx_t *ctx, int1_ctrl.int1_fifo_full = val->fifo_full; int1_ctrl.int1_cnt_bdr = val->cnt_bdr; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_INT1_CTRL, (uint8_t *)&int1_ctrl, 1); + ret = lsm6dsv_write_reg(ctx, LSM6DSV_INT1_CTRL, (uint8_t *)&int1_ctrl, 1); if (ret != 0) { return ret; } - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_MD1_CFG, (uint8_t *)&md1_cfg, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_MD1_CFG, (uint8_t *)&md1_cfg, 1); if (ret != 0) { return ret; } md1_cfg.int1_shub = val->shub; @@ -1831,7 +1822,7 @@ int32_t lsm6dsv16x_pin_int1_route_set(stmdev_ctx_t *ctx, md1_cfg.int1_ff = val->freefall; md1_cfg.int1_sleep_change = val->sleep_change; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_MD1_CFG, (uint8_t *)&md1_cfg, 1); + ret = lsm6dsv_write_reg(ctx, LSM6DSV_MD1_CFG, (uint8_t *)&md1_cfg, 1); return ret; } @@ -1844,14 +1835,14 @@ int32_t lsm6dsv16x_pin_int1_route_set(stmdev_ctx_t *ctx, * @retval Interface status (MANDATORY: return 0 -> no Error). * */ -int32_t lsm6dsv16x_pin_int1_route_get(stmdev_ctx_t *ctx, - lsm6dsv16x_pin_int_route_t *val) +int32_t lsm6dsv_pin_int1_route_get(stmdev_ctx_t *ctx, + lsm6dsv_pin_int_route_t *val) { - lsm6dsv16x_int1_ctrl_t int1_ctrl; - lsm6dsv16x_md1_cfg_t md1_cfg; + lsm6dsv_int1_ctrl_t int1_ctrl; + lsm6dsv_md1_cfg_t md1_cfg; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_INT1_CTRL, (uint8_t *)&int1_ctrl, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_INT1_CTRL, (uint8_t *)&int1_ctrl, 1); if (ret != 0) { return ret; } val->drdy_xl = int1_ctrl.int1_drdy_xl; @@ -1861,7 +1852,7 @@ int32_t lsm6dsv16x_pin_int1_route_get(stmdev_ctx_t *ctx, val->fifo_full = int1_ctrl.int1_fifo_full; val->cnt_bdr = int1_ctrl.int1_cnt_bdr; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_MD1_CFG, (uint8_t *)&md1_cfg, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_MD1_CFG, (uint8_t *)&md1_cfg, 1); if (ret != 0) { return ret; } val->shub = md1_cfg.int1_shub; @@ -1884,14 +1875,14 @@ int32_t lsm6dsv16x_pin_int1_route_get(stmdev_ctx_t *ctx, * @retval Interface status (MANDATORY: return 0 -> no Error). * */ -int32_t lsm6dsv16x_pin_int2_route_set(stmdev_ctx_t *ctx, - lsm6dsv16x_pin_int_route_t *val) +int32_t lsm6dsv_pin_int2_route_set(stmdev_ctx_t *ctx, + lsm6dsv_pin_int_route_t *val) { - lsm6dsv16x_int2_ctrl_t int2_ctrl; - lsm6dsv16x_md2_cfg_t md2_cfg; + lsm6dsv_int2_ctrl_t int2_ctrl; + lsm6dsv_md2_cfg_t md2_cfg; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_INT2_CTRL, (uint8_t *)&int2_ctrl, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_INT2_CTRL, (uint8_t *)&int2_ctrl, 1); if (ret != 0) { return ret; } int2_ctrl.int2_drdy_xl = val->drdy_xl; @@ -1903,10 +1894,10 @@ int32_t lsm6dsv16x_pin_int2_route_set(stmdev_ctx_t *ctx, int2_ctrl.int2_drdy_g_eis = val->drdy_g_eis; int2_ctrl.int2_emb_func_endop = val->emb_func_endop; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_INT2_CTRL, (uint8_t *)&int2_ctrl, 1); + ret = lsm6dsv_write_reg(ctx, LSM6DSV_INT2_CTRL, (uint8_t *)&int2_ctrl, 1); if (ret != 0) { return ret; } - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_MD2_CFG, (uint8_t *)&md2_cfg, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_MD2_CFG, (uint8_t *)&md2_cfg, 1); if (ret != 0) { return ret; } md2_cfg.int2_timestamp = val->timestamp; @@ -1918,7 +1909,7 @@ int32_t lsm6dsv16x_pin_int2_route_set(stmdev_ctx_t *ctx, md2_cfg.int2_ff = val->freefall; md2_cfg.int2_sleep_change = val->sleep_change; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_MD2_CFG, (uint8_t *)&md2_cfg, 1); + ret = lsm6dsv_write_reg(ctx, LSM6DSV_MD2_CFG, (uint8_t *)&md2_cfg, 1); return ret; } @@ -1931,14 +1922,14 @@ int32_t lsm6dsv16x_pin_int2_route_set(stmdev_ctx_t *ctx, * @retval Interface status (MANDATORY: return 0 -> no Error). * */ -int32_t lsm6dsv16x_pin_int2_route_get(stmdev_ctx_t *ctx, - lsm6dsv16x_pin_int_route_t *val) +int32_t lsm6dsv_pin_int2_route_get(stmdev_ctx_t *ctx, + lsm6dsv_pin_int_route_t *val) { - lsm6dsv16x_int2_ctrl_t int2_ctrl; - lsm6dsv16x_md2_cfg_t md2_cfg; + lsm6dsv_int2_ctrl_t int2_ctrl; + lsm6dsv_md2_cfg_t md2_cfg; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_INT2_CTRL, (uint8_t *)&int2_ctrl, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_INT2_CTRL, (uint8_t *)&int2_ctrl, 1); if (ret != 0) { return ret; } val->drdy_xl = int2_ctrl.int2_drdy_xl; @@ -1950,7 +1941,7 @@ int32_t lsm6dsv16x_pin_int2_route_get(stmdev_ctx_t *ctx, val->drdy_g_eis = int2_ctrl.int2_drdy_g_eis; val->emb_func_endop = int2_ctrl.int2_emb_func_endop; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_MD2_CFG, (uint8_t *)&md2_cfg, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_MD2_CFG, (uint8_t *)&md2_cfg, 1); if (ret != 0) { return ret; } val->timestamp = md2_cfg.int2_timestamp; @@ -1978,32 +1969,30 @@ int32_t lsm6dsv16x_pin_int2_route_get(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_all_sources_get(stmdev_ctx_t *ctx, - lsm6dsv16x_all_sources_t *val) +int32_t lsm6dsv_all_sources_get(stmdev_ctx_t *ctx, lsm6dsv_all_sources_t *val) { - lsm6dsv16x_emb_func_status_mainpage_t emb_func_status_mainpage; - lsm6dsv16x_emb_func_exec_status_t emb_func_exec_status; - lsm6dsv16x_fsm_status_mainpage_t fsm_status_mainpage; - lsm6dsv16x_mlc_status_mainpage_t mlc_status_mainpage; - lsm6dsv16x_functions_enable_t functions_enable; - lsm6dsv16x_emb_func_src_t emb_func_src; - lsm6dsv16x_fifo_status2_t fifo_status2; - lsm6dsv16x_all_int_src_t all_int_src; - lsm6dsv16x_wake_up_src_t wake_up_src; - lsm6dsv16x_status_reg_t status_reg; - lsm6dsv16x_d6d_src_t d6d_src; - lsm6dsv16x_tap_src_t tap_src; - lsm6dsv16x_ui_status_reg_ois_t status_reg_ois; - lsm6dsv16x_status_master_t status_shub; - uint8_t buff[8]; + lsm6dsv_emb_func_status_mainpage_t emb_func_status_mainpage; + lsm6dsv_emb_func_exec_status_t emb_func_exec_status; + lsm6dsv_fsm_status_mainpage_t fsm_status_mainpage; + lsm6dsv_functions_enable_t functions_enable; + lsm6dsv_emb_func_src_t emb_func_src; + lsm6dsv_fifo_status2_t fifo_status2; + lsm6dsv_all_int_src_t all_int_src; + lsm6dsv_wake_up_src_t wake_up_src; + lsm6dsv_status_reg_t status_reg; + lsm6dsv_d6d_src_t d6d_src; + lsm6dsv_tap_src_t tap_src; + lsm6dsv_ui_status_reg_ois_t status_reg_ois; + lsm6dsv_status_master_t status_shub; + uint8_t buff[7]; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FUNCTIONS_ENABLE, (uint8_t *)&functions_enable, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_FUNCTIONS_ENABLE, (uint8_t *)&functions_enable, 1); functions_enable.dis_rst_lir_all_int = PROPERTY_ENABLE; - ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_FUNCTIONS_ENABLE, (uint8_t *)&functions_enable, 1); + ret += lsm6dsv_write_reg(ctx, LSM6DSV_FUNCTIONS_ENABLE, (uint8_t *)&functions_enable, 1); if (ret != 0) { return ret; } - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FIFO_STATUS1, (uint8_t *)&buff, 4); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_FIFO_STATUS1, (uint8_t *)&buff, 4); if (ret != 0) { return ret; } bytecpy((uint8_t *)&fifo_status2, &buff[1]); @@ -2022,26 +2011,24 @@ int32_t lsm6dsv16x_all_sources_get(stmdev_ctx_t *ctx, val->drdy_xl = status_reg.xlda; val->drdy_gy = status_reg.gda; val->drdy_temp = status_reg.tda; - val->drdy_ah_qvar = status_reg.ah_qvarda; val->drdy_eis = status_reg.gda_eis; val->drdy_ois = status_reg.ois_drdy; val->timestamp = status_reg.timestamp_endcount; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FUNCTIONS_ENABLE, (uint8_t *)&functions_enable, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_FUNCTIONS_ENABLE, (uint8_t *)&functions_enable, 1); functions_enable.dis_rst_lir_all_int = PROPERTY_DISABLE; - ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_FUNCTIONS_ENABLE, (uint8_t *)&functions_enable, 1); + ret += lsm6dsv_write_reg(ctx, LSM6DSV_FUNCTIONS_ENABLE, (uint8_t *)&functions_enable, 1); if (ret != 0) { return ret; } - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_UI_STATUS_REG_OIS, (uint8_t *)&buff, 8); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_UI_STATUS_REG_OIS, (uint8_t *)&buff, 8); if (ret != 0) { return ret; } bytecpy((uint8_t *)&status_reg_ois, &buff[0]); bytecpy((uint8_t *)&wake_up_src, &buff[1]); bytecpy((uint8_t *)&tap_src, &buff[2]); bytecpy((uint8_t *)&d6d_src, &buff[3]); - bytecpy((uint8_t *)&emb_func_status_mainpage, &buff[5]); - bytecpy((uint8_t *)&fsm_status_mainpage, &buff[6]); - bytecpy((uint8_t *)&mlc_status_mainpage, &buff[7]); + bytecpy((uint8_t *)&emb_func_status_mainpage, &buff[4]); + bytecpy((uint8_t *)&fsm_status_mainpage, &buff[5]); val->gy_settling = status_reg_ois.gyro_settling; val->sleep_change = wake_up_src.sleep_change_ia; @@ -2078,17 +2065,13 @@ int32_t lsm6dsv16x_all_sources_get(stmdev_ctx_t *ctx, val->fsm7 = fsm_status_mainpage.is_fsm7; val->fsm8 = fsm_status_mainpage.is_fsm8; - val->mlc1 = mlc_status_mainpage.is_mlc1; - val->mlc2 = mlc_status_mainpage.is_mlc2; - val->mlc3 = mlc_status_mainpage.is_mlc3; - val->mlc4 = mlc_status_mainpage.is_mlc4; /* embedded func */ - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK); - ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_EMB_FUNC_EXEC_STATUS, (uint8_t *)&emb_func_exec_status, 1); - ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_EMB_FUNC_SRC, (uint8_t *)&emb_func_src, 1); - ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + ret = lsm6dsv_mem_bank_set(ctx, LSM6DSV_EMBED_FUNC_MEM_BANK); + ret += lsm6dsv_read_reg(ctx, LSM6DSV_EMB_FUNC_EXEC_STATUS, (uint8_t *)&emb_func_exec_status, 1); + ret += lsm6dsv_read_reg(ctx, LSM6DSV_EMB_FUNC_SRC, (uint8_t *)&emb_func_src, 1); + ret += lsm6dsv_mem_bank_set(ctx, LSM6DSV_MAIN_MEM_BANK); if (ret != 0) { return ret; } val->emb_func_stand_by = emb_func_exec_status.emb_func_endop; @@ -2100,7 +2083,7 @@ int32_t lsm6dsv16x_all_sources_get(stmdev_ctx_t *ctx, val->step_detector = emb_func_src.step_detected; /* sensor hub */ - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_STATUS_MASTER_MAINPAGE, (uint8_t *)&status_shub, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_STATUS_MASTER_MAINPAGE, (uint8_t *)&status_shub, 1); if (ret != 0) { return ret; } val->sh_endop = status_shub.sens_hub_endop; @@ -2113,13 +2096,13 @@ int32_t lsm6dsv16x_all_sources_get(stmdev_ctx_t *ctx, return ret; } -int32_t lsm6dsv16x_flag_data_ready_get(stmdev_ctx_t *ctx, - lsm6dsv16x_data_ready_t *val) +int32_t lsm6dsv_flag_data_ready_get(stmdev_ctx_t *ctx, + lsm6dsv_data_ready_t *val) { - lsm6dsv16x_status_reg_t status; + lsm6dsv_status_reg_t status; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_STATUS_REG, (uint8_t *)&status, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_STATUS_REG, (uint8_t *)&status, 1); if (ret != 0) { return ret; } val->drdy_xl = status.xlda; @@ -2137,11 +2120,11 @@ int32_t lsm6dsv16x_flag_data_ready_get(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_int_ack_mask_set(stmdev_ctx_t *ctx, uint8_t val) +int32_t lsm6dsv_int_ack_mask_set(stmdev_ctx_t *ctx, uint8_t val) { int32_t ret; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_INT_ACK_MASK, &val, 1); + ret = lsm6dsv_write_reg(ctx, LSM6DSV_INT_ACK_MASK, &val, 1); return ret; } @@ -2154,11 +2137,11 @@ int32_t lsm6dsv16x_int_ack_mask_set(stmdev_ctx_t *ctx, uint8_t val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_int_ack_mask_get(stmdev_ctx_t *ctx, uint8_t *val) +int32_t lsm6dsv_int_ack_mask_get(stmdev_ctx_t *ctx, uint8_t *val) { int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_INT_ACK_MASK, val, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_INT_ACK_MASK, val, 1); return ret; } @@ -2171,12 +2154,12 @@ int32_t lsm6dsv16x_int_ack_mask_get(stmdev_ctx_t *ctx, uint8_t *val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_temperature_raw_get(stmdev_ctx_t *ctx, int16_t *val) +int32_t lsm6dsv_temperature_raw_get(stmdev_ctx_t *ctx, int16_t *val) { uint8_t buff[2]; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_OUT_TEMP_L, &buff[0], 2); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_OUT_TEMP_L, &buff[0], 2); if (ret != 0) { return ret; } *val = (int16_t)buff[1]; @@ -2193,12 +2176,12 @@ int32_t lsm6dsv16x_temperature_raw_get(stmdev_ctx_t *ctx, int16_t *val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_angular_rate_raw_get(stmdev_ctx_t *ctx, int16_t *val) +int32_t lsm6dsv_angular_rate_raw_get(stmdev_ctx_t *ctx, int16_t *val) { uint8_t buff[6]; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_OUTX_L_G, &buff[0], 6); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_OUTX_L_G, &buff[0], 6); if (ret != 0) { return ret; } val[0] = (int16_t)buff[1]; @@ -2219,12 +2202,12 @@ int32_t lsm6dsv16x_angular_rate_raw_get(stmdev_ctx_t *ctx, int16_t *val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_ois_angular_rate_raw_get(stmdev_ctx_t *ctx, int16_t *val) +int32_t lsm6dsv_ois_angular_rate_raw_get(stmdev_ctx_t *ctx, int16_t *val) { uint8_t buff[6]; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_SPI2_OUTX_L_G_OIS, &buff[0], 6); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_SPI2_OUTX_L_G_OIS, &buff[0], 6); if (ret != 0) { return ret; } val[0] = (int16_t)buff[1]; @@ -2245,12 +2228,12 @@ int32_t lsm6dsv16x_ois_angular_rate_raw_get(stmdev_ctx_t *ctx, int16_t *val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_ois_eis_angular_rate_raw_get(stmdev_ctx_t *ctx, int16_t *val) +int32_t lsm6dsv_ois_eis_angular_rate_raw_get(stmdev_ctx_t *ctx, int16_t *val) { uint8_t buff[6]; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_UI_OUTX_L_G_OIS_EIS, &buff[0], 6); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_UI_OUTX_L_G_OIS_EIS, &buff[0], 6); if (ret != 0) { return ret; } val[0] = (int16_t)buff[1]; @@ -2271,12 +2254,12 @@ int32_t lsm6dsv16x_ois_eis_angular_rate_raw_get(stmdev_ctx_t *ctx, int16_t *val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_acceleration_raw_get(stmdev_ctx_t *ctx, int16_t *val) +int32_t lsm6dsv_acceleration_raw_get(stmdev_ctx_t *ctx, int16_t *val) { uint8_t buff[6]; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_OUTX_L_A, &buff[0], 6); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_OUTX_L_A, &buff[0], 6); if (ret != 0) { return ret; } val[0] = (int16_t)buff[1]; @@ -2297,12 +2280,12 @@ int32_t lsm6dsv16x_acceleration_raw_get(stmdev_ctx_t *ctx, int16_t *val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_dual_acceleration_raw_get(stmdev_ctx_t *ctx, int16_t *val) +int32_t lsm6dsv_dual_acceleration_raw_get(stmdev_ctx_t *ctx, int16_t *val) { uint8_t buff[6]; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_UI_OUTX_L_A_OIS_DUALC, &buff[0], 6); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_UI_OUTX_L_A_OIS_DUALC, &buff[0], 6); if (ret != 0) { return ret; } val[0] = (int16_t)buff[1]; @@ -2315,28 +2298,6 @@ int32_t lsm6dsv16x_dual_acceleration_raw_get(stmdev_ctx_t *ctx, int16_t *val) return ret; } -/** - * @brief ah_qvar data output register.[get] - * - * @param ctx read / write interface definitions - * @param val ah_qvar data output register. - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6dsv16x_ah_qvar_raw_get(stmdev_ctx_t *ctx, int16_t *val) -{ - uint8_t buff[2]; - int32_t ret; - - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_AH_QVAR_OUT_L, &buff[0], 2); - if (ret != 0) { return ret; } - - *val = (int16_t)buff[1]; - *val = (*val * 256) + (int16_t)buff[0]; - - return ret; -} - /** * @brief Difference in percentage of the effective ODR (and timestamp rate) with respect to the typical. Step: 0.13%. 8-bit format, 2's complement.[get] * @@ -2345,12 +2306,12 @@ int32_t lsm6dsv16x_ah_qvar_raw_get(stmdev_ctx_t *ctx, int16_t *val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_odr_cal_reg_get(stmdev_ctx_t *ctx, int8_t *val) +int32_t lsm6dsv_odr_cal_reg_get(stmdev_ctx_t *ctx, int8_t *val) { - lsm6dsv16x_internal_freq_t internal_freq; + lsm6dsv_internal_freq_t internal_freq; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_INTERNAL_FREQ, (uint8_t *)&internal_freq, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_INTERNAL_FREQ, (uint8_t *)&internal_freq, 1); *val = (int8_t)internal_freq.freq_fine; return ret; @@ -2364,12 +2325,12 @@ int32_t lsm6dsv16x_odr_cal_reg_get(stmdev_ctx_t *ctx, int8_t *val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_ln_pg_write(stmdev_ctx_t *ctx, uint16_t address, - uint8_t *buf, uint8_t len) +int32_t lsm6dsv_ln_pg_write(stmdev_ctx_t *ctx, uint16_t address, + uint8_t *buf, uint8_t len) { - lsm6dsv16x_page_address_t page_address; - lsm6dsv16x_page_sel_t page_sel; - lsm6dsv16x_page_rw_t page_rw; + lsm6dsv_page_address_t page_address; + lsm6dsv_page_sel_t page_sel; + lsm6dsv_page_rw_t page_rw; uint8_t msb; uint8_t lsb; int32_t ret; @@ -2378,32 +2339,32 @@ int32_t lsm6dsv16x_ln_pg_write(stmdev_ctx_t *ctx, uint16_t address, msb = ((uint8_t)(address >> 8) & 0x0FU); lsb = (uint8_t)address & 0xFFU; - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK); + ret = lsm6dsv_mem_bank_set(ctx, LSM6DSV_EMBED_FUNC_MEM_BANK); if (ret != 0) { return ret; } /* set page write */ - ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_PAGE_RW, (uint8_t *)&page_rw, 1); + ret += lsm6dsv_read_reg(ctx, LSM6DSV_PAGE_RW, (uint8_t *)&page_rw, 1); page_rw.page_read = PROPERTY_DISABLE; page_rw.page_write = PROPERTY_ENABLE; - ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_PAGE_RW, (uint8_t *)&page_rw, 1); + ret += lsm6dsv_write_reg(ctx, LSM6DSV_PAGE_RW, (uint8_t *)&page_rw, 1); if (ret != 0) { goto exit; } /* select page */ - ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_PAGE_SEL, (uint8_t *)&page_sel, 1); + ret += lsm6dsv_read_reg(ctx, LSM6DSV_PAGE_SEL, (uint8_t *)&page_sel, 1); page_sel.page_sel = msb; page_sel.not_used0 = 1; // Default value - ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_PAGE_SEL, (uint8_t *)&page_sel, 1); + ret += lsm6dsv_write_reg(ctx, LSM6DSV_PAGE_SEL, (uint8_t *)&page_sel, 1); if (ret != 0) { goto exit; } /* set page addr */ page_address.page_addr = lsb; - ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_PAGE_ADDRESS, - (uint8_t *)&page_address, 1); + ret += lsm6dsv_write_reg(ctx, LSM6DSV_PAGE_ADDRESS, + (uint8_t *)&page_address, 1); if (ret != 0) { goto exit; } for (i = 0; ((i < len) && (ret == 0)); i++) { - ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_PAGE_VALUE, &buf[i], 1); + ret += lsm6dsv_write_reg(ctx, LSM6DSV_PAGE_VALUE, &buf[i], 1); if (ret != 0) { goto exit; } lsb++; @@ -2412,29 +2373,29 @@ int32_t lsm6dsv16x_ln_pg_write(stmdev_ctx_t *ctx, uint16_t address, if (((lsb & 0xFFU) == 0x00U) && (ret == 0)) { msb++; - ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_PAGE_SEL, (uint8_t *)&page_sel, 1); + ret += lsm6dsv_read_reg(ctx, LSM6DSV_PAGE_SEL, (uint8_t *)&page_sel, 1); if (ret != 0) { goto exit; } page_sel.page_sel = msb; page_sel.not_used0 = 1; // Default value - ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_PAGE_SEL, (uint8_t *)&page_sel, 1); + ret += lsm6dsv_write_reg(ctx, LSM6DSV_PAGE_SEL, (uint8_t *)&page_sel, 1); if (ret != 0) { goto exit; } } } page_sel.page_sel = 0; page_sel.not_used0 = 1;// Default value - ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_PAGE_SEL, (uint8_t *)&page_sel, 1); + ret += lsm6dsv_write_reg(ctx, LSM6DSV_PAGE_SEL, (uint8_t *)&page_sel, 1); if (ret != 0) { goto exit; } /* unset page write */ - ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_PAGE_RW, (uint8_t *)&page_rw, 1); + ret += lsm6dsv_read_reg(ctx, LSM6DSV_PAGE_RW, (uint8_t *)&page_rw, 1); page_rw.page_read = PROPERTY_DISABLE; page_rw.page_write = PROPERTY_DISABLE; - ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_PAGE_RW, (uint8_t *)&page_rw, 1); + ret += lsm6dsv_write_reg(ctx, LSM6DSV_PAGE_RW, (uint8_t *)&page_rw, 1); exit: - ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + ret += lsm6dsv_mem_bank_set(ctx, LSM6DSV_MAIN_MEM_BANK); return ret; } @@ -2454,12 +2415,12 @@ int32_t lsm6dsv16x_ln_pg_write(stmdev_ctx_t *ctx, uint16_t address, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_ln_pg_read(stmdev_ctx_t *ctx, uint16_t address, uint8_t *buf, - uint8_t len) +int32_t lsm6dsv_ln_pg_read(stmdev_ctx_t *ctx, uint16_t address, uint8_t *buf, + uint8_t len) { - lsm6dsv16x_page_address_t page_address; - lsm6dsv16x_page_sel_t page_sel; - lsm6dsv16x_page_rw_t page_rw; + lsm6dsv_page_address_t page_address; + lsm6dsv_page_sel_t page_sel; + lsm6dsv_page_rw_t page_rw; uint8_t msb; uint8_t lsb; int32_t ret; @@ -2468,32 +2429,32 @@ int32_t lsm6dsv16x_ln_pg_read(stmdev_ctx_t *ctx, uint16_t address, uint8_t *buf, msb = ((uint8_t)(address >> 8) & 0x0FU); lsb = (uint8_t)address & 0xFFU; - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK); + ret = lsm6dsv_mem_bank_set(ctx, LSM6DSV_EMBED_FUNC_MEM_BANK); if (ret != 0) { return ret; } /* set page write */ - ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_PAGE_RW, (uint8_t *)&page_rw, 1); + ret += lsm6dsv_read_reg(ctx, LSM6DSV_PAGE_RW, (uint8_t *)&page_rw, 1); page_rw.page_read = PROPERTY_ENABLE; page_rw.page_write = PROPERTY_DISABLE; - ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_PAGE_RW, (uint8_t *)&page_rw, 1); + ret += lsm6dsv_write_reg(ctx, LSM6DSV_PAGE_RW, (uint8_t *)&page_rw, 1); if (ret != 0) { goto exit; } /* select page */ - ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_PAGE_SEL, (uint8_t *)&page_sel, 1); + ret += lsm6dsv_read_reg(ctx, LSM6DSV_PAGE_SEL, (uint8_t *)&page_sel, 1); page_sel.page_sel = msb; page_sel.not_used0 = 1; // Default value - ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_PAGE_SEL, (uint8_t *)&page_sel, 1); + ret += lsm6dsv_write_reg(ctx, LSM6DSV_PAGE_SEL, (uint8_t *)&page_sel, 1); if (ret != 0) { goto exit; } /* set page addr */ page_address.page_addr = lsb; - ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_PAGE_ADDRESS, - (uint8_t *)&page_address, 1); + ret += lsm6dsv_write_reg(ctx, LSM6DSV_PAGE_ADDRESS, + (uint8_t *)&page_address, 1); if (ret != 0) { goto exit; } for (i = 0; ((i < len) && (ret == 0)); i++) { - ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_PAGE_VALUE, &buf[i], 1); + ret += lsm6dsv_read_reg(ctx, LSM6DSV_PAGE_VALUE, &buf[i], 1); if (ret != 0) { goto exit; } lsb++; @@ -2502,29 +2463,29 @@ int32_t lsm6dsv16x_ln_pg_read(stmdev_ctx_t *ctx, uint16_t address, uint8_t *buf, if (((lsb & 0xFFU) == 0x00U) && (ret == 0)) { msb++; - ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_PAGE_SEL, (uint8_t *)&page_sel, 1); + ret += lsm6dsv_read_reg(ctx, LSM6DSV_PAGE_SEL, (uint8_t *)&page_sel, 1); if (ret != 0) { goto exit; } page_sel.page_sel = msb; page_sel.not_used0 = 1; // Default value - ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_PAGE_SEL, (uint8_t *)&page_sel, 1); + ret += lsm6dsv_write_reg(ctx, LSM6DSV_PAGE_SEL, (uint8_t *)&page_sel, 1); if (ret != 0) { goto exit; } } } page_sel.page_sel = 0; page_sel.not_used0 = 1;// Default value - ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_PAGE_SEL, (uint8_t *)&page_sel, 1); + ret += lsm6dsv_write_reg(ctx, LSM6DSV_PAGE_SEL, (uint8_t *)&page_sel, 1); if (ret != 0) { goto exit; } /* unset page write */ - ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_PAGE_RW, (uint8_t *)&page_rw, 1); + ret += lsm6dsv_read_reg(ctx, LSM6DSV_PAGE_RW, (uint8_t *)&page_rw, 1); page_rw.page_read = PROPERTY_DISABLE; page_rw.page_write = PROPERTY_DISABLE; - ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_PAGE_RW, (uint8_t *)&page_rw, 1); + ret += lsm6dsv_write_reg(ctx, LSM6DSV_PAGE_RW, (uint8_t *)&page_rw, 1); exit: - ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + ret += lsm6dsv_mem_bank_set(ctx, LSM6DSV_MAIN_MEM_BANK); return ret; } @@ -2537,17 +2498,17 @@ int32_t lsm6dsv16x_ln_pg_read(stmdev_ctx_t *ctx, uint16_t address, uint8_t *buf, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_emb_function_dbg_set(stmdev_ctx_t *ctx, uint8_t val) +int32_t lsm6dsv_emb_function_dbg_set(stmdev_ctx_t *ctx, uint8_t val) { - lsm6dsv16x_ctrl10_t ctrl10; + lsm6dsv_ctrl10_t ctrl10; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL10, (uint8_t *)&ctrl10, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_CTRL10, (uint8_t *)&ctrl10, 1); if (ret == 0) { ctrl10.emb_func_debug = val; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_CTRL10, (uint8_t *)&ctrl10, 1); + ret = lsm6dsv_write_reg(ctx, LSM6DSV_CTRL10, (uint8_t *)&ctrl10, 1); } return ret; @@ -2561,12 +2522,12 @@ int32_t lsm6dsv16x_emb_function_dbg_set(stmdev_ctx_t *ctx, uint8_t val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_emb_function_dbg_get(stmdev_ctx_t *ctx, uint8_t *val) +int32_t lsm6dsv_emb_function_dbg_get(stmdev_ctx_t *ctx, uint8_t *val) { - lsm6dsv16x_ctrl10_t ctrl10; + lsm6dsv_ctrl10_t ctrl10; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL10, (uint8_t *)&ctrl10, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_CTRL10, (uint8_t *)&ctrl10, 1); if (ret != 0) { return ret; } *val = ctrl10.emb_func_debug; @@ -2595,18 +2556,17 @@ int32_t lsm6dsv16x_emb_function_dbg_get(stmdev_ctx_t *ctx, uint8_t *val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_den_polarity_set(stmdev_ctx_t *ctx, - lsm6dsv16x_den_polarity_t val) +int32_t lsm6dsv_den_polarity_set(stmdev_ctx_t *ctx, lsm6dsv_den_polarity_t val) { - lsm6dsv16x_ctrl4_t ctrl4; + lsm6dsv_ctrl4_t ctrl4; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL4, (uint8_t *)&ctrl4, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_CTRL4, (uint8_t *)&ctrl4, 1); if (ret == 0) { ctrl4.int2_in_lh = (uint8_t)val & 0x1U; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_CTRL4, (uint8_t *)&ctrl4, 1); + ret = lsm6dsv_write_reg(ctx, LSM6DSV_CTRL4, (uint8_t *)&ctrl4, 1); } return ret; @@ -2620,27 +2580,26 @@ int32_t lsm6dsv16x_den_polarity_set(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_den_polarity_get(stmdev_ctx_t *ctx, - lsm6dsv16x_den_polarity_t *val) +int32_t lsm6dsv_den_polarity_get(stmdev_ctx_t *ctx, lsm6dsv_den_polarity_t *val) { - lsm6dsv16x_ctrl4_t ctrl4; + lsm6dsv_ctrl4_t ctrl4; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL4, (uint8_t *)&ctrl4, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_CTRL4, (uint8_t *)&ctrl4, 1); if (ret != 0) { return ret; } switch (ctrl4.int2_in_lh) { - case LSM6DSV16X_DEN_ACT_LOW: - *val = LSM6DSV16X_DEN_ACT_LOW; + case LSM6DSV_DEN_ACT_LOW: + *val = LSM6DSV_DEN_ACT_LOW; break; - case LSM6DSV16X_DEN_ACT_HIGH: - *val = LSM6DSV16X_DEN_ACT_HIGH; + case LSM6DSV_DEN_ACT_HIGH: + *val = LSM6DSV_DEN_ACT_HIGH; break; default: - *val = LSM6DSV16X_DEN_ACT_LOW; + *val = LSM6DSV_DEN_ACT_LOW; break; } @@ -2655,12 +2614,12 @@ int32_t lsm6dsv16x_den_polarity_get(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_den_conf_set(stmdev_ctx_t *ctx, lsm6dsv16x_den_conf_t val) +int32_t lsm6dsv_den_conf_set(stmdev_ctx_t *ctx, lsm6dsv_den_conf_t val) { - lsm6dsv16x_den_t den; + lsm6dsv_den_t den; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_DEN, (uint8_t *)&den, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_DEN, (uint8_t *)&den, 1); if (ret != 0) { return ret; } den.den_z = val.den_z; @@ -2696,7 +2655,7 @@ int32_t lsm6dsv16x_den_conf_set(stmdev_ctx_t *ctx, lsm6dsv16x_den_conf_t val) den.lvl1_en = PROPERTY_DISABLE; } - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_DEN, (uint8_t *)&den, 1); + ret = lsm6dsv_write_reg(ctx, LSM6DSV_DEN, (uint8_t *)&den, 1); return ret; } @@ -2710,12 +2669,12 @@ int32_t lsm6dsv16x_den_conf_set(stmdev_ctx_t *ctx, lsm6dsv16x_den_conf_t val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_den_conf_get(stmdev_ctx_t *ctx, lsm6dsv16x_den_conf_t *val) +int32_t lsm6dsv_den_conf_get(stmdev_ctx_t *ctx, lsm6dsv_den_conf_t *val) { - lsm6dsv16x_den_t den; + lsm6dsv_den_t den; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_DEN, (uint8_t *)&den, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_DEN, (uint8_t *)&den, 1); if (ret != 0) { return ret; } val->den_z = den.den_z; @@ -2748,16 +2707,16 @@ int32_t lsm6dsv16x_den_conf_get(stmdev_ctx_t *ctx, lsm6dsv16x_den_conf_t *val) switch ((den.lvl1_en << 1) + den.lvl2_en) { - case LEVEL_TRIGGER: - val->mode = LEVEL_TRIGGER; + case LSM6DSV_LEVEL_TRIGGER: + val->mode = LSM6DSV_LEVEL_TRIGGER; break; - case LEVEL_LATCHED: - val->mode = LEVEL_LATCHED; + case LSM6DSV_LEVEL_LATCHED: + val->mode = LSM6DSV_LEVEL_LATCHED; break; default: - val->mode = DEN_NOT_DEFINED; + val->mode = LSM6DSV_DEN_NOT_DEFINED; break; } @@ -2784,18 +2743,18 @@ int32_t lsm6dsv16x_den_conf_get(stmdev_ctx_t *ctx, lsm6dsv16x_den_conf_t *val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_eis_gy_full_scale_set(stmdev_ctx_t *ctx, - lsm6dsv16x_eis_gy_full_scale_t val) +int32_t lsm6dsv_eis_gy_full_scale_set(stmdev_ctx_t *ctx, + lsm6dsv_eis_gy_full_scale_t val) { - lsm6dsv16x_ctrl_eis_t ctrl_eis; + lsm6dsv_ctrl_eis_t ctrl_eis; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL_EIS, (uint8_t *)&ctrl_eis, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_CTRL_EIS, (uint8_t *)&ctrl_eis, 1); if (ret == 0) { ctrl_eis.fs_g_eis = (uint8_t)val & 0x7U; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_CTRL_EIS, (uint8_t *)&ctrl_eis, 1); + ret = lsm6dsv_write_reg(ctx, LSM6DSV_CTRL_EIS, (uint8_t *)&ctrl_eis, 1); } return ret; @@ -2809,39 +2768,39 @@ int32_t lsm6dsv16x_eis_gy_full_scale_set(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_eis_gy_full_scale_get(stmdev_ctx_t *ctx, - lsm6dsv16x_eis_gy_full_scale_t *val) +int32_t lsm6dsv_eis_gy_full_scale_get(stmdev_ctx_t *ctx, + lsm6dsv_eis_gy_full_scale_t *val) { - lsm6dsv16x_ctrl_eis_t ctrl_eis; + lsm6dsv_ctrl_eis_t ctrl_eis; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL_EIS, (uint8_t *)&ctrl_eis, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_CTRL_EIS, (uint8_t *)&ctrl_eis, 1); if (ret != 0) { return ret; } switch (ctrl_eis.fs_g_eis) { - case LSM6DSV16X_EIS_125dps: - *val = LSM6DSV16X_EIS_125dps; + case LSM6DSV_EIS_125dps: + *val = LSM6DSV_EIS_125dps; break; - case LSM6DSV16X_EIS_250dps: - *val = LSM6DSV16X_EIS_250dps; + case LSM6DSV_EIS_250dps: + *val = LSM6DSV_EIS_250dps; break; - case LSM6DSV16X_EIS_500dps: - *val = LSM6DSV16X_EIS_500dps; + case LSM6DSV_EIS_500dps: + *val = LSM6DSV_EIS_500dps; break; - case LSM6DSV16X_EIS_1000dps: - *val = LSM6DSV16X_EIS_1000dps; + case LSM6DSV_EIS_1000dps: + *val = LSM6DSV_EIS_1000dps; break; - case LSM6DSV16X_EIS_2000dps: - *val = LSM6DSV16X_EIS_2000dps; + case LSM6DSV_EIS_2000dps: + *val = LSM6DSV_EIS_2000dps; break; default: - *val = LSM6DSV16X_EIS_125dps; + *val = LSM6DSV_EIS_125dps; break; } return ret; @@ -2855,17 +2814,17 @@ int32_t lsm6dsv16x_eis_gy_full_scale_get(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_eis_gy_on_spi2_set(stmdev_ctx_t *ctx, uint8_t val) +int32_t lsm6dsv_eis_gy_on_spi2_set(stmdev_ctx_t *ctx, uint8_t val) { - lsm6dsv16x_ctrl_eis_t ctrl_eis; + lsm6dsv_ctrl_eis_t ctrl_eis; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL_EIS, (uint8_t *)&ctrl_eis, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_CTRL_EIS, (uint8_t *)&ctrl_eis, 1); if (ret == 0) { ctrl_eis.g_eis_on_g_ois_out_reg = val; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_CTRL_EIS, (uint8_t *)&ctrl_eis, 1); + ret = lsm6dsv_write_reg(ctx, LSM6DSV_CTRL_EIS, (uint8_t *)&ctrl_eis, 1); } return ret; @@ -2879,12 +2838,12 @@ int32_t lsm6dsv16x_eis_gy_on_spi2_set(stmdev_ctx_t *ctx, uint8_t val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_eis_gy_on_spi2_get(stmdev_ctx_t *ctx, uint8_t *val) +int32_t lsm6dsv_eis_gy_on_spi2_get(stmdev_ctx_t *ctx, uint8_t *val) { - lsm6dsv16x_ctrl_eis_t ctrl_eis; + lsm6dsv_ctrl_eis_t ctrl_eis; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL_EIS, (uint8_t *)&ctrl_eis, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_CTRL_EIS, (uint8_t *)&ctrl_eis, 1); *val = ctrl_eis.g_eis_on_g_ois_out_reg; return ret; @@ -2898,18 +2857,18 @@ int32_t lsm6dsv16x_eis_gy_on_spi2_get(stmdev_ctx_t *ctx, uint8_t *val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_gy_eis_data_rate_set(stmdev_ctx_t *ctx, - lsm6dsv16x_gy_eis_data_rate_t val) +int32_t lsm6dsv_gy_eis_data_rate_set(stmdev_ctx_t *ctx, + lsm6dsv_gy_eis_data_rate_t val) { - lsm6dsv16x_ctrl_eis_t ctrl_eis; + lsm6dsv_ctrl_eis_t ctrl_eis; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL_EIS, (uint8_t *)&ctrl_eis, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_CTRL_EIS, (uint8_t *)&ctrl_eis, 1); if (ret == 0) { ctrl_eis.odr_g_eis = (uint8_t)val & 0x03U; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_CTRL_EIS, (uint8_t *)&ctrl_eis, 1); + ret = lsm6dsv_write_reg(ctx, LSM6DSV_CTRL_EIS, (uint8_t *)&ctrl_eis, 1); } return ret; @@ -2923,31 +2882,31 @@ int32_t lsm6dsv16x_gy_eis_data_rate_set(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_gy_eis_data_rate_get(stmdev_ctx_t *ctx, - lsm6dsv16x_gy_eis_data_rate_t *val) +int32_t lsm6dsv_gy_eis_data_rate_get(stmdev_ctx_t *ctx, + lsm6dsv_gy_eis_data_rate_t *val) { - lsm6dsv16x_ctrl_eis_t ctrl_eis; + lsm6dsv_ctrl_eis_t ctrl_eis; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL_EIS, (uint8_t *)&ctrl_eis, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_CTRL_EIS, (uint8_t *)&ctrl_eis, 1); if (ret != 0) { return ret; } switch (ctrl_eis.odr_g_eis) { - case LSM6DSV16X_EIS_ODR_OFF: - *val = LSM6DSV16X_EIS_ODR_OFF; + case LSM6DSV_EIS_ODR_OFF: + *val = LSM6DSV_EIS_ODR_OFF; break; - case LSM6DSV16X_EIS_1920Hz: - *val = LSM6DSV16X_EIS_1920Hz; + case LSM6DSV_EIS_1920Hz: + *val = LSM6DSV_EIS_1920Hz; break; - case LSM6DSV16X_EIS_960Hz: - *val = LSM6DSV16X_EIS_960Hz; + case LSM6DSV_EIS_960Hz: + *val = LSM6DSV_EIS_960Hz; break; default: - *val = LSM6DSV16X_EIS_1920Hz; + *val = LSM6DSV_EIS_1920Hz; break; } @@ -2974,17 +2933,17 @@ int32_t lsm6dsv16x_gy_eis_data_rate_get(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fifo_watermark_set(stmdev_ctx_t *ctx, uint8_t val) +int32_t lsm6dsv_fifo_watermark_set(stmdev_ctx_t *ctx, uint8_t val) { - lsm6dsv16x_fifo_ctrl1_t fifo_ctrl1; + lsm6dsv_fifo_ctrl1_t fifo_ctrl1; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FIFO_CTRL1, (uint8_t *)&fifo_ctrl1, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_FIFO_CTRL1, (uint8_t *)&fifo_ctrl1, 1); if (ret == 0) { fifo_ctrl1.wtm = val; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_FIFO_CTRL1, (uint8_t *)&fifo_ctrl1, 1); + ret = lsm6dsv_write_reg(ctx, LSM6DSV_FIFO_CTRL1, (uint8_t *)&fifo_ctrl1, 1); } return ret; @@ -2998,12 +2957,12 @@ int32_t lsm6dsv16x_fifo_watermark_set(stmdev_ctx_t *ctx, uint8_t val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fifo_watermark_get(stmdev_ctx_t *ctx, uint8_t *val) +int32_t lsm6dsv_fifo_watermark_get(stmdev_ctx_t *ctx, uint8_t *val) { - lsm6dsv16x_fifo_ctrl1_t fifo_ctrl1; + lsm6dsv_fifo_ctrl1_t fifo_ctrl1; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FIFO_CTRL1, (uint8_t *)&fifo_ctrl1, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_FIFO_CTRL1, (uint8_t *)&fifo_ctrl1, 1); *val = fifo_ctrl1.wtm; return ret; @@ -3017,16 +2976,16 @@ int32_t lsm6dsv16x_fifo_watermark_get(stmdev_ctx_t *ctx, uint8_t *val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fifo_xl_dual_fsm_batch_set(stmdev_ctx_t *ctx, uint8_t val) +int32_t lsm6dsv_fifo_xl_dual_fsm_batch_set(stmdev_ctx_t *ctx, uint8_t val) { - lsm6dsv16x_fifo_ctrl2_t fifo_ctrl2; + lsm6dsv_fifo_ctrl2_t fifo_ctrl2; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FIFO_CTRL2, (uint8_t *)&fifo_ctrl2, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_FIFO_CTRL2, (uint8_t *)&fifo_ctrl2, 1); if (ret == 0) { fifo_ctrl2.xl_dualc_batch_from_fsm = val; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_FIFO_CTRL2, (uint8_t *)&fifo_ctrl2, 1); + ret = lsm6dsv_write_reg(ctx, LSM6DSV_FIFO_CTRL2, (uint8_t *)&fifo_ctrl2, 1); } return ret; @@ -3040,12 +2999,12 @@ int32_t lsm6dsv16x_fifo_xl_dual_fsm_batch_set(stmdev_ctx_t *ctx, uint8_t val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fifo_xl_dual_fsm_batch_get(stmdev_ctx_t *ctx, uint8_t *val) +int32_t lsm6dsv_fifo_xl_dual_fsm_batch_get(stmdev_ctx_t *ctx, uint8_t *val) { - lsm6dsv16x_fifo_ctrl2_t fifo_ctrl2; + lsm6dsv_fifo_ctrl2_t fifo_ctrl2; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FIFO_CTRL2, (uint8_t *)&fifo_ctrl2, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_FIFO_CTRL2, (uint8_t *)&fifo_ctrl2, 1); *val = fifo_ctrl2.xl_dualc_batch_from_fsm; return ret; @@ -3059,17 +3018,17 @@ int32_t lsm6dsv16x_fifo_xl_dual_fsm_batch_get(stmdev_ctx_t *ctx, uint8_t *val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fifo_compress_algo_set(stmdev_ctx_t *ctx, - lsm6dsv16x_fifo_compress_algo_t val) +int32_t lsm6dsv_fifo_compress_algo_set(stmdev_ctx_t *ctx, + lsm6dsv_fifo_compress_algo_t val) { - lsm6dsv16x_fifo_ctrl2_t fifo_ctrl2; + lsm6dsv_fifo_ctrl2_t fifo_ctrl2; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FIFO_CTRL2, (uint8_t *)&fifo_ctrl2, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_FIFO_CTRL2, (uint8_t *)&fifo_ctrl2, 1); if (ret == 0) { fifo_ctrl2.uncompr_rate = (uint8_t)val & 0x03U; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_FIFO_CTRL2, (uint8_t *)&fifo_ctrl2, 1); + ret = lsm6dsv_write_reg(ctx, LSM6DSV_FIFO_CTRL2, (uint8_t *)&fifo_ctrl2, 1); } return ret; @@ -3083,35 +3042,35 @@ int32_t lsm6dsv16x_fifo_compress_algo_set(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fifo_compress_algo_get(stmdev_ctx_t *ctx, - lsm6dsv16x_fifo_compress_algo_t *val) +int32_t lsm6dsv_fifo_compress_algo_get(stmdev_ctx_t *ctx, + lsm6dsv_fifo_compress_algo_t *val) { - lsm6dsv16x_fifo_ctrl2_t fifo_ctrl2; + lsm6dsv_fifo_ctrl2_t fifo_ctrl2; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FIFO_CTRL2, (uint8_t *)&fifo_ctrl2, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_FIFO_CTRL2, (uint8_t *)&fifo_ctrl2, 1); if (ret != 0) { return ret; } switch (fifo_ctrl2.uncompr_rate) { - case LSM6DSV16X_CMP_DISABLE: - *val = LSM6DSV16X_CMP_DISABLE; + case LSM6DSV_CMP_DISABLE: + *val = LSM6DSV_CMP_DISABLE; break; - case LSM6DSV16X_CMP_8_TO_1: - *val = LSM6DSV16X_CMP_8_TO_1; + case LSM6DSV_CMP_8_TO_1: + *val = LSM6DSV_CMP_8_TO_1; break; - case LSM6DSV16X_CMP_16_TO_1: - *val = LSM6DSV16X_CMP_16_TO_1; + case LSM6DSV_CMP_16_TO_1: + *val = LSM6DSV_CMP_16_TO_1; break; - case LSM6DSV16X_CMP_32_TO_1: - *val = LSM6DSV16X_CMP_32_TO_1; + case LSM6DSV_CMP_32_TO_1: + *val = LSM6DSV_CMP_32_TO_1; break; default: - *val = LSM6DSV16X_CMP_DISABLE; + *val = LSM6DSV_CMP_DISABLE; break; } return ret; @@ -3125,16 +3084,16 @@ int32_t lsm6dsv16x_fifo_compress_algo_get(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fifo_virtual_sens_odr_chg_set(stmdev_ctx_t *ctx, uint8_t val) +int32_t lsm6dsv_fifo_virtual_sens_odr_chg_set(stmdev_ctx_t *ctx, uint8_t val) { - lsm6dsv16x_fifo_ctrl2_t fifo_ctrl2; + lsm6dsv_fifo_ctrl2_t fifo_ctrl2; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FIFO_CTRL2, (uint8_t *)&fifo_ctrl2, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_FIFO_CTRL2, (uint8_t *)&fifo_ctrl2, 1); if (ret == 0) { fifo_ctrl2.odr_chg_en = val; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_FIFO_CTRL2, (uint8_t *)&fifo_ctrl2, 1); + ret = lsm6dsv_write_reg(ctx, LSM6DSV_FIFO_CTRL2, (uint8_t *)&fifo_ctrl2, 1); } return ret; @@ -3148,13 +3107,12 @@ int32_t lsm6dsv16x_fifo_virtual_sens_odr_chg_set(stmdev_ctx_t *ctx, uint8_t val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fifo_virtual_sens_odr_chg_get(stmdev_ctx_t *ctx, - uint8_t *val) +int32_t lsm6dsv_fifo_virtual_sens_odr_chg_get(stmdev_ctx_t *ctx, uint8_t *val) { - lsm6dsv16x_fifo_ctrl2_t fifo_ctrl2; + lsm6dsv_fifo_ctrl2_t fifo_ctrl2; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FIFO_CTRL2, (uint8_t *)&fifo_ctrl2, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_FIFO_CTRL2, (uint8_t *)&fifo_ctrl2, 1); *val = fifo_ctrl2.odr_chg_en; return ret; @@ -3168,26 +3126,25 @@ int32_t lsm6dsv16x_fifo_virtual_sens_odr_chg_get(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fifo_compress_algo_real_time_set(stmdev_ctx_t *ctx, - uint8_t val) +int32_t lsm6dsv_fifo_compress_algo_real_time_set(stmdev_ctx_t *ctx, uint8_t val) { - lsm6dsv16x_emb_func_en_b_t emb_func_en_b; - lsm6dsv16x_fifo_ctrl2_t fifo_ctrl2; + lsm6dsv_emb_func_en_b_t emb_func_en_b; + lsm6dsv_fifo_ctrl2_t fifo_ctrl2; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FIFO_CTRL2, (uint8_t *)&fifo_ctrl2, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_FIFO_CTRL2, (uint8_t *)&fifo_ctrl2, 1); fifo_ctrl2.fifo_compr_rt_en = val; - ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_FIFO_CTRL2, (uint8_t *)&fifo_ctrl2, 1); + ret += lsm6dsv_write_reg(ctx, LSM6DSV_FIFO_CTRL2, (uint8_t *)&fifo_ctrl2, 1); if (ret != 0) { return ret; } - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK); + ret = lsm6dsv_mem_bank_set(ctx, LSM6DSV_EMBED_FUNC_MEM_BANK); if (ret != 0) { return ret; } - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_EMB_FUNC_EN_B, (uint8_t *)&emb_func_en_b, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_EMB_FUNC_EN_B, (uint8_t *)&emb_func_en_b, 1); emb_func_en_b.fifo_compr_en = val; - ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_EMB_FUNC_EN_B, (uint8_t *)&emb_func_en_b, 1); - ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + ret += lsm6dsv_write_reg(ctx, LSM6DSV_EMB_FUNC_EN_B, (uint8_t *)&emb_func_en_b, 1); + ret += lsm6dsv_mem_bank_set(ctx, LSM6DSV_MAIN_MEM_BANK); return ret; } @@ -3200,13 +3157,13 @@ int32_t lsm6dsv16x_fifo_compress_algo_real_time_set(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fifo_compress_algo_real_time_get(stmdev_ctx_t *ctx, - uint8_t *val) +int32_t lsm6dsv_fifo_compress_algo_real_time_get(stmdev_ctx_t *ctx, + uint8_t *val) { - lsm6dsv16x_fifo_ctrl2_t fifo_ctrl2; + lsm6dsv_fifo_ctrl2_t fifo_ctrl2; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FIFO_CTRL2, (uint8_t *)&fifo_ctrl2, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_FIFO_CTRL2, (uint8_t *)&fifo_ctrl2, 1); *val = fifo_ctrl2.fifo_compr_rt_en; @@ -3221,16 +3178,16 @@ int32_t lsm6dsv16x_fifo_compress_algo_real_time_get(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fifo_stop_on_wtm_set(stmdev_ctx_t *ctx, uint8_t val) +int32_t lsm6dsv_fifo_stop_on_wtm_set(stmdev_ctx_t *ctx, uint8_t val) { - lsm6dsv16x_fifo_ctrl2_t fifo_ctrl2; + lsm6dsv_fifo_ctrl2_t fifo_ctrl2; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FIFO_CTRL2, (uint8_t *)&fifo_ctrl2, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_FIFO_CTRL2, (uint8_t *)&fifo_ctrl2, 1); if (ret == 0) { fifo_ctrl2.stop_on_wtm = val; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_FIFO_CTRL2, (uint8_t *)&fifo_ctrl2, 1); + ret = lsm6dsv_write_reg(ctx, LSM6DSV_FIFO_CTRL2, (uint8_t *)&fifo_ctrl2, 1); } return ret; @@ -3244,12 +3201,12 @@ int32_t lsm6dsv16x_fifo_stop_on_wtm_set(stmdev_ctx_t *ctx, uint8_t val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fifo_stop_on_wtm_get(stmdev_ctx_t *ctx, uint8_t *val) +int32_t lsm6dsv_fifo_stop_on_wtm_get(stmdev_ctx_t *ctx, uint8_t *val) { - lsm6dsv16x_fifo_ctrl2_t fifo_ctrl2; + lsm6dsv_fifo_ctrl2_t fifo_ctrl2; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FIFO_CTRL2, (uint8_t *)&fifo_ctrl2, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_FIFO_CTRL2, (uint8_t *)&fifo_ctrl2, 1); *val = fifo_ctrl2.stop_on_wtm; return ret; @@ -3263,17 +3220,17 @@ int32_t lsm6dsv16x_fifo_stop_on_wtm_get(stmdev_ctx_t *ctx, uint8_t *val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fifo_xl_batch_set(stmdev_ctx_t *ctx, - lsm6dsv16x_fifo_xl_batch_t val) +int32_t lsm6dsv_fifo_xl_batch_set(stmdev_ctx_t *ctx, + lsm6dsv_fifo_xl_batch_t val) { - lsm6dsv16x_fifo_ctrl3_t fifo_ctrl3; + lsm6dsv_fifo_ctrl3_t fifo_ctrl3; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FIFO_CTRL3, (uint8_t *)&fifo_ctrl3, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_FIFO_CTRL3, (uint8_t *)&fifo_ctrl3, 1); if (ret == 0) { fifo_ctrl3.bdr_xl = (uint8_t)val & 0xFu; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_FIFO_CTRL3, (uint8_t *)&fifo_ctrl3, 1); + ret = lsm6dsv_write_reg(ctx, LSM6DSV_FIFO_CTRL3, (uint8_t *)&fifo_ctrl3, 1); } return ret; @@ -3287,71 +3244,71 @@ int32_t lsm6dsv16x_fifo_xl_batch_set(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fifo_xl_batch_get(stmdev_ctx_t *ctx, - lsm6dsv16x_fifo_xl_batch_t *val) +int32_t lsm6dsv_fifo_xl_batch_get(stmdev_ctx_t *ctx, + lsm6dsv_fifo_xl_batch_t *val) { - lsm6dsv16x_fifo_ctrl3_t fifo_ctrl3; + lsm6dsv_fifo_ctrl3_t fifo_ctrl3; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FIFO_CTRL3, (uint8_t *)&fifo_ctrl3, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_FIFO_CTRL3, (uint8_t *)&fifo_ctrl3, 1); if (ret != 0) { return ret; } switch (fifo_ctrl3.bdr_xl) { - case LSM6DSV16X_XL_NOT_BATCHED: - *val = LSM6DSV16X_XL_NOT_BATCHED; + case LSM6DSV_XL_NOT_BATCHED: + *val = LSM6DSV_XL_NOT_BATCHED; break; - case LSM6DSV16X_XL_BATCHED_AT_1Hz875: - *val = LSM6DSV16X_XL_BATCHED_AT_1Hz875; + case LSM6DSV_XL_BATCHED_AT_1Hz875: + *val = LSM6DSV_XL_BATCHED_AT_1Hz875; break; - case LSM6DSV16X_XL_BATCHED_AT_7Hz5: - *val = LSM6DSV16X_XL_BATCHED_AT_7Hz5; + case LSM6DSV_XL_BATCHED_AT_7Hz5: + *val = LSM6DSV_XL_BATCHED_AT_7Hz5; break; - case LSM6DSV16X_XL_BATCHED_AT_15Hz: - *val = LSM6DSV16X_XL_BATCHED_AT_15Hz; + case LSM6DSV_XL_BATCHED_AT_15Hz: + *val = LSM6DSV_XL_BATCHED_AT_15Hz; break; - case LSM6DSV16X_XL_BATCHED_AT_30Hz: - *val = LSM6DSV16X_XL_BATCHED_AT_30Hz; + case LSM6DSV_XL_BATCHED_AT_30Hz: + *val = LSM6DSV_XL_BATCHED_AT_30Hz; break; - case LSM6DSV16X_XL_BATCHED_AT_60Hz: - *val = LSM6DSV16X_XL_BATCHED_AT_60Hz; + case LSM6DSV_XL_BATCHED_AT_60Hz: + *val = LSM6DSV_XL_BATCHED_AT_60Hz; break; - case LSM6DSV16X_XL_BATCHED_AT_120Hz: - *val = LSM6DSV16X_XL_BATCHED_AT_120Hz; + case LSM6DSV_XL_BATCHED_AT_120Hz: + *val = LSM6DSV_XL_BATCHED_AT_120Hz; break; - case LSM6DSV16X_XL_BATCHED_AT_240Hz: - *val = LSM6DSV16X_XL_BATCHED_AT_240Hz; + case LSM6DSV_XL_BATCHED_AT_240Hz: + *val = LSM6DSV_XL_BATCHED_AT_240Hz; break; - case LSM6DSV16X_XL_BATCHED_AT_480Hz: - *val = LSM6DSV16X_XL_BATCHED_AT_480Hz; + case LSM6DSV_XL_BATCHED_AT_480Hz: + *val = LSM6DSV_XL_BATCHED_AT_480Hz; break; - case LSM6DSV16X_XL_BATCHED_AT_960Hz: - *val = LSM6DSV16X_XL_BATCHED_AT_960Hz; + case LSM6DSV_XL_BATCHED_AT_960Hz: + *val = LSM6DSV_XL_BATCHED_AT_960Hz; break; - case LSM6DSV16X_XL_BATCHED_AT_1920Hz: - *val = LSM6DSV16X_XL_BATCHED_AT_1920Hz; + case LSM6DSV_XL_BATCHED_AT_1920Hz: + *val = LSM6DSV_XL_BATCHED_AT_1920Hz; break; - case LSM6DSV16X_XL_BATCHED_AT_3840Hz: - *val = LSM6DSV16X_XL_BATCHED_AT_3840Hz; + case LSM6DSV_XL_BATCHED_AT_3840Hz: + *val = LSM6DSV_XL_BATCHED_AT_3840Hz; break; - case LSM6DSV16X_XL_BATCHED_AT_7680Hz: - *val = LSM6DSV16X_XL_BATCHED_AT_7680Hz; + case LSM6DSV_XL_BATCHED_AT_7680Hz: + *val = LSM6DSV_XL_BATCHED_AT_7680Hz; break; default: - *val = LSM6DSV16X_XL_NOT_BATCHED; + *val = LSM6DSV_XL_NOT_BATCHED; break; } @@ -3366,17 +3323,17 @@ int32_t lsm6dsv16x_fifo_xl_batch_get(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fifo_gy_batch_set(stmdev_ctx_t *ctx, - lsm6dsv16x_fifo_gy_batch_t val) +int32_t lsm6dsv_fifo_gy_batch_set(stmdev_ctx_t *ctx, + lsm6dsv_fifo_gy_batch_t val) { - lsm6dsv16x_fifo_ctrl3_t fifo_ctrl3; + lsm6dsv_fifo_ctrl3_t fifo_ctrl3; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FIFO_CTRL3, (uint8_t *)&fifo_ctrl3, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_FIFO_CTRL3, (uint8_t *)&fifo_ctrl3, 1); if (ret == 0) { fifo_ctrl3.bdr_gy = (uint8_t)val & 0x0Fu; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_FIFO_CTRL3, (uint8_t *)&fifo_ctrl3, 1); + ret = lsm6dsv_write_reg(ctx, LSM6DSV_FIFO_CTRL3, (uint8_t *)&fifo_ctrl3, 1); } return ret; @@ -3390,71 +3347,71 @@ int32_t lsm6dsv16x_fifo_gy_batch_set(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fifo_gy_batch_get(stmdev_ctx_t *ctx, - lsm6dsv16x_fifo_gy_batch_t *val) +int32_t lsm6dsv_fifo_gy_batch_get(stmdev_ctx_t *ctx, + lsm6dsv_fifo_gy_batch_t *val) { - lsm6dsv16x_fifo_ctrl3_t fifo_ctrl3; + lsm6dsv_fifo_ctrl3_t fifo_ctrl3; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FIFO_CTRL3, (uint8_t *)&fifo_ctrl3, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_FIFO_CTRL3, (uint8_t *)&fifo_ctrl3, 1); if (ret != 0) { return ret; } switch (fifo_ctrl3.bdr_gy) { - case LSM6DSV16X_GY_NOT_BATCHED: - *val = LSM6DSV16X_GY_NOT_BATCHED; + case LSM6DSV_GY_NOT_BATCHED: + *val = LSM6DSV_GY_NOT_BATCHED; break; - case LSM6DSV16X_GY_BATCHED_AT_1Hz875: - *val = LSM6DSV16X_GY_BATCHED_AT_1Hz875; + case LSM6DSV_GY_BATCHED_AT_1Hz875: + *val = LSM6DSV_GY_BATCHED_AT_1Hz875; break; - case LSM6DSV16X_GY_BATCHED_AT_7Hz5: - *val = LSM6DSV16X_GY_BATCHED_AT_7Hz5; + case LSM6DSV_GY_BATCHED_AT_7Hz5: + *val = LSM6DSV_GY_BATCHED_AT_7Hz5; break; - case LSM6DSV16X_GY_BATCHED_AT_15Hz: - *val = LSM6DSV16X_GY_BATCHED_AT_15Hz; + case LSM6DSV_GY_BATCHED_AT_15Hz: + *val = LSM6DSV_GY_BATCHED_AT_15Hz; break; - case LSM6DSV16X_GY_BATCHED_AT_30Hz: - *val = LSM6DSV16X_GY_BATCHED_AT_30Hz; + case LSM6DSV_GY_BATCHED_AT_30Hz: + *val = LSM6DSV_GY_BATCHED_AT_30Hz; break; - case LSM6DSV16X_GY_BATCHED_AT_60Hz: - *val = LSM6DSV16X_GY_BATCHED_AT_60Hz; + case LSM6DSV_GY_BATCHED_AT_60Hz: + *val = LSM6DSV_GY_BATCHED_AT_60Hz; break; - case LSM6DSV16X_GY_BATCHED_AT_120Hz: - *val = LSM6DSV16X_GY_BATCHED_AT_120Hz; + case LSM6DSV_GY_BATCHED_AT_120Hz: + *val = LSM6DSV_GY_BATCHED_AT_120Hz; break; - case LSM6DSV16X_GY_BATCHED_AT_240Hz: - *val = LSM6DSV16X_GY_BATCHED_AT_240Hz; + case LSM6DSV_GY_BATCHED_AT_240Hz: + *val = LSM6DSV_GY_BATCHED_AT_240Hz; break; - case LSM6DSV16X_GY_BATCHED_AT_480Hz: - *val = LSM6DSV16X_GY_BATCHED_AT_480Hz; + case LSM6DSV_GY_BATCHED_AT_480Hz: + *val = LSM6DSV_GY_BATCHED_AT_480Hz; break; - case LSM6DSV16X_GY_BATCHED_AT_960Hz: - *val = LSM6DSV16X_GY_BATCHED_AT_960Hz; + case LSM6DSV_GY_BATCHED_AT_960Hz: + *val = LSM6DSV_GY_BATCHED_AT_960Hz; break; - case LSM6DSV16X_GY_BATCHED_AT_1920Hz: - *val = LSM6DSV16X_GY_BATCHED_AT_1920Hz; + case LSM6DSV_GY_BATCHED_AT_1920Hz: + *val = LSM6DSV_GY_BATCHED_AT_1920Hz; break; - case LSM6DSV16X_GY_BATCHED_AT_3840Hz: - *val = LSM6DSV16X_GY_BATCHED_AT_3840Hz; + case LSM6DSV_GY_BATCHED_AT_3840Hz: + *val = LSM6DSV_GY_BATCHED_AT_3840Hz; break; - case LSM6DSV16X_GY_BATCHED_AT_7680Hz: - *val = LSM6DSV16X_GY_BATCHED_AT_7680Hz; + case LSM6DSV_GY_BATCHED_AT_7680Hz: + *val = LSM6DSV_GY_BATCHED_AT_7680Hz; break; default: - *val = LSM6DSV16X_GY_NOT_BATCHED; + *val = LSM6DSV_GY_NOT_BATCHED; break; } return ret; @@ -3469,16 +3426,16 @@ int32_t lsm6dsv16x_fifo_gy_batch_get(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fifo_mode_set(stmdev_ctx_t *ctx, lsm6dsv16x_fifo_mode_t val) +int32_t lsm6dsv_fifo_mode_set(stmdev_ctx_t *ctx, lsm6dsv_fifo_mode_t val) { - lsm6dsv16x_fifo_ctrl4_t fifo_ctrl4; + lsm6dsv_fifo_ctrl4_t fifo_ctrl4; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FIFO_CTRL4, (uint8_t *)&fifo_ctrl4, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_FIFO_CTRL4, (uint8_t *)&fifo_ctrl4, 1); if (ret == 0) { fifo_ctrl4.fifo_mode = (uint8_t)val & 0x07U; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_FIFO_CTRL4, (uint8_t *)&fifo_ctrl4, 1); + ret = lsm6dsv_write_reg(ctx, LSM6DSV_FIFO_CTRL4, (uint8_t *)&fifo_ctrl4, 1); } return ret; @@ -3492,46 +3449,46 @@ int32_t lsm6dsv16x_fifo_mode_set(stmdev_ctx_t *ctx, lsm6dsv16x_fifo_mode_t val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fifo_mode_get(stmdev_ctx_t *ctx, lsm6dsv16x_fifo_mode_t *val) +int32_t lsm6dsv_fifo_mode_get(stmdev_ctx_t *ctx, lsm6dsv_fifo_mode_t *val) { - lsm6dsv16x_fifo_ctrl4_t fifo_ctrl4; + lsm6dsv_fifo_ctrl4_t fifo_ctrl4; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FIFO_CTRL4, (uint8_t *)&fifo_ctrl4, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_FIFO_CTRL4, (uint8_t *)&fifo_ctrl4, 1); if (ret != 0) { return ret; } switch (fifo_ctrl4.fifo_mode) { - case LSM6DSV16X_BYPASS_MODE: - *val = LSM6DSV16X_BYPASS_MODE; + case LSM6DSV_BYPASS_MODE: + *val = LSM6DSV_BYPASS_MODE; break; - case LSM6DSV16X_FIFO_MODE: - *val = LSM6DSV16X_FIFO_MODE; + case LSM6DSV_FIFO_MODE: + *val = LSM6DSV_FIFO_MODE; break; - case LSM6DSV16X_STREAM_WTM_TO_FULL_MODE: - *val = LSM6DSV16X_STREAM_WTM_TO_FULL_MODE; + case LSM6DSV_STREAM_WTM_TO_FULL_MODE: + *val = LSM6DSV_STREAM_WTM_TO_FULL_MODE; break; - case LSM6DSV16X_STREAM_TO_FIFO_MODE: - *val = LSM6DSV16X_STREAM_TO_FIFO_MODE; + case LSM6DSV_STREAM_TO_FIFO_MODE: + *val = LSM6DSV_STREAM_TO_FIFO_MODE; break; - case LSM6DSV16X_BYPASS_TO_STREAM_MODE: - *val = LSM6DSV16X_BYPASS_TO_STREAM_MODE; + case LSM6DSV_BYPASS_TO_STREAM_MODE: + *val = LSM6DSV_BYPASS_TO_STREAM_MODE; break; - case LSM6DSV16X_STREAM_MODE: - *val = LSM6DSV16X_STREAM_MODE; + case LSM6DSV_STREAM_MODE: + *val = LSM6DSV_STREAM_MODE; break; - case LSM6DSV16X_BYPASS_TO_FIFO_MODE: - *val = LSM6DSV16X_BYPASS_TO_FIFO_MODE; + case LSM6DSV_BYPASS_TO_FIFO_MODE: + *val = LSM6DSV_BYPASS_TO_FIFO_MODE; break; default: - *val = LSM6DSV16X_BYPASS_MODE; + *val = LSM6DSV_BYPASS_MODE; break; } return ret; @@ -3545,16 +3502,16 @@ int32_t lsm6dsv16x_fifo_mode_get(stmdev_ctx_t *ctx, lsm6dsv16x_fifo_mode_t *val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fifo_gy_eis_batch_set(stmdev_ctx_t *ctx, uint8_t val) +int32_t lsm6dsv_fifo_gy_eis_batch_set(stmdev_ctx_t *ctx, uint8_t val) { - lsm6dsv16x_fifo_ctrl4_t fifo_ctrl4; + lsm6dsv_fifo_ctrl4_t fifo_ctrl4; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FIFO_CTRL4, (uint8_t *)&fifo_ctrl4, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_FIFO_CTRL4, (uint8_t *)&fifo_ctrl4, 1); if (ret == 0) { fifo_ctrl4.g_eis_fifo_en = val; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_FIFO_CTRL4, (uint8_t *)&fifo_ctrl4, 1); + ret = lsm6dsv_write_reg(ctx, LSM6DSV_FIFO_CTRL4, (uint8_t *)&fifo_ctrl4, 1); } return ret; @@ -3568,12 +3525,12 @@ int32_t lsm6dsv16x_fifo_gy_eis_batch_set(stmdev_ctx_t *ctx, uint8_t val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fifo_gy_eis_batch_get(stmdev_ctx_t *ctx, uint8_t *val) +int32_t lsm6dsv_fifo_gy_eis_batch_get(stmdev_ctx_t *ctx, uint8_t *val) { - lsm6dsv16x_fifo_ctrl4_t fifo_ctrl4; + lsm6dsv_fifo_ctrl4_t fifo_ctrl4; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FIFO_CTRL4, (uint8_t *)&fifo_ctrl4, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_FIFO_CTRL4, (uint8_t *)&fifo_ctrl4, 1); *val = fifo_ctrl4.g_eis_fifo_en; return ret; @@ -3587,17 +3544,17 @@ int32_t lsm6dsv16x_fifo_gy_eis_batch_get(stmdev_ctx_t *ctx, uint8_t *val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fifo_temp_batch_set(stmdev_ctx_t *ctx, - lsm6dsv16x_fifo_temp_batch_t val) +int32_t lsm6dsv_fifo_temp_batch_set(stmdev_ctx_t *ctx, + lsm6dsv_fifo_temp_batch_t val) { - lsm6dsv16x_fifo_ctrl4_t fifo_ctrl4; + lsm6dsv_fifo_ctrl4_t fifo_ctrl4; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FIFO_CTRL4, (uint8_t *)&fifo_ctrl4, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_FIFO_CTRL4, (uint8_t *)&fifo_ctrl4, 1); if (ret == 0) { fifo_ctrl4.odr_t_batch = (uint8_t)val & 0x03U; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_FIFO_CTRL4, (uint8_t *)&fifo_ctrl4, 1); + ret = lsm6dsv_write_reg(ctx, LSM6DSV_FIFO_CTRL4, (uint8_t *)&fifo_ctrl4, 1); } return ret; @@ -3611,35 +3568,35 @@ int32_t lsm6dsv16x_fifo_temp_batch_set(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fifo_temp_batch_get(stmdev_ctx_t *ctx, - lsm6dsv16x_fifo_temp_batch_t *val) +int32_t lsm6dsv_fifo_temp_batch_get(stmdev_ctx_t *ctx, + lsm6dsv_fifo_temp_batch_t *val) { - lsm6dsv16x_fifo_ctrl4_t fifo_ctrl4; + lsm6dsv_fifo_ctrl4_t fifo_ctrl4; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FIFO_CTRL4, (uint8_t *)&fifo_ctrl4, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_FIFO_CTRL4, (uint8_t *)&fifo_ctrl4, 1); if (ret != 0) { return ret; } switch (fifo_ctrl4.odr_t_batch) { - case LSM6DSV16X_TEMP_NOT_BATCHED: - *val = LSM6DSV16X_TEMP_NOT_BATCHED; + case LSM6DSV_TEMP_NOT_BATCHED: + *val = LSM6DSV_TEMP_NOT_BATCHED; break; - case LSM6DSV16X_TEMP_BATCHED_AT_1Hz875: - *val = LSM6DSV16X_TEMP_BATCHED_AT_1Hz875; + case LSM6DSV_TEMP_BATCHED_AT_1Hz875: + *val = LSM6DSV_TEMP_BATCHED_AT_1Hz875; break; - case LSM6DSV16X_TEMP_BATCHED_AT_15Hz: - *val = LSM6DSV16X_TEMP_BATCHED_AT_15Hz; + case LSM6DSV_TEMP_BATCHED_AT_15Hz: + *val = LSM6DSV_TEMP_BATCHED_AT_15Hz; break; - case LSM6DSV16X_TEMP_BATCHED_AT_60Hz: - *val = LSM6DSV16X_TEMP_BATCHED_AT_60Hz; + case LSM6DSV_TEMP_BATCHED_AT_60Hz: + *val = LSM6DSV_TEMP_BATCHED_AT_60Hz; break; default: - *val = LSM6DSV16X_TEMP_NOT_BATCHED; + *val = LSM6DSV_TEMP_NOT_BATCHED; break; } return ret; @@ -3653,17 +3610,17 @@ int32_t lsm6dsv16x_fifo_temp_batch_get(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fifo_timestamp_batch_set(stmdev_ctx_t *ctx, - lsm6dsv16x_fifo_timestamp_batch_t val) +int32_t lsm6dsv_fifo_timestamp_batch_set(stmdev_ctx_t *ctx, + lsm6dsv_fifo_timestamp_batch_t val) { - lsm6dsv16x_fifo_ctrl4_t fifo_ctrl4; + lsm6dsv_fifo_ctrl4_t fifo_ctrl4; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FIFO_CTRL4, (uint8_t *)&fifo_ctrl4, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_FIFO_CTRL4, (uint8_t *)&fifo_ctrl4, 1); if (ret == 0) { fifo_ctrl4.dec_ts_batch = (uint8_t)val & 0x03U; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_FIFO_CTRL4, (uint8_t *)&fifo_ctrl4, 1); + ret = lsm6dsv_write_reg(ctx, LSM6DSV_FIFO_CTRL4, (uint8_t *)&fifo_ctrl4, 1); } return ret; @@ -3677,35 +3634,35 @@ int32_t lsm6dsv16x_fifo_timestamp_batch_set(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fifo_timestamp_batch_get(stmdev_ctx_t *ctx, - lsm6dsv16x_fifo_timestamp_batch_t *val) +int32_t lsm6dsv_fifo_timestamp_batch_get(stmdev_ctx_t *ctx, + lsm6dsv_fifo_timestamp_batch_t *val) { - lsm6dsv16x_fifo_ctrl4_t fifo_ctrl4; + lsm6dsv_fifo_ctrl4_t fifo_ctrl4; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FIFO_CTRL4, (uint8_t *)&fifo_ctrl4, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_FIFO_CTRL4, (uint8_t *)&fifo_ctrl4, 1); if (ret != 0) { return ret; } switch (fifo_ctrl4.dec_ts_batch) { - case LSM6DSV16X_TMSTMP_NOT_BATCHED: - *val = LSM6DSV16X_TMSTMP_NOT_BATCHED; + case LSM6DSV_TMSTMP_NOT_BATCHED: + *val = LSM6DSV_TMSTMP_NOT_BATCHED; break; - case LSM6DSV16X_TMSTMP_DEC_1: - *val = LSM6DSV16X_TMSTMP_DEC_1; + case LSM6DSV_TMSTMP_DEC_1: + *val = LSM6DSV_TMSTMP_DEC_1; break; - case LSM6DSV16X_TMSTMP_DEC_8: - *val = LSM6DSV16X_TMSTMP_DEC_8; + case LSM6DSV_TMSTMP_DEC_8: + *val = LSM6DSV_TMSTMP_DEC_8; break; - case LSM6DSV16X_TMSTMP_DEC_32: - *val = LSM6DSV16X_TMSTMP_DEC_32; + case LSM6DSV_TMSTMP_DEC_32: + *val = LSM6DSV_TMSTMP_DEC_32; break; default: - *val = LSM6DSV16X_TMSTMP_NOT_BATCHED; + *val = LSM6DSV_TMSTMP_NOT_BATCHED; break; } @@ -3720,15 +3677,15 @@ int32_t lsm6dsv16x_fifo_timestamp_batch_get(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fifo_batch_counter_threshold_set(stmdev_ctx_t *ctx, - uint16_t val) +int32_t lsm6dsv_fifo_batch_counter_threshold_set(stmdev_ctx_t *ctx, + uint16_t val) { uint8_t buff[2]; int32_t ret; buff[1] = (uint8_t)(val / 256U); buff[0] = (uint8_t)(val - (buff[1] * 256U)); - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_COUNTER_BDR_REG1, (uint8_t *)&buff[0], 2); + ret = lsm6dsv_write_reg(ctx, LSM6DSV_COUNTER_BDR_REG1, (uint8_t *)&buff[0], 2); return ret; } @@ -3741,13 +3698,13 @@ int32_t lsm6dsv16x_fifo_batch_counter_threshold_set(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fifo_batch_counter_threshold_get(stmdev_ctx_t *ctx, - uint16_t *val) +int32_t lsm6dsv_fifo_batch_counter_threshold_get(stmdev_ctx_t *ctx, + uint16_t *val) { uint8_t buff[2]; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_COUNTER_BDR_REG1, &buff[0], 2); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_COUNTER_BDR_REG1, &buff[0], 2); if (ret != 0) { return ret; } *val = buff[1]; @@ -3764,17 +3721,17 @@ int32_t lsm6dsv16x_fifo_batch_counter_threshold_get(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fifo_batch_cnt_event_set(stmdev_ctx_t *ctx, - lsm6dsv16x_fifo_batch_cnt_event_t val) +int32_t lsm6dsv_fifo_batch_cnt_event_set(stmdev_ctx_t *ctx, + lsm6dsv_fifo_batch_cnt_event_t val) { - lsm6dsv16x_counter_bdr_reg1_t counter_bdr_reg1; + lsm6dsv_counter_bdr_reg1_t counter_bdr_reg1; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_COUNTER_BDR_REG1, (uint8_t *)&counter_bdr_reg1, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_COUNTER_BDR_REG1, (uint8_t *)&counter_bdr_reg1, 1); if (ret == 0) { counter_bdr_reg1.trig_counter_bdr = (uint8_t)val & 0x03U; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_COUNTER_BDR_REG1, (uint8_t *)&counter_bdr_reg1, 1); + ret = lsm6dsv_write_reg(ctx, LSM6DSV_COUNTER_BDR_REG1, (uint8_t *)&counter_bdr_reg1, 1); } return ret; @@ -3788,45 +3745,45 @@ int32_t lsm6dsv16x_fifo_batch_cnt_event_set(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fifo_batch_cnt_event_get(stmdev_ctx_t *ctx, - lsm6dsv16x_fifo_batch_cnt_event_t *val) +int32_t lsm6dsv_fifo_batch_cnt_event_get(stmdev_ctx_t *ctx, + lsm6dsv_fifo_batch_cnt_event_t *val) { - lsm6dsv16x_counter_bdr_reg1_t counter_bdr_reg1; + lsm6dsv_counter_bdr_reg1_t counter_bdr_reg1; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_COUNTER_BDR_REG1, (uint8_t *)&counter_bdr_reg1, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_COUNTER_BDR_REG1, (uint8_t *)&counter_bdr_reg1, 1); if (ret != 0) { return ret; } switch (counter_bdr_reg1.trig_counter_bdr) { - case LSM6DSV16X_XL_BATCH_EVENT: - *val = LSM6DSV16X_XL_BATCH_EVENT; + case LSM6DSV_XL_BATCH_EVENT: + *val = LSM6DSV_XL_BATCH_EVENT; break; - case LSM6DSV16X_GY_BATCH_EVENT: - *val = LSM6DSV16X_GY_BATCH_EVENT; + case LSM6DSV_GY_BATCH_EVENT: + *val = LSM6DSV_GY_BATCH_EVENT; break; - case LSM6DSV16X_GY_EIS_BATCH_EVENT: - *val = LSM6DSV16X_GY_EIS_BATCH_EVENT; + case LSM6DSV_GY_EIS_BATCH_EVENT: + *val = LSM6DSV_GY_EIS_BATCH_EVENT; break; default: - *val = LSM6DSV16X_XL_BATCH_EVENT; + *val = LSM6DSV_XL_BATCH_EVENT; break; } return ret; } -int32_t lsm6dsv16x_fifo_status_get(stmdev_ctx_t *ctx, - lsm6dsv16x_fifo_status_t *val) +int32_t lsm6dsv_fifo_status_get(stmdev_ctx_t *ctx, + lsm6dsv_fifo_status_t *val) { uint8_t buff[2]; - lsm6dsv16x_fifo_status2_t status; + lsm6dsv_fifo_status2_t status; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FIFO_STATUS1, (uint8_t *)&buff[0], 2); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_FIFO_STATUS1, (uint8_t *)&buff[0], 2); if (ret != 0) { return ret; } bytecpy((uint8_t *)&status, &buff[1]); @@ -3853,139 +3810,127 @@ int32_t lsm6dsv16x_fifo_status_get(stmdev_ctx_t *ctx, GY_NC_T_1_TAG, GY_2XC_TAG, GY_3XC_TAG, SENSORHUB_SLAVE0_TAG, SENSORHUB_SLAVE1_TAG, SENSORHUB_SLAVE2_TAG, SENSORHUB_SLAVE3_TAG, STEP_COUNTER_TAG, SFLP_GAME_ROTATION_VECTOR_TAG, SFLP_GYROSCOPE_BIAS_TAG, - SFLP_GRAVITY_VECTOR_TAG, SENSORHUB_NACK_TAG, MLC_RESULT_TAG, - MLC_FILTER, MLC_FEATURE, XL_DUAL_CORE, GY_ENHANCED_EIS, + SFLP_GRAVITY_VECTOR_TAG, SENSORHUB_NACK_TAG, XL_DUAL_CORE, + GY_ENHANCED_EIS, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fifo_out_raw_get(stmdev_ctx_t *ctx, - lsm6dsv16x_fifo_out_raw_t *val) +int32_t lsm6dsv_fifo_out_raw_get(stmdev_ctx_t *ctx, + lsm6dsv_fifo_out_raw_t *val) { - lsm6dsv16x_fifo_data_out_tag_t fifo_data_out_tag; + lsm6dsv_fifo_data_out_tag_t fifo_data_out_tag; uint8_t buff[7]; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FIFO_DATA_OUT_TAG, buff, 7); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_FIFO_DATA_OUT_TAG, buff, 7); if (ret != 0) { return ret; } bytecpy((uint8_t *)&fifo_data_out_tag, &buff[0]); switch (fifo_data_out_tag.tag_sensor) { - case LSM6DSV16X_FIFO_EMPTY: - val->tag = LSM6DSV16X_FIFO_EMPTY; - break; - - case LSM6DSV16X_GY_NC_TAG: - val->tag = LSM6DSV16X_GY_NC_TAG; + case LSM6DSV_FIFO_EMPTY: + val->tag = LSM6DSV_FIFO_EMPTY; break; - case LSM6DSV16X_XL_NC_TAG: - val->tag = LSM6DSV16X_XL_NC_TAG; + case LSM6DSV_GY_NC_TAG: + val->tag = LSM6DSV_GY_NC_TAG; break; - case LSM6DSV16X_TIMESTAMP_TAG: - val->tag = LSM6DSV16X_TIMESTAMP_TAG; + case LSM6DSV_XL_NC_TAG: + val->tag = LSM6DSV_XL_NC_TAG; break; - case LSM6DSV16X_TEMPERATURE_TAG: - val->tag = LSM6DSV16X_TEMPERATURE_TAG; + case LSM6DSV_TIMESTAMP_TAG: + val->tag = LSM6DSV_TIMESTAMP_TAG; break; - case LSM6DSV16X_CFG_CHANGE_TAG: - val->tag = LSM6DSV16X_CFG_CHANGE_TAG; + case LSM6DSV_TEMPERATURE_TAG: + val->tag = LSM6DSV_TEMPERATURE_TAG; break; - case LSM6DSV16X_XL_NC_T_2_TAG: - val->tag = LSM6DSV16X_XL_NC_T_2_TAG; + case LSM6DSV_CFG_CHANGE_TAG: + val->tag = LSM6DSV_CFG_CHANGE_TAG; break; - case LSM6DSV16X_XL_NC_T_1_TAG: - val->tag = LSM6DSV16X_XL_NC_T_1_TAG; + case LSM6DSV_XL_NC_T_2_TAG: + val->tag = LSM6DSV_XL_NC_T_2_TAG; break; - case LSM6DSV16X_XL_2XC_TAG: - val->tag = LSM6DSV16X_XL_2XC_TAG; + case LSM6DSV_XL_NC_T_1_TAG: + val->tag = LSM6DSV_XL_NC_T_1_TAG; break; - case LSM6DSV16X_XL_3XC_TAG: - val->tag = LSM6DSV16X_XL_3XC_TAG; + case LSM6DSV_XL_2XC_TAG: + val->tag = LSM6DSV_XL_2XC_TAG; break; - case LSM6DSV16X_GY_NC_T_2_TAG: - val->tag = LSM6DSV16X_GY_NC_T_2_TAG; + case LSM6DSV_XL_3XC_TAG: + val->tag = LSM6DSV_XL_3XC_TAG; break; - case LSM6DSV16X_GY_NC_T_1_TAG: - val->tag = LSM6DSV16X_GY_NC_T_1_TAG; + case LSM6DSV_GY_NC_T_2_TAG: + val->tag = LSM6DSV_GY_NC_T_2_TAG; break; - case LSM6DSV16X_GY_2XC_TAG: - val->tag = LSM6DSV16X_GY_2XC_TAG; + case LSM6DSV_GY_NC_T_1_TAG: + val->tag = LSM6DSV_GY_NC_T_1_TAG; break; - case LSM6DSV16X_GY_3XC_TAG: - val->tag = LSM6DSV16X_GY_3XC_TAG; + case LSM6DSV_GY_2XC_TAG: + val->tag = LSM6DSV_GY_2XC_TAG; break; - case LSM6DSV16X_SENSORHUB_SLAVE0_TAG: - val->tag = LSM6DSV16X_SENSORHUB_SLAVE0_TAG; + case LSM6DSV_GY_3XC_TAG: + val->tag = LSM6DSV_GY_3XC_TAG; break; - case LSM6DSV16X_SENSORHUB_SLAVE1_TAG: - val->tag = LSM6DSV16X_SENSORHUB_SLAVE1_TAG; + case LSM6DSV_SENSORHUB_SLAVE0_TAG: + val->tag = LSM6DSV_SENSORHUB_SLAVE0_TAG; break; - case LSM6DSV16X_SENSORHUB_SLAVE2_TAG: - val->tag = LSM6DSV16X_SENSORHUB_SLAVE2_TAG; + case LSM6DSV_SENSORHUB_SLAVE1_TAG: + val->tag = LSM6DSV_SENSORHUB_SLAVE1_TAG; break; - case LSM6DSV16X_SENSORHUB_SLAVE3_TAG: - val->tag = LSM6DSV16X_SENSORHUB_SLAVE3_TAG; + case LSM6DSV_SENSORHUB_SLAVE2_TAG: + val->tag = LSM6DSV_SENSORHUB_SLAVE2_TAG; break; - case LSM6DSV16X_STEP_COUNTER_TAG: - val->tag = LSM6DSV16X_STEP_COUNTER_TAG; + case LSM6DSV_SENSORHUB_SLAVE3_TAG: + val->tag = LSM6DSV_SENSORHUB_SLAVE3_TAG; break; - case LSM6DSV16X_SFLP_GAME_ROTATION_VECTOR_TAG: - val->tag = LSM6DSV16X_SFLP_GAME_ROTATION_VECTOR_TAG; + case LSM6DSV_STEP_COUNTER_TAG: + val->tag = LSM6DSV_STEP_COUNTER_TAG; break; - case LSM6DSV16X_SFLP_GYROSCOPE_BIAS_TAG: - val->tag = LSM6DSV16X_SFLP_GYROSCOPE_BIAS_TAG; + case LSM6DSV_SFLP_GAME_ROTATION_VECTOR_TAG: + val->tag = LSM6DSV_SFLP_GAME_ROTATION_VECTOR_TAG; break; - case LSM6DSV16X_SFLP_GRAVITY_VECTOR_TAG: - val->tag = LSM6DSV16X_SFLP_GRAVITY_VECTOR_TAG; + case LSM6DSV_SFLP_GYROSCOPE_BIAS_TAG: + val->tag = LSM6DSV_SFLP_GYROSCOPE_BIAS_TAG; break; - case LSM6DSV16X_SENSORHUB_NACK_TAG: - val->tag = LSM6DSV16X_SENSORHUB_NACK_TAG; + case LSM6DSV_SFLP_GRAVITY_VECTOR_TAG: + val->tag = LSM6DSV_SFLP_GRAVITY_VECTOR_TAG; break; - case LSM6DSV16X_MLC_RESULT_TAG: - val->tag = LSM6DSV16X_MLC_RESULT_TAG; + case LSM6DSV_SENSORHUB_NACK_TAG: + val->tag = LSM6DSV_SENSORHUB_NACK_TAG; break; - case LSM6DSV16X_MLC_FILTER: - val->tag = LSM6DSV16X_MLC_FILTER; + case LSM6DSV_XL_DUAL_CORE: + val->tag = LSM6DSV_XL_DUAL_CORE; break; - case LSM6DSV16X_MLC_FEATURE: - val->tag = LSM6DSV16X_MLC_FEATURE; - break; - - case LSM6DSV16X_XL_DUAL_CORE: - val->tag = LSM6DSV16X_XL_DUAL_CORE; - break; - - case LSM6DSV16X_GY_ENHANCED_EIS: - val->tag = LSM6DSV16X_GY_ENHANCED_EIS; + case LSM6DSV_GY_ENHANCED_EIS: + val->tag = LSM6DSV_GY_ENHANCED_EIS; break; default: - val->tag = LSM6DSV16X_FIFO_EMPTY; + val->tag = LSM6DSV_FIFO_EMPTY; break; } @@ -4009,19 +3954,19 @@ int32_t lsm6dsv16x_fifo_out_raw_get(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fifo_stpcnt_batch_set(stmdev_ctx_t *ctx, uint8_t val) +int32_t lsm6dsv_fifo_stpcnt_batch_set(stmdev_ctx_t *ctx, uint8_t val) { - lsm6dsv16x_emb_func_fifo_en_a_t emb_func_fifo_en_a; + lsm6dsv_emb_func_fifo_en_a_t emb_func_fifo_en_a; int32_t ret; - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK); + ret = lsm6dsv_mem_bank_set(ctx, LSM6DSV_EMBED_FUNC_MEM_BANK); if (ret != 0) { return ret; } - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_EMB_FUNC_FIFO_EN_A, (uint8_t *)&emb_func_fifo_en_a, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_EMB_FUNC_FIFO_EN_A, (uint8_t *)&emb_func_fifo_en_a, 1); emb_func_fifo_en_a.step_counter_fifo_en = val; - ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_EMB_FUNC_FIFO_EN_A, (uint8_t *)&emb_func_fifo_en_a, 1); + ret += lsm6dsv_write_reg(ctx, LSM6DSV_EMB_FUNC_FIFO_EN_A, (uint8_t *)&emb_func_fifo_en_a, 1); - ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + ret += lsm6dsv_mem_bank_set(ctx, LSM6DSV_MAIN_MEM_BANK); return ret; } @@ -4034,116 +3979,18 @@ int32_t lsm6dsv16x_fifo_stpcnt_batch_set(stmdev_ctx_t *ctx, uint8_t val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fifo_stpcnt_batch_get(stmdev_ctx_t *ctx, uint8_t *val) +int32_t lsm6dsv_fifo_stpcnt_batch_get(stmdev_ctx_t *ctx, uint8_t *val) { - lsm6dsv16x_emb_func_fifo_en_a_t emb_func_fifo_en_a; + lsm6dsv_emb_func_fifo_en_a_t emb_func_fifo_en_a; int32_t ret; - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK); + ret = lsm6dsv_mem_bank_set(ctx, LSM6DSV_EMBED_FUNC_MEM_BANK); if (ret != 0) { return ret; } - ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_EMB_FUNC_FIFO_EN_A, (uint8_t *)&emb_func_fifo_en_a, 1); + ret += lsm6dsv_read_reg(ctx, LSM6DSV_EMB_FUNC_FIFO_EN_A, (uint8_t *)&emb_func_fifo_en_a, 1); *val = emb_func_fifo_en_a.step_counter_fifo_en; - ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); - - return ret; -} - -/** - * @brief Batching in FIFO buffer of machine learning core results.[set] - * - * @param ctx read / write interface definitions - * @param val Batching in FIFO buffer of machine learning core results. - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6dsv16x_fifo_mlc_batch_set(stmdev_ctx_t *ctx, uint8_t val) -{ - lsm6dsv16x_emb_func_fifo_en_a_t emb_func_fifo_en_a; - int32_t ret; - - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK); - if (ret != 0) { return ret; } - - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_EMB_FUNC_FIFO_EN_A, (uint8_t *)&emb_func_fifo_en_a, 1); - emb_func_fifo_en_a.mlc_fifo_en = val; - ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_EMB_FUNC_FIFO_EN_A, (uint8_t *)&emb_func_fifo_en_a, 1); - - ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); - - return ret; -} - -/** - * @brief Batching in FIFO buffer of machine learning core results.[get] - * - * @param ctx read / write interface definitions - * @param val Batching in FIFO buffer of machine learning core results. - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6dsv16x_fifo_mlc_batch_get(stmdev_ctx_t *ctx, uint8_t *val) -{ - lsm6dsv16x_emb_func_fifo_en_a_t emb_func_fifo_en_a; - int32_t ret; - - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK); - if (ret != 0) { return ret; } - - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_EMB_FUNC_FIFO_EN_A, (uint8_t *)&emb_func_fifo_en_a, 1); - *val = emb_func_fifo_en_a.mlc_fifo_en; - - ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); - - return ret; -} - -/** - * @brief Enables batching in FIFO buffer of machine learning core filters and features.[set] - * - * @param ctx read / write interface definitions - * @param val Enables batching in FIFO buffer of machine learning core filters and features. - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6dsv16x_fifo_mlc_filt_batch_set(stmdev_ctx_t *ctx, uint8_t val) -{ - lsm6dsv16x_emb_func_fifo_en_b_t emb_func_fifo_en_b; - int32_t ret; - - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK); - if (ret != 0) { return ret; } - - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_EMB_FUNC_FIFO_EN_B, (uint8_t *)&emb_func_fifo_en_b, 1); - emb_func_fifo_en_b.mlc_filter_feature_fifo_en = val; - ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_EMB_FUNC_FIFO_EN_B, (uint8_t *)&emb_func_fifo_en_b, 1); - - ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); - - return ret; -} - -/** - * @brief Enables batching in FIFO buffer of machine learning core filters and features.[get] - * - * @param ctx read / write interface definitions - * @param val Enables batching in FIFO buffer of machine learning core filters and features. - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6dsv16x_fifo_mlc_filt_batch_get(stmdev_ctx_t *ctx, uint8_t *val) -{ - lsm6dsv16x_emb_func_fifo_en_b_t emb_func_fifo_en_b; - int32_t ret; - - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK); - if (ret != 0) { return ret; } - - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_EMB_FUNC_FIFO_EN_B, (uint8_t *)&emb_func_fifo_en_b, 1); - *val = emb_func_fifo_en_b.mlc_filter_feature_fifo_en; - - ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + ret += lsm6dsv_mem_bank_set(ctx, LSM6DSV_MAIN_MEM_BANK); return ret; } @@ -4156,19 +4003,19 @@ int32_t lsm6dsv16x_fifo_mlc_filt_batch_get(stmdev_ctx_t *ctx, uint8_t *val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fifo_sh_batch_slave_set(stmdev_ctx_t *ctx, uint8_t idx, uint8_t val) +int32_t lsm6dsv_fifo_sh_batch_slave_set(stmdev_ctx_t *ctx, uint8_t idx, uint8_t val) { - lsm6dsv16x_slv0_config_t slv_config; + lsm6dsv_slv0_config_t slv_config; int32_t ret; - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_SENSOR_HUB_MEM_BANK); + ret = lsm6dsv_mem_bank_set(ctx, LSM6DSV_SENSOR_HUB_MEM_BANK); if (ret != 0) { return ret; } - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_SLV0_CONFIG + idx*3U, (uint8_t *)&slv_config, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_SLV0_CONFIG + idx*3U, (uint8_t *)&slv_config, 1); slv_config.batch_ext_sens_0_en = val; - ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_SLV0_CONFIG + idx*3U, (uint8_t *)&slv_config, 1); + ret += lsm6dsv_write_reg(ctx, LSM6DSV_SLV0_CONFIG + idx*3U, (uint8_t *)&slv_config, 1); - ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + ret += lsm6dsv_mem_bank_set(ctx, LSM6DSV_MAIN_MEM_BANK); return ret; } @@ -4181,18 +4028,18 @@ int32_t lsm6dsv16x_fifo_sh_batch_slave_set(stmdev_ctx_t *ctx, uint8_t idx, uint8 * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fifo_sh_batch_slave_get(stmdev_ctx_t *ctx, uint8_t idx, uint8_t *val) +int32_t lsm6dsv_fifo_sh_batch_slave_get(stmdev_ctx_t *ctx, uint8_t idx, uint8_t *val) { - lsm6dsv16x_slv0_config_t slv_config; + lsm6dsv_slv0_config_t slv_config; int32_t ret; - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_SENSOR_HUB_MEM_BANK); + ret = lsm6dsv_mem_bank_set(ctx, LSM6DSV_SENSOR_HUB_MEM_BANK); if (ret != 0) { return ret; } - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_SLV0_CONFIG + idx*3U, (uint8_t *)&slv_config, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_SLV0_CONFIG + idx*3U, (uint8_t *)&slv_config, 1); *val = slv_config.batch_ext_sens_0_en; - ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + ret += lsm6dsv_mem_bank_set(ctx, LSM6DSV_MAIN_MEM_BANK); return ret; } @@ -4205,24 +4052,24 @@ int32_t lsm6dsv16x_fifo_sh_batch_slave_get(stmdev_ctx_t *ctx, uint8_t idx, uint8 * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fifo_sflp_batch_set(stmdev_ctx_t *ctx, - lsm6dsv16x_fifo_sflp_raw_t val) +int32_t lsm6dsv_fifo_sflp_batch_set(stmdev_ctx_t *ctx, + lsm6dsv_fifo_sflp_raw_t val) { - lsm6dsv16x_emb_func_fifo_en_a_t emb_func_fifo_en_a; + lsm6dsv_emb_func_fifo_en_a_t emb_func_fifo_en_a; int32_t ret; - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK); + ret = lsm6dsv_mem_bank_set(ctx, LSM6DSV_EMBED_FUNC_MEM_BANK); if (ret == 0) { - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_EMB_FUNC_FIFO_EN_A, (uint8_t *)&emb_func_fifo_en_a, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_EMB_FUNC_FIFO_EN_A, (uint8_t *)&emb_func_fifo_en_a, 1); emb_func_fifo_en_a.sflp_game_fifo_en = val.game_rotation; emb_func_fifo_en_a.sflp_gravity_fifo_en = val.gravity; emb_func_fifo_en_a.sflp_gbias_fifo_en = val.gbias; - ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_EMB_FUNC_FIFO_EN_A, + ret += lsm6dsv_write_reg(ctx, LSM6DSV_EMB_FUNC_FIFO_EN_A, (uint8_t *)&emb_func_fifo_en_a, 1); } - ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + ret += lsm6dsv_mem_bank_set(ctx, LSM6DSV_MAIN_MEM_BANK); return ret; } @@ -4235,23 +4082,23 @@ int32_t lsm6dsv16x_fifo_sflp_batch_set(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fifo_sflp_batch_get(stmdev_ctx_t *ctx, - lsm6dsv16x_fifo_sflp_raw_t *val) +int32_t lsm6dsv_fifo_sflp_batch_get(stmdev_ctx_t *ctx, + lsm6dsv_fifo_sflp_raw_t *val) { - lsm6dsv16x_emb_func_fifo_en_a_t emb_func_fifo_en_a; + lsm6dsv_emb_func_fifo_en_a_t emb_func_fifo_en_a; int32_t ret; - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK); + ret = lsm6dsv_mem_bank_set(ctx, LSM6DSV_EMBED_FUNC_MEM_BANK); if (ret == 0) { - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_EMB_FUNC_FIFO_EN_A, (uint8_t *)&emb_func_fifo_en_a, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_EMB_FUNC_FIFO_EN_A, (uint8_t *)&emb_func_fifo_en_a, 1); val->game_rotation = emb_func_fifo_en_a.sflp_game_fifo_en; val->gravity = emb_func_fifo_en_a.sflp_gravity_fifo_en; val->gbias = emb_func_fifo_en_a.sflp_gbias_fifo_en; } - ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + ret += lsm6dsv_mem_bank_set(ctx, LSM6DSV_MAIN_MEM_BANK); return ret; } @@ -4277,18 +4124,18 @@ int32_t lsm6dsv16x_fifo_sflp_batch_get(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_filt_anti_spike_set(stmdev_ctx_t *ctx, - lsm6dsv16x_filt_anti_spike_t val) +int32_t lsm6dsv_filt_anti_spike_set(stmdev_ctx_t *ctx, + lsm6dsv_filt_anti_spike_t val) { - lsm6dsv16x_if_cfg_t if_cfg; + lsm6dsv_if_cfg_t if_cfg; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_IF_CFG, (uint8_t *)&if_cfg, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_IF_CFG, (uint8_t *)&if_cfg, 1); if (ret == 0) { if_cfg.asf_ctrl = (uint8_t)val & 0x01U; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_IF_CFG, (uint8_t *)&if_cfg, 1); + ret = lsm6dsv_write_reg(ctx, LSM6DSV_IF_CFG, (uint8_t *)&if_cfg, 1); } return ret; @@ -4302,27 +4149,27 @@ int32_t lsm6dsv16x_filt_anti_spike_set(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_filt_anti_spike_get(stmdev_ctx_t *ctx, - lsm6dsv16x_filt_anti_spike_t *val) +int32_t lsm6dsv_filt_anti_spike_get(stmdev_ctx_t *ctx, + lsm6dsv_filt_anti_spike_t *val) { - lsm6dsv16x_if_cfg_t if_cfg; + lsm6dsv_if_cfg_t if_cfg; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_IF_CFG, (uint8_t *)&if_cfg, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_IF_CFG, (uint8_t *)&if_cfg, 1); if (ret != 0) { return ret; } switch (if_cfg.asf_ctrl) { - case LSM6DSV16X_AUTO: - *val = LSM6DSV16X_AUTO; + case LSM6DSV_AUTO: + *val = LSM6DSV_AUTO; break; - case LSM6DSV16X_ALWAYS_ACTIVE: - *val = LSM6DSV16X_ALWAYS_ACTIVE; + case LSM6DSV_ALWAYS_ACTIVE: + *val = LSM6DSV_ALWAYS_ACTIVE; break; default: - *val = LSM6DSV16X_AUTO; + *val = LSM6DSV_AUTO; break; } @@ -4337,28 +4184,28 @@ int32_t lsm6dsv16x_filt_anti_spike_get(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_filt_settling_mask_set(stmdev_ctx_t *ctx, - lsm6dsv16x_filt_settling_mask_t val) +int32_t lsm6dsv_filt_settling_mask_set(stmdev_ctx_t *ctx, + lsm6dsv_filt_settling_mask_t val) { - lsm6dsv16x_emb_func_cfg_t emb_func_cfg; - lsm6dsv16x_ui_int_ois_t ui_int_ois; - lsm6dsv16x_ctrl4_t ctrl4; + lsm6dsv_emb_func_cfg_t emb_func_cfg; + lsm6dsv_ui_int_ois_t ui_int_ois; + lsm6dsv_ctrl4_t ctrl4; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL4, (uint8_t *)&ctrl4, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_CTRL4, (uint8_t *)&ctrl4, 1); ctrl4.drdy_mask = val.drdy; - ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_CTRL4, (uint8_t *)&ctrl4, 1); + ret += lsm6dsv_write_reg(ctx, LSM6DSV_CTRL4, (uint8_t *)&ctrl4, 1); if (ret != 0) { return ret; } - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_EMB_FUNC_CFG, (uint8_t *)&emb_func_cfg, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_EMB_FUNC_CFG, (uint8_t *)&emb_func_cfg, 1); emb_func_cfg.emb_func_irq_mask_xl_settl = val.irq_xl; emb_func_cfg.emb_func_irq_mask_g_settl = val.irq_g; - ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_EMB_FUNC_CFG, (uint8_t *)&emb_func_cfg, 1); + ret += lsm6dsv_write_reg(ctx, LSM6DSV_EMB_FUNC_CFG, (uint8_t *)&emb_func_cfg, 1); if (ret != 0) { return ret; } - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_UI_INT_OIS, (uint8_t *)&ui_int_ois, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_UI_INT_OIS, (uint8_t *)&ui_int_ois, 1); ui_int_ois.drdy_mask_ois = val.ois_drdy; - ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_UI_INT_OIS, (uint8_t *)&ui_int_ois, 1); + ret += lsm6dsv_write_reg(ctx, LSM6DSV_UI_INT_OIS, (uint8_t *)&ui_int_ois, 1); return ret; } @@ -4371,17 +4218,17 @@ int32_t lsm6dsv16x_filt_settling_mask_set(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_filt_settling_mask_get(stmdev_ctx_t *ctx, - lsm6dsv16x_filt_settling_mask_t *val) +int32_t lsm6dsv_filt_settling_mask_get(stmdev_ctx_t *ctx, + lsm6dsv_filt_settling_mask_t *val) { - lsm6dsv16x_emb_func_cfg_t emb_func_cfg; - lsm6dsv16x_ui_int_ois_t ui_int_ois; - lsm6dsv16x_ctrl4_t ctrl4; + lsm6dsv_emb_func_cfg_t emb_func_cfg; + lsm6dsv_ui_int_ois_t ui_int_ois; + lsm6dsv_ctrl4_t ctrl4; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL4, (uint8_t *)&ctrl4, 1); - ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_EMB_FUNC_CFG, (uint8_t *)&emb_func_cfg, 1); - ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_UI_INT_OIS, (uint8_t *)&ui_int_ois, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_CTRL4, (uint8_t *)&ctrl4, 1); + ret += lsm6dsv_read_reg(ctx, LSM6DSV_EMB_FUNC_CFG, (uint8_t *)&emb_func_cfg, 1); + ret += lsm6dsv_read_reg(ctx, LSM6DSV_UI_INT_OIS, (uint8_t *)&ui_int_ois, 1); val->irq_xl = emb_func_cfg.emb_func_irq_mask_xl_settl; val->irq_g = emb_func_cfg.emb_func_irq_mask_g_settl; @@ -4398,18 +4245,18 @@ int32_t lsm6dsv16x_filt_settling_mask_get(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_filt_ois_settling_mask_set(stmdev_ctx_t *ctx, - lsm6dsv16x_filt_ois_settling_mask_t val) +int32_t lsm6dsv_filt_ois_settling_mask_set(stmdev_ctx_t *ctx, + lsm6dsv_filt_ois_settling_mask_t val) { - lsm6dsv16x_spi2_int_ois_t spi2_int_ois; + lsm6dsv_spi2_int_ois_t spi2_int_ois; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_SPI2_INT_OIS, (uint8_t *)&spi2_int_ois, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_SPI2_INT_OIS, (uint8_t *)&spi2_int_ois, 1); if (ret == 0) { spi2_int_ois.drdy_mask_ois = val.ois_drdy; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_SPI2_INT_OIS, (uint8_t *)&spi2_int_ois, 1); + ret = lsm6dsv_write_reg(ctx, LSM6DSV_SPI2_INT_OIS, (uint8_t *)&spi2_int_ois, 1); } return ret; @@ -4423,14 +4270,14 @@ int32_t lsm6dsv16x_filt_ois_settling_mask_set(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_filt_ois_settling_mask_get(stmdev_ctx_t *ctx, - lsm6dsv16x_filt_ois_settling_mask_t *val) +int32_t lsm6dsv_filt_ois_settling_mask_get(stmdev_ctx_t *ctx, + lsm6dsv_filt_ois_settling_mask_t *val) { - lsm6dsv16x_spi2_int_ois_t spi2_int_ois; + lsm6dsv_spi2_int_ois_t spi2_int_ois; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_SPI2_INT_OIS, (uint8_t *)&spi2_int_ois, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_SPI2_INT_OIS, (uint8_t *)&spi2_int_ois, 1); val->ois_drdy = spi2_int_ois.drdy_mask_ois; return ret; @@ -4444,17 +4291,17 @@ int32_t lsm6dsv16x_filt_ois_settling_mask_get(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_filt_gy_lp1_bandwidth_set(stmdev_ctx_t *ctx, - lsm6dsv16x_filt_gy_lp1_bandwidth_t val) +int32_t lsm6dsv_filt_gy_lp1_bandwidth_set(stmdev_ctx_t *ctx, + lsm6dsv_filt_gy_lp1_bandwidth_t val) { - lsm6dsv16x_ctrl6_t ctrl6; + lsm6dsv_ctrl6_t ctrl6; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL6, (uint8_t *)&ctrl6, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_CTRL6, (uint8_t *)&ctrl6, 1); if (ret == 0) { ctrl6.lpf1_g_bw = (uint8_t)val & 0x0Fu; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_CTRL6, (uint8_t *)&ctrl6, 1); + ret = lsm6dsv_write_reg(ctx, LSM6DSV_CTRL6, (uint8_t *)&ctrl6, 1); } return ret; @@ -4468,51 +4315,51 @@ int32_t lsm6dsv16x_filt_gy_lp1_bandwidth_set(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_filt_gy_lp1_bandwidth_get(stmdev_ctx_t *ctx, - lsm6dsv16x_filt_gy_lp1_bandwidth_t *val) +int32_t lsm6dsv_filt_gy_lp1_bandwidth_get(stmdev_ctx_t *ctx, + lsm6dsv_filt_gy_lp1_bandwidth_t *val) { - lsm6dsv16x_ctrl6_t ctrl6; + lsm6dsv_ctrl6_t ctrl6; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL6, (uint8_t *)&ctrl6, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_CTRL6, (uint8_t *)&ctrl6, 1); if (ret != 0) { return ret; } switch (ctrl6.lpf1_g_bw) { - case LSM6DSV16X_GY_ULTRA_LIGHT: - *val = LSM6DSV16X_GY_ULTRA_LIGHT; + case LSM6DSV_GY_ULTRA_LIGHT: + *val = LSM6DSV_GY_ULTRA_LIGHT; break; - case LSM6DSV16X_GY_VERY_LIGHT: - *val = LSM6DSV16X_GY_VERY_LIGHT; + case LSM6DSV_GY_VERY_LIGHT: + *val = LSM6DSV_GY_VERY_LIGHT; break; - case LSM6DSV16X_GY_LIGHT: - *val = LSM6DSV16X_GY_LIGHT; + case LSM6DSV_GY_LIGHT: + *val = LSM6DSV_GY_LIGHT; break; - case LSM6DSV16X_GY_MEDIUM: - *val = LSM6DSV16X_GY_MEDIUM; + case LSM6DSV_GY_MEDIUM: + *val = LSM6DSV_GY_MEDIUM; break; - case LSM6DSV16X_GY_STRONG: - *val = LSM6DSV16X_GY_STRONG; + case LSM6DSV_GY_STRONG: + *val = LSM6DSV_GY_STRONG; break; - case LSM6DSV16X_GY_VERY_STRONG: - *val = LSM6DSV16X_GY_VERY_STRONG; + case LSM6DSV_GY_VERY_STRONG: + *val = LSM6DSV_GY_VERY_STRONG; break; - case LSM6DSV16X_GY_AGGRESSIVE: - *val = LSM6DSV16X_GY_AGGRESSIVE; + case LSM6DSV_GY_AGGRESSIVE: + *val = LSM6DSV_GY_AGGRESSIVE; break; - case LSM6DSV16X_GY_XTREME: - *val = LSM6DSV16X_GY_XTREME; + case LSM6DSV_GY_XTREME: + *val = LSM6DSV_GY_XTREME; break; default: - *val = LSM6DSV16X_GY_ULTRA_LIGHT; + *val = LSM6DSV_GY_ULTRA_LIGHT; break; } @@ -4527,16 +4374,16 @@ int32_t lsm6dsv16x_filt_gy_lp1_bandwidth_get(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_filt_gy_lp1_set(stmdev_ctx_t *ctx, uint8_t val) +int32_t lsm6dsv_filt_gy_lp1_set(stmdev_ctx_t *ctx, uint8_t val) { - lsm6dsv16x_ctrl7_t ctrl7; + lsm6dsv_ctrl7_t ctrl7; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL7, (uint8_t *)&ctrl7, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_CTRL7, (uint8_t *)&ctrl7, 1); if (ret == 0) { ctrl7.lpf1_g_en = val; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_CTRL7, (uint8_t *)&ctrl7, 1); + ret = lsm6dsv_write_reg(ctx, LSM6DSV_CTRL7, (uint8_t *)&ctrl7, 1); } return ret; @@ -4551,12 +4398,12 @@ int32_t lsm6dsv16x_filt_gy_lp1_set(stmdev_ctx_t *ctx, uint8_t val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_filt_gy_lp1_get(stmdev_ctx_t *ctx, uint8_t *val) +int32_t lsm6dsv_filt_gy_lp1_get(stmdev_ctx_t *ctx, uint8_t *val) { - lsm6dsv16x_ctrl7_t ctrl7; + lsm6dsv_ctrl7_t ctrl7; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL7, (uint8_t *)&ctrl7, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_CTRL7, (uint8_t *)&ctrl7, 1); *val = ctrl7.lpf1_g_en; return ret; @@ -4570,17 +4417,17 @@ int32_t lsm6dsv16x_filt_gy_lp1_get(stmdev_ctx_t *ctx, uint8_t *val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_filt_xl_lp2_bandwidth_set(stmdev_ctx_t *ctx, - lsm6dsv16x_filt_xl_lp2_bandwidth_t val) +int32_t lsm6dsv_filt_xl_lp2_bandwidth_set(stmdev_ctx_t *ctx, + lsm6dsv_filt_xl_lp2_bandwidth_t val) { - lsm6dsv16x_ctrl8_t ctrl8; + lsm6dsv_ctrl8_t ctrl8; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL8, (uint8_t *)&ctrl8, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_CTRL8, (uint8_t *)&ctrl8, 1); if (ret == 0) { ctrl8.hp_lpf2_xl_bw = (uint8_t)val & 0x07U; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_CTRL8, (uint8_t *)&ctrl8, 1); + ret = lsm6dsv_write_reg(ctx, LSM6DSV_CTRL8, (uint8_t *)&ctrl8, 1); } return ret; @@ -4594,51 +4441,51 @@ int32_t lsm6dsv16x_filt_xl_lp2_bandwidth_set(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_filt_xl_lp2_bandwidth_get(stmdev_ctx_t *ctx, - lsm6dsv16x_filt_xl_lp2_bandwidth_t *val) +int32_t lsm6dsv_filt_xl_lp2_bandwidth_get(stmdev_ctx_t *ctx, + lsm6dsv_filt_xl_lp2_bandwidth_t *val) { - lsm6dsv16x_ctrl8_t ctrl8; + lsm6dsv_ctrl8_t ctrl8; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL8, (uint8_t *)&ctrl8, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_CTRL8, (uint8_t *)&ctrl8, 1); if (ret != 0) { return ret; } switch (ctrl8.hp_lpf2_xl_bw) { - case LSM6DSV16X_XL_ULTRA_LIGHT: - *val = LSM6DSV16X_XL_ULTRA_LIGHT; + case LSM6DSV_XL_ULTRA_LIGHT: + *val = LSM6DSV_XL_ULTRA_LIGHT; break; - case LSM6DSV16X_XL_VERY_LIGHT: - *val = LSM6DSV16X_XL_VERY_LIGHT; + case LSM6DSV_XL_VERY_LIGHT: + *val = LSM6DSV_XL_VERY_LIGHT; break; - case LSM6DSV16X_XL_LIGHT: - *val = LSM6DSV16X_XL_LIGHT; + case LSM6DSV_XL_LIGHT: + *val = LSM6DSV_XL_LIGHT; break; - case LSM6DSV16X_XL_MEDIUM: - *val = LSM6DSV16X_XL_MEDIUM; + case LSM6DSV_XL_MEDIUM: + *val = LSM6DSV_XL_MEDIUM; break; - case LSM6DSV16X_XL_STRONG: - *val = LSM6DSV16X_XL_STRONG; + case LSM6DSV_XL_STRONG: + *val = LSM6DSV_XL_STRONG; break; - case LSM6DSV16X_XL_VERY_STRONG: - *val = LSM6DSV16X_XL_VERY_STRONG; + case LSM6DSV_XL_VERY_STRONG: + *val = LSM6DSV_XL_VERY_STRONG; break; - case LSM6DSV16X_XL_AGGRESSIVE: - *val = LSM6DSV16X_XL_AGGRESSIVE; + case LSM6DSV_XL_AGGRESSIVE: + *val = LSM6DSV_XL_AGGRESSIVE; break; - case LSM6DSV16X_XL_XTREME: - *val = LSM6DSV16X_XL_XTREME; + case LSM6DSV_XL_XTREME: + *val = LSM6DSV_XL_XTREME; break; default: - *val = LSM6DSV16X_XL_ULTRA_LIGHT; + *val = LSM6DSV_XL_ULTRA_LIGHT; break; } @@ -4653,16 +4500,16 @@ int32_t lsm6dsv16x_filt_xl_lp2_bandwidth_get(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_filt_xl_lp2_set(stmdev_ctx_t *ctx, uint8_t val) +int32_t lsm6dsv_filt_xl_lp2_set(stmdev_ctx_t *ctx, uint8_t val) { - lsm6dsv16x_ctrl9_t ctrl9; + lsm6dsv_ctrl9_t ctrl9; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL9, (uint8_t *)&ctrl9, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_CTRL9, (uint8_t *)&ctrl9, 1); if (ret == 0) { ctrl9.lpf2_xl_en = val; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_CTRL9, (uint8_t *)&ctrl9, 1); + ret = lsm6dsv_write_reg(ctx, LSM6DSV_CTRL9, (uint8_t *)&ctrl9, 1); } return ret; @@ -4676,12 +4523,12 @@ int32_t lsm6dsv16x_filt_xl_lp2_set(stmdev_ctx_t *ctx, uint8_t val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_filt_xl_lp2_get(stmdev_ctx_t *ctx, uint8_t *val) +int32_t lsm6dsv_filt_xl_lp2_get(stmdev_ctx_t *ctx, uint8_t *val) { - lsm6dsv16x_ctrl9_t ctrl9; + lsm6dsv_ctrl9_t ctrl9; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL9, (uint8_t *)&ctrl9, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_CTRL9, (uint8_t *)&ctrl9, 1); *val = ctrl9.lpf2_xl_en; return ret; @@ -4695,16 +4542,16 @@ int32_t lsm6dsv16x_filt_xl_lp2_get(stmdev_ctx_t *ctx, uint8_t *val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_filt_xl_hp_set(stmdev_ctx_t *ctx, uint8_t val) +int32_t lsm6dsv_filt_xl_hp_set(stmdev_ctx_t *ctx, uint8_t val) { - lsm6dsv16x_ctrl9_t ctrl9; + lsm6dsv_ctrl9_t ctrl9; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL9, (uint8_t *)&ctrl9, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_CTRL9, (uint8_t *)&ctrl9, 1); if (ret == 0) { ctrl9.hp_slope_xl_en = val; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_CTRL9, (uint8_t *)&ctrl9, 1); + ret = lsm6dsv_write_reg(ctx, LSM6DSV_CTRL9, (uint8_t *)&ctrl9, 1); } return ret; @@ -4718,12 +4565,12 @@ int32_t lsm6dsv16x_filt_xl_hp_set(stmdev_ctx_t *ctx, uint8_t val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_filt_xl_hp_get(stmdev_ctx_t *ctx, uint8_t *val) +int32_t lsm6dsv_filt_xl_hp_get(stmdev_ctx_t *ctx, uint8_t *val) { - lsm6dsv16x_ctrl9_t ctrl9; + lsm6dsv_ctrl9_t ctrl9; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL9, (uint8_t *)&ctrl9, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_CTRL9, (uint8_t *)&ctrl9, 1); *val = ctrl9.hp_slope_xl_en; return ret; @@ -4737,16 +4584,16 @@ int32_t lsm6dsv16x_filt_xl_hp_get(stmdev_ctx_t *ctx, uint8_t *val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_filt_xl_fast_settling_set(stmdev_ctx_t *ctx, uint8_t val) +int32_t lsm6dsv_filt_xl_fast_settling_set(stmdev_ctx_t *ctx, uint8_t val) { - lsm6dsv16x_ctrl9_t ctrl9; + lsm6dsv_ctrl9_t ctrl9; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL9, (uint8_t *)&ctrl9, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_CTRL9, (uint8_t *)&ctrl9, 1); if (ret == 0) { ctrl9.xl_fastsettl_mode = val; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_CTRL9, (uint8_t *)&ctrl9, 1); + ret = lsm6dsv_write_reg(ctx, LSM6DSV_CTRL9, (uint8_t *)&ctrl9, 1); } return ret; @@ -4760,12 +4607,12 @@ int32_t lsm6dsv16x_filt_xl_fast_settling_set(stmdev_ctx_t *ctx, uint8_t val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_filt_xl_fast_settling_get(stmdev_ctx_t *ctx, uint8_t *val) +int32_t lsm6dsv_filt_xl_fast_settling_get(stmdev_ctx_t *ctx, uint8_t *val) { - lsm6dsv16x_ctrl9_t ctrl9; + lsm6dsv_ctrl9_t ctrl9; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL9, (uint8_t *)&ctrl9, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_CTRL9, (uint8_t *)&ctrl9, 1); *val = ctrl9.xl_fastsettl_mode; return ret; @@ -4779,17 +4626,17 @@ int32_t lsm6dsv16x_filt_xl_fast_settling_get(stmdev_ctx_t *ctx, uint8_t *val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_filt_xl_hp_mode_set(stmdev_ctx_t *ctx, - lsm6dsv16x_filt_xl_hp_mode_t val) +int32_t lsm6dsv_filt_xl_hp_mode_set(stmdev_ctx_t *ctx, + lsm6dsv_filt_xl_hp_mode_t val) { - lsm6dsv16x_ctrl9_t ctrl9; + lsm6dsv_ctrl9_t ctrl9; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL9, (uint8_t *)&ctrl9, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_CTRL9, (uint8_t *)&ctrl9, 1); if (ret == 0) { ctrl9.hp_ref_mode_xl = (uint8_t)val & 0x01U; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_CTRL9, (uint8_t *)&ctrl9, 1); + ret = lsm6dsv_write_reg(ctx, LSM6DSV_CTRL9, (uint8_t *)&ctrl9, 1); } return ret; @@ -4803,27 +4650,27 @@ int32_t lsm6dsv16x_filt_xl_hp_mode_set(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_filt_xl_hp_mode_get(stmdev_ctx_t *ctx, - lsm6dsv16x_filt_xl_hp_mode_t *val) +int32_t lsm6dsv_filt_xl_hp_mode_get(stmdev_ctx_t *ctx, + lsm6dsv_filt_xl_hp_mode_t *val) { - lsm6dsv16x_ctrl9_t ctrl9; + lsm6dsv_ctrl9_t ctrl9; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL9, (uint8_t *)&ctrl9, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_CTRL9, (uint8_t *)&ctrl9, 1); if (ret != 0) { return ret; } switch (ctrl9.hp_ref_mode_xl) { - case LSM6DSV16X_HP_MD_NORMAL: - *val = LSM6DSV16X_HP_MD_NORMAL; + case LSM6DSV_HP_MD_NORMAL: + *val = LSM6DSV_HP_MD_NORMAL; break; - case LSM6DSV16X_HP_MD_REFERENCE: - *val = LSM6DSV16X_HP_MD_REFERENCE; + case LSM6DSV_HP_MD_REFERENCE: + *val = LSM6DSV_HP_MD_REFERENCE; break; default: - *val = LSM6DSV16X_HP_MD_NORMAL; + *val = LSM6DSV_HP_MD_NORMAL; break; } @@ -4838,23 +4685,23 @@ int32_t lsm6dsv16x_filt_xl_hp_mode_get(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_filt_wkup_act_feed_set(stmdev_ctx_t *ctx, - lsm6dsv16x_filt_wkup_act_feed_t val) +int32_t lsm6dsv_filt_wkup_act_feed_set(stmdev_ctx_t *ctx, + lsm6dsv_filt_wkup_act_feed_t val) { - lsm6dsv16x_wake_up_ths_t wake_up_ths; - lsm6dsv16x_tap_cfg0_t tap_cfg0; + lsm6dsv_wake_up_ths_t wake_up_ths; + lsm6dsv_tap_cfg0_t tap_cfg0; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_WAKE_UP_THS, (uint8_t *)&wake_up_ths, 1); - ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_TAP_CFG0, (uint8_t *)&tap_cfg0, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_WAKE_UP_THS, (uint8_t *)&wake_up_ths, 1); + ret += lsm6dsv_read_reg(ctx, LSM6DSV_TAP_CFG0, (uint8_t *)&tap_cfg0, 1); if (ret != 0) { return ret; } tap_cfg0.slope_fds = (uint8_t)val & 0x01U; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_TAP_CFG0, (uint8_t *)&tap_cfg0, 1); + ret = lsm6dsv_write_reg(ctx, LSM6DSV_TAP_CFG0, (uint8_t *)&tap_cfg0, 1); if (ret != 0) { return ret; } wake_up_ths.usr_off_on_wu = ((uint8_t)val & 0x02U) >> 1; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_WAKE_UP_THS, (uint8_t *)&wake_up_ths, 1); + ret = lsm6dsv_write_reg(ctx, LSM6DSV_WAKE_UP_THS, (uint8_t *)&wake_up_ths, 1); return ret; } @@ -4867,33 +4714,33 @@ int32_t lsm6dsv16x_filt_wkup_act_feed_set(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_filt_wkup_act_feed_get(stmdev_ctx_t *ctx, - lsm6dsv16x_filt_wkup_act_feed_t *val) +int32_t lsm6dsv_filt_wkup_act_feed_get(stmdev_ctx_t *ctx, + lsm6dsv_filt_wkup_act_feed_t *val) { - lsm6dsv16x_wake_up_ths_t wake_up_ths; - lsm6dsv16x_tap_cfg0_t tap_cfg0; + lsm6dsv_wake_up_ths_t wake_up_ths; + lsm6dsv_tap_cfg0_t tap_cfg0; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_WAKE_UP_THS, (uint8_t *)&wake_up_ths, 1); - ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_TAP_CFG0, (uint8_t *)&tap_cfg0, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_WAKE_UP_THS, (uint8_t *)&wake_up_ths, 1); + ret += lsm6dsv_read_reg(ctx, LSM6DSV_TAP_CFG0, (uint8_t *)&tap_cfg0, 1); if (ret != 0) { return ret; } switch ((wake_up_ths.usr_off_on_wu << 1) + tap_cfg0.slope_fds) { - case LSM6DSV16X_WK_FEED_SLOPE: - *val = LSM6DSV16X_WK_FEED_SLOPE; + case LSM6DSV_WK_FEED_SLOPE: + *val = LSM6DSV_WK_FEED_SLOPE; break; - case LSM6DSV16X_WK_FEED_HIGH_PASS: - *val = LSM6DSV16X_WK_FEED_HIGH_PASS; + case LSM6DSV_WK_FEED_HIGH_PASS: + *val = LSM6DSV_WK_FEED_HIGH_PASS; break; - case LSM6DSV16X_WK_FEED_LP_WITH_OFFSET: - *val = LSM6DSV16X_WK_FEED_LP_WITH_OFFSET; + case LSM6DSV_WK_FEED_LP_WITH_OFFSET: + *val = LSM6DSV_WK_FEED_LP_WITH_OFFSET; break; default: - *val = LSM6DSV16X_WK_FEED_SLOPE; + *val = LSM6DSV_WK_FEED_SLOPE; break; } @@ -4908,17 +4755,17 @@ int32_t lsm6dsv16x_filt_wkup_act_feed_get(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_mask_trigger_xl_settl_set(stmdev_ctx_t *ctx, uint8_t val) +int32_t lsm6dsv_mask_trigger_xl_settl_set(stmdev_ctx_t *ctx, uint8_t val) { - lsm6dsv16x_tap_cfg0_t tap_cfg0; + lsm6dsv_tap_cfg0_t tap_cfg0; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_TAP_CFG0, (uint8_t *)&tap_cfg0, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_TAP_CFG0, (uint8_t *)&tap_cfg0, 1); if (ret == 0) { tap_cfg0.hw_func_mask_xl_settl = val & 0x01U; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_TAP_CFG0, (uint8_t *)&tap_cfg0, 1); + ret = lsm6dsv_write_reg(ctx, LSM6DSV_TAP_CFG0, (uint8_t *)&tap_cfg0, 1); } return ret; @@ -4932,12 +4779,12 @@ int32_t lsm6dsv16x_mask_trigger_xl_settl_set(stmdev_ctx_t *ctx, uint8_t val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_mask_trigger_xl_settl_get(stmdev_ctx_t *ctx, uint8_t *val) +int32_t lsm6dsv_mask_trigger_xl_settl_get(stmdev_ctx_t *ctx, uint8_t *val) { - lsm6dsv16x_tap_cfg0_t tap_cfg0; + lsm6dsv_tap_cfg0_t tap_cfg0; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_TAP_CFG0, (uint8_t *)&tap_cfg0, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_TAP_CFG0, (uint8_t *)&tap_cfg0, 1); *val = tap_cfg0.hw_func_mask_xl_settl; return ret; @@ -4951,18 +4798,18 @@ int32_t lsm6dsv16x_mask_trigger_xl_settl_get(stmdev_ctx_t *ctx, uint8_t *val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_filt_sixd_feed_set(stmdev_ctx_t *ctx, - lsm6dsv16x_filt_sixd_feed_t val) +int32_t lsm6dsv_filt_sixd_feed_set(stmdev_ctx_t *ctx, + lsm6dsv_filt_sixd_feed_t val) { - lsm6dsv16x_tap_cfg0_t tap_cfg0; + lsm6dsv_tap_cfg0_t tap_cfg0; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_TAP_CFG0, (uint8_t *)&tap_cfg0, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_TAP_CFG0, (uint8_t *)&tap_cfg0, 1); if (ret == 0) { tap_cfg0.low_pass_on_6d = (uint8_t)val & 0x01U; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_TAP_CFG0, (uint8_t *)&tap_cfg0, 1); + ret = lsm6dsv_write_reg(ctx, LSM6DSV_TAP_CFG0, (uint8_t *)&tap_cfg0, 1); } return ret; @@ -4976,27 +4823,27 @@ int32_t lsm6dsv16x_filt_sixd_feed_set(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_filt_sixd_feed_get(stmdev_ctx_t *ctx, - lsm6dsv16x_filt_sixd_feed_t *val) +int32_t lsm6dsv_filt_sixd_feed_get(stmdev_ctx_t *ctx, + lsm6dsv_filt_sixd_feed_t *val) { - lsm6dsv16x_tap_cfg0_t tap_cfg0; + lsm6dsv_tap_cfg0_t tap_cfg0; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_TAP_CFG0, (uint8_t *)&tap_cfg0, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_TAP_CFG0, (uint8_t *)&tap_cfg0, 1); if (ret != 0) { return ret; } switch (tap_cfg0.low_pass_on_6d) { - case LSM6DSV16X_SIXD_FEED_ODR_DIV_2: - *val = LSM6DSV16X_SIXD_FEED_ODR_DIV_2; + case LSM6DSV_SIXD_FEED_ODR_DIV_2: + *val = LSM6DSV_SIXD_FEED_ODR_DIV_2; break; - case LSM6DSV16X_SIXD_FEED_LOW_PASS: - *val = LSM6DSV16X_SIXD_FEED_LOW_PASS; + case LSM6DSV_SIXD_FEED_LOW_PASS: + *val = LSM6DSV_SIXD_FEED_LOW_PASS; break; default: - *val = LSM6DSV16X_SIXD_FEED_ODR_DIV_2; + *val = LSM6DSV_SIXD_FEED_ODR_DIV_2; break; } @@ -5011,18 +4858,18 @@ int32_t lsm6dsv16x_filt_sixd_feed_get(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_filt_gy_eis_lp_bandwidth_set(stmdev_ctx_t *ctx, - lsm6dsv16x_filt_gy_eis_lp_bandwidth_t val) +int32_t lsm6dsv_filt_gy_eis_lp_bandwidth_set(stmdev_ctx_t *ctx, + lsm6dsv_filt_gy_eis_lp_bandwidth_t val) { - lsm6dsv16x_ctrl_eis_t ctrl_eis; + lsm6dsv_ctrl_eis_t ctrl_eis; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL_EIS, (uint8_t *)&ctrl_eis, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_CTRL_EIS, (uint8_t *)&ctrl_eis, 1); if (ret == 0) { ctrl_eis.lpf_g_eis_bw = (uint8_t)val & 0x01U; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_CTRL_EIS, (uint8_t *)&ctrl_eis, 1); + ret = lsm6dsv_write_reg(ctx, LSM6DSV_CTRL_EIS, (uint8_t *)&ctrl_eis, 1); } return ret; @@ -5036,27 +4883,27 @@ int32_t lsm6dsv16x_filt_gy_eis_lp_bandwidth_set(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_filt_gy_eis_lp_bandwidth_get(stmdev_ctx_t *ctx, - lsm6dsv16x_filt_gy_eis_lp_bandwidth_t *val) +int32_t lsm6dsv_filt_gy_eis_lp_bandwidth_get(stmdev_ctx_t *ctx, + lsm6dsv_filt_gy_eis_lp_bandwidth_t *val) { - lsm6dsv16x_ctrl_eis_t ctrl_eis; + lsm6dsv_ctrl_eis_t ctrl_eis; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL_EIS, (uint8_t *)&ctrl_eis, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_CTRL_EIS, (uint8_t *)&ctrl_eis, 1); if (ret != 0) { return ret; } switch (ctrl_eis.lpf_g_eis_bw) { - case LSM6DSV16X_EIS_LP_NORMAL: - *val = LSM6DSV16X_EIS_LP_NORMAL; + case LSM6DSV_EIS_LP_NORMAL: + *val = LSM6DSV_EIS_LP_NORMAL; break; - case LSM6DSV16X_EIS_LP_LIGHT: - *val = LSM6DSV16X_EIS_LP_LIGHT; + case LSM6DSV_EIS_LP_LIGHT: + *val = LSM6DSV_EIS_LP_LIGHT; break; default: - *val = LSM6DSV16X_EIS_LP_NORMAL; + *val = LSM6DSV_EIS_LP_NORMAL; break; } @@ -5071,18 +4918,18 @@ int32_t lsm6dsv16x_filt_gy_eis_lp_bandwidth_get(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_filt_gy_ois_lp_bandwidth_set(stmdev_ctx_t *ctx, - lsm6dsv16x_filt_gy_ois_lp_bandwidth_t val) +int32_t lsm6dsv_filt_gy_ois_lp_bandwidth_set(stmdev_ctx_t *ctx, + lsm6dsv_filt_gy_ois_lp_bandwidth_t val) { - lsm6dsv16x_ui_ctrl2_ois_t ui_ctrl2_ois; + lsm6dsv_ui_ctrl2_ois_t ui_ctrl2_ois; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_UI_CTRL2_OIS, (uint8_t *)&ui_ctrl2_ois, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_UI_CTRL2_OIS, (uint8_t *)&ui_ctrl2_ois, 1); if (ret == 0) { ui_ctrl2_ois.lpf1_g_ois_bw = (uint8_t)val & 0x03U; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_UI_CTRL2_OIS, (uint8_t *)&ui_ctrl2_ois, 1); + ret = lsm6dsv_write_reg(ctx, LSM6DSV_UI_CTRL2_OIS, (uint8_t *)&ui_ctrl2_ois, 1); } return ret; @@ -5096,36 +4943,36 @@ int32_t lsm6dsv16x_filt_gy_ois_lp_bandwidth_set(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_filt_gy_ois_lp_bandwidth_get(stmdev_ctx_t *ctx, - lsm6dsv16x_filt_gy_ois_lp_bandwidth_t *val) +int32_t lsm6dsv_filt_gy_ois_lp_bandwidth_get(stmdev_ctx_t *ctx, + lsm6dsv_filt_gy_ois_lp_bandwidth_t *val) { - lsm6dsv16x_ui_ctrl2_ois_t ui_ctrl2_ois; + lsm6dsv_ui_ctrl2_ois_t ui_ctrl2_ois; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_UI_CTRL2_OIS, (uint8_t *)&ui_ctrl2_ois, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_UI_CTRL2_OIS, (uint8_t *)&ui_ctrl2_ois, 1); if (ret != 0) { return ret; } switch (ui_ctrl2_ois.lpf1_g_ois_bw) { - case LSM6DSV16X_OIS_GY_LP_NORMAL: - *val = LSM6DSV16X_OIS_GY_LP_NORMAL; + case LSM6DSV_OIS_GY_LP_NORMAL: + *val = LSM6DSV_OIS_GY_LP_NORMAL; break; - case LSM6DSV16X_OIS_GY_LP_STRONG: - *val = LSM6DSV16X_OIS_GY_LP_STRONG; + case LSM6DSV_OIS_GY_LP_STRONG: + *val = LSM6DSV_OIS_GY_LP_STRONG; break; - case LSM6DSV16X_OIS_GY_LP_AGGRESSIVE: - *val = LSM6DSV16X_OIS_GY_LP_AGGRESSIVE; + case LSM6DSV_OIS_GY_LP_AGGRESSIVE: + *val = LSM6DSV_OIS_GY_LP_AGGRESSIVE; break; - case LSM6DSV16X_OIS_GY_LP_LIGHT: - *val = LSM6DSV16X_OIS_GY_LP_LIGHT; + case LSM6DSV_OIS_GY_LP_LIGHT: + *val = LSM6DSV_OIS_GY_LP_LIGHT; break; default: - *val = LSM6DSV16X_OIS_GY_LP_NORMAL; + *val = LSM6DSV_OIS_GY_LP_NORMAL; break; } @@ -5140,18 +4987,18 @@ int32_t lsm6dsv16x_filt_gy_ois_lp_bandwidth_get(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_filt_xl_ois_lp_bandwidth_set(stmdev_ctx_t *ctx, - lsm6dsv16x_filt_xl_ois_lp_bandwidth_t val) +int32_t lsm6dsv_filt_xl_ois_lp_bandwidth_set(stmdev_ctx_t *ctx, + lsm6dsv_filt_xl_ois_lp_bandwidth_t val) { - lsm6dsv16x_ui_ctrl3_ois_t ui_ctrl3_ois; + lsm6dsv_ui_ctrl3_ois_t ui_ctrl3_ois; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_UI_CTRL3_OIS, (uint8_t *)&ui_ctrl3_ois, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_UI_CTRL3_OIS, (uint8_t *)&ui_ctrl3_ois, 1); if (ret == 0) { ui_ctrl3_ois.lpf_xl_ois_bw = (uint8_t)val & 0x07U; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_UI_CTRL3_OIS, (uint8_t *)&ui_ctrl3_ois, 1); + ret = lsm6dsv_write_reg(ctx, LSM6DSV_UI_CTRL3_OIS, (uint8_t *)&ui_ctrl3_ois, 1); } return ret; @@ -5165,51 +5012,51 @@ int32_t lsm6dsv16x_filt_xl_ois_lp_bandwidth_set(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_filt_xl_ois_lp_bandwidth_get(stmdev_ctx_t *ctx, - lsm6dsv16x_filt_xl_ois_lp_bandwidth_t *val) +int32_t lsm6dsv_filt_xl_ois_lp_bandwidth_get(stmdev_ctx_t *ctx, + lsm6dsv_filt_xl_ois_lp_bandwidth_t *val) { - lsm6dsv16x_ui_ctrl3_ois_t ui_ctrl3_ois; + lsm6dsv_ui_ctrl3_ois_t ui_ctrl3_ois; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_UI_CTRL3_OIS, (uint8_t *)&ui_ctrl3_ois, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_UI_CTRL3_OIS, (uint8_t *)&ui_ctrl3_ois, 1); if (ret != 0) { return ret; } switch (ui_ctrl3_ois.lpf_xl_ois_bw) { - case LSM6DSV16X_OIS_XL_LP_ULTRA_LIGHT: - *val = LSM6DSV16X_OIS_XL_LP_ULTRA_LIGHT; + case LSM6DSV_OIS_XL_LP_ULTRA_LIGHT: + *val = LSM6DSV_OIS_XL_LP_ULTRA_LIGHT; break; - case LSM6DSV16X_OIS_XL_LP_VERY_LIGHT: - *val = LSM6DSV16X_OIS_XL_LP_VERY_LIGHT; + case LSM6DSV_OIS_XL_LP_VERY_LIGHT: + *val = LSM6DSV_OIS_XL_LP_VERY_LIGHT; break; - case LSM6DSV16X_OIS_XL_LP_LIGHT: - *val = LSM6DSV16X_OIS_XL_LP_LIGHT; + case LSM6DSV_OIS_XL_LP_LIGHT: + *val = LSM6DSV_OIS_XL_LP_LIGHT; break; - case LSM6DSV16X_OIS_XL_LP_NORMAL: - *val = LSM6DSV16X_OIS_XL_LP_NORMAL; + case LSM6DSV_OIS_XL_LP_NORMAL: + *val = LSM6DSV_OIS_XL_LP_NORMAL; break; - case LSM6DSV16X_OIS_XL_LP_STRONG: - *val = LSM6DSV16X_OIS_XL_LP_STRONG; + case LSM6DSV_OIS_XL_LP_STRONG: + *val = LSM6DSV_OIS_XL_LP_STRONG; break; - case LSM6DSV16X_OIS_XL_LP_VERY_STRONG: - *val = LSM6DSV16X_OIS_XL_LP_VERY_STRONG; + case LSM6DSV_OIS_XL_LP_VERY_STRONG: + *val = LSM6DSV_OIS_XL_LP_VERY_STRONG; break; - case LSM6DSV16X_OIS_XL_LP_AGGRESSIVE: - *val = LSM6DSV16X_OIS_XL_LP_AGGRESSIVE; + case LSM6DSV_OIS_XL_LP_AGGRESSIVE: + *val = LSM6DSV_OIS_XL_LP_AGGRESSIVE; break; - case LSM6DSV16X_OIS_XL_LP_XTREME: - *val = LSM6DSV16X_OIS_XL_LP_XTREME; + case LSM6DSV_OIS_XL_LP_XTREME: + *val = LSM6DSV_OIS_XL_LP_XTREME; break; default: - *val = LSM6DSV16X_OIS_XL_LP_ULTRA_LIGHT; + *val = LSM6DSV_OIS_XL_LP_ULTRA_LIGHT; break; } @@ -5237,18 +5084,18 @@ int32_t lsm6dsv16x_filt_xl_ois_lp_bandwidth_get(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fsm_permission_set(stmdev_ctx_t *ctx, - lsm6dsv16x_fsm_permission_t val) +int32_t lsm6dsv_fsm_permission_set(stmdev_ctx_t *ctx, + lsm6dsv_fsm_permission_t val) { - lsm6dsv16x_func_cfg_access_t func_cfg_access; + lsm6dsv_func_cfg_access_t func_cfg_access; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FUNC_CFG_ACCESS, (uint8_t *)&func_cfg_access, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_FUNC_CFG_ACCESS, (uint8_t *)&func_cfg_access, 1); if (ret == 0) { func_cfg_access.fsm_wr_ctrl_en = (uint8_t)val & 0x01U; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_FUNC_CFG_ACCESS, (uint8_t *)&func_cfg_access, 1); + ret = lsm6dsv_write_reg(ctx, LSM6DSV_FUNC_CFG_ACCESS, (uint8_t *)&func_cfg_access, 1); } return ret; @@ -5262,27 +5109,27 @@ int32_t lsm6dsv16x_fsm_permission_set(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fsm_permission_get(stmdev_ctx_t *ctx, - lsm6dsv16x_fsm_permission_t *val) +int32_t lsm6dsv_fsm_permission_get(stmdev_ctx_t *ctx, + lsm6dsv_fsm_permission_t *val) { - lsm6dsv16x_func_cfg_access_t func_cfg_access; + lsm6dsv_func_cfg_access_t func_cfg_access; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FUNC_CFG_ACCESS, (uint8_t *)&func_cfg_access, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_FUNC_CFG_ACCESS, (uint8_t *)&func_cfg_access, 1); if (ret != 0) { return ret; } switch (func_cfg_access.fsm_wr_ctrl_en) { - case LSM6DSV16X_PROTECT_CTRL_REGS: - *val = LSM6DSV16X_PROTECT_CTRL_REGS; + case LSM6DSV_PROTECT_CTRL_REGS: + *val = LSM6DSV_PROTECT_CTRL_REGS; break; - case LSM6DSV16X_WRITE_CTRL_REG: - *val = LSM6DSV16X_WRITE_CTRL_REG; + case LSM6DSV_WRITE_CTRL_REG: + *val = LSM6DSV_WRITE_CTRL_REG; break; default: - *val = LSM6DSV16X_PROTECT_CTRL_REGS; + *val = LSM6DSV_PROTECT_CTRL_REGS; break; } @@ -5297,12 +5144,12 @@ int32_t lsm6dsv16x_fsm_permission_get(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fsm_permission_status(stmdev_ctx_t *ctx, uint8_t *val) +int32_t lsm6dsv_fsm_permission_status(stmdev_ctx_t *ctx, uint8_t *val) { - lsm6dsv16x_ctrl_status_t ctrl_status; + lsm6dsv_ctrl_status_t ctrl_status; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL_STATUS, (uint8_t *)&ctrl_status, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_CTRL_STATUS, (uint8_t *)&ctrl_status, 1); *val = ctrl_status.fsm_wr_ctrl_status; @@ -5317,17 +5164,17 @@ int32_t lsm6dsv16x_fsm_permission_status(stmdev_ctx_t *ctx, uint8_t *val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fsm_mode_set(stmdev_ctx_t *ctx, lsm6dsv16x_fsm_mode_t val) +int32_t lsm6dsv_fsm_mode_set(stmdev_ctx_t *ctx, lsm6dsv_fsm_mode_t val) { - lsm6dsv16x_emb_func_en_b_t emb_func_en_b; - lsm6dsv16x_fsm_enable_t fsm_enable; + lsm6dsv_emb_func_en_b_t emb_func_en_b; + lsm6dsv_fsm_enable_t fsm_enable; int32_t ret; - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK); + ret = lsm6dsv_mem_bank_set(ctx, LSM6DSV_EMBED_FUNC_MEM_BANK); if (ret != 0) { return ret; } - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_EMB_FUNC_EN_B, (uint8_t *)&emb_func_en_b, 1); - ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FSM_ENABLE, (uint8_t *)&fsm_enable, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_EMB_FUNC_EN_B, (uint8_t *)&emb_func_en_b, 1); + ret += lsm6dsv_read_reg(ctx, LSM6DSV_FSM_ENABLE, (uint8_t *)&fsm_enable, 1); if (ret != 0) { goto exit; } if ((val.fsm1_en | val.fsm2_en | val.fsm1_en | val.fsm1_en @@ -5349,11 +5196,11 @@ int32_t lsm6dsv16x_fsm_mode_set(stmdev_ctx_t *ctx, lsm6dsv16x_fsm_mode_t val) fsm_enable.fsm7_en = val.fsm7_en; fsm_enable.fsm8_en = val.fsm8_en; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_FSM_ENABLE, (uint8_t *)&fsm_enable, 1); - ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_EMB_FUNC_EN_B, (uint8_t *)&emb_func_en_b, 1); + ret = lsm6dsv_write_reg(ctx, LSM6DSV_FSM_ENABLE, (uint8_t *)&fsm_enable, 1); + ret += lsm6dsv_write_reg(ctx, LSM6DSV_EMB_FUNC_EN_B, (uint8_t *)&emb_func_en_b, 1); exit: - ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + ret += lsm6dsv_mem_bank_set(ctx, LSM6DSV_MAIN_MEM_BANK); return ret; } @@ -5366,14 +5213,14 @@ int32_t lsm6dsv16x_fsm_mode_set(stmdev_ctx_t *ctx, lsm6dsv16x_fsm_mode_t val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fsm_mode_get(stmdev_ctx_t *ctx, lsm6dsv16x_fsm_mode_t *val) +int32_t lsm6dsv_fsm_mode_get(stmdev_ctx_t *ctx, lsm6dsv_fsm_mode_t *val) { - lsm6dsv16x_fsm_enable_t fsm_enable; + lsm6dsv_fsm_enable_t fsm_enable; int32_t ret; - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK); - ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FSM_ENABLE, (uint8_t *)&fsm_enable, 1); - ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + ret = lsm6dsv_mem_bank_set(ctx, LSM6DSV_EMBED_FUNC_MEM_BANK); + ret += lsm6dsv_read_reg(ctx, LSM6DSV_FSM_ENABLE, (uint8_t *)&fsm_enable, 1); + ret += lsm6dsv_mem_bank_set(ctx, LSM6DSV_MAIN_MEM_BANK); if (ret != 0) { return ret; } val->fsm1_en = fsm_enable.fsm1_en; @@ -5396,7 +5243,7 @@ int32_t lsm6dsv16x_fsm_mode_get(stmdev_ctx_t *ctx, lsm6dsv16x_fsm_mode_t *val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fsm_long_cnt_set(stmdev_ctx_t *ctx, uint16_t val) +int32_t lsm6dsv_fsm_long_cnt_set(stmdev_ctx_t *ctx, uint16_t val) { uint8_t buff[2]; int32_t ret; @@ -5404,9 +5251,9 @@ int32_t lsm6dsv16x_fsm_long_cnt_set(stmdev_ctx_t *ctx, uint16_t val) buff[1] = (uint8_t)(val / 256U); buff[0] = (uint8_t)(val - (buff[1] * 256U)); - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK); - ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_FSM_LONG_COUNTER_L, (uint8_t *)&buff[0], 2); - ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + ret = lsm6dsv_mem_bank_set(ctx, LSM6DSV_EMBED_FUNC_MEM_BANK); + ret += lsm6dsv_write_reg(ctx, LSM6DSV_FSM_LONG_COUNTER_L, (uint8_t *)&buff[0], 2); + ret += lsm6dsv_mem_bank_set(ctx, LSM6DSV_MAIN_MEM_BANK); return ret; } @@ -5419,14 +5266,14 @@ int32_t lsm6dsv16x_fsm_long_cnt_set(stmdev_ctx_t *ctx, uint16_t val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fsm_long_cnt_get(stmdev_ctx_t *ctx, uint16_t *val) +int32_t lsm6dsv_fsm_long_cnt_get(stmdev_ctx_t *ctx, uint16_t *val) { uint8_t buff[2]; int32_t ret; - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK); - ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FSM_LONG_COUNTER_L, &buff[0], 2); - ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + ret = lsm6dsv_mem_bank_set(ctx, LSM6DSV_EMBED_FUNC_MEM_BANK); + ret += lsm6dsv_read_reg(ctx, LSM6DSV_FSM_LONG_COUNTER_L, &buff[0], 2); + ret += lsm6dsv_mem_bank_set(ctx, LSM6DSV_MAIN_MEM_BANK); if (ret != 0) { return ret; } *val = buff[1]; @@ -5443,13 +5290,13 @@ int32_t lsm6dsv16x_fsm_long_cnt_get(stmdev_ctx_t *ctx, uint16_t *val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fsm_out_get(stmdev_ctx_t *ctx, lsm6dsv16x_fsm_out_t *val) +int32_t lsm6dsv_fsm_out_get(stmdev_ctx_t *ctx, lsm6dsv_fsm_out_t *val) { int32_t ret; - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK); - ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FSM_OUTS1, (uint8_t *)val, 8); - ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + ret = lsm6dsv_mem_bank_set(ctx, LSM6DSV_EMBED_FUNC_MEM_BANK); + ret += lsm6dsv_read_reg(ctx, LSM6DSV_FSM_OUTS1, (uint8_t *)val, 8); + ret += lsm6dsv_mem_bank_set(ctx, LSM6DSV_MAIN_MEM_BANK); return ret; } @@ -5462,21 +5309,21 @@ int32_t lsm6dsv16x_fsm_out_get(stmdev_ctx_t *ctx, lsm6dsv16x_fsm_out_t *val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fsm_data_rate_set(stmdev_ctx_t *ctx, - lsm6dsv16x_fsm_data_rate_t val) +int32_t lsm6dsv_fsm_data_rate_set(stmdev_ctx_t *ctx, + lsm6dsv_fsm_data_rate_t val) { - lsm6dsv16x_fsm_odr_t fsm_odr; + lsm6dsv_fsm_odr_t fsm_odr; int32_t ret; - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK); - ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FSM_ODR, (uint8_t *)&fsm_odr, 1); + ret = lsm6dsv_mem_bank_set(ctx, LSM6DSV_EMBED_FUNC_MEM_BANK); + ret += lsm6dsv_read_reg(ctx, LSM6DSV_FSM_ODR, (uint8_t *)&fsm_odr, 1); if (ret != 0) { goto exit; } fsm_odr.fsm_odr = (uint8_t)val & 0x07U; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_FSM_ODR, (uint8_t *)&fsm_odr, 1); + ret = lsm6dsv_write_reg(ctx, LSM6DSV_FSM_ODR, (uint8_t *)&fsm_odr, 1); exit: - ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + ret += lsm6dsv_mem_bank_set(ctx, LSM6DSV_MAIN_MEM_BANK); return ret; } @@ -5489,49 +5336,49 @@ int32_t lsm6dsv16x_fsm_data_rate_set(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fsm_data_rate_get(stmdev_ctx_t *ctx, - lsm6dsv16x_fsm_data_rate_t *val) +int32_t lsm6dsv_fsm_data_rate_get(stmdev_ctx_t *ctx, + lsm6dsv_fsm_data_rate_t *val) { - lsm6dsv16x_fsm_odr_t fsm_odr; + lsm6dsv_fsm_odr_t fsm_odr; int32_t ret; - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK); - ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FSM_ODR, (uint8_t *)&fsm_odr, 1); - ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + ret = lsm6dsv_mem_bank_set(ctx, LSM6DSV_EMBED_FUNC_MEM_BANK); + ret += lsm6dsv_read_reg(ctx, LSM6DSV_FSM_ODR, (uint8_t *)&fsm_odr, 1); + ret += lsm6dsv_mem_bank_set(ctx, LSM6DSV_MAIN_MEM_BANK); if (ret != 0) { return ret; } switch (fsm_odr.fsm_odr) { - case LSM6DSV16X_FSM_15Hz: - *val = LSM6DSV16X_FSM_15Hz; + case LSM6DSV_FSM_15Hz: + *val = LSM6DSV_FSM_15Hz; break; - case LSM6DSV16X_FSM_30Hz: - *val = LSM6DSV16X_FSM_30Hz; + case LSM6DSV_FSM_30Hz: + *val = LSM6DSV_FSM_30Hz; break; - case LSM6DSV16X_FSM_60Hz: - *val = LSM6DSV16X_FSM_60Hz; + case LSM6DSV_FSM_60Hz: + *val = LSM6DSV_FSM_60Hz; break; - case LSM6DSV16X_FSM_120Hz: - *val = LSM6DSV16X_FSM_120Hz; + case LSM6DSV_FSM_120Hz: + *val = LSM6DSV_FSM_120Hz; break; - case LSM6DSV16X_FSM_240Hz: - *val = LSM6DSV16X_FSM_240Hz; + case LSM6DSV_FSM_240Hz: + *val = LSM6DSV_FSM_240Hz; break; - case LSM6DSV16X_FSM_480Hz: - *val = LSM6DSV16X_FSM_480Hz; + case LSM6DSV_FSM_480Hz: + *val = LSM6DSV_FSM_480Hz; break; - case LSM6DSV16X_FSM_960Hz: - *val = LSM6DSV16X_FSM_960Hz; + case LSM6DSV_FSM_960Hz: + *val = LSM6DSV_FSM_960Hz; break; default: - *val = LSM6DSV16X_FSM_15Hz; + *val = LSM6DSV_FSM_15Hz; break; } @@ -5705,14 +5552,14 @@ static uint16_t npy_float_to_half(float_t f) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_sflp_game_gbias_set(stmdev_ctx_t *ctx, - lsm6dsv16x_sflp_gbias_t *val) +int32_t lsm6dsv_sflp_game_gbias_set(stmdev_ctx_t *ctx, + lsm6dsv_sflp_gbias_t *val) { - lsm6dsv16x_sflp_data_rate_t sflp_odr; - lsm6dsv16x_emb_func_exec_status_t emb_func_sts; - lsm6dsv16x_data_ready_t drdy; - lsm6dsv16x_xl_full_scale_t xl_fs; - lsm6dsv16x_ctrl10_t ctrl10; + lsm6dsv_sflp_data_rate_t sflp_odr; + lsm6dsv_emb_func_exec_status_t emb_func_sts; + lsm6dsv_data_ready_t drdy; + lsm6dsv_xl_full_scale_t xl_fs; + lsm6dsv_ctrl10_t ctrl10; uint8_t master_config; uint8_t emb_func_en_saved[2]; uint8_t conf_saved[2]; @@ -5725,29 +5572,29 @@ int32_t lsm6dsv16x_sflp_game_gbias_set(stmdev_ctx_t *ctx, uint8_t i, j; int32_t ret; - ret = lsm6dsv16x_sflp_data_rate_get(ctx, &sflp_odr); + ret = lsm6dsv_sflp_data_rate_get(ctx, &sflp_odr); if (ret != 0) { return ret; } /* Calculate k factor */ switch (sflp_odr) { default: - case LSM6DSV16X_SFLP_15Hz: + case LSM6DSV_SFLP_15Hz: k = 0.04f; break; - case LSM6DSV16X_SFLP_30Hz: + case LSM6DSV_SFLP_30Hz: k = 0.02f; break; - case LSM6DSV16X_SFLP_60Hz: + case LSM6DSV_SFLP_60Hz: k = 0.01f; break; - case LSM6DSV16X_SFLP_120Hz: + case LSM6DSV_SFLP_120Hz: k = 0.005f; break; - case LSM6DSV16X_SFLP_240Hz: + case LSM6DSV_SFLP_240Hz: k = 0.0025f; break; - case LSM6DSV16X_SFLP_480Hz: + case LSM6DSV_SFLP_480Hz: k = 0.00125f; break; } @@ -5758,100 +5605,100 @@ int32_t lsm6dsv16x_sflp_game_gbias_set(stmdev_ctx_t *ctx, gbias_hf[2] = npy_float_to_half(val->gbias_z * (3.14159265358979323846f / 180.0f) / k); /* Save sensor configuration and set high-performance mode (if the sensor is in power-down mode, turn it on) */ - ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL1, conf_saved, 2); - ret += lsm6dsv16x_xl_mode_set(ctx, LSM6DSV16X_XL_HIGH_PERFORMANCE_MD); - ret += lsm6dsv16x_gy_mode_set(ctx, LSM6DSV16X_GY_HIGH_PERFORMANCE_MD); - if (((uint8_t)conf_saved[0] & 0x0FU) == (uint8_t)LSM6DSV16X_ODR_OFF) + ret += lsm6dsv_read_reg(ctx, LSM6DSV_CTRL1, conf_saved, 2); + ret += lsm6dsv_xl_mode_set(ctx, LSM6DSV_XL_HIGH_PERFORMANCE_MD); + ret += lsm6dsv_gy_mode_set(ctx, LSM6DSV_GY_HIGH_PERFORMANCE_MD); + if (((uint8_t)conf_saved[0] & 0x0FU) == (uint8_t)LSM6DSV_ODR_OFF) { - ret += lsm6dsv16x_xl_data_rate_set(ctx, LSM6DSV16X_ODR_AT_120Hz); + ret += lsm6dsv_xl_data_rate_set(ctx, LSM6DSV_ODR_AT_120Hz); } /* Make sure to turn the sensor-hub master off */ - ret += lsm6dsv16x_sh_master_get(ctx, &master_config); - ret += lsm6dsv16x_sh_master_set(ctx, 0); + ret += lsm6dsv_sh_master_get(ctx, &master_config); + ret += lsm6dsv_sh_master_set(ctx, 0); /* disable algos */ - ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK); - ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_EMB_FUNC_EN_A, emb_func_en_saved, 2); - ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_EMB_FUNC_EN_A, reg_zero, 2); + ret += lsm6dsv_mem_bank_set(ctx, LSM6DSV_EMBED_FUNC_MEM_BANK); + ret += lsm6dsv_read_reg(ctx, LSM6DSV_EMB_FUNC_EN_A, emb_func_en_saved, 2); + ret += lsm6dsv_write_reg(ctx, LSM6DSV_EMB_FUNC_EN_A, reg_zero, 2); do { - ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_EMB_FUNC_EXEC_STATUS, + ret += lsm6dsv_read_reg(ctx, LSM6DSV_EMB_FUNC_EXEC_STATUS, (uint8_t *)&emb_func_sts, 1); } while (emb_func_sts.emb_func_endop != 1U); - ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + ret += lsm6dsv_mem_bank_set(ctx, LSM6DSV_MAIN_MEM_BANK); // enable gbias setting - ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL10, (uint8_t *)&ctrl10, 1); + ret += lsm6dsv_read_reg(ctx, LSM6DSV_CTRL10, (uint8_t *)&ctrl10, 1); ctrl10.emb_func_debug = 1; - ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_CTRL10, (uint8_t *)&ctrl10, 1); + ret += lsm6dsv_write_reg(ctx, LSM6DSV_CTRL10, (uint8_t *)&ctrl10, 1); /* enable algos */ - ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK); + ret += lsm6dsv_mem_bank_set(ctx, LSM6DSV_EMBED_FUNC_MEM_BANK); emb_func_en_saved[0] |= 0x02U; /* force SFLP GAME en */ - ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_EMB_FUNC_EN_A, emb_func_en_saved, + ret += lsm6dsv_write_reg(ctx, LSM6DSV_EMB_FUNC_EN_A, emb_func_en_saved, 2); - ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + ret += lsm6dsv_mem_bank_set(ctx, LSM6DSV_MAIN_MEM_BANK); - ret += lsm6dsv16x_xl_full_scale_get(ctx, &xl_fs); + ret += lsm6dsv_xl_full_scale_get(ctx, &xl_fs); /* Read XL data */ do { - ret += lsm6dsv16x_flag_data_ready_get(ctx, &drdy); + ret += lsm6dsv_flag_data_ready_get(ctx, &drdy); } while (drdy.drdy_xl != 1U); - ret += lsm6dsv16x_acceleration_raw_get(ctx, xl_data); + ret += lsm6dsv_acceleration_raw_get(ctx, xl_data); /* force sflp initialization */ - ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_SENSOR_HUB_MEM_BANK); + ret += lsm6dsv_mem_bank_set(ctx, LSM6DSV_SENSOR_HUB_MEM_BANK); for (i = 0; i < 3U; i++) { j = 0; data_tmp = (int32_t)xl_data[i]; data_tmp <<= xl_fs; // shift based on current fs - ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_SENSOR_HUB_1 + 3U * i, + ret += lsm6dsv_write_reg(ctx, LSM6DSV_SENSOR_HUB_1 + 3U * i, &data_ptr[j++], 1); - ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_SENSOR_HUB_2 + 3U * i, + ret += lsm6dsv_write_reg(ctx, LSM6DSV_SENSOR_HUB_2 + 3U * i, &data_ptr[j++], 1); - ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_SENSOR_HUB_3 + 3U * i, &data_ptr[j], + ret += lsm6dsv_write_reg(ctx, LSM6DSV_SENSOR_HUB_3 + 3U * i, &data_ptr[j], 1); } for (i = 0; i < 3U; i++) { j = 0; data_tmp = 0; - ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_SENSOR_HUB_10 + 3U * i, + ret += lsm6dsv_write_reg(ctx, LSM6DSV_SENSOR_HUB_10 + 3U * i, &data_ptr[j++], 1); - ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_SENSOR_HUB_11 + 3U * i, + ret += lsm6dsv_write_reg(ctx, LSM6DSV_SENSOR_HUB_11 + 3U * i, &data_ptr[j++], 1); - ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_SENSOR_HUB_12 + 3U * i, &data_ptr[j], + ret += lsm6dsv_write_reg(ctx, LSM6DSV_SENSOR_HUB_12 + 3U * i, &data_ptr[j], 1); } - ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + ret += lsm6dsv_mem_bank_set(ctx, LSM6DSV_MAIN_MEM_BANK); // wait end_op (and at least 30 us) ctx->mdelay(1); - ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK); + ret += lsm6dsv_mem_bank_set(ctx, LSM6DSV_EMBED_FUNC_MEM_BANK); do { - ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_EMB_FUNC_EXEC_STATUS, + ret += lsm6dsv_read_reg(ctx, LSM6DSV_EMB_FUNC_EXEC_STATUS, (uint8_t *)&emb_func_sts, 1); } while (emb_func_sts.emb_func_endop != 1U); - ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + ret += lsm6dsv_mem_bank_set(ctx, LSM6DSV_MAIN_MEM_BANK); /* write gbias in embedded advanced features registers */ - ret += lsm6dsv16x_ln_pg_write(ctx, LSM6DSV16X_SFLP_GAME_GBIASX_L, + ret += lsm6dsv_ln_pg_write(ctx, LSM6DSV_SFLP_GAME_GBIASX_L, (uint8_t *)gbias_hf, 6); /* reload previous sensor configuration */ - ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_CTRL1, conf_saved, 2); + ret += lsm6dsv_write_reg(ctx, LSM6DSV_CTRL1, conf_saved, 2); // disable gbias setting ctrl10.emb_func_debug = 0; - ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_CTRL10, (uint8_t *)&ctrl10, 1); - + ret += lsm6dsv_write_reg(ctx, LSM6DSV_CTRL10, (uint8_t *)&ctrl10, 1); + /* reload previous master configuration */ - ret += lsm6dsv16x_sh_master_set(ctx, master_config); + ret += lsm6dsv_sh_master_set(ctx, master_config); return ret; } @@ -5864,14 +5711,14 @@ int32_t lsm6dsv16x_sflp_game_gbias_set(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fsm_ext_sens_sensitivity_set(stmdev_ctx_t *ctx, uint16_t val) +int32_t lsm6dsv_fsm_ext_sens_sensitivity_set(stmdev_ctx_t *ctx, uint16_t val) { uint8_t buff[2]; int32_t ret; buff[1] = (uint8_t)(val / 256U); buff[0] = (uint8_t)(val - (buff[1] * 256U)); - ret = lsm6dsv16x_ln_pg_write(ctx, LSM6DSV16X_FSM_EXT_SENSITIVITY_L, (uint8_t *)&buff[0], 2); + ret = lsm6dsv_ln_pg_write(ctx, LSM6DSV_FSM_EXT_SENSITIVITY_L, (uint8_t *)&buff[0], 2); return ret; } @@ -5884,13 +5731,12 @@ int32_t lsm6dsv16x_fsm_ext_sens_sensitivity_set(stmdev_ctx_t *ctx, uint16_t val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fsm_ext_sens_sensitivity_get(stmdev_ctx_t *ctx, - uint16_t *val) +int32_t lsm6dsv_fsm_ext_sens_sensitivity_get(stmdev_ctx_t *ctx, uint16_t *val) { uint8_t buff[2]; int32_t ret; - ret = lsm6dsv16x_ln_pg_read(ctx, LSM6DSV16X_FSM_EXT_SENSITIVITY_L, &buff[0], 2); + ret = lsm6dsv_ln_pg_read(ctx, LSM6DSV_FSM_EXT_SENSITIVITY_L, &buff[0], 2); if (ret != 0) { return ret; } *val = buff[1]; @@ -5907,8 +5753,8 @@ int32_t lsm6dsv16x_fsm_ext_sens_sensitivity_get(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fsm_ext_sens_offset_set(stmdev_ctx_t *ctx, - lsm6dsv16x_xl_fsm_ext_sens_offset_t val) +int32_t lsm6dsv_fsm_ext_sens_offset_set(stmdev_ctx_t *ctx, + lsm6dsv_xl_fsm_ext_sens_offset_t val) { uint8_t buff[6]; int32_t ret; @@ -5919,7 +5765,7 @@ int32_t lsm6dsv16x_fsm_ext_sens_offset_set(stmdev_ctx_t *ctx, buff[2] = (uint8_t)(val.y - (buff[3] * 256U)); buff[5] = (uint8_t)(val.z / 256U); buff[4] = (uint8_t)(val.z - (buff[5] * 256U)); - ret = lsm6dsv16x_ln_pg_write(ctx, LSM6DSV16X_FSM_EXT_OFFX_L, (uint8_t *)&buff[0], 6); + ret = lsm6dsv_ln_pg_write(ctx, LSM6DSV_FSM_EXT_OFFX_L, (uint8_t *)&buff[0], 6); return ret; } @@ -5932,13 +5778,13 @@ int32_t lsm6dsv16x_fsm_ext_sens_offset_set(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fsm_ext_sens_offset_get(stmdev_ctx_t *ctx, - lsm6dsv16x_xl_fsm_ext_sens_offset_t *val) +int32_t lsm6dsv_fsm_ext_sens_offset_get(stmdev_ctx_t *ctx, + lsm6dsv_xl_fsm_ext_sens_offset_t *val) { uint8_t buff[6]; int32_t ret; - ret = lsm6dsv16x_ln_pg_read(ctx, LSM6DSV16X_FSM_EXT_OFFX_L, &buff[0], 6); + ret = lsm6dsv_ln_pg_read(ctx, LSM6DSV_FSM_EXT_OFFX_L, &buff[0], 6); if (ret != 0) { return ret; } val->x = buff[1]; @@ -5959,8 +5805,8 @@ int32_t lsm6dsv16x_fsm_ext_sens_offset_get(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fsm_ext_sens_matrix_set(stmdev_ctx_t *ctx, - lsm6dsv16x_xl_fsm_ext_sens_matrix_t val) +int32_t lsm6dsv_fsm_ext_sens_matrix_set(stmdev_ctx_t *ctx, + lsm6dsv_xl_fsm_ext_sens_matrix_t val) { uint8_t buff[12]; int32_t ret; @@ -5977,7 +5823,7 @@ int32_t lsm6dsv16x_fsm_ext_sens_matrix_set(stmdev_ctx_t *ctx, buff[8] = (uint8_t)(val.yz - (buff[9] * 256U)); buff[11] = (uint8_t)(val.zz / 256U); buff[10] = (uint8_t)(val.zz - (buff[11] * 256U)); - ret = lsm6dsv16x_ln_pg_write(ctx, LSM6DSV16X_FSM_EXT_MATRIX_XX_L, (uint8_t *)&buff[0], 12); + ret = lsm6dsv_ln_pg_write(ctx, LSM6DSV_FSM_EXT_MATRIX_XX_L, (uint8_t *)&buff[0], 12); return ret; } @@ -5990,13 +5836,13 @@ int32_t lsm6dsv16x_fsm_ext_sens_matrix_set(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fsm_ext_sens_matrix_get(stmdev_ctx_t *ctx, - lsm6dsv16x_xl_fsm_ext_sens_matrix_t *val) +int32_t lsm6dsv_fsm_ext_sens_matrix_get(stmdev_ctx_t *ctx, + lsm6dsv_xl_fsm_ext_sens_matrix_t *val) { uint8_t buff[12]; int32_t ret; - ret = lsm6dsv16x_ln_pg_read(ctx, LSM6DSV16X_FSM_EXT_MATRIX_XX_L, &buff[0], 12); + ret = lsm6dsv_ln_pg_read(ctx, LSM6DSV_FSM_EXT_MATRIX_XX_L, &buff[0], 12); if (ret != 0) { return ret; } val->xx = buff[1]; @@ -6023,15 +5869,15 @@ int32_t lsm6dsv16x_fsm_ext_sens_matrix_get(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fsm_ext_sens_z_orient_set(stmdev_ctx_t *ctx, - lsm6dsv16x_fsm_ext_sens_z_orient_t val) +int32_t lsm6dsv_fsm_ext_sens_z_orient_set(stmdev_ctx_t *ctx, + lsm6dsv_fsm_ext_sens_z_orient_t val) { - lsm6dsv16x_ext_cfg_a_t ext_cfg_a; + lsm6dsv_ext_cfg_a_t ext_cfg_a; int32_t ret; - ret = lsm6dsv16x_ln_pg_read(ctx, LSM6DSV16X_EXT_CFG_A, (uint8_t *)&ext_cfg_a, 1); + ret = lsm6dsv_ln_pg_read(ctx, LSM6DSV_EXT_CFG_A, (uint8_t *)&ext_cfg_a, 1); ext_cfg_a.ext_z_axis = (uint8_t)val & 0x07U; - ret += lsm6dsv16x_ln_pg_write(ctx, LSM6DSV16X_EXT_CFG_A, (uint8_t *)&ext_cfg_a, 1); + ret += lsm6dsv_ln_pg_write(ctx, LSM6DSV_EXT_CFG_A, (uint8_t *)&ext_cfg_a, 1); return ret; } @@ -6044,43 +5890,43 @@ int32_t lsm6dsv16x_fsm_ext_sens_z_orient_set(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fsm_ext_sens_z_orient_get(stmdev_ctx_t *ctx, - lsm6dsv16x_fsm_ext_sens_z_orient_t *val) +int32_t lsm6dsv_fsm_ext_sens_z_orient_get(stmdev_ctx_t *ctx, + lsm6dsv_fsm_ext_sens_z_orient_t *val) { - lsm6dsv16x_ext_cfg_a_t ext_cfg_a; + lsm6dsv_ext_cfg_a_t ext_cfg_a; int32_t ret; - ret = lsm6dsv16x_ln_pg_read(ctx, LSM6DSV16X_EXT_CFG_A, (uint8_t *)&ext_cfg_a, 1); + ret = lsm6dsv_ln_pg_read(ctx, LSM6DSV_EXT_CFG_A, (uint8_t *)&ext_cfg_a, 1); if (ret != 0) { return ret; } switch (ext_cfg_a.ext_z_axis) { - case LSM6DSV16X_Z_EQ_Y: - *val = LSM6DSV16X_Z_EQ_Y; + case LSM6DSV_Z_EQ_Y: + *val = LSM6DSV_Z_EQ_Y; break; - case LSM6DSV16X_Z_EQ_MIN_Y: - *val = LSM6DSV16X_Z_EQ_MIN_Y; + case LSM6DSV_Z_EQ_MIN_Y: + *val = LSM6DSV_Z_EQ_MIN_Y; break; - case LSM6DSV16X_Z_EQ_X: - *val = LSM6DSV16X_Z_EQ_X; + case LSM6DSV_Z_EQ_X: + *val = LSM6DSV_Z_EQ_X; break; - case LSM6DSV16X_Z_EQ_MIN_X: - *val = LSM6DSV16X_Z_EQ_MIN_X; + case LSM6DSV_Z_EQ_MIN_X: + *val = LSM6DSV_Z_EQ_MIN_X; break; - case LSM6DSV16X_Z_EQ_MIN_Z: - *val = LSM6DSV16X_Z_EQ_MIN_Z; + case LSM6DSV_Z_EQ_MIN_Z: + *val = LSM6DSV_Z_EQ_MIN_Z; break; - case LSM6DSV16X_Z_EQ_Z: - *val = LSM6DSV16X_Z_EQ_Z; + case LSM6DSV_Z_EQ_Z: + *val = LSM6DSV_Z_EQ_Z; break; default: - *val = LSM6DSV16X_Z_EQ_Y; + *val = LSM6DSV_Z_EQ_Y; break; } @@ -6095,17 +5941,17 @@ int32_t lsm6dsv16x_fsm_ext_sens_z_orient_get(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fsm_ext_sens_y_orient_set(stmdev_ctx_t *ctx, - lsm6dsv16x_fsm_ext_sens_y_orient_t val) +int32_t lsm6dsv_fsm_ext_sens_y_orient_set(stmdev_ctx_t *ctx, + lsm6dsv_fsm_ext_sens_y_orient_t val) { - lsm6dsv16x_ext_cfg_a_t ext_cfg_a; + lsm6dsv_ext_cfg_a_t ext_cfg_a; int32_t ret; - ret = lsm6dsv16x_ln_pg_read(ctx, LSM6DSV16X_EXT_CFG_A, (uint8_t *)&ext_cfg_a, 1); + ret = lsm6dsv_ln_pg_read(ctx, LSM6DSV_EXT_CFG_A, (uint8_t *)&ext_cfg_a, 1); if (ret == 0) { ext_cfg_a.ext_y_axis = (uint8_t)val & 0x7U; - ret = lsm6dsv16x_ln_pg_write(ctx, LSM6DSV16X_EXT_CFG_A, (uint8_t *)&ext_cfg_a, 1); + ret = lsm6dsv_ln_pg_write(ctx, LSM6DSV_EXT_CFG_A, (uint8_t *)&ext_cfg_a, 1); } return ret; @@ -6119,43 +5965,43 @@ int32_t lsm6dsv16x_fsm_ext_sens_y_orient_set(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fsm_ext_sens_y_orient_get(stmdev_ctx_t *ctx, - lsm6dsv16x_fsm_ext_sens_y_orient_t *val) +int32_t lsm6dsv_fsm_ext_sens_y_orient_get(stmdev_ctx_t *ctx, + lsm6dsv_fsm_ext_sens_y_orient_t *val) { - lsm6dsv16x_ext_cfg_a_t ext_cfg_a; + lsm6dsv_ext_cfg_a_t ext_cfg_a; int32_t ret; - ret = lsm6dsv16x_ln_pg_read(ctx, LSM6DSV16X_EXT_CFG_A, (uint8_t *)&ext_cfg_a, 1); + ret = lsm6dsv_ln_pg_read(ctx, LSM6DSV_EXT_CFG_A, (uint8_t *)&ext_cfg_a, 1); if (ret != 0) { return ret; } switch (ext_cfg_a.ext_y_axis) { - case LSM6DSV16X_Y_EQ_Y: - *val = LSM6DSV16X_Y_EQ_Y; + case LSM6DSV_Y_EQ_Y: + *val = LSM6DSV_Y_EQ_Y; break; - case LSM6DSV16X_Y_EQ_MIN_Y: - *val = LSM6DSV16X_Y_EQ_MIN_Y; + case LSM6DSV_Y_EQ_MIN_Y: + *val = LSM6DSV_Y_EQ_MIN_Y; break; - case LSM6DSV16X_Y_EQ_X: - *val = LSM6DSV16X_Y_EQ_X; + case LSM6DSV_Y_EQ_X: + *val = LSM6DSV_Y_EQ_X; break; - case LSM6DSV16X_Y_EQ_MIN_X: - *val = LSM6DSV16X_Y_EQ_MIN_X; + case LSM6DSV_Y_EQ_MIN_X: + *val = LSM6DSV_Y_EQ_MIN_X; break; - case LSM6DSV16X_Y_EQ_MIN_Z: - *val = LSM6DSV16X_Y_EQ_MIN_Z; + case LSM6DSV_Y_EQ_MIN_Z: + *val = LSM6DSV_Y_EQ_MIN_Z; break; - case LSM6DSV16X_Y_EQ_Z: - *val = LSM6DSV16X_Y_EQ_Z; + case LSM6DSV_Y_EQ_Z: + *val = LSM6DSV_Y_EQ_Z; break; default: - *val = LSM6DSV16X_Y_EQ_Y; + *val = LSM6DSV_Y_EQ_Y; break; } @@ -6170,17 +6016,17 @@ int32_t lsm6dsv16x_fsm_ext_sens_y_orient_get(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fsm_ext_sens_x_orient_set(stmdev_ctx_t *ctx, - lsm6dsv16x_fsm_ext_sens_x_orient_t val) +int32_t lsm6dsv_fsm_ext_sens_x_orient_set(stmdev_ctx_t *ctx, + lsm6dsv_fsm_ext_sens_x_orient_t val) { - lsm6dsv16x_ext_cfg_b_t ext_cfg_b; + lsm6dsv_ext_cfg_b_t ext_cfg_b; int32_t ret; - ret = lsm6dsv16x_ln_pg_read(ctx, LSM6DSV16X_EXT_CFG_B, (uint8_t *)&ext_cfg_b, 1); + ret = lsm6dsv_ln_pg_read(ctx, LSM6DSV_EXT_CFG_B, (uint8_t *)&ext_cfg_b, 1); if (ret == 0) { ext_cfg_b.ext_x_axis = (uint8_t)val & 0x7U; - ret = lsm6dsv16x_ln_pg_write(ctx, LSM6DSV16X_EXT_CFG_B, (uint8_t *)&ext_cfg_b, 1); + ret = lsm6dsv_ln_pg_write(ctx, LSM6DSV_EXT_CFG_B, (uint8_t *)&ext_cfg_b, 1); } return ret; @@ -6194,43 +6040,43 @@ int32_t lsm6dsv16x_fsm_ext_sens_x_orient_set(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fsm_ext_sens_x_orient_get(stmdev_ctx_t *ctx, - lsm6dsv16x_fsm_ext_sens_x_orient_t *val) +int32_t lsm6dsv_fsm_ext_sens_x_orient_get(stmdev_ctx_t *ctx, + lsm6dsv_fsm_ext_sens_x_orient_t *val) { - lsm6dsv16x_ext_cfg_b_t ext_cfg_b; + lsm6dsv_ext_cfg_b_t ext_cfg_b; int32_t ret; - ret = lsm6dsv16x_ln_pg_read(ctx, LSM6DSV16X_EXT_CFG_B, (uint8_t *)&ext_cfg_b, 1); + ret = lsm6dsv_ln_pg_read(ctx, LSM6DSV_EXT_CFG_B, (uint8_t *)&ext_cfg_b, 1); if (ret != 0) { return ret; } switch (ext_cfg_b.ext_x_axis) { - case LSM6DSV16X_X_EQ_Y: - *val = LSM6DSV16X_X_EQ_Y; + case LSM6DSV_X_EQ_Y: + *val = LSM6DSV_X_EQ_Y; break; - case LSM6DSV16X_X_EQ_MIN_Y: - *val = LSM6DSV16X_X_EQ_MIN_Y; + case LSM6DSV_X_EQ_MIN_Y: + *val = LSM6DSV_X_EQ_MIN_Y; break; - case LSM6DSV16X_X_EQ_X: - *val = LSM6DSV16X_X_EQ_X; + case LSM6DSV_X_EQ_X: + *val = LSM6DSV_X_EQ_X; break; - case LSM6DSV16X_X_EQ_MIN_X: - *val = LSM6DSV16X_X_EQ_MIN_X; + case LSM6DSV_X_EQ_MIN_X: + *val = LSM6DSV_X_EQ_MIN_X; break; - case LSM6DSV16X_X_EQ_MIN_Z: - *val = LSM6DSV16X_X_EQ_MIN_Z; + case LSM6DSV_X_EQ_MIN_Z: + *val = LSM6DSV_X_EQ_MIN_Z; break; - case LSM6DSV16X_X_EQ_Z: - *val = LSM6DSV16X_X_EQ_Z; + case LSM6DSV_X_EQ_Z: + *val = LSM6DSV_X_EQ_Z; break; default: - *val = LSM6DSV16X_X_EQ_Y; + *val = LSM6DSV_X_EQ_Y; break; } @@ -6245,14 +6091,14 @@ int32_t lsm6dsv16x_fsm_ext_sens_x_orient_get(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fsm_long_cnt_timeout_set(stmdev_ctx_t *ctx, uint16_t val) +int32_t lsm6dsv_fsm_long_cnt_timeout_set(stmdev_ctx_t *ctx, uint16_t val) { uint8_t buff[2]; int32_t ret; buff[1] = (uint8_t)(val / 256U); buff[0] = (uint8_t)(val - (buff[1] * 256U)); - ret = lsm6dsv16x_ln_pg_write(ctx, LSM6DSV16X_EMB_ADV_PG_1 + LSM6DSV16X_FSM_LC_TIMEOUT_L, (uint8_t *)&buff[0], 2); + ret = lsm6dsv_ln_pg_write(ctx, LSM6DSV_EMB_ADV_PG_1 + LSM6DSV_FSM_LC_TIMEOUT_L, (uint8_t *)&buff[0], 2); return ret; } @@ -6265,12 +6111,12 @@ int32_t lsm6dsv16x_fsm_long_cnt_timeout_set(stmdev_ctx_t *ctx, uint16_t val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fsm_long_cnt_timeout_get(stmdev_ctx_t *ctx, uint16_t *val) +int32_t lsm6dsv_fsm_long_cnt_timeout_get(stmdev_ctx_t *ctx, uint16_t *val) { uint8_t buff[2]; int32_t ret; - ret = lsm6dsv16x_ln_pg_read(ctx, LSM6DSV16X_EMB_ADV_PG_1 + LSM6DSV16X_FSM_LC_TIMEOUT_L, &buff[0], 2); + ret = lsm6dsv_ln_pg_read(ctx, LSM6DSV_EMB_ADV_PG_1 + LSM6DSV_FSM_LC_TIMEOUT_L, &buff[0], 2); if (ret != 0) { return ret; } *val = buff[1]; @@ -6287,16 +6133,16 @@ int32_t lsm6dsv16x_fsm_long_cnt_timeout_get(stmdev_ctx_t *ctx, uint16_t *val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fsm_number_of_programs_set(stmdev_ctx_t *ctx, uint8_t val) +int32_t lsm6dsv_fsm_number_of_programs_set(stmdev_ctx_t *ctx, uint8_t val) { - lsm6dsv16x_fsm_programs_t fsm_programs; + lsm6dsv_fsm_programs_t fsm_programs; int32_t ret; - ret = lsm6dsv16x_ln_pg_read(ctx, LSM6DSV16X_EMB_ADV_PG_1 + LSM6DSV16X_FSM_PROGRAMS, (uint8_t *)&fsm_programs, 1); + ret = lsm6dsv_ln_pg_read(ctx, LSM6DSV_EMB_ADV_PG_1 + LSM6DSV_FSM_PROGRAMS, (uint8_t *)&fsm_programs, 1); if (ret == 0) { fsm_programs.fsm_n_prog = val; - ret = lsm6dsv16x_ln_pg_write(ctx, LSM6DSV16X_EMB_ADV_PG_1 + LSM6DSV16X_FSM_PROGRAMS, (uint8_t *)&fsm_programs, 1); + ret = lsm6dsv_ln_pg_write(ctx, LSM6DSV_EMB_ADV_PG_1 + LSM6DSV_FSM_PROGRAMS, (uint8_t *)&fsm_programs, 1); } return ret; @@ -6310,12 +6156,12 @@ int32_t lsm6dsv16x_fsm_number_of_programs_set(stmdev_ctx_t *ctx, uint8_t val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fsm_number_of_programs_get(stmdev_ctx_t *ctx, uint8_t *val) +int32_t lsm6dsv_fsm_number_of_programs_get(stmdev_ctx_t *ctx, uint8_t *val) { - lsm6dsv16x_fsm_programs_t fsm_programs; + lsm6dsv_fsm_programs_t fsm_programs; int32_t ret; - ret = lsm6dsv16x_ln_pg_read(ctx, LSM6DSV16X_EMB_ADV_PG_1 + LSM6DSV16X_FSM_PROGRAMS, (uint8_t *)&fsm_programs, 1); + ret = lsm6dsv_ln_pg_read(ctx, LSM6DSV_EMB_ADV_PG_1 + LSM6DSV_FSM_PROGRAMS, (uint8_t *)&fsm_programs, 1); *val = fsm_programs.fsm_n_prog; return ret; @@ -6329,14 +6175,14 @@ int32_t lsm6dsv16x_fsm_number_of_programs_get(stmdev_ctx_t *ctx, uint8_t *val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fsm_start_address_set(stmdev_ctx_t *ctx, uint16_t val) +int32_t lsm6dsv_fsm_start_address_set(stmdev_ctx_t *ctx, uint16_t val) { uint8_t buff[2]; int32_t ret; buff[1] = (uint8_t)(val / 256U); buff[0] = (uint8_t)(val - (buff[1] * 256U)); - ret = lsm6dsv16x_ln_pg_write(ctx, LSM6DSV16X_EMB_ADV_PG_1 + LSM6DSV16X_FSM_START_ADD_L, (uint8_t *)&buff[0], 2); + ret = lsm6dsv_ln_pg_write(ctx, LSM6DSV_EMB_ADV_PG_1 + LSM6DSV_FSM_START_ADD_L, (uint8_t *)&buff[0], 2); return ret; } @@ -6349,12 +6195,12 @@ int32_t lsm6dsv16x_fsm_start_address_set(stmdev_ctx_t *ctx, uint16_t val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_fsm_start_address_get(stmdev_ctx_t *ctx, uint16_t *val) +int32_t lsm6dsv_fsm_start_address_get(stmdev_ctx_t *ctx, uint16_t *val) { uint8_t buff[2]; int32_t ret; - ret = lsm6dsv16x_ln_pg_read(ctx, LSM6DSV16X_EMB_ADV_PG_1 + LSM6DSV16X_FSM_START_ADD_L, &buff[0], 2); + ret = lsm6dsv_ln_pg_read(ctx, LSM6DSV_EMB_ADV_PG_1 + LSM6DSV_FSM_START_ADD_L, &buff[0], 2); if (ret != 0) { return ret; } *val = buff[1]; @@ -6384,20 +6230,20 @@ int32_t lsm6dsv16x_fsm_start_address_get(stmdev_ctx_t *ctx, uint16_t *val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_ff_time_windows_set(stmdev_ctx_t *ctx, uint8_t val) +int32_t lsm6dsv_ff_time_windows_set(stmdev_ctx_t *ctx, uint8_t val) { - lsm6dsv16x_wake_up_dur_t wake_up_dur; - lsm6dsv16x_free_fall_t free_fall; + lsm6dsv_wake_up_dur_t wake_up_dur; + lsm6dsv_free_fall_t free_fall; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_WAKE_UP_DUR, (uint8_t *)&wake_up_dur, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_WAKE_UP_DUR, (uint8_t *)&wake_up_dur, 1); wake_up_dur.ff_dur = ((uint8_t)val & 0x20U) >> 5; - ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_WAKE_UP_DUR, (uint8_t *)&wake_up_dur, 1); + ret += lsm6dsv_write_reg(ctx, LSM6DSV_WAKE_UP_DUR, (uint8_t *)&wake_up_dur, 1); if (ret != 0) { return ret; } - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FREE_FALL, (uint8_t *)&free_fall, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_FREE_FALL, (uint8_t *)&free_fall, 1); free_fall.ff_dur = (uint8_t)val & 0x1FU; - ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_FREE_FALL, (uint8_t *)&free_fall, 1); + ret += lsm6dsv_write_reg(ctx, LSM6DSV_FREE_FALL, (uint8_t *)&free_fall, 1); return ret; } @@ -6410,14 +6256,14 @@ int32_t lsm6dsv16x_ff_time_windows_set(stmdev_ctx_t *ctx, uint8_t val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_ff_time_windows_get(stmdev_ctx_t *ctx, uint8_t *val) +int32_t lsm6dsv_ff_time_windows_get(stmdev_ctx_t *ctx, uint8_t *val) { - lsm6dsv16x_wake_up_dur_t wake_up_dur; - lsm6dsv16x_free_fall_t free_fall; + lsm6dsv_wake_up_dur_t wake_up_dur; + lsm6dsv_free_fall_t free_fall; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_WAKE_UP_DUR, (uint8_t *)&wake_up_dur, 1); - ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FREE_FALL, (uint8_t *)&free_fall, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_WAKE_UP_DUR, (uint8_t *)&wake_up_dur, 1); + ret += lsm6dsv_read_reg(ctx, LSM6DSV_FREE_FALL, (uint8_t *)&free_fall, 1); *val = (wake_up_dur.ff_dur << 5) + free_fall.ff_dur; @@ -6432,17 +6278,17 @@ int32_t lsm6dsv16x_ff_time_windows_get(stmdev_ctx_t *ctx, uint8_t *val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_ff_thresholds_set(stmdev_ctx_t *ctx, - lsm6dsv16x_ff_thresholds_t val) +int32_t lsm6dsv_ff_thresholds_set(stmdev_ctx_t *ctx, + lsm6dsv_ff_thresholds_t val) { - lsm6dsv16x_free_fall_t free_fall; + lsm6dsv_free_fall_t free_fall; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FREE_FALL, (uint8_t *)&free_fall, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_FREE_FALL, (uint8_t *)&free_fall, 1); if (ret == 0) { free_fall.ff_ths = (uint8_t)val & 0x7U; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_FREE_FALL, (uint8_t *)&free_fall, 1); + ret = lsm6dsv_write_reg(ctx, LSM6DSV_FREE_FALL, (uint8_t *)&free_fall, 1); } return ret; @@ -6456,305 +6302,53 @@ int32_t lsm6dsv16x_ff_thresholds_set(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_ff_thresholds_get(stmdev_ctx_t *ctx, - lsm6dsv16x_ff_thresholds_t *val) +int32_t lsm6dsv_ff_thresholds_get(stmdev_ctx_t *ctx, + lsm6dsv_ff_thresholds_t *val) { - lsm6dsv16x_free_fall_t free_fall; + lsm6dsv_free_fall_t free_fall; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FREE_FALL, (uint8_t *)&free_fall, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_FREE_FALL, (uint8_t *)&free_fall, 1); if (ret != 0) { return ret; } switch (free_fall.ff_ths) { - case LSM6DSV16X_156_mg: - *val = LSM6DSV16X_156_mg; + case LSM6DSV_156_mg: + *val = LSM6DSV_156_mg; break; - case LSM6DSV16X_219_mg: - *val = LSM6DSV16X_219_mg; + case LSM6DSV_219_mg: + *val = LSM6DSV_219_mg; break; - case LSM6DSV16X_250_mg: - *val = LSM6DSV16X_250_mg; + case LSM6DSV_250_mg: + *val = LSM6DSV_250_mg; break; - case LSM6DSV16X_312_mg: - *val = LSM6DSV16X_312_mg; + case LSM6DSV_312_mg: + *val = LSM6DSV_312_mg; break; - case LSM6DSV16X_344_mg: - *val = LSM6DSV16X_344_mg; + case LSM6DSV_344_mg: + *val = LSM6DSV_344_mg; break; - case LSM6DSV16X_406_mg: - *val = LSM6DSV16X_406_mg; + case LSM6DSV_406_mg: + *val = LSM6DSV_406_mg; break; - case LSM6DSV16X_469_mg: - *val = LSM6DSV16X_469_mg; + case LSM6DSV_469_mg: + *val = LSM6DSV_469_mg; break; - case LSM6DSV16X_500_mg: - *val = LSM6DSV16X_500_mg; + case LSM6DSV_500_mg: + *val = LSM6DSV_500_mg; break; default: - *val = LSM6DSV16X_156_mg; + *val = LSM6DSV_156_mg; break; } - - return ret; -} - -/** - * @} - * - */ - -/** - * @defgroup Machine Learning Core (MLC) - * @brief This section group all the functions concerning the - * usage of Machine Learning Core - * @{ - * - */ - -/** - * @brief It enables Machine Learning Core feature (MLC). When the Machine Learning Core is enabled the Finite State Machine (FSM) programs are executed before executing the MLC algorithms.[set] - * - * @param ctx read / write interface definitions - * @param val MLC_OFF, MLC_ON, MLC_BEFORE_FSM, - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6dsv16x_mlc_set(stmdev_ctx_t *ctx, lsm6dsv16x_mlc_mode_t val) -{ - lsm6dsv16x_emb_func_en_b_t emb_en_b; - lsm6dsv16x_emb_func_en_a_t emb_en_a; - int32_t ret; - - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK); - ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_EMB_FUNC_EN_A, (uint8_t *)&emb_en_a, 1); - ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_EMB_FUNC_EN_B, (uint8_t *)&emb_en_b, 1); - if (ret != 0) { goto exit; } - - switch(val) - { - case LSM6DSV16X_MLC_OFF: - emb_en_a.mlc_before_fsm_en = 0; - emb_en_b.mlc_en = 0; - break; - case LSM6DSV16X_MLC_ON: - emb_en_a.mlc_before_fsm_en = 0; - emb_en_b.mlc_en = 1; - break; - case LSM6DSV16X_MLC_ON_BEFORE_FSM: - emb_en_a.mlc_before_fsm_en = 1; - emb_en_b.mlc_en = 0; - break; - default: - break; - } - - ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_EMB_FUNC_EN_A, (uint8_t *)&emb_en_a, 1); - ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_EMB_FUNC_EN_B, (uint8_t *)&emb_en_b, 1); - -exit: - ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); - - return ret; -} - -/** - * @brief It enables Machine Learning Core feature (MLC). When the Machine Learning Core is enabled the Finite State Machine (FSM) programs are executed before executing the MLC algorithms.[get] - * - * @param ctx read / write interface definitions - * @param val MLC_OFF, MLC_ON, MLC_BEFORE_FSM, - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6dsv16x_mlc_get(stmdev_ctx_t *ctx, lsm6dsv16x_mlc_mode_t *val) -{ - lsm6dsv16x_emb_func_en_b_t emb_en_b; - lsm6dsv16x_emb_func_en_a_t emb_en_a; - int32_t ret; - - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK); - ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_EMB_FUNC_EN_A, (uint8_t *)&emb_en_a, 1); - ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_EMB_FUNC_EN_B, (uint8_t *)&emb_en_b, 1); - if (ret != 0) { goto exit; } - - if (emb_en_a.mlc_before_fsm_en == 0U && emb_en_b.mlc_en == 0U) - { - *val = LSM6DSV16X_MLC_OFF; - } - else if (emb_en_a.mlc_before_fsm_en == 0U && emb_en_b.mlc_en == 1U) - { - *val = LSM6DSV16X_MLC_ON; - } - else if (emb_en_a.mlc_before_fsm_en == 1U) - { - *val = LSM6DSV16X_MLC_ON_BEFORE_FSM; - } - else - { - /* Do nothing */ - } - -exit: - ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); - - return ret; -} - -/** - * @brief Machine Learning Core Output Data Rate (ODR) configuration.[set] - * - * @param ctx read / write interface definitions - * @param val MLC_15Hz, MLC_30Hz, MLC_60Hz, MLC_120Hz, MLC_240Hz, MLC_480Hz, MLC_960Hz, - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6dsv16x_mlc_data_rate_set(stmdev_ctx_t *ctx, - lsm6dsv16x_mlc_data_rate_t val) -{ - lsm6dsv16x_mlc_odr_t mlc_odr; - int32_t ret; - - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK); - ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_MLC_ODR, (uint8_t *)&mlc_odr, 1); - if (ret != 0) { goto exit; } - - mlc_odr.mlc_odr = (uint8_t)val & 0x07U; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_MLC_ODR, (uint8_t *)&mlc_odr, 1); - -exit: - ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); - - return ret; -} - -/** - * @brief Machine Learning Core Output Data Rate (ODR) configuration.[get] - * - * @param ctx read / write interface definitions - * @param val MLC_15Hz, MLC_30Hz, MLC_60Hz, MLC_120Hz, MLC_240Hz, MLC_480Hz, MLC_960Hz, - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6dsv16x_mlc_data_rate_get(stmdev_ctx_t *ctx, - lsm6dsv16x_mlc_data_rate_t *val) -{ - lsm6dsv16x_mlc_odr_t mlc_odr; - int32_t ret; - - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK); - ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_MLC_ODR, (uint8_t *)&mlc_odr, 1); - ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); - if (ret != 0) { return ret; } - - switch (mlc_odr.mlc_odr) - { - case LSM6DSV16X_MLC_15Hz: - *val = LSM6DSV16X_MLC_15Hz; - break; - - case LSM6DSV16X_MLC_30Hz: - *val = LSM6DSV16X_MLC_30Hz; - break; - - case LSM6DSV16X_MLC_60Hz: - *val = LSM6DSV16X_MLC_60Hz; - break; - - case LSM6DSV16X_MLC_120Hz: - *val = LSM6DSV16X_MLC_120Hz; - break; - - case LSM6DSV16X_MLC_240Hz: - *val = LSM6DSV16X_MLC_240Hz; - break; - - case LSM6DSV16X_MLC_480Hz: - *val = LSM6DSV16X_MLC_480Hz; - break; - - case LSM6DSV16X_MLC_960Hz: - *val = LSM6DSV16X_MLC_960Hz; - break; - - default: - *val = LSM6DSV16X_MLC_15Hz; - break; - } - - return ret; -} - -/** - * @brief Output value of all MLC decision trees.[get] - * - * @param ctx read / write interface definitions - * @param val Output value of all MLC decision trees. - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6dsv16x_mlc_out_get(stmdev_ctx_t *ctx, lsm6dsv16x_mlc_out_t *val) -{ - int32_t ret; - - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK); - if (ret == 0) - { - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_MLC1_SRC, (uint8_t *)val, 4); - } - ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); - - return ret; -} - -/** - * @brief External sensor sensitivity value register for the Machine Learning Core. This register corresponds to the conversion value of the external sensor. The register value is expressed as half-precision floating-point format: SEEEEEFFFFFFFFFF (S: 1 sign bit; E: 5 exponent bits; F: 10 fraction bits).Default value is 0x3C00 (when using an external magnetometer this value corresponds to 1 gauss/LSB).[set] - * - * @param ctx read / write interface definitions - * @param val External sensor sensitivity value register for the Machine Learning Core. This register corresponds to the conversion value of the external sensor. The register value is expressed as half-precision floating-point format: SEEEEEFFFFFFFFFF (S: 1 sign bit; E: 5 exponent bits; F: 10 fraction bits).Default value is 0x3C00 (when using an external magnetometer this value corresponds to 1 gauss/LSB). - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6dsv16x_mlc_ext_sens_sensitivity_set(stmdev_ctx_t *ctx, uint16_t val) -{ - uint8_t buff[2]; - int32_t ret; - - buff[1] = (uint8_t)(val / 256U); - buff[0] = (uint8_t)(val - (buff[1] * 256U)); - - ret = lsm6dsv16x_ln_pg_write(ctx, LSM6DSV16X_EMB_ADV_PG_1 + LSM6DSV16X_MLC_EXT_SENSITIVITY_L, (uint8_t *)&buff[0], 2); - - return ret; -} - -/** - * @brief External sensor sensitivity value register for the Machine Learning Core. This register corresponds to the conversion value of the external sensor. The register value is expressed as half-precision floating-point format: SEEEEEFFFFFFFFFF (S: 1 sign bit; E: 5 exponent bits; F: 10 fraction bits).Default value is 0x3C00 (when using an external magnetometer this value corresponds to 1 gauss/LSB).[get] - * - * @param ctx read / write interface definitions - * @param val External sensor sensitivity value register for the Machine Learning Core. This register corresponds to the conversion value of the external sensor. The register value is expressed as half-precision floating-point format: SEEEEEFFFFFFFFFF (S: 1 sign bit; E: 5 exponent bits; F: 10 fraction bits).Default value is 0x3C00 (when using an external magnetometer this value corresponds to 1 gauss/LSB). - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6dsv16x_mlc_ext_sens_sensitivity_get(stmdev_ctx_t *ctx, - uint16_t *val) -{ - uint8_t buff[2]; - int32_t ret; - - ret = lsm6dsv16x_ln_pg_read(ctx, LSM6DSV16X_EMB_ADV_PG_1 + LSM6DSV16X_MLC_EXT_SENSITIVITY_L, &buff[0], 2); - if (ret != 0) { return ret; } - - *val = buff[1]; - *val = (*val * 256U) + buff[0]; - return ret; } @@ -6779,17 +6373,17 @@ int32_t lsm6dsv16x_mlc_ext_sens_sensitivity_get(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_ois_ctrl_mode_set(stmdev_ctx_t *ctx, - lsm6dsv16x_ois_ctrl_mode_t val) +int32_t lsm6dsv_ois_ctrl_mode_set(stmdev_ctx_t *ctx, + lsm6dsv_ois_ctrl_mode_t val) { - lsm6dsv16x_func_cfg_access_t func_cfg_access; + lsm6dsv_func_cfg_access_t func_cfg_access; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FUNC_CFG_ACCESS, (uint8_t *)&func_cfg_access, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_FUNC_CFG_ACCESS, (uint8_t *)&func_cfg_access, 1); if (ret == 0) { func_cfg_access.ois_ctrl_from_ui = (uint8_t)val & 0x1U; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_FUNC_CFG_ACCESS, (uint8_t *)&func_cfg_access, 1); + ret = lsm6dsv_write_reg(ctx, LSM6DSV_FUNC_CFG_ACCESS, (uint8_t *)&func_cfg_access, 1); } return ret; @@ -6803,27 +6397,27 @@ int32_t lsm6dsv16x_ois_ctrl_mode_set(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_ois_ctrl_mode_get(stmdev_ctx_t *ctx, - lsm6dsv16x_ois_ctrl_mode_t *val) +int32_t lsm6dsv_ois_ctrl_mode_get(stmdev_ctx_t *ctx, + lsm6dsv_ois_ctrl_mode_t *val) { - lsm6dsv16x_func_cfg_access_t func_cfg_access; + lsm6dsv_func_cfg_access_t func_cfg_access; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FUNC_CFG_ACCESS, (uint8_t *)&func_cfg_access, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_FUNC_CFG_ACCESS, (uint8_t *)&func_cfg_access, 1); if (ret != 0) { return ret; } switch (func_cfg_access.ois_ctrl_from_ui) { - case LSM6DSV16X_OIS_CTRL_FROM_OIS: - *val = LSM6DSV16X_OIS_CTRL_FROM_OIS; + case LSM6DSV_OIS_CTRL_FROM_OIS: + *val = LSM6DSV_OIS_CTRL_FROM_OIS; break; - case LSM6DSV16X_OIS_CTRL_FROM_UI: - *val = LSM6DSV16X_OIS_CTRL_FROM_UI; + case LSM6DSV_OIS_CTRL_FROM_UI: + *val = LSM6DSV_OIS_CTRL_FROM_UI; break; default: - *val = LSM6DSV16X_OIS_CTRL_FROM_OIS; + *val = LSM6DSV_OIS_CTRL_FROM_OIS; break; } @@ -6838,16 +6432,16 @@ int32_t lsm6dsv16x_ois_ctrl_mode_get(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_ois_reset_set(stmdev_ctx_t *ctx, int8_t val) +int32_t lsm6dsv_ois_reset_set(stmdev_ctx_t *ctx, int8_t val) { - lsm6dsv16x_func_cfg_access_t func_cfg_access; + lsm6dsv_func_cfg_access_t func_cfg_access; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FUNC_CFG_ACCESS, (uint8_t *)&func_cfg_access, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_FUNC_CFG_ACCESS, (uint8_t *)&func_cfg_access, 1); if (ret == 0) { func_cfg_access.spi2_reset = (uint8_t)val; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_FUNC_CFG_ACCESS, (uint8_t *)&func_cfg_access, 1); + ret = lsm6dsv_write_reg(ctx, LSM6DSV_FUNC_CFG_ACCESS, (uint8_t *)&func_cfg_access, 1); } return ret; @@ -6861,12 +6455,12 @@ int32_t lsm6dsv16x_ois_reset_set(stmdev_ctx_t *ctx, int8_t val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_ois_reset_get(stmdev_ctx_t *ctx, int8_t *val) +int32_t lsm6dsv_ois_reset_get(stmdev_ctx_t *ctx, int8_t *val) { - lsm6dsv16x_func_cfg_access_t func_cfg_access; + lsm6dsv_func_cfg_access_t func_cfg_access; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FUNC_CFG_ACCESS, (uint8_t *)&func_cfg_access, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_FUNC_CFG_ACCESS, (uint8_t *)&func_cfg_access, 1); *val = (int8_t)func_cfg_access.spi2_reset; return ret; @@ -6880,16 +6474,16 @@ int32_t lsm6dsv16x_ois_reset_get(stmdev_ctx_t *ctx, int8_t *val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_ois_interface_pull_up_set(stmdev_ctx_t *ctx, uint8_t val) +int32_t lsm6dsv_ois_interface_pull_up_set(stmdev_ctx_t *ctx, uint8_t val) { - lsm6dsv16x_pin_ctrl_t pin_ctrl; + lsm6dsv_pin_ctrl_t pin_ctrl; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_PIN_CTRL, (uint8_t *)&pin_ctrl, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_PIN_CTRL, (uint8_t *)&pin_ctrl, 1); if (ret == 0) { pin_ctrl.ois_pu_dis = val; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_PIN_CTRL, (uint8_t *)&pin_ctrl, 1); + ret = lsm6dsv_write_reg(ctx, LSM6DSV_PIN_CTRL, (uint8_t *)&pin_ctrl, 1); } return ret; @@ -6903,12 +6497,12 @@ int32_t lsm6dsv16x_ois_interface_pull_up_set(stmdev_ctx_t *ctx, uint8_t val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_ois_interface_pull_up_get(stmdev_ctx_t *ctx, uint8_t *val) +int32_t lsm6dsv_ois_interface_pull_up_get(stmdev_ctx_t *ctx, uint8_t *val) { - lsm6dsv16x_pin_ctrl_t pin_ctrl; + lsm6dsv_pin_ctrl_t pin_ctrl; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_PIN_CTRL, (uint8_t *)&pin_ctrl, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_PIN_CTRL, (uint8_t *)&pin_ctrl, 1); *val = pin_ctrl.ois_pu_dis; return ret; @@ -6922,18 +6516,18 @@ int32_t lsm6dsv16x_ois_interface_pull_up_get(stmdev_ctx_t *ctx, uint8_t *val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_ois_handshake_from_ui_set(stmdev_ctx_t *ctx, - lsm6dsv16x_ois_handshake_t val) +int32_t lsm6dsv_ois_handshake_from_ui_set(stmdev_ctx_t *ctx, + lsm6dsv_ois_handshake_t val) { - lsm6dsv16x_ui_handshake_ctrl_t ui_handshake_ctrl; + lsm6dsv_ui_handshake_ctrl_t ui_handshake_ctrl; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_UI_HANDSHAKE_CTRL, (uint8_t *)&ui_handshake_ctrl, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_UI_HANDSHAKE_CTRL, (uint8_t *)&ui_handshake_ctrl, 1); if (ret == 0) { ui_handshake_ctrl.ui_shared_ack = val.ack; ui_handshake_ctrl.ui_shared_req = val.req; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_UI_HANDSHAKE_CTRL, (uint8_t *)&ui_handshake_ctrl, 1); + ret = lsm6dsv_write_reg(ctx, LSM6DSV_UI_HANDSHAKE_CTRL, (uint8_t *)&ui_handshake_ctrl, 1); } return ret; @@ -6947,13 +6541,13 @@ int32_t lsm6dsv16x_ois_handshake_from_ui_set(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_ois_handshake_from_ui_get(stmdev_ctx_t *ctx, - lsm6dsv16x_ois_handshake_t *val) +int32_t lsm6dsv_ois_handshake_from_ui_get(stmdev_ctx_t *ctx, + lsm6dsv_ois_handshake_t *val) { - lsm6dsv16x_ui_handshake_ctrl_t ui_handshake_ctrl; + lsm6dsv_ui_handshake_ctrl_t ui_handshake_ctrl; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_UI_HANDSHAKE_CTRL, (uint8_t *)&ui_handshake_ctrl, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_UI_HANDSHAKE_CTRL, (uint8_t *)&ui_handshake_ctrl, 1); if (ret != 0) { return ret; } val->ack = ui_handshake_ctrl.ui_shared_ack; @@ -6970,18 +6564,18 @@ int32_t lsm6dsv16x_ois_handshake_from_ui_get(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_ois_handshake_from_ois_set(stmdev_ctx_t *ctx, - lsm6dsv16x_ois_handshake_t val) +int32_t lsm6dsv_ois_handshake_from_ois_set(stmdev_ctx_t *ctx, + lsm6dsv_ois_handshake_t val) { - lsm6dsv16x_spi2_handshake_ctrl_t spi2_handshake_ctrl; + lsm6dsv_spi2_handshake_ctrl_t spi2_handshake_ctrl; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_SPI2_HANDSHAKE_CTRL, (uint8_t *)&spi2_handshake_ctrl, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_SPI2_HANDSHAKE_CTRL, (uint8_t *)&spi2_handshake_ctrl, 1); if (ret == 0) { spi2_handshake_ctrl.spi2_shared_ack = val.ack; spi2_handshake_ctrl.spi2_shared_req = val.req; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_SPI2_HANDSHAKE_CTRL, (uint8_t *)&spi2_handshake_ctrl, 1); + ret = lsm6dsv_write_reg(ctx, LSM6DSV_SPI2_HANDSHAKE_CTRL, (uint8_t *)&spi2_handshake_ctrl, 1); } return ret; @@ -6995,13 +6589,13 @@ int32_t lsm6dsv16x_ois_handshake_from_ois_set(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_ois_handshake_from_ois_get(stmdev_ctx_t *ctx, - lsm6dsv16x_ois_handshake_t *val) +int32_t lsm6dsv_ois_handshake_from_ois_get(stmdev_ctx_t *ctx, + lsm6dsv_ois_handshake_t *val) { - lsm6dsv16x_spi2_handshake_ctrl_t spi2_handshake_ctrl; + lsm6dsv_spi2_handshake_ctrl_t spi2_handshake_ctrl; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_SPI2_HANDSHAKE_CTRL, (uint8_t *)&spi2_handshake_ctrl, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_SPI2_HANDSHAKE_CTRL, (uint8_t *)&spi2_handshake_ctrl, 1); if (ret != 0) { return ret; } val->ack = spi2_handshake_ctrl.spi2_shared_ack; @@ -7018,11 +6612,11 @@ int32_t lsm6dsv16x_ois_handshake_from_ois_get(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_ois_shared_set(stmdev_ctx_t *ctx, uint8_t val[6]) +int32_t lsm6dsv_ois_shared_set(stmdev_ctx_t *ctx, uint8_t val[6]) { int32_t ret; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_UI_SPI2_SHARED_0, val, 6); + ret = lsm6dsv_write_reg(ctx, LSM6DSV_UI_SPI2_SHARED_0, val, 6); return ret; } @@ -7035,11 +6629,11 @@ int32_t lsm6dsv16x_ois_shared_set(stmdev_ctx_t *ctx, uint8_t val[6]) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_ois_shared_get(stmdev_ctx_t *ctx, uint8_t val[6]) +int32_t lsm6dsv_ois_shared_get(stmdev_ctx_t *ctx, uint8_t val[6]) { int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_UI_SPI2_SHARED_0, val, 6); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_UI_SPI2_SHARED_0, val, 6); return ret; } @@ -7052,16 +6646,16 @@ int32_t lsm6dsv16x_ois_shared_get(stmdev_ctx_t *ctx, uint8_t val[6]) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_ois_on_spi2_set(stmdev_ctx_t *ctx, uint8_t val) +int32_t lsm6dsv_ois_on_spi2_set(stmdev_ctx_t *ctx, uint8_t val) { - lsm6dsv16x_ui_ctrl1_ois_t ui_ctrl1_ois; + lsm6dsv_ui_ctrl1_ois_t ui_ctrl1_ois; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_UI_CTRL1_OIS, (uint8_t *)&ui_ctrl1_ois, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_UI_CTRL1_OIS, (uint8_t *)&ui_ctrl1_ois, 1); if (ret == 0) { ui_ctrl1_ois.spi2_read_en = val; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_UI_CTRL1_OIS, (uint8_t *)&ui_ctrl1_ois, 1); + ret = lsm6dsv_write_reg(ctx, LSM6DSV_UI_CTRL1_OIS, (uint8_t *)&ui_ctrl1_ois, 1); } return ret; @@ -7075,12 +6669,12 @@ int32_t lsm6dsv16x_ois_on_spi2_set(stmdev_ctx_t *ctx, uint8_t val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_ois_on_spi2_get(stmdev_ctx_t *ctx, uint8_t *val) +int32_t lsm6dsv_ois_on_spi2_get(stmdev_ctx_t *ctx, uint8_t *val) { - lsm6dsv16x_ui_ctrl1_ois_t ui_ctrl1_ois; + lsm6dsv_ui_ctrl1_ois_t ui_ctrl1_ois; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_UI_CTRL1_OIS, (uint8_t *)&ui_ctrl1_ois, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_UI_CTRL1_OIS, (uint8_t *)&ui_ctrl1_ois, 1); *val = ui_ctrl1_ois.spi2_read_en; return ret; @@ -7094,17 +6688,17 @@ int32_t lsm6dsv16x_ois_on_spi2_get(stmdev_ctx_t *ctx, uint8_t *val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_ois_chain_set(stmdev_ctx_t *ctx, lsm6dsv16x_ois_chain_t val) +int32_t lsm6dsv_ois_chain_set(stmdev_ctx_t *ctx, lsm6dsv_ois_chain_t val) { - lsm6dsv16x_ui_ctrl1_ois_t ui_ctrl1_ois; + lsm6dsv_ui_ctrl1_ois_t ui_ctrl1_ois; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_UI_CTRL1_OIS, (uint8_t *)&ui_ctrl1_ois, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_UI_CTRL1_OIS, (uint8_t *)&ui_ctrl1_ois, 1); if (ret == 0) { ui_ctrl1_ois.ois_g_en = val.gy; ui_ctrl1_ois.ois_xl_en = val.xl; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_UI_CTRL1_OIS, (uint8_t *)&ui_ctrl1_ois, 1); + ret = lsm6dsv_write_reg(ctx, LSM6DSV_UI_CTRL1_OIS, (uint8_t *)&ui_ctrl1_ois, 1); } return ret; @@ -7118,12 +6712,12 @@ int32_t lsm6dsv16x_ois_chain_set(stmdev_ctx_t *ctx, lsm6dsv16x_ois_chain_t val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_ois_chain_get(stmdev_ctx_t *ctx, lsm6dsv16x_ois_chain_t *val) +int32_t lsm6dsv_ois_chain_get(stmdev_ctx_t *ctx, lsm6dsv_ois_chain_t *val) { - lsm6dsv16x_ui_ctrl1_ois_t ui_ctrl1_ois; + lsm6dsv_ui_ctrl1_ois_t ui_ctrl1_ois; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_UI_CTRL1_OIS, (uint8_t *)&ui_ctrl1_ois, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_UI_CTRL1_OIS, (uint8_t *)&ui_ctrl1_ois, 1); if (ret != 0) { return ret; } val->gy = ui_ctrl1_ois.ois_g_en; @@ -7140,17 +6734,17 @@ int32_t lsm6dsv16x_ois_chain_get(stmdev_ctx_t *ctx, lsm6dsv16x_ois_chain_t *val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_ois_gy_full_scale_set(stmdev_ctx_t *ctx, - lsm6dsv16x_ois_gy_full_scale_t val) +int32_t lsm6dsv_ois_gy_full_scale_set(stmdev_ctx_t *ctx, + lsm6dsv_ois_gy_full_scale_t val) { - lsm6dsv16x_ui_ctrl2_ois_t ui_ctrl2_ois; + lsm6dsv_ui_ctrl2_ois_t ui_ctrl2_ois; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_UI_CTRL2_OIS, (uint8_t *)&ui_ctrl2_ois, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_UI_CTRL2_OIS, (uint8_t *)&ui_ctrl2_ois, 1); if (ret == 0) { ui_ctrl2_ois.fs_g_ois = (uint8_t)val & 0x03U; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_UI_CTRL2_OIS, (uint8_t *)&ui_ctrl2_ois, 1); + ret = lsm6dsv_write_reg(ctx, LSM6DSV_UI_CTRL2_OIS, (uint8_t *)&ui_ctrl2_ois, 1); } return ret; @@ -7164,39 +6758,39 @@ int32_t lsm6dsv16x_ois_gy_full_scale_set(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_ois_gy_full_scale_get(stmdev_ctx_t *ctx, - lsm6dsv16x_ois_gy_full_scale_t *val) +int32_t lsm6dsv_ois_gy_full_scale_get(stmdev_ctx_t *ctx, + lsm6dsv_ois_gy_full_scale_t *val) { - lsm6dsv16x_ui_ctrl2_ois_t ui_ctrl2_ois; + lsm6dsv_ui_ctrl2_ois_t ui_ctrl2_ois; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_UI_CTRL2_OIS, (uint8_t *)&ui_ctrl2_ois, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_UI_CTRL2_OIS, (uint8_t *)&ui_ctrl2_ois, 1); if (ret != 0) { return ret; } switch (ui_ctrl2_ois.fs_g_ois) { - case LSM6DSV16X_OIS_125dps: - *val = LSM6DSV16X_OIS_125dps; + case LSM6DSV_OIS_125dps: + *val = LSM6DSV_OIS_125dps; break; - case LSM6DSV16X_OIS_250dps: - *val = LSM6DSV16X_OIS_250dps; + case LSM6DSV_OIS_250dps: + *val = LSM6DSV_OIS_250dps; break; - case LSM6DSV16X_OIS_500dps: - *val = LSM6DSV16X_OIS_500dps; + case LSM6DSV_OIS_500dps: + *val = LSM6DSV_OIS_500dps; break; - case LSM6DSV16X_OIS_1000dps: - *val = LSM6DSV16X_OIS_1000dps; + case LSM6DSV_OIS_1000dps: + *val = LSM6DSV_OIS_1000dps; break; - case LSM6DSV16X_OIS_2000dps: - *val = LSM6DSV16X_OIS_2000dps; + case LSM6DSV_OIS_2000dps: + *val = LSM6DSV_OIS_2000dps; break; default: - *val = LSM6DSV16X_OIS_125dps; + *val = LSM6DSV_OIS_125dps; break; } @@ -7211,17 +6805,17 @@ int32_t lsm6dsv16x_ois_gy_full_scale_get(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_ois_xl_full_scale_set(stmdev_ctx_t *ctx, - lsm6dsv16x_ois_xl_full_scale_t val) +int32_t lsm6dsv_ois_xl_full_scale_set(stmdev_ctx_t *ctx, + lsm6dsv_ois_xl_full_scale_t val) { - lsm6dsv16x_ui_ctrl3_ois_t ui_ctrl3_ois; + lsm6dsv_ui_ctrl3_ois_t ui_ctrl3_ois; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_UI_CTRL3_OIS, (uint8_t *)&ui_ctrl3_ois, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_UI_CTRL3_OIS, (uint8_t *)&ui_ctrl3_ois, 1); if (ret == 0) { ui_ctrl3_ois.fs_xl_ois = (uint8_t)val & 0x3U; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_UI_CTRL3_OIS, (uint8_t *)&ui_ctrl3_ois, 1); + ret = lsm6dsv_write_reg(ctx, LSM6DSV_UI_CTRL3_OIS, (uint8_t *)&ui_ctrl3_ois, 1); } return ret; @@ -7235,35 +6829,35 @@ int32_t lsm6dsv16x_ois_xl_full_scale_set(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_ois_xl_full_scale_get(stmdev_ctx_t *ctx, - lsm6dsv16x_ois_xl_full_scale_t *val) +int32_t lsm6dsv_ois_xl_full_scale_get(stmdev_ctx_t *ctx, + lsm6dsv_ois_xl_full_scale_t *val) { - lsm6dsv16x_ui_ctrl3_ois_t ui_ctrl3_ois; + lsm6dsv_ui_ctrl3_ois_t ui_ctrl3_ois; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_UI_CTRL3_OIS, (uint8_t *)&ui_ctrl3_ois, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_UI_CTRL3_OIS, (uint8_t *)&ui_ctrl3_ois, 1); if (ret != 0) { return ret; } switch (ui_ctrl3_ois.fs_xl_ois) { - case LSM6DSV16X_OIS_2g: - *val = LSM6DSV16X_OIS_2g; + case LSM6DSV_OIS_2g: + *val = LSM6DSV_OIS_2g; break; - case LSM6DSV16X_OIS_4g: - *val = LSM6DSV16X_OIS_4g; + case LSM6DSV_OIS_4g: + *val = LSM6DSV_OIS_4g; break; - case LSM6DSV16X_OIS_8g: - *val = LSM6DSV16X_OIS_8g; + case LSM6DSV_OIS_8g: + *val = LSM6DSV_OIS_8g; break; - case LSM6DSV16X_OIS_16g: - *val = LSM6DSV16X_OIS_16g; + case LSM6DSV_OIS_16g: + *val = LSM6DSV_OIS_16g; break; default: - *val = LSM6DSV16X_OIS_2g; + *val = LSM6DSV_OIS_2g; break; } @@ -7291,17 +6885,16 @@ int32_t lsm6dsv16x_ois_xl_full_scale_get(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_6d_threshold_set(stmdev_ctx_t *ctx, - lsm6dsv16x_6d_threshold_t val) +int32_t lsm6dsv_6d_threshold_set(stmdev_ctx_t *ctx, lsm6dsv_6d_threshold_t val) { - lsm6dsv16x_tap_ths_6d_t tap_ths_6d; + lsm6dsv_tap_ths_6d_t tap_ths_6d; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_TAP_THS_6D, (uint8_t *)&tap_ths_6d, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_TAP_THS_6D, (uint8_t *)&tap_ths_6d, 1); if (ret == 0) { tap_ths_6d.sixd_ths = (uint8_t)val & 0x03U; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_TAP_THS_6D, (uint8_t *)&tap_ths_6d, 1); + ret = lsm6dsv_write_reg(ctx, LSM6DSV_TAP_THS_6D, (uint8_t *)&tap_ths_6d, 1); } return ret; @@ -7315,35 +6908,34 @@ int32_t lsm6dsv16x_6d_threshold_set(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_6d_threshold_get(stmdev_ctx_t *ctx, - lsm6dsv16x_6d_threshold_t *val) +int32_t lsm6dsv_6d_threshold_get(stmdev_ctx_t *ctx, lsm6dsv_6d_threshold_t *val) { - lsm6dsv16x_tap_ths_6d_t tap_ths_6d; + lsm6dsv_tap_ths_6d_t tap_ths_6d; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_TAP_THS_6D, (uint8_t *)&tap_ths_6d, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_TAP_THS_6D, (uint8_t *)&tap_ths_6d, 1); if (ret != 0) { return ret; } switch (tap_ths_6d.sixd_ths) { - case LSM6DSV16X_DEG_80: - *val = LSM6DSV16X_DEG_80; + case LSM6DSV_DEG_80: + *val = LSM6DSV_DEG_80; break; - case LSM6DSV16X_DEG_70: - *val = LSM6DSV16X_DEG_70; + case LSM6DSV_DEG_70: + *val = LSM6DSV_DEG_70; break; - case LSM6DSV16X_DEG_60: - *val = LSM6DSV16X_DEG_60; + case LSM6DSV_DEG_60: + *val = LSM6DSV_DEG_60; break; - case LSM6DSV16X_DEG_50: - *val = LSM6DSV16X_DEG_50; + case LSM6DSV_DEG_50: + *val = LSM6DSV_DEG_50; break; default: - *val = LSM6DSV16X_DEG_80; + *val = LSM6DSV_DEG_80; break; } @@ -7358,16 +6950,16 @@ int32_t lsm6dsv16x_6d_threshold_get(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_4d_mode_set(stmdev_ctx_t *ctx, uint8_t val) +int32_t lsm6dsv_4d_mode_set(stmdev_ctx_t *ctx, uint8_t val) { - lsm6dsv16x_tap_ths_6d_t tap_ths_6d; + lsm6dsv_tap_ths_6d_t tap_ths_6d; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_TAP_THS_6D, (uint8_t *)&tap_ths_6d, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_TAP_THS_6D, (uint8_t *)&tap_ths_6d, 1); if (ret == 0) { tap_ths_6d.d4d_en = val; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_TAP_THS_6D, (uint8_t *)&tap_ths_6d, 1); + ret = lsm6dsv_write_reg(ctx, LSM6DSV_TAP_THS_6D, (uint8_t *)&tap_ths_6d, 1); } return ret; @@ -7381,141 +6973,17 @@ int32_t lsm6dsv16x_4d_mode_set(stmdev_ctx_t *ctx, uint8_t val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_4d_mode_get(stmdev_ctx_t *ctx, uint8_t *val) +int32_t lsm6dsv_4d_mode_get(stmdev_ctx_t *ctx, uint8_t *val) { - lsm6dsv16x_tap_ths_6d_t tap_ths_6d; + lsm6dsv_tap_ths_6d_t tap_ths_6d; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_TAP_THS_6D, (uint8_t *)&tap_ths_6d, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_TAP_THS_6D, (uint8_t *)&tap_ths_6d, 1); *val = tap_ths_6d.d4d_en; return ret; } -/** - * @} - * - */ - -/** - * @defgroup AH_QVAR - * @brief This section group all the functions concerning the - * usage of AH_QVAR - * @{ - * - */ - -/** - * @brief Configures the equivalent input impedance of the AH_QVAR buffers.[set] - * - * @param ctx read / write interface definitions - * @param val 2400MOhm, 730MOhm, 300MOhm, 255MOhm, - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6dsv16x_ah_qvar_zin_set(stmdev_ctx_t *ctx, - lsm6dsv16x_ah_qvar_zin_t val) -{ - lsm6dsv16x_ctrl7_t ctrl7; - int32_t ret; - - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL7, (uint8_t *)&ctrl7, 1); - if (ret == 0) - { - ctrl7.ah_qvar_c_zin = (uint8_t)val & 0x03U; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_CTRL7, (uint8_t *)&ctrl7, 1); - } - - return ret; -} - -/** - * @brief Configures the equivalent input impedance of the AH_QVAR buffers.[get] - * - * @param ctx read / write interface definitions - * @param val 2400MOhm, 730MOhm, 300MOhm, 255MOhm, - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6dsv16x_ah_qvar_zin_get(stmdev_ctx_t *ctx, - lsm6dsv16x_ah_qvar_zin_t *val) -{ - lsm6dsv16x_ctrl7_t ctrl7; - int32_t ret; - - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL7, (uint8_t *)&ctrl7, 1); - if (ret != 0) { return ret; } - - switch (ctrl7.ah_qvar_c_zin) - { - case LSM6DSV16X_2400MOhm: - *val = LSM6DSV16X_2400MOhm; - break; - - case LSM6DSV16X_730MOhm: - *val = LSM6DSV16X_730MOhm; - break; - - case LSM6DSV16X_300MOhm: - *val = LSM6DSV16X_300MOhm; - break; - - case LSM6DSV16X_255MOhm: - *val = LSM6DSV16X_255MOhm; - break; - - default: - *val = LSM6DSV16X_2400MOhm; - break; - } - - return ret; -} - -/** - * @brief Enables AH_QVAR chain. When this bit is set to ‘1’, the AH_QVAR buffers are connected to the SDx/AH1/Qvar1 and SCx/AH2/Qvar2 pins. Before setting this bit to 1, the accelerometer and gyroscope sensor have to be configured in power-down mode.[set] - * - * @param ctx read / write interface definitions - * @param val Enables AH_QVAR chain. When this bit is set to ‘1’, the AH_QVAR buffers are connected to the SDx/AH1/Qvar1 and SCx/AH2/Qvar2 pins. Before setting this bit to 1, the accelerometer and gyroscope sensor have to be configured in power-down mode. - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6dsv16x_ah_qvar_mode_set(stmdev_ctx_t *ctx, - lsm6dsv16x_ah_qvar_mode_t val) -{ - lsm6dsv16x_ctrl7_t ctrl7; - int32_t ret; - - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL7, (uint8_t *)&ctrl7, 1); - if (ret == 0) - { - ctrl7.ah_qvar_en = val.ah_qvar_en; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_CTRL7, (uint8_t *)&ctrl7, 1); - } - - return ret; -} - -/** - * @brief Enables AH_QVAR chain. When this bit is set to ‘1’, the AH_QVAR buffers are connected to the SDx/AH1/Qvar1 and SCx/AH2/Qvar2 pins. Before setting this bit to 1, the accelerometer and gyroscope sensor have to be configured in power-down mode.[get] - * - * @param ctx read / write interface definitions - * @param val Enables AH_QVAR chain. When this bit is set to ‘1’, the AH_QVAR buffers are connected to the SDx/AH1/Qvar1 and SCx/AH2/Qvar2 pins. Before setting this bit to 1, the accelerometer and gyroscope sensor have to be configured in power-down mode. - * @retval interface status (MANDATORY: return 0 -> no Error) - * - */ -int32_t lsm6dsv16x_ah_qvar_mode_get(stmdev_ctx_t *ctx, - lsm6dsv16x_ah_qvar_mode_t *val) -{ - lsm6dsv16x_ctrl7_t ctrl7; - int32_t ret; - - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL7, (uint8_t *)&ctrl7, 1); - val->ah_qvar_en = ctrl7.ah_qvar_en; - - return ret; -} - /** * @} * @@ -7537,17 +7005,17 @@ int32_t lsm6dsv16x_ah_qvar_mode_get(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_i3c_reset_mode_set(stmdev_ctx_t *ctx, - lsm6dsv16x_i3c_reset_mode_t val) +int32_t lsm6dsv_i3c_reset_mode_set(stmdev_ctx_t *ctx, + lsm6dsv_i3c_reset_mode_t val) { - lsm6dsv16x_pin_ctrl_t pin_ctrl; + lsm6dsv_pin_ctrl_t pin_ctrl; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_PIN_CTRL, (uint8_t *)&pin_ctrl, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_PIN_CTRL, (uint8_t *)&pin_ctrl, 1); if (ret == 0) { pin_ctrl.ibhr_por_en = (uint8_t)val & 0x01U; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_PIN_CTRL, (uint8_t *)&pin_ctrl, 1); + ret = lsm6dsv_write_reg(ctx, LSM6DSV_PIN_CTRL, (uint8_t *)&pin_ctrl, 1); } return ret; @@ -7561,27 +7029,27 @@ int32_t lsm6dsv16x_i3c_reset_mode_set(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_i3c_reset_mode_get(stmdev_ctx_t *ctx, - lsm6dsv16x_i3c_reset_mode_t *val) +int32_t lsm6dsv_i3c_reset_mode_get(stmdev_ctx_t *ctx, + lsm6dsv_i3c_reset_mode_t *val) { - lsm6dsv16x_pin_ctrl_t pin_ctrl; + lsm6dsv_pin_ctrl_t pin_ctrl; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_PIN_CTRL, (uint8_t *)&pin_ctrl, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_PIN_CTRL, (uint8_t *)&pin_ctrl, 1); if (ret != 0) { return ret; } switch (pin_ctrl.ibhr_por_en) { - case LSM6DSV16X_SW_RST_DYN_ADDRESS_RST: - *val = LSM6DSV16X_SW_RST_DYN_ADDRESS_RST; + case LSM6DSV_SW_RST_DYN_ADDRESS_RST: + *val = LSM6DSV_SW_RST_DYN_ADDRESS_RST; break; - case LSM6DSV16X_I3C_GLOBAL_RST: - *val = LSM6DSV16X_I3C_GLOBAL_RST; + case LSM6DSV_I3C_GLOBAL_RST: + *val = LSM6DSV_I3C_GLOBAL_RST; break; default: - *val = LSM6DSV16X_SW_RST_DYN_ADDRESS_RST; + *val = LSM6DSV_SW_RST_DYN_ADDRESS_RST; break; } @@ -7596,17 +7064,16 @@ int32_t lsm6dsv16x_i3c_reset_mode_get(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_i3c_ibi_time_set(stmdev_ctx_t *ctx, - lsm6dsv16x_i3c_ibi_time_t val) +int32_t lsm6dsv_i3c_ibi_time_set(stmdev_ctx_t *ctx, lsm6dsv_i3c_ibi_time_t val) { - lsm6dsv16x_ctrl5_t ctrl5; + lsm6dsv_ctrl5_t ctrl5; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL5, (uint8_t *)&ctrl5, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_CTRL5, (uint8_t *)&ctrl5, 1); if (ret == 0) { ctrl5.bus_act_sel = (uint8_t)val & 0x03U; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_CTRL5, (uint8_t *)&ctrl5, 1); + ret = lsm6dsv_write_reg(ctx, LSM6DSV_CTRL5, (uint8_t *)&ctrl5, 1); } return ret; @@ -7620,35 +7087,34 @@ int32_t lsm6dsv16x_i3c_ibi_time_set(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_i3c_ibi_time_get(stmdev_ctx_t *ctx, - lsm6dsv16x_i3c_ibi_time_t *val) +int32_t lsm6dsv_i3c_ibi_time_get(stmdev_ctx_t *ctx, lsm6dsv_i3c_ibi_time_t *val) { - lsm6dsv16x_ctrl5_t ctrl5; + lsm6dsv_ctrl5_t ctrl5; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_CTRL5, (uint8_t *)&ctrl5, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_CTRL5, (uint8_t *)&ctrl5, 1); if (ret != 0) { return ret; } switch (ctrl5.bus_act_sel) { - case LSM6DSV16X_IBI_2us: - *val = LSM6DSV16X_IBI_2us; + case LSM6DSV_IBI_2us: + *val = LSM6DSV_IBI_2us; break; - case LSM6DSV16X_IBI_50us: - *val = LSM6DSV16X_IBI_50us; + case LSM6DSV_IBI_50us: + *val = LSM6DSV_IBI_50us; break; - case LSM6DSV16X_IBI_1ms: - *val = LSM6DSV16X_IBI_1ms; + case LSM6DSV_IBI_1ms: + *val = LSM6DSV_IBI_1ms; break; - case LSM6DSV16X_IBI_25ms: - *val = LSM6DSV16X_IBI_25ms; + case LSM6DSV_IBI_25ms: + *val = LSM6DSV_IBI_25ms; break; default: - *val = LSM6DSV16X_IBI_2us; + *val = LSM6DSV_IBI_2us; break; } @@ -7676,17 +7142,16 @@ int32_t lsm6dsv16x_i3c_ibi_time_get(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_sh_master_interface_pull_up_set(stmdev_ctx_t *ctx, - uint8_t val) +int32_t lsm6dsv_sh_master_interface_pull_up_set(stmdev_ctx_t *ctx, uint8_t val) { - lsm6dsv16x_if_cfg_t if_cfg; + lsm6dsv_if_cfg_t if_cfg; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_IF_CFG, (uint8_t *)&if_cfg, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_IF_CFG, (uint8_t *)&if_cfg, 1); if (ret == 0) { if_cfg.shub_pu_en = val; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_IF_CFG, (uint8_t *)&if_cfg, 1); + ret = lsm6dsv_write_reg(ctx, LSM6DSV_IF_CFG, (uint8_t *)&if_cfg, 1); } return ret; @@ -7700,13 +7165,12 @@ int32_t lsm6dsv16x_sh_master_interface_pull_up_set(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_sh_master_interface_pull_up_get(stmdev_ctx_t *ctx, - uint8_t *val) +int32_t lsm6dsv_sh_master_interface_pull_up_get(stmdev_ctx_t *ctx, uint8_t *val) { - lsm6dsv16x_if_cfg_t if_cfg; + lsm6dsv_if_cfg_t if_cfg; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_IF_CFG, (uint8_t *)&if_cfg, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_IF_CFG, (uint8_t *)&if_cfg, 1); *val = if_cfg.shub_pu_en; return ret; @@ -7720,14 +7184,14 @@ int32_t lsm6dsv16x_sh_master_interface_pull_up_get(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_sh_read_data_raw_get(stmdev_ctx_t *ctx, uint8_t *val, - uint8_t len) +int32_t lsm6dsv_sh_read_data_raw_get(stmdev_ctx_t *ctx, uint8_t *val, + uint8_t len) { int32_t ret; - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_SENSOR_HUB_MEM_BANK); - ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_SENSOR_HUB_1, val, len); - ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + ret = lsm6dsv_mem_bank_set(ctx, LSM6DSV_SENSOR_HUB_MEM_BANK); + ret += lsm6dsv_read_reg(ctx, LSM6DSV_SENSOR_HUB_1, val, len); + ret += lsm6dsv_mem_bank_set(ctx, LSM6DSV_MAIN_MEM_BANK); return ret; } @@ -7740,21 +7204,21 @@ int32_t lsm6dsv16x_sh_read_data_raw_get(stmdev_ctx_t *ctx, uint8_t *val, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_sh_slave_connected_set(stmdev_ctx_t *ctx, - lsm6dsv16x_sh_slave_connected_t val) +int32_t lsm6dsv_sh_slave_connected_set(stmdev_ctx_t *ctx, + lsm6dsv_sh_slave_connected_t val) { - lsm6dsv16x_master_config_t master_config; + lsm6dsv_master_config_t master_config; int32_t ret; - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_SENSOR_HUB_MEM_BANK); - ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_MASTER_CONFIG, (uint8_t *)&master_config, 1); + ret = lsm6dsv_mem_bank_set(ctx, LSM6DSV_SENSOR_HUB_MEM_BANK); + ret += lsm6dsv_read_reg(ctx, LSM6DSV_MASTER_CONFIG, (uint8_t *)&master_config, 1); if (ret != 0) { goto exit; } master_config.aux_sens_on = (uint8_t)val & 0x3U; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_MASTER_CONFIG, (uint8_t *)&master_config, 1); + ret = lsm6dsv_write_reg(ctx, LSM6DSV_MASTER_CONFIG, (uint8_t *)&master_config, 1); exit: - ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + ret += lsm6dsv_mem_bank_set(ctx, LSM6DSV_MAIN_MEM_BANK); return ret; } @@ -7767,37 +7231,37 @@ int32_t lsm6dsv16x_sh_slave_connected_set(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_sh_slave_connected_get(stmdev_ctx_t *ctx, - lsm6dsv16x_sh_slave_connected_t *val) +int32_t lsm6dsv_sh_slave_connected_get(stmdev_ctx_t *ctx, + lsm6dsv_sh_slave_connected_t *val) { - lsm6dsv16x_master_config_t master_config; + lsm6dsv_master_config_t master_config; int32_t ret; - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_SENSOR_HUB_MEM_BANK); - ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_MASTER_CONFIG, (uint8_t *)&master_config, 1); - ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + ret = lsm6dsv_mem_bank_set(ctx, LSM6DSV_SENSOR_HUB_MEM_BANK); + ret += lsm6dsv_read_reg(ctx, LSM6DSV_MASTER_CONFIG, (uint8_t *)&master_config, 1); + ret += lsm6dsv_mem_bank_set(ctx, LSM6DSV_MAIN_MEM_BANK); if (ret != 0) { return ret; } switch (master_config.aux_sens_on) { - case LSM6DSV16X_SLV_0: - *val = LSM6DSV16X_SLV_0; + case LSM6DSV_SLV_0: + *val = LSM6DSV_SLV_0; break; - case LSM6DSV16X_SLV_0_1: - *val = LSM6DSV16X_SLV_0_1; + case LSM6DSV_SLV_0_1: + *val = LSM6DSV_SLV_0_1; break; - case LSM6DSV16X_SLV_0_1_2: - *val = LSM6DSV16X_SLV_0_1_2; + case LSM6DSV_SLV_0_1_2: + *val = LSM6DSV_SLV_0_1_2; break; - case LSM6DSV16X_SLV_0_1_2_3: - *val = LSM6DSV16X_SLV_0_1_2_3; + case LSM6DSV_SLV_0_1_2_3: + *val = LSM6DSV_SLV_0_1_2_3; break; default: - *val = LSM6DSV16X_SLV_0; + *val = LSM6DSV_SLV_0; break; } @@ -7812,20 +7276,20 @@ int32_t lsm6dsv16x_sh_slave_connected_get(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_sh_master_set(stmdev_ctx_t *ctx, uint8_t val) +int32_t lsm6dsv_sh_master_set(stmdev_ctx_t *ctx, uint8_t val) { - lsm6dsv16x_master_config_t master_config; + lsm6dsv_master_config_t master_config; int32_t ret; - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_SENSOR_HUB_MEM_BANK); - ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_MASTER_CONFIG, (uint8_t *)&master_config, 1); + ret = lsm6dsv_mem_bank_set(ctx, LSM6DSV_SENSOR_HUB_MEM_BANK); + ret += lsm6dsv_read_reg(ctx, LSM6DSV_MASTER_CONFIG, (uint8_t *)&master_config, 1); if (ret != 0) { goto exit; } master_config.master_on = val; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_MASTER_CONFIG, (uint8_t *)&master_config, 1); + ret = lsm6dsv_write_reg(ctx, LSM6DSV_MASTER_CONFIG, (uint8_t *)&master_config, 1); exit: - ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + ret += lsm6dsv_mem_bank_set(ctx, LSM6DSV_MAIN_MEM_BANK); return ret; } @@ -7838,17 +7302,17 @@ int32_t lsm6dsv16x_sh_master_set(stmdev_ctx_t *ctx, uint8_t val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_sh_master_get(stmdev_ctx_t *ctx, uint8_t *val) +int32_t lsm6dsv_sh_master_get(stmdev_ctx_t *ctx, uint8_t *val) { - lsm6dsv16x_master_config_t master_config; + lsm6dsv_master_config_t master_config; int32_t ret; - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_SENSOR_HUB_MEM_BANK); - ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_MASTER_CONFIG, (uint8_t *)&master_config, 1); + ret = lsm6dsv_mem_bank_set(ctx, LSM6DSV_SENSOR_HUB_MEM_BANK); + ret += lsm6dsv_read_reg(ctx, LSM6DSV_MASTER_CONFIG, (uint8_t *)&master_config, 1); *val = master_config.master_on; - ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + ret += lsm6dsv_mem_bank_set(ctx, LSM6DSV_MAIN_MEM_BANK); return ret; } @@ -7861,20 +7325,20 @@ int32_t lsm6dsv16x_sh_master_get(stmdev_ctx_t *ctx, uint8_t *val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_sh_pass_through_set(stmdev_ctx_t *ctx, uint8_t val) +int32_t lsm6dsv_sh_pass_through_set(stmdev_ctx_t *ctx, uint8_t val) { - lsm6dsv16x_master_config_t master_config; + lsm6dsv_master_config_t master_config; int32_t ret; - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_SENSOR_HUB_MEM_BANK); - ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_MASTER_CONFIG, (uint8_t *)&master_config, 1); + ret = lsm6dsv_mem_bank_set(ctx, LSM6DSV_SENSOR_HUB_MEM_BANK); + ret += lsm6dsv_read_reg(ctx, LSM6DSV_MASTER_CONFIG, (uint8_t *)&master_config, 1); if (ret != 0) { goto exit; } master_config.pass_through_mode = val; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_MASTER_CONFIG, (uint8_t *)&master_config, 1); + ret = lsm6dsv_write_reg(ctx, LSM6DSV_MASTER_CONFIG, (uint8_t *)&master_config, 1); exit: - ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + ret += lsm6dsv_mem_bank_set(ctx, LSM6DSV_MAIN_MEM_BANK); return ret; } @@ -7887,17 +7351,17 @@ int32_t lsm6dsv16x_sh_pass_through_set(stmdev_ctx_t *ctx, uint8_t val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_sh_pass_through_get(stmdev_ctx_t *ctx, uint8_t *val) +int32_t lsm6dsv_sh_pass_through_get(stmdev_ctx_t *ctx, uint8_t *val) { - lsm6dsv16x_master_config_t master_config; + lsm6dsv_master_config_t master_config; int32_t ret; - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_SENSOR_HUB_MEM_BANK); - ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_MASTER_CONFIG, (uint8_t *)&master_config, 1); + ret = lsm6dsv_mem_bank_set(ctx, LSM6DSV_SENSOR_HUB_MEM_BANK); + ret += lsm6dsv_read_reg(ctx, LSM6DSV_MASTER_CONFIG, (uint8_t *)&master_config, 1); *val = master_config.pass_through_mode; - ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + ret += lsm6dsv_mem_bank_set(ctx, LSM6DSV_MAIN_MEM_BANK); return ret; } @@ -7910,21 +7374,21 @@ int32_t lsm6dsv16x_sh_pass_through_get(stmdev_ctx_t *ctx, uint8_t *val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_sh_syncro_mode_set(stmdev_ctx_t *ctx, - lsm6dsv16x_sh_syncro_mode_t val) +int32_t lsm6dsv_sh_syncro_mode_set(stmdev_ctx_t *ctx, + lsm6dsv_sh_syncro_mode_t val) { - lsm6dsv16x_master_config_t master_config; + lsm6dsv_master_config_t master_config; int32_t ret; - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_SENSOR_HUB_MEM_BANK); - ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_MASTER_CONFIG, (uint8_t *)&master_config, 1); + ret = lsm6dsv_mem_bank_set(ctx, LSM6DSV_SENSOR_HUB_MEM_BANK); + ret += lsm6dsv_read_reg(ctx, LSM6DSV_MASTER_CONFIG, (uint8_t *)&master_config, 1); if (ret != 0) { goto exit; } master_config.start_config = (uint8_t)val & 0x01U; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_MASTER_CONFIG, (uint8_t *)&master_config, 1); + ret = lsm6dsv_write_reg(ctx, LSM6DSV_MASTER_CONFIG, (uint8_t *)&master_config, 1); exit: - ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + ret += lsm6dsv_mem_bank_set(ctx, LSM6DSV_MAIN_MEM_BANK); return ret; } @@ -7937,29 +7401,29 @@ int32_t lsm6dsv16x_sh_syncro_mode_set(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_sh_syncro_mode_get(stmdev_ctx_t *ctx, - lsm6dsv16x_sh_syncro_mode_t *val) +int32_t lsm6dsv_sh_syncro_mode_get(stmdev_ctx_t *ctx, + lsm6dsv_sh_syncro_mode_t *val) { - lsm6dsv16x_master_config_t master_config; + lsm6dsv_master_config_t master_config; int32_t ret; - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_SENSOR_HUB_MEM_BANK); - ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_MASTER_CONFIG, (uint8_t *)&master_config, 1); - ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + ret = lsm6dsv_mem_bank_set(ctx, LSM6DSV_SENSOR_HUB_MEM_BANK); + ret += lsm6dsv_read_reg(ctx, LSM6DSV_MASTER_CONFIG, (uint8_t *)&master_config, 1); + ret += lsm6dsv_mem_bank_set(ctx, LSM6DSV_MAIN_MEM_BANK); if (ret != 0) { return ret; } switch (master_config.start_config) { - case LSM6DSV16X_SH_TRG_XL_GY_DRDY: - *val = LSM6DSV16X_SH_TRG_XL_GY_DRDY; + case LSM6DSV_SH_TRG_XL_GY_DRDY: + *val = LSM6DSV_SH_TRG_XL_GY_DRDY; break; - case LSM6DSV16X_SH_TRIG_INT2: - *val = LSM6DSV16X_SH_TRIG_INT2; + case LSM6DSV_SH_TRIG_INT2: + *val = LSM6DSV_SH_TRIG_INT2; break; default: - *val = LSM6DSV16X_SH_TRG_XL_GY_DRDY; + *val = LSM6DSV_SH_TRG_XL_GY_DRDY; break; } @@ -7974,21 +7438,21 @@ int32_t lsm6dsv16x_sh_syncro_mode_get(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_sh_write_mode_set(stmdev_ctx_t *ctx, - lsm6dsv16x_sh_write_mode_t val) +int32_t lsm6dsv_sh_write_mode_set(stmdev_ctx_t *ctx, + lsm6dsv_sh_write_mode_t val) { - lsm6dsv16x_master_config_t master_config; + lsm6dsv_master_config_t master_config; int32_t ret; - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_SENSOR_HUB_MEM_BANK); - ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_MASTER_CONFIG, (uint8_t *)&master_config, 1); + ret = lsm6dsv_mem_bank_set(ctx, LSM6DSV_SENSOR_HUB_MEM_BANK); + ret += lsm6dsv_read_reg(ctx, LSM6DSV_MASTER_CONFIG, (uint8_t *)&master_config, 1); if (ret != 0) { goto exit; } master_config.write_once = (uint8_t)val & 0x01U; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_MASTER_CONFIG, (uint8_t *)&master_config, 1); + ret = lsm6dsv_write_reg(ctx, LSM6DSV_MASTER_CONFIG, (uint8_t *)&master_config, 1); exit: - ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + ret += lsm6dsv_mem_bank_set(ctx, LSM6DSV_MAIN_MEM_BANK); return ret; } @@ -8001,29 +7465,29 @@ int32_t lsm6dsv16x_sh_write_mode_set(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_sh_write_mode_get(stmdev_ctx_t *ctx, - lsm6dsv16x_sh_write_mode_t *val) +int32_t lsm6dsv_sh_write_mode_get(stmdev_ctx_t *ctx, + lsm6dsv_sh_write_mode_t *val) { - lsm6dsv16x_master_config_t master_config; + lsm6dsv_master_config_t master_config; int32_t ret; - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_SENSOR_HUB_MEM_BANK); - ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_MASTER_CONFIG, (uint8_t *)&master_config, 1); - ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + ret = lsm6dsv_mem_bank_set(ctx, LSM6DSV_SENSOR_HUB_MEM_BANK); + ret += lsm6dsv_read_reg(ctx, LSM6DSV_MASTER_CONFIG, (uint8_t *)&master_config, 1); + ret += lsm6dsv_mem_bank_set(ctx, LSM6DSV_MAIN_MEM_BANK); if (ret != 0) { return ret; } switch (master_config.write_once) { - case LSM6DSV16X_EACH_SH_CYCLE: - *val = LSM6DSV16X_EACH_SH_CYCLE; + case LSM6DSV_EACH_SH_CYCLE: + *val = LSM6DSV_EACH_SH_CYCLE; break; - case LSM6DSV16X_ONLY_FIRST_CYCLE: - *val = LSM6DSV16X_ONLY_FIRST_CYCLE; + case LSM6DSV_ONLY_FIRST_CYCLE: + *val = LSM6DSV_ONLY_FIRST_CYCLE; break; default: - *val = LSM6DSV16X_EACH_SH_CYCLE; + *val = LSM6DSV_EACH_SH_CYCLE; break; } @@ -8038,20 +7502,20 @@ int32_t lsm6dsv16x_sh_write_mode_get(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_sh_reset_set(stmdev_ctx_t *ctx, uint8_t val) +int32_t lsm6dsv_sh_reset_set(stmdev_ctx_t *ctx, uint8_t val) { - lsm6dsv16x_master_config_t master_config; + lsm6dsv_master_config_t master_config; int32_t ret; - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_SENSOR_HUB_MEM_BANK); - ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_MASTER_CONFIG, (uint8_t *)&master_config, 1); + ret = lsm6dsv_mem_bank_set(ctx, LSM6DSV_SENSOR_HUB_MEM_BANK); + ret += lsm6dsv_read_reg(ctx, LSM6DSV_MASTER_CONFIG, (uint8_t *)&master_config, 1); if (ret != 0) { goto exit; } master_config.rst_master_regs = val; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_MASTER_CONFIG, (uint8_t *)&master_config, 1); + ret = lsm6dsv_write_reg(ctx, LSM6DSV_MASTER_CONFIG, (uint8_t *)&master_config, 1); exit: - ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + ret += lsm6dsv_mem_bank_set(ctx, LSM6DSV_MAIN_MEM_BANK); return ret; } @@ -8064,17 +7528,17 @@ int32_t lsm6dsv16x_sh_reset_set(stmdev_ctx_t *ctx, uint8_t val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_sh_reset_get(stmdev_ctx_t *ctx, uint8_t *val) +int32_t lsm6dsv_sh_reset_get(stmdev_ctx_t *ctx, uint8_t *val) { - lsm6dsv16x_master_config_t master_config; + lsm6dsv_master_config_t master_config; int32_t ret; - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_SENSOR_HUB_MEM_BANK); - ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_MASTER_CONFIG, (uint8_t *)&master_config, 1); + ret = lsm6dsv_mem_bank_set(ctx, LSM6DSV_SENSOR_HUB_MEM_BANK); + ret += lsm6dsv_read_reg(ctx, LSM6DSV_MASTER_CONFIG, (uint8_t *)&master_config, 1); *val = master_config.rst_master_regs; - ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + ret += lsm6dsv_mem_bank_set(ctx, LSM6DSV_MAIN_MEM_BANK); return ret; } @@ -8090,29 +7554,29 @@ int32_t lsm6dsv16x_sh_reset_get(stmdev_ctx_t *ctx, uint8_t *val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_sh_cfg_write(stmdev_ctx_t *ctx, - lsm6dsv16x_sh_cfg_write_t *val) +int32_t lsm6dsv_sh_cfg_write(stmdev_ctx_t *ctx, + lsm6dsv_sh_cfg_write_t *val) { - lsm6dsv16x_slv0_add_t reg; + lsm6dsv_slv0_add_t reg; int32_t ret; - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_SENSOR_HUB_MEM_BANK); + ret = lsm6dsv_mem_bank_set(ctx, LSM6DSV_SENSOR_HUB_MEM_BANK); if (ret != 0) { return ret; } reg.slave0_add = val->slv0_add; reg.rw_0 = 0; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_SLV0_ADD, (uint8_t *)®, 1); + ret = lsm6dsv_write_reg(ctx, LSM6DSV_SLV0_ADD, (uint8_t *)®, 1); if (ret != 0) { goto exit; } - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_SLV0_SUBADD, + ret = lsm6dsv_write_reg(ctx, LSM6DSV_SLV0_SUBADD, &(val->slv0_subadd), 1); if (ret != 0) { goto exit; } - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_DATAWRITE_SLV0, + ret = lsm6dsv_write_reg(ctx, LSM6DSV_DATAWRITE_SLV0, &(val->slv0_data), 1); exit: - ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + ret += lsm6dsv_mem_bank_set(ctx, LSM6DSV_MAIN_MEM_BANK); return ret; } @@ -8125,21 +7589,20 @@ int32_t lsm6dsv16x_sh_cfg_write(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_sh_data_rate_set(stmdev_ctx_t *ctx, - lsm6dsv16x_sh_data_rate_t val) +int32_t lsm6dsv_sh_data_rate_set(stmdev_ctx_t *ctx, lsm6dsv_sh_data_rate_t val) { - lsm6dsv16x_slv0_config_t slv0_config; + lsm6dsv_slv0_config_t slv0_config; int32_t ret; - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_SENSOR_HUB_MEM_BANK); - ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_SLV0_CONFIG, (uint8_t *)&slv0_config, 1); + ret = lsm6dsv_mem_bank_set(ctx, LSM6DSV_SENSOR_HUB_MEM_BANK); + ret += lsm6dsv_read_reg(ctx, LSM6DSV_SLV0_CONFIG, (uint8_t *)&slv0_config, 1); if (ret != 0) { goto exit; } slv0_config.shub_odr = (uint8_t)val & 0x07U; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_SLV0_CONFIG, (uint8_t *)&slv0_config, 1); + ret = lsm6dsv_write_reg(ctx, LSM6DSV_SLV0_CONFIG, (uint8_t *)&slv0_config, 1); exit: - ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + ret += lsm6dsv_mem_bank_set(ctx, LSM6DSV_MAIN_MEM_BANK); return ret; } @@ -8152,45 +7615,44 @@ int32_t lsm6dsv16x_sh_data_rate_set(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_sh_data_rate_get(stmdev_ctx_t *ctx, - lsm6dsv16x_sh_data_rate_t *val) +int32_t lsm6dsv_sh_data_rate_get(stmdev_ctx_t *ctx, lsm6dsv_sh_data_rate_t *val) { - lsm6dsv16x_slv0_config_t slv0_config; + lsm6dsv_slv0_config_t slv0_config; int32_t ret; - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_SENSOR_HUB_MEM_BANK); - ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_SLV0_CONFIG, (uint8_t *)&slv0_config, 1); - ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + ret = lsm6dsv_mem_bank_set(ctx, LSM6DSV_SENSOR_HUB_MEM_BANK); + ret += lsm6dsv_read_reg(ctx, LSM6DSV_SLV0_CONFIG, (uint8_t *)&slv0_config, 1); + ret += lsm6dsv_mem_bank_set(ctx, LSM6DSV_MAIN_MEM_BANK); if (ret != 0) { return ret; } switch (slv0_config.shub_odr) { - case LSM6DSV16X_SH_15Hz: - *val = LSM6DSV16X_SH_15Hz; + case LSM6DSV_SH_15Hz: + *val = LSM6DSV_SH_15Hz; break; - case LSM6DSV16X_SH_30Hz: - *val = LSM6DSV16X_SH_30Hz; + case LSM6DSV_SH_30Hz: + *val = LSM6DSV_SH_30Hz; break; - case LSM6DSV16X_SH_60Hz: - *val = LSM6DSV16X_SH_60Hz; + case LSM6DSV_SH_60Hz: + *val = LSM6DSV_SH_60Hz; break; - case LSM6DSV16X_SH_120Hz: - *val = LSM6DSV16X_SH_120Hz; + case LSM6DSV_SH_120Hz: + *val = LSM6DSV_SH_120Hz; break; - case LSM6DSV16X_SH_240Hz: - *val = LSM6DSV16X_SH_240Hz; + case LSM6DSV_SH_240Hz: + *val = LSM6DSV_SH_240Hz; break; - case LSM6DSV16X_SH_480Hz: - *val = LSM6DSV16X_SH_480Hz; + case LSM6DSV_SH_480Hz: + *val = LSM6DSV_SH_480Hz; break; default: - *val = LSM6DSV16X_SH_15Hz; + *val = LSM6DSV_SH_15Hz; break; } @@ -8208,36 +7670,36 @@ int32_t lsm6dsv16x_sh_data_rate_get(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_sh_slv_cfg_read(stmdev_ctx_t *ctx, uint8_t idx, - lsm6dsv16x_sh_cfg_read_t *val) +int32_t lsm6dsv_sh_slv_cfg_read(stmdev_ctx_t *ctx, uint8_t idx, + lsm6dsv_sh_cfg_read_t *val) { - lsm6dsv16x_slv0_add_t slv_add; - lsm6dsv16x_slv0_config_t slv_config; + lsm6dsv_slv0_add_t slv_add; + lsm6dsv_slv0_config_t slv_config; int32_t ret; - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_SENSOR_HUB_MEM_BANK); + ret = lsm6dsv_mem_bank_set(ctx, LSM6DSV_SENSOR_HUB_MEM_BANK); if (ret != 0) { return ret; } slv_add.slave0_add = val->slv_add; slv_add.rw_0 = 1; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_SLV0_ADD + idx*3U, + ret = lsm6dsv_write_reg(ctx, LSM6DSV_SLV0_ADD + idx*3U, (uint8_t *)&slv_add, 1); if (ret != 0) { goto exit; } - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_SLV0_SUBADD + idx*3U, + ret = lsm6dsv_write_reg(ctx, LSM6DSV_SLV0_SUBADD + idx*3U, &(val->slv_subadd), 1); if (ret != 0) { goto exit; } - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_SLV0_CONFIG + idx*3U, + ret = lsm6dsv_read_reg(ctx, LSM6DSV_SLV0_CONFIG + idx*3U, (uint8_t *)&slv_config, 1); if (ret != 0) { goto exit; } slv_config.slave0_numop = val->slv_len; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_SLV0_CONFIG + idx*3U, + ret = lsm6dsv_write_reg(ctx, LSM6DSV_SLV0_CONFIG + idx*3U, (uint8_t *)&slv_config, 1); exit: - ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + ret += lsm6dsv_mem_bank_set(ctx, LSM6DSV_MAIN_MEM_BANK); return ret; } @@ -8250,12 +7712,12 @@ int32_t lsm6dsv16x_sh_slv_cfg_read(stmdev_ctx_t *ctx, uint8_t idx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_sh_status_get(stmdev_ctx_t *ctx, - lsm6dsv16x_status_master_t *val) +int32_t lsm6dsv_sh_status_get(stmdev_ctx_t *ctx, + lsm6dsv_status_master_t *val) { int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_STATUS_MASTER_MAINPAGE, (uint8_t *) val, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_STATUS_MASTER_MAINPAGE, (uint8_t *) val, 1); return ret; } @@ -8281,16 +7743,16 @@ int32_t lsm6dsv16x_sh_status_get(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_ui_sdo_pull_up_set(stmdev_ctx_t *ctx, uint8_t val) +int32_t lsm6dsv_ui_sdo_pull_up_set(stmdev_ctx_t *ctx, uint8_t val) { - lsm6dsv16x_pin_ctrl_t pin_ctrl; + lsm6dsv_pin_ctrl_t pin_ctrl; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_PIN_CTRL, (uint8_t *)&pin_ctrl, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_PIN_CTRL, (uint8_t *)&pin_ctrl, 1); if (ret == 0) { pin_ctrl.sdo_pu_en = val; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_PIN_CTRL, (uint8_t *)&pin_ctrl, 1); + ret = lsm6dsv_write_reg(ctx, LSM6DSV_PIN_CTRL, (uint8_t *)&pin_ctrl, 1); } return ret; @@ -8304,12 +7766,12 @@ int32_t lsm6dsv16x_ui_sdo_pull_up_set(stmdev_ctx_t *ctx, uint8_t val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_ui_sdo_pull_up_get(stmdev_ctx_t *ctx, uint8_t *val) +int32_t lsm6dsv_ui_sdo_pull_up_get(stmdev_ctx_t *ctx, uint8_t *val) { - lsm6dsv16x_pin_ctrl_t pin_ctrl; + lsm6dsv_pin_ctrl_t pin_ctrl; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_PIN_CTRL, (uint8_t *)&pin_ctrl, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_PIN_CTRL, (uint8_t *)&pin_ctrl, 1); *val = pin_ctrl.sdo_pu_en; return ret; @@ -8323,17 +7785,17 @@ int32_t lsm6dsv16x_ui_sdo_pull_up_get(stmdev_ctx_t *ctx, uint8_t *val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_ui_i2c_i3c_mode_set(stmdev_ctx_t *ctx, - lsm6dsv16x_ui_i2c_i3c_mode_t val) +int32_t lsm6dsv_ui_i2c_i3c_mode_set(stmdev_ctx_t *ctx, + lsm6dsv_ui_i2c_i3c_mode_t val) { - lsm6dsv16x_if_cfg_t if_cfg; + lsm6dsv_if_cfg_t if_cfg; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_IF_CFG, (uint8_t *)&if_cfg, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_IF_CFG, (uint8_t *)&if_cfg, 1); if (ret == 0) { if_cfg.i2c_i3c_disable = (uint8_t)val & 0x1U; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_IF_CFG, (uint8_t *)&if_cfg, 1); + ret = lsm6dsv_write_reg(ctx, LSM6DSV_IF_CFG, (uint8_t *)&if_cfg, 1); } return ret; @@ -8347,27 +7809,27 @@ int32_t lsm6dsv16x_ui_i2c_i3c_mode_set(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_ui_i2c_i3c_mode_get(stmdev_ctx_t *ctx, - lsm6dsv16x_ui_i2c_i3c_mode_t *val) +int32_t lsm6dsv_ui_i2c_i3c_mode_get(stmdev_ctx_t *ctx, + lsm6dsv_ui_i2c_i3c_mode_t *val) { - lsm6dsv16x_if_cfg_t if_cfg; + lsm6dsv_if_cfg_t if_cfg; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_IF_CFG, (uint8_t *)&if_cfg, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_IF_CFG, (uint8_t *)&if_cfg, 1); if (ret != 0) { return ret; } switch (if_cfg.i2c_i3c_disable) { - case LSM6DSV16X_I2C_I3C_ENABLE: - *val = LSM6DSV16X_I2C_I3C_ENABLE; + case LSM6DSV_I2C_I3C_ENABLE: + *val = LSM6DSV_I2C_I3C_ENABLE; break; - case LSM6DSV16X_I2C_I3C_DISABLE: - *val = LSM6DSV16X_I2C_I3C_DISABLE; + case LSM6DSV_I2C_I3C_DISABLE: + *val = LSM6DSV_I2C_I3C_DISABLE; break; default: - *val = LSM6DSV16X_I2C_I3C_ENABLE; + *val = LSM6DSV_I2C_I3C_ENABLE; break; } @@ -8382,16 +7844,16 @@ int32_t lsm6dsv16x_ui_i2c_i3c_mode_get(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_spi_mode_set(stmdev_ctx_t *ctx, lsm6dsv16x_spi_mode_t val) +int32_t lsm6dsv_spi_mode_set(stmdev_ctx_t *ctx, lsm6dsv_spi_mode_t val) { - lsm6dsv16x_if_cfg_t if_cfg; + lsm6dsv_if_cfg_t if_cfg; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_IF_CFG, (uint8_t *)&if_cfg, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_IF_CFG, (uint8_t *)&if_cfg, 1); if (ret == 0) { if_cfg.sim = (uint8_t)val & 0x01U; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_IF_CFG, (uint8_t *)&if_cfg, 1); + ret = lsm6dsv_write_reg(ctx, LSM6DSV_IF_CFG, (uint8_t *)&if_cfg, 1); } return ret; @@ -8405,26 +7867,26 @@ int32_t lsm6dsv16x_spi_mode_set(stmdev_ctx_t *ctx, lsm6dsv16x_spi_mode_t val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_spi_mode_get(stmdev_ctx_t *ctx, lsm6dsv16x_spi_mode_t *val) +int32_t lsm6dsv_spi_mode_get(stmdev_ctx_t *ctx, lsm6dsv_spi_mode_t *val) { - lsm6dsv16x_if_cfg_t if_cfg; + lsm6dsv_if_cfg_t if_cfg; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_IF_CFG, (uint8_t *)&if_cfg, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_IF_CFG, (uint8_t *)&if_cfg, 1); if (ret != 0) { return ret; } switch (if_cfg.sim) { - case LSM6DSV16X_SPI_4_WIRE: - *val = LSM6DSV16X_SPI_4_WIRE; + case LSM6DSV_SPI_4_WIRE: + *val = LSM6DSV_SPI_4_WIRE; break; - case LSM6DSV16X_SPI_3_WIRE: - *val = LSM6DSV16X_SPI_3_WIRE; + case LSM6DSV_SPI_3_WIRE: + *val = LSM6DSV_SPI_3_WIRE; break; default: - *val = LSM6DSV16X_SPI_4_WIRE; + *val = LSM6DSV_SPI_4_WIRE; break; } @@ -8439,16 +7901,16 @@ int32_t lsm6dsv16x_spi_mode_get(stmdev_ctx_t *ctx, lsm6dsv16x_spi_mode_t *val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_ui_sda_pull_up_set(stmdev_ctx_t *ctx, uint8_t val) +int32_t lsm6dsv_ui_sda_pull_up_set(stmdev_ctx_t *ctx, uint8_t val) { - lsm6dsv16x_if_cfg_t if_cfg; + lsm6dsv_if_cfg_t if_cfg; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_IF_CFG, (uint8_t *)&if_cfg, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_IF_CFG, (uint8_t *)&if_cfg, 1); if (ret == 0) { if_cfg.sda_pu_en = val; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_IF_CFG, (uint8_t *)&if_cfg, 1); + ret = lsm6dsv_write_reg(ctx, LSM6DSV_IF_CFG, (uint8_t *)&if_cfg, 1); } return ret; @@ -8462,12 +7924,12 @@ int32_t lsm6dsv16x_ui_sda_pull_up_set(stmdev_ctx_t *ctx, uint8_t val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_ui_sda_pull_up_get(stmdev_ctx_t *ctx, uint8_t *val) +int32_t lsm6dsv_ui_sda_pull_up_get(stmdev_ctx_t *ctx, uint8_t *val) { - lsm6dsv16x_if_cfg_t if_cfg; + lsm6dsv_if_cfg_t if_cfg; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_IF_CFG, (uint8_t *)&if_cfg, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_IF_CFG, (uint8_t *)&if_cfg, 1); *val = if_cfg.sda_pu_en; return ret; @@ -8481,16 +7943,16 @@ int32_t lsm6dsv16x_ui_sda_pull_up_get(stmdev_ctx_t *ctx, uint8_t *val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_spi2_mode_set(stmdev_ctx_t *ctx, lsm6dsv16x_spi2_mode_t val) +int32_t lsm6dsv_spi2_mode_set(stmdev_ctx_t *ctx, lsm6dsv_spi2_mode_t val) { - lsm6dsv16x_ui_ctrl1_ois_t ui_ctrl1_ois; + lsm6dsv_ui_ctrl1_ois_t ui_ctrl1_ois; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_UI_CTRL1_OIS, (uint8_t *)&ui_ctrl1_ois, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_UI_CTRL1_OIS, (uint8_t *)&ui_ctrl1_ois, 1); if (ret == 0) { ui_ctrl1_ois.sim_ois = (uint8_t)val & 0x01U; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_UI_CTRL1_OIS, (uint8_t *)&ui_ctrl1_ois, 1); + ret = lsm6dsv_write_reg(ctx, LSM6DSV_UI_CTRL1_OIS, (uint8_t *)&ui_ctrl1_ois, 1); } return ret; @@ -8504,26 +7966,26 @@ int32_t lsm6dsv16x_spi2_mode_set(stmdev_ctx_t *ctx, lsm6dsv16x_spi2_mode_t val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_spi2_mode_get(stmdev_ctx_t *ctx, lsm6dsv16x_spi2_mode_t *val) +int32_t lsm6dsv_spi2_mode_get(stmdev_ctx_t *ctx, lsm6dsv_spi2_mode_t *val) { - lsm6dsv16x_ui_ctrl1_ois_t ui_ctrl1_ois; + lsm6dsv_ui_ctrl1_ois_t ui_ctrl1_ois; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_UI_CTRL1_OIS, (uint8_t *)&ui_ctrl1_ois, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_UI_CTRL1_OIS, (uint8_t *)&ui_ctrl1_ois, 1); if (ret != 0) { return ret; } switch (ui_ctrl1_ois.sim_ois) { - case LSM6DSV16X_SPI2_4_WIRE: - *val = LSM6DSV16X_SPI2_4_WIRE; + case LSM6DSV_SPI2_4_WIRE: + *val = LSM6DSV_SPI2_4_WIRE; break; - case LSM6DSV16X_SPI2_3_WIRE: - *val = LSM6DSV16X_SPI2_3_WIRE; + case LSM6DSV_SPI2_3_WIRE: + *val = LSM6DSV_SPI2_3_WIRE; break; default: - *val = LSM6DSV16X_SPI2_4_WIRE; + *val = LSM6DSV_SPI2_4_WIRE; break; } @@ -8552,19 +8014,19 @@ int32_t lsm6dsv16x_spi2_mode_get(stmdev_ctx_t *ctx, lsm6dsv16x_spi2_mode_t *val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_sigmot_mode_set(stmdev_ctx_t *ctx, uint8_t val) +int32_t lsm6dsv_sigmot_mode_set(stmdev_ctx_t *ctx, uint8_t val) { - lsm6dsv16x_emb_func_en_a_t emb_func_en_a; + lsm6dsv_emb_func_en_a_t emb_func_en_a; int32_t ret; - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK); + ret = lsm6dsv_mem_bank_set(ctx, LSM6DSV_EMBED_FUNC_MEM_BANK); if (ret != 0) { return ret; } - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_EMB_FUNC_EN_A, (uint8_t *)&emb_func_en_a, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_EMB_FUNC_EN_A, (uint8_t *)&emb_func_en_a, 1); emb_func_en_a.sign_motion_en = val; - ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_EMB_FUNC_EN_A, (uint8_t *)&emb_func_en_a, 1); + ret += lsm6dsv_write_reg(ctx, LSM6DSV_EMB_FUNC_EN_A, (uint8_t *)&emb_func_en_a, 1); - ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + ret += lsm6dsv_mem_bank_set(ctx, LSM6DSV_MAIN_MEM_BANK); return ret; } @@ -8577,18 +8039,18 @@ int32_t lsm6dsv16x_sigmot_mode_set(stmdev_ctx_t *ctx, uint8_t val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_sigmot_mode_get(stmdev_ctx_t *ctx, uint8_t *val) +int32_t lsm6dsv_sigmot_mode_get(stmdev_ctx_t *ctx, uint8_t *val) { - lsm6dsv16x_emb_func_en_a_t emb_func_en_a; + lsm6dsv_emb_func_en_a_t emb_func_en_a; int32_t ret; - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK); + ret = lsm6dsv_mem_bank_set(ctx, LSM6DSV_EMBED_FUNC_MEM_BANK); if (ret != 0) { return ret; } - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_EMB_FUNC_EN_A, (uint8_t *)&emb_func_en_a, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_EMB_FUNC_EN_A, (uint8_t *)&emb_func_en_a, 1); *val = emb_func_en_a.sign_motion_en; - ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + ret += lsm6dsv_mem_bank_set(ctx, LSM6DSV_MAIN_MEM_BANK); return ret; } @@ -8613,40 +8075,20 @@ int32_t lsm6dsv16x_sigmot_mode_get(stmdev_ctx_t *ctx, uint8_t *val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_stpcnt_mode_set(stmdev_ctx_t *ctx, - lsm6dsv16x_stpcnt_mode_t val) +int32_t lsm6dsv_stpcnt_mode_set(stmdev_ctx_t *ctx, + lsm6dsv_stpcnt_mode_t val) { - lsm6dsv16x_emb_func_en_a_t emb_func_en_a; - lsm6dsv16x_emb_func_en_b_t emb_func_en_b; - lsm6dsv16x_pedo_cmd_reg_t pedo_cmd_reg; + lsm6dsv_emb_func_en_a_t emb_func_en_a; int32_t ret; - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK); + ret = lsm6dsv_mem_bank_set(ctx, LSM6DSV_EMBED_FUNC_MEM_BANK); if (ret != 0) { return ret; } - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_EMB_FUNC_EN_A, (uint8_t *)&emb_func_en_a, 1); - ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_EMB_FUNC_EN_B, (uint8_t *)&emb_func_en_b, 1); - if (ret != 0) { goto exit; } - - if ((val.false_step_rej == PROPERTY_ENABLE) - && ((emb_func_en_a.mlc_before_fsm_en & emb_func_en_b.mlc_en) == - PROPERTY_DISABLE)) - { - emb_func_en_a.mlc_before_fsm_en = PROPERTY_ENABLE; - } - + ret = lsm6dsv_read_reg(ctx, LSM6DSV_EMB_FUNC_EN_A, (uint8_t *)&emb_func_en_a, 1); emb_func_en_a.pedo_en = val.step_counter_enable; - ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_EMB_FUNC_EN_A, (uint8_t *)&emb_func_en_a, 1); + ret += lsm6dsv_write_reg(ctx, LSM6DSV_EMB_FUNC_EN_A, (uint8_t *)&emb_func_en_a, 1); -exit: - ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); - - if (ret == 0) - { - ret = lsm6dsv16x_ln_pg_read(ctx, LSM6DSV16X_EMB_ADV_PG_1 + LSM6DSV16X_PEDO_CMD_REG, (uint8_t *)&pedo_cmd_reg, 1); - pedo_cmd_reg.fp_rejection_en = val.false_step_rej; - ret += lsm6dsv16x_ln_pg_write(ctx, LSM6DSV16X_EMB_ADV_PG_1 + LSM6DSV16X_PEDO_CMD_REG, (uint8_t *)&pedo_cmd_reg, 1); - } + ret += lsm6dsv_mem_bank_set(ctx, LSM6DSV_MAIN_MEM_BANK); return ret; } @@ -8659,24 +8101,17 @@ int32_t lsm6dsv16x_stpcnt_mode_set(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_stpcnt_mode_get(stmdev_ctx_t *ctx, - lsm6dsv16x_stpcnt_mode_t *val) +int32_t lsm6dsv_stpcnt_mode_get(stmdev_ctx_t *ctx, + lsm6dsv_stpcnt_mode_t *val) { - lsm6dsv16x_emb_func_en_a_t emb_func_en_a; - lsm6dsv16x_pedo_cmd_reg_t pedo_cmd_reg; + lsm6dsv_emb_func_en_a_t emb_func_en_a; int32_t ret; - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK); - ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_EMB_FUNC_EN_A, (uint8_t *)&emb_func_en_a, 1); - ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); - if (ret != 0) { return ret; } - - ret = lsm6dsv16x_ln_pg_read(ctx, LSM6DSV16X_EMB_ADV_PG_1 + LSM6DSV16X_PEDO_CMD_REG, (uint8_t *)&pedo_cmd_reg, 1); + ret = lsm6dsv_mem_bank_set(ctx, LSM6DSV_EMBED_FUNC_MEM_BANK); + ret += lsm6dsv_read_reg(ctx, LSM6DSV_EMB_FUNC_EN_A, (uint8_t *)&emb_func_en_a, 1); + ret += lsm6dsv_mem_bank_set(ctx, LSM6DSV_MAIN_MEM_BANK); if (ret != 0) { return ret; } - val->false_step_rej = pedo_cmd_reg.fp_rejection_en; - val->step_counter_enable = emb_func_en_a.pedo_en; - return ret; } @@ -8688,14 +8123,14 @@ int32_t lsm6dsv16x_stpcnt_mode_get(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_stpcnt_steps_get(stmdev_ctx_t *ctx, uint16_t *val) +int32_t lsm6dsv_stpcnt_steps_get(stmdev_ctx_t *ctx, uint16_t *val) { uint8_t buff[2]; int32_t ret; - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK); - ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_STEP_COUNTER_L, &buff[0], 2); - ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + ret = lsm6dsv_mem_bank_set(ctx, LSM6DSV_EMBED_FUNC_MEM_BANK); + ret += lsm6dsv_read_reg(ctx, LSM6DSV_STEP_COUNTER_L, &buff[0], 2); + ret += lsm6dsv_mem_bank_set(ctx, LSM6DSV_MAIN_MEM_BANK); if (ret != 0) { return ret; } *val = buff[1]; @@ -8712,22 +8147,22 @@ int32_t lsm6dsv16x_stpcnt_steps_get(stmdev_ctx_t *ctx, uint16_t *val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_stpcnt_rst_step_set(stmdev_ctx_t *ctx, uint8_t val) +int32_t lsm6dsv_stpcnt_rst_step_set(stmdev_ctx_t *ctx, uint8_t val) { - lsm6dsv16x_emb_func_src_t emb_func_src; + lsm6dsv_emb_func_src_t emb_func_src; int32_t ret; - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK); + ret = lsm6dsv_mem_bank_set(ctx, LSM6DSV_EMBED_FUNC_MEM_BANK); if (ret != 0) { return ret; } - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_EMB_FUNC_SRC, (uint8_t *)&emb_func_src, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_EMB_FUNC_SRC, (uint8_t *)&emb_func_src, 1); if (ret != 0) { goto exit; } emb_func_src.pedo_rst_step = val; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_EMB_FUNC_SRC, (uint8_t *)&emb_func_src, 1); + ret = lsm6dsv_write_reg(ctx, LSM6DSV_EMB_FUNC_SRC, (uint8_t *)&emb_func_src, 1); exit: - ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + ret += lsm6dsv_mem_bank_set(ctx, LSM6DSV_MAIN_MEM_BANK); return ret; } @@ -8740,18 +8175,18 @@ int32_t lsm6dsv16x_stpcnt_rst_step_set(stmdev_ctx_t *ctx, uint8_t val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_stpcnt_rst_step_get(stmdev_ctx_t *ctx, uint8_t *val) +int32_t lsm6dsv_stpcnt_rst_step_get(stmdev_ctx_t *ctx, uint8_t *val) { - lsm6dsv16x_emb_func_src_t emb_func_src; + lsm6dsv_emb_func_src_t emb_func_src; int32_t ret; - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK); + ret = lsm6dsv_mem_bank_set(ctx, LSM6DSV_EMBED_FUNC_MEM_BANK); if (ret != 0) { return ret; } - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_EMB_FUNC_SRC, (uint8_t *)&emb_func_src, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_EMB_FUNC_SRC, (uint8_t *)&emb_func_src, 1); *val = emb_func_src.pedo_rst_step; - ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + ret += lsm6dsv_mem_bank_set(ctx, LSM6DSV_MAIN_MEM_BANK); return ret; } @@ -8764,16 +8199,16 @@ int32_t lsm6dsv16x_stpcnt_rst_step_get(stmdev_ctx_t *ctx, uint8_t *val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_stpcnt_debounce_set(stmdev_ctx_t *ctx, uint8_t val) +int32_t lsm6dsv_stpcnt_debounce_set(stmdev_ctx_t *ctx, uint8_t val) { - lsm6dsv16x_pedo_deb_steps_conf_t pedo_deb_steps_conf; + lsm6dsv_pedo_deb_steps_conf_t pedo_deb_steps_conf; int32_t ret; - ret = lsm6dsv16x_ln_pg_read(ctx, LSM6DSV16X_EMB_ADV_PG_1 + LSM6DSV16X_PEDO_DEB_STEPS_CONF, (uint8_t *)&pedo_deb_steps_conf, 1); + ret = lsm6dsv_ln_pg_read(ctx, LSM6DSV_EMB_ADV_PG_1 + LSM6DSV_PEDO_DEB_STEPS_CONF, (uint8_t *)&pedo_deb_steps_conf, 1); if (ret == 0) { pedo_deb_steps_conf.deb_step = val; - ret = lsm6dsv16x_ln_pg_write(ctx, LSM6DSV16X_EMB_ADV_PG_1 + LSM6DSV16X_PEDO_DEB_STEPS_CONF, (uint8_t *)&pedo_deb_steps_conf, 1); + ret = lsm6dsv_ln_pg_write(ctx, LSM6DSV_EMB_ADV_PG_1 + LSM6DSV_PEDO_DEB_STEPS_CONF, (uint8_t *)&pedo_deb_steps_conf, 1); } return ret; @@ -8787,12 +8222,12 @@ int32_t lsm6dsv16x_stpcnt_debounce_set(stmdev_ctx_t *ctx, uint8_t val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_stpcnt_debounce_get(stmdev_ctx_t *ctx, uint8_t *val) +int32_t lsm6dsv_stpcnt_debounce_get(stmdev_ctx_t *ctx, uint8_t *val) { - lsm6dsv16x_pedo_deb_steps_conf_t pedo_deb_steps_conf; + lsm6dsv_pedo_deb_steps_conf_t pedo_deb_steps_conf; int32_t ret; - ret = lsm6dsv16x_ln_pg_read(ctx, LSM6DSV16X_EMB_ADV_PG_1 + LSM6DSV16X_PEDO_DEB_STEPS_CONF, (uint8_t *)&pedo_deb_steps_conf, 1); + ret = lsm6dsv_ln_pg_read(ctx, LSM6DSV_EMB_ADV_PG_1 + LSM6DSV_PEDO_DEB_STEPS_CONF, (uint8_t *)&pedo_deb_steps_conf, 1); *val = pedo_deb_steps_conf.deb_step; return ret; @@ -8806,14 +8241,14 @@ int32_t lsm6dsv16x_stpcnt_debounce_get(stmdev_ctx_t *ctx, uint8_t *val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_stpcnt_period_set(stmdev_ctx_t *ctx, uint16_t val) +int32_t lsm6dsv_stpcnt_period_set(stmdev_ctx_t *ctx, uint16_t val) { uint8_t buff[2]; int32_t ret; buff[1] = (uint8_t)(val / 256U); buff[0] = (uint8_t)(val - (buff[1] * 256U)); - ret = lsm6dsv16x_ln_pg_write(ctx, LSM6DSV16X_EMB_ADV_PG_1 + LSM6DSV16X_PEDO_SC_DELTAT_L, (uint8_t *)&buff[0], 2); + ret = lsm6dsv_ln_pg_write(ctx, LSM6DSV_EMB_ADV_PG_1 + LSM6DSV_PEDO_SC_DELTAT_L, (uint8_t *)&buff[0], 2); return ret; } @@ -8826,12 +8261,12 @@ int32_t lsm6dsv16x_stpcnt_period_set(stmdev_ctx_t *ctx, uint16_t val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_stpcnt_period_get(stmdev_ctx_t *ctx, uint16_t *val) +int32_t lsm6dsv_stpcnt_period_get(stmdev_ctx_t *ctx, uint16_t *val) { uint8_t buff[2]; int32_t ret; - ret = lsm6dsv16x_ln_pg_read(ctx, LSM6DSV16X_EMB_ADV_PG_1 + LSM6DSV16X_PEDO_SC_DELTAT_L, &buff[0], 2); + ret = lsm6dsv_ln_pg_read(ctx, LSM6DSV_EMB_ADV_PG_1 + LSM6DSV_PEDO_SC_DELTAT_L, &buff[0], 2); if (ret != 0) { return ret; } *val = buff[1]; @@ -8860,23 +8295,23 @@ int32_t lsm6dsv16x_stpcnt_period_get(stmdev_ctx_t *ctx, uint16_t *val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_sflp_game_rotation_set(stmdev_ctx_t *ctx, uint8_t val) +int32_t lsm6dsv_sflp_game_rotation_set(stmdev_ctx_t *ctx, uint8_t val) { - lsm6dsv16x_emb_func_en_a_t emb_func_en_a; + lsm6dsv_emb_func_en_a_t emb_func_en_a; int32_t ret; - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK); + ret = lsm6dsv_mem_bank_set(ctx, LSM6DSV_EMBED_FUNC_MEM_BANK); if (ret != 0) { return ret; } - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_EMB_FUNC_EN_A, (uint8_t *)&emb_func_en_a, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_EMB_FUNC_EN_A, (uint8_t *)&emb_func_en_a, 1); if (ret != 0) { goto exit; } emb_func_en_a.sflp_game_en = val; - ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_EMB_FUNC_EN_A, + ret += lsm6dsv_write_reg(ctx, LSM6DSV_EMB_FUNC_EN_A, (uint8_t *)&emb_func_en_a, 1); exit: - ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + ret += lsm6dsv_mem_bank_set(ctx, LSM6DSV_MAIN_MEM_BANK); return ret; } @@ -8889,18 +8324,18 @@ int32_t lsm6dsv16x_sflp_game_rotation_set(stmdev_ctx_t *ctx, uint8_t val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_sflp_game_rotation_get(stmdev_ctx_t *ctx, uint8_t *val) +int32_t lsm6dsv_sflp_game_rotation_get(stmdev_ctx_t *ctx, uint8_t *val) { - lsm6dsv16x_emb_func_en_a_t emb_func_en_a; + lsm6dsv_emb_func_en_a_t emb_func_en_a; int32_t ret; - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK); + ret = lsm6dsv_mem_bank_set(ctx, LSM6DSV_EMBED_FUNC_MEM_BANK); if (ret != 0) { return ret; } - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_EMB_FUNC_EN_A, (uint8_t *)&emb_func_en_a, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_EMB_FUNC_EN_A, (uint8_t *)&emb_func_en_a, 1); *val = emb_func_en_a.sflp_game_en; - ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + ret += lsm6dsv_mem_bank_set(ctx, LSM6DSV_MAIN_MEM_BANK); return ret; } @@ -8913,23 +8348,23 @@ int32_t lsm6dsv16x_sflp_game_rotation_get(stmdev_ctx_t *ctx, uint8_t *val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_sflp_data_rate_set(stmdev_ctx_t *ctx, - lsm6dsv16x_sflp_data_rate_t val) +int32_t lsm6dsv_sflp_data_rate_set(stmdev_ctx_t *ctx, + lsm6dsv_sflp_data_rate_t val) { - lsm6dsv16x_sflp_odr_t sflp_odr; + lsm6dsv_sflp_odr_t sflp_odr; int32_t ret; - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK); + ret = lsm6dsv_mem_bank_set(ctx, LSM6DSV_EMBED_FUNC_MEM_BANK); if (ret != 0) { return ret; } - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_SFLP_ODR, (uint8_t *)&sflp_odr, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_SFLP_ODR, (uint8_t *)&sflp_odr, 1); if (ret != 0) { goto exit; } sflp_odr.sflp_game_odr = (uint8_t)val & 0x07U; - ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_SFLP_ODR, (uint8_t *)&sflp_odr, 1); + ret += lsm6dsv_write_reg(ctx, LSM6DSV_SFLP_ODR, (uint8_t *)&sflp_odr, 1); exit: - ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + ret += lsm6dsv_mem_bank_set(ctx, LSM6DSV_MAIN_MEM_BANK); return ret; } @@ -8942,45 +8377,45 @@ int32_t lsm6dsv16x_sflp_data_rate_set(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_sflp_data_rate_get(stmdev_ctx_t *ctx, - lsm6dsv16x_sflp_data_rate_t *val) +int32_t lsm6dsv_sflp_data_rate_get(stmdev_ctx_t *ctx, + lsm6dsv_sflp_data_rate_t *val) { - lsm6dsv16x_sflp_odr_t sflp_odr; + lsm6dsv_sflp_odr_t sflp_odr; int32_t ret; - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK); - ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_SFLP_ODR, (uint8_t *)&sflp_odr, 1); - ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + ret = lsm6dsv_mem_bank_set(ctx, LSM6DSV_EMBED_FUNC_MEM_BANK); + ret += lsm6dsv_read_reg(ctx, LSM6DSV_SFLP_ODR, (uint8_t *)&sflp_odr, 1); + ret += lsm6dsv_mem_bank_set(ctx, LSM6DSV_MAIN_MEM_BANK); if (ret != 0) { return ret; } switch (sflp_odr.sflp_game_odr) { - case LSM6DSV16X_SFLP_15Hz: - *val = LSM6DSV16X_SFLP_15Hz; + case LSM6DSV_SFLP_15Hz: + *val = LSM6DSV_SFLP_15Hz; break; - case LSM6DSV16X_SFLP_30Hz: - *val = LSM6DSV16X_SFLP_30Hz; + case LSM6DSV_SFLP_30Hz: + *val = LSM6DSV_SFLP_30Hz; break; - case LSM6DSV16X_SFLP_60Hz: - *val = LSM6DSV16X_SFLP_60Hz; + case LSM6DSV_SFLP_60Hz: + *val = LSM6DSV_SFLP_60Hz; break; - case LSM6DSV16X_SFLP_120Hz: - *val = LSM6DSV16X_SFLP_120Hz; + case LSM6DSV_SFLP_120Hz: + *val = LSM6DSV_SFLP_120Hz; break; - case LSM6DSV16X_SFLP_240Hz: - *val = LSM6DSV16X_SFLP_240Hz; + case LSM6DSV_SFLP_240Hz: + *val = LSM6DSV_SFLP_240Hz; break; - case LSM6DSV16X_SFLP_480Hz: - *val = LSM6DSV16X_SFLP_480Hz; + case LSM6DSV_SFLP_480Hz: + *val = LSM6DSV_SFLP_480Hz; break; default: - *val = LSM6DSV16X_SFLP_15Hz; + *val = LSM6DSV_SFLP_15Hz; break; } @@ -9008,19 +8443,19 @@ int32_t lsm6dsv16x_sflp_data_rate_get(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_tap_detection_set(stmdev_ctx_t *ctx, - lsm6dsv16x_tap_detection_t val) +int32_t lsm6dsv_tap_detection_set(stmdev_ctx_t *ctx, + lsm6dsv_tap_detection_t val) { - lsm6dsv16x_tap_cfg0_t tap_cfg0; + lsm6dsv_tap_cfg0_t tap_cfg0; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_TAP_CFG0, (uint8_t *)&tap_cfg0, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_TAP_CFG0, (uint8_t *)&tap_cfg0, 1); if (ret == 0) { tap_cfg0.tap_x_en = val.tap_x_en; tap_cfg0.tap_y_en = val.tap_y_en; tap_cfg0.tap_z_en = val.tap_z_en; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_TAP_CFG0, (uint8_t *)&tap_cfg0, 1); + ret = lsm6dsv_write_reg(ctx, LSM6DSV_TAP_CFG0, (uint8_t *)&tap_cfg0, 1); } return ret; @@ -9034,13 +8469,13 @@ int32_t lsm6dsv16x_tap_detection_set(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_tap_detection_get(stmdev_ctx_t *ctx, - lsm6dsv16x_tap_detection_t *val) +int32_t lsm6dsv_tap_detection_get(stmdev_ctx_t *ctx, + lsm6dsv_tap_detection_t *val) { - lsm6dsv16x_tap_cfg0_t tap_cfg0; + lsm6dsv_tap_cfg0_t tap_cfg0; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_TAP_CFG0, (uint8_t *)&tap_cfg0, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_TAP_CFG0, (uint8_t *)&tap_cfg0, 1); if (ret != 0) { return ret; } val->tap_x_en = tap_cfg0.tap_x_en; @@ -9058,26 +8493,26 @@ int32_t lsm6dsv16x_tap_detection_get(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_tap_thresholds_set(stmdev_ctx_t *ctx, - lsm6dsv16x_tap_thresholds_t val) +int32_t lsm6dsv_tap_thresholds_set(stmdev_ctx_t *ctx, + lsm6dsv_tap_thresholds_t val) { - lsm6dsv16x_tap_ths_6d_t tap_ths_6d; - lsm6dsv16x_tap_cfg2_t tap_cfg2; - lsm6dsv16x_tap_cfg1_t tap_cfg1; + lsm6dsv_tap_ths_6d_t tap_ths_6d; + lsm6dsv_tap_cfg2_t tap_cfg2; + lsm6dsv_tap_cfg1_t tap_cfg1; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_TAP_CFG1, (uint8_t *)&tap_cfg1, 1); - ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_TAP_CFG2, (uint8_t *)&tap_cfg2, 1); - ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_TAP_THS_6D, (uint8_t *)&tap_ths_6d, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_TAP_CFG1, (uint8_t *)&tap_cfg1, 1); + ret += lsm6dsv_read_reg(ctx, LSM6DSV_TAP_CFG2, (uint8_t *)&tap_cfg2, 1); + ret += lsm6dsv_read_reg(ctx, LSM6DSV_TAP_THS_6D, (uint8_t *)&tap_ths_6d, 1); if (ret != 0) { return ret; } tap_cfg1.tap_ths_x = val.x; tap_cfg2.tap_ths_y = val.y; tap_ths_6d.tap_ths_z = val.z; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_TAP_THS_6D, (uint8_t *)&tap_ths_6d, 1); - ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_TAP_CFG2, (uint8_t *)&tap_cfg2, 1); - ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_TAP_CFG1, (uint8_t *)&tap_cfg1, 1); + ret = lsm6dsv_write_reg(ctx, LSM6DSV_TAP_THS_6D, (uint8_t *)&tap_ths_6d, 1); + ret += lsm6dsv_write_reg(ctx, LSM6DSV_TAP_CFG2, (uint8_t *)&tap_cfg2, 1); + ret += lsm6dsv_write_reg(ctx, LSM6DSV_TAP_CFG1, (uint8_t *)&tap_cfg1, 1); return ret; } @@ -9090,17 +8525,17 @@ int32_t lsm6dsv16x_tap_thresholds_set(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_tap_thresholds_get(stmdev_ctx_t *ctx, - lsm6dsv16x_tap_thresholds_t *val) +int32_t lsm6dsv_tap_thresholds_get(stmdev_ctx_t *ctx, + lsm6dsv_tap_thresholds_t *val) { - lsm6dsv16x_tap_ths_6d_t tap_ths_6d; - lsm6dsv16x_tap_cfg2_t tap_cfg2; - lsm6dsv16x_tap_cfg1_t tap_cfg1; + lsm6dsv_tap_ths_6d_t tap_ths_6d; + lsm6dsv_tap_cfg2_t tap_cfg2; + lsm6dsv_tap_cfg1_t tap_cfg1; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_TAP_CFG1, (uint8_t *)&tap_cfg1, 1); - ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_TAP_CFG2, (uint8_t *)&tap_cfg2, 1); - ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_TAP_THS_6D, (uint8_t *)&tap_ths_6d, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_TAP_CFG1, (uint8_t *)&tap_cfg1, 1); + ret += lsm6dsv_read_reg(ctx, LSM6DSV_TAP_CFG2, (uint8_t *)&tap_cfg2, 1); + ret += lsm6dsv_read_reg(ctx, LSM6DSV_TAP_THS_6D, (uint8_t *)&tap_ths_6d, 1); if (ret != 0) { return ret; } val->x = tap_cfg1.tap_ths_x; @@ -9118,17 +8553,17 @@ int32_t lsm6dsv16x_tap_thresholds_get(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_tap_axis_priority_set(stmdev_ctx_t *ctx, - lsm6dsv16x_tap_axis_priority_t val) +int32_t lsm6dsv_tap_axis_priority_set(stmdev_ctx_t *ctx, + lsm6dsv_tap_axis_priority_t val) { - lsm6dsv16x_tap_cfg1_t tap_cfg1; + lsm6dsv_tap_cfg1_t tap_cfg1; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_TAP_CFG1, (uint8_t *)&tap_cfg1, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_TAP_CFG1, (uint8_t *)&tap_cfg1, 1); if (ret == 0) { tap_cfg1.tap_priority = (uint8_t)val & 0x7U; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_TAP_CFG1, (uint8_t *)&tap_cfg1, 1); + ret = lsm6dsv_write_reg(ctx, LSM6DSV_TAP_CFG1, (uint8_t *)&tap_cfg1, 1); } return ret; @@ -9142,43 +8577,43 @@ int32_t lsm6dsv16x_tap_axis_priority_set(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_tap_axis_priority_get(stmdev_ctx_t *ctx, - lsm6dsv16x_tap_axis_priority_t *val) +int32_t lsm6dsv_tap_axis_priority_get(stmdev_ctx_t *ctx, + lsm6dsv_tap_axis_priority_t *val) { - lsm6dsv16x_tap_cfg1_t tap_cfg1; + lsm6dsv_tap_cfg1_t tap_cfg1; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_TAP_CFG1, (uint8_t *)&tap_cfg1, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_TAP_CFG1, (uint8_t *)&tap_cfg1, 1); if (ret != 0) { return ret; } switch (tap_cfg1.tap_priority) { - case LSM6DSV16X_XYZ : - *val = LSM6DSV16X_XYZ ; + case LSM6DSV_XYZ : + *val = LSM6DSV_XYZ ; break; - case LSM6DSV16X_YXZ : - *val = LSM6DSV16X_YXZ ; + case LSM6DSV_YXZ : + *val = LSM6DSV_YXZ ; break; - case LSM6DSV16X_XZY: - *val = LSM6DSV16X_XZY; + case LSM6DSV_XZY: + *val = LSM6DSV_XZY; break; - case LSM6DSV16X_ZYX : - *val = LSM6DSV16X_ZYX ; + case LSM6DSV_ZYX : + *val = LSM6DSV_ZYX ; break; - case LSM6DSV16X_YZX : - *val = LSM6DSV16X_YZX ; + case LSM6DSV_YZX : + *val = LSM6DSV_YZX ; break; - case LSM6DSV16X_ZXY : - *val = LSM6DSV16X_ZXY ; + case LSM6DSV_ZXY : + *val = LSM6DSV_ZXY ; break; default: - *val = LSM6DSV16X_XYZ ; + *val = LSM6DSV_XYZ ; break; } @@ -9193,19 +8628,19 @@ int32_t lsm6dsv16x_tap_axis_priority_get(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_tap_time_windows_set(stmdev_ctx_t *ctx, - lsm6dsv16x_tap_time_windows_t val) +int32_t lsm6dsv_tap_time_windows_set(stmdev_ctx_t *ctx, + lsm6dsv_tap_time_windows_t val) { - lsm6dsv16x_tap_dur_t tap_dur; + lsm6dsv_tap_dur_t tap_dur; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_TAP_DUR, (uint8_t *)&tap_dur, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_TAP_DUR, (uint8_t *)&tap_dur, 1); if (ret == 0) { tap_dur.shock = val.shock; tap_dur.quiet = val.quiet; tap_dur.dur = val.tap_gap; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_TAP_DUR, (uint8_t *)&tap_dur, 1); + ret = lsm6dsv_write_reg(ctx, LSM6DSV_TAP_DUR, (uint8_t *)&tap_dur, 1); } return ret; @@ -9219,13 +8654,13 @@ int32_t lsm6dsv16x_tap_time_windows_set(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_tap_time_windows_get(stmdev_ctx_t *ctx, - lsm6dsv16x_tap_time_windows_t *val) +int32_t lsm6dsv_tap_time_windows_get(stmdev_ctx_t *ctx, + lsm6dsv_tap_time_windows_t *val) { - lsm6dsv16x_tap_dur_t tap_dur; + lsm6dsv_tap_dur_t tap_dur; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_TAP_DUR, (uint8_t *)&tap_dur, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_TAP_DUR, (uint8_t *)&tap_dur, 1); if (ret != 0) { return ret; } val->shock = tap_dur.shock; @@ -9243,16 +8678,16 @@ int32_t lsm6dsv16x_tap_time_windows_get(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_tap_mode_set(stmdev_ctx_t *ctx, lsm6dsv16x_tap_mode_t val) +int32_t lsm6dsv_tap_mode_set(stmdev_ctx_t *ctx, lsm6dsv_tap_mode_t val) { - lsm6dsv16x_wake_up_ths_t wake_up_ths; + lsm6dsv_wake_up_ths_t wake_up_ths; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_WAKE_UP_THS, (uint8_t *)&wake_up_ths, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_WAKE_UP_THS, (uint8_t *)&wake_up_ths, 1); if (ret == 0) { wake_up_ths.single_double_tap = (uint8_t)val & 0x01U; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_WAKE_UP_THS, (uint8_t *)&wake_up_ths, 1); + ret = lsm6dsv_write_reg(ctx, LSM6DSV_WAKE_UP_THS, (uint8_t *)&wake_up_ths, 1); } return ret; @@ -9266,26 +8701,26 @@ int32_t lsm6dsv16x_tap_mode_set(stmdev_ctx_t *ctx, lsm6dsv16x_tap_mode_t val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_tap_mode_get(stmdev_ctx_t *ctx, lsm6dsv16x_tap_mode_t *val) +int32_t lsm6dsv_tap_mode_get(stmdev_ctx_t *ctx, lsm6dsv_tap_mode_t *val) { - lsm6dsv16x_wake_up_ths_t wake_up_ths; + lsm6dsv_wake_up_ths_t wake_up_ths; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_WAKE_UP_THS, (uint8_t *)&wake_up_ths, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_WAKE_UP_THS, (uint8_t *)&wake_up_ths, 1); if (ret != 0) { return ret; } switch (wake_up_ths.single_double_tap) { - case LSM6DSV16X_ONLY_SINGLE: - *val = LSM6DSV16X_ONLY_SINGLE; + case LSM6DSV_ONLY_SINGLE: + *val = LSM6DSV_ONLY_SINGLE; break; - case LSM6DSV16X_BOTH_SINGLE_DOUBLE: - *val = LSM6DSV16X_BOTH_SINGLE_DOUBLE; + case LSM6DSV_BOTH_SINGLE_DOUBLE: + *val = LSM6DSV_BOTH_SINGLE_DOUBLE; break; default: - *val = LSM6DSV16X_ONLY_SINGLE; + *val = LSM6DSV_ONLY_SINGLE; break; } @@ -9313,19 +8748,19 @@ int32_t lsm6dsv16x_tap_mode_get(stmdev_ctx_t *ctx, lsm6dsv16x_tap_mode_t *val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_tilt_mode_set(stmdev_ctx_t *ctx, uint8_t val) +int32_t lsm6dsv_tilt_mode_set(stmdev_ctx_t *ctx, uint8_t val) { - lsm6dsv16x_emb_func_en_a_t emb_func_en_a; + lsm6dsv_emb_func_en_a_t emb_func_en_a; int32_t ret; - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK); + ret = lsm6dsv_mem_bank_set(ctx, LSM6DSV_EMBED_FUNC_MEM_BANK); if (ret != 0) { return ret; } - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_EMB_FUNC_EN_A, (uint8_t *)&emb_func_en_a, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_EMB_FUNC_EN_A, (uint8_t *)&emb_func_en_a, 1); emb_func_en_a.tilt_en = val; - ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_EMB_FUNC_EN_A, (uint8_t *)&emb_func_en_a, 1); + ret += lsm6dsv_write_reg(ctx, LSM6DSV_EMB_FUNC_EN_A, (uint8_t *)&emb_func_en_a, 1); - ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + ret += lsm6dsv_mem_bank_set(ctx, LSM6DSV_MAIN_MEM_BANK); return ret; } @@ -9338,18 +8773,18 @@ int32_t lsm6dsv16x_tilt_mode_set(stmdev_ctx_t *ctx, uint8_t val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_tilt_mode_get(stmdev_ctx_t *ctx, uint8_t *val) +int32_t lsm6dsv_tilt_mode_get(stmdev_ctx_t *ctx, uint8_t *val) { - lsm6dsv16x_emb_func_en_a_t emb_func_en_a; + lsm6dsv_emb_func_en_a_t emb_func_en_a; int32_t ret; - ret = lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_EMBED_FUNC_MEM_BANK); + ret = lsm6dsv_mem_bank_set(ctx, LSM6DSV_EMBED_FUNC_MEM_BANK); if (ret != 0) { return ret; } - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_EMB_FUNC_EN_A, (uint8_t *)&emb_func_en_a, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_EMB_FUNC_EN_A, (uint8_t *)&emb_func_en_a, 1); *val = emb_func_en_a.tilt_en; - ret += lsm6dsv16x_mem_bank_set(ctx, LSM6DSV16X_MAIN_MEM_BANK); + ret += lsm6dsv_mem_bank_set(ctx, LSM6DSV_MAIN_MEM_BANK); return ret; } @@ -9375,12 +8810,12 @@ int32_t lsm6dsv16x_tilt_mode_get(stmdev_ctx_t *ctx, uint8_t *val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_timestamp_raw_get(stmdev_ctx_t *ctx, uint32_t *val) +int32_t lsm6dsv_timestamp_raw_get(stmdev_ctx_t *ctx, uint32_t *val) { uint8_t buff[4]; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_TIMESTAMP0, &buff[0], 4); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_TIMESTAMP0, &buff[0], 4); if (ret != 0) { return ret; } *val = buff[3]; @@ -9399,16 +8834,16 @@ int32_t lsm6dsv16x_timestamp_raw_get(stmdev_ctx_t *ctx, uint32_t *val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_timestamp_set(stmdev_ctx_t *ctx, uint8_t val) +int32_t lsm6dsv_timestamp_set(stmdev_ctx_t *ctx, uint8_t val) { - lsm6dsv16x_functions_enable_t functions_enable; + lsm6dsv_functions_enable_t functions_enable; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FUNCTIONS_ENABLE, (uint8_t *)&functions_enable, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_FUNCTIONS_ENABLE, (uint8_t *)&functions_enable, 1); if (ret == 0) { functions_enable.timestamp_en = val; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_FUNCTIONS_ENABLE, (uint8_t *)&functions_enable, 1); + ret = lsm6dsv_write_reg(ctx, LSM6DSV_FUNCTIONS_ENABLE, (uint8_t *)&functions_enable, 1); } return ret; @@ -9422,12 +8857,12 @@ int32_t lsm6dsv16x_timestamp_set(stmdev_ctx_t *ctx, uint8_t val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_timestamp_get(stmdev_ctx_t *ctx, uint8_t *val) +int32_t lsm6dsv_timestamp_get(stmdev_ctx_t *ctx, uint8_t *val) { - lsm6dsv16x_functions_enable_t functions_enable; + lsm6dsv_functions_enable_t functions_enable; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FUNCTIONS_ENABLE, (uint8_t *)&functions_enable, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_FUNCTIONS_ENABLE, (uint8_t *)&functions_enable, 1); *val = functions_enable.timestamp_en; return ret; @@ -9454,16 +8889,16 @@ int32_t lsm6dsv16x_timestamp_get(stmdev_ctx_t *ctx, uint8_t *val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_act_mode_set(stmdev_ctx_t *ctx, lsm6dsv16x_act_mode_t val) +int32_t lsm6dsv_act_mode_set(stmdev_ctx_t *ctx, lsm6dsv_act_mode_t val) { - lsm6dsv16x_functions_enable_t functions_enable; + lsm6dsv_functions_enable_t functions_enable; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FUNCTIONS_ENABLE, (uint8_t *)&functions_enable, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_FUNCTIONS_ENABLE, (uint8_t *)&functions_enable, 1); if (ret == 0) { functions_enable.inact_en = (uint8_t)val & 0x3U; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_FUNCTIONS_ENABLE, (uint8_t *)&functions_enable, 1); + ret = lsm6dsv_write_reg(ctx, LSM6DSV_FUNCTIONS_ENABLE, (uint8_t *)&functions_enable, 1); } return ret; @@ -9477,34 +8912,34 @@ int32_t lsm6dsv16x_act_mode_set(stmdev_ctx_t *ctx, lsm6dsv16x_act_mode_t val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_act_mode_get(stmdev_ctx_t *ctx, lsm6dsv16x_act_mode_t *val) +int32_t lsm6dsv_act_mode_get(stmdev_ctx_t *ctx, lsm6dsv_act_mode_t *val) { - lsm6dsv16x_functions_enable_t functions_enable; + lsm6dsv_functions_enable_t functions_enable; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_FUNCTIONS_ENABLE, (uint8_t *)&functions_enable, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_FUNCTIONS_ENABLE, (uint8_t *)&functions_enable, 1); if (ret != 0) { return ret; } switch (functions_enable.inact_en) { - case LSM6DSV16X_XL_AND_GY_NOT_AFFECTED: - *val = LSM6DSV16X_XL_AND_GY_NOT_AFFECTED; + case LSM6DSV_XL_AND_GY_NOT_AFFECTED: + *val = LSM6DSV_XL_AND_GY_NOT_AFFECTED; break; - case LSM6DSV16X_XL_LOW_POWER_GY_NOT_AFFECTED: - *val = LSM6DSV16X_XL_LOW_POWER_GY_NOT_AFFECTED; + case LSM6DSV_XL_LOW_POWER_GY_NOT_AFFECTED: + *val = LSM6DSV_XL_LOW_POWER_GY_NOT_AFFECTED; break; - case LSM6DSV16X_XL_LOW_POWER_GY_SLEEP: - *val = LSM6DSV16X_XL_LOW_POWER_GY_SLEEP; + case LSM6DSV_XL_LOW_POWER_GY_SLEEP: + *val = LSM6DSV_XL_LOW_POWER_GY_SLEEP; break; - case LSM6DSV16X_XL_LOW_POWER_GY_POWER_DOWN: - *val = LSM6DSV16X_XL_LOW_POWER_GY_POWER_DOWN; + case LSM6DSV_XL_LOW_POWER_GY_POWER_DOWN: + *val = LSM6DSV_XL_LOW_POWER_GY_POWER_DOWN; break; default: - *val = LSM6DSV16X_XL_AND_GY_NOT_AFFECTED; + *val = LSM6DSV_XL_AND_GY_NOT_AFFECTED; break; } @@ -9519,17 +8954,17 @@ int32_t lsm6dsv16x_act_mode_get(stmdev_ctx_t *ctx, lsm6dsv16x_act_mode_t *val) * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_act_from_sleep_to_act_dur_set(stmdev_ctx_t *ctx, - lsm6dsv16x_act_from_sleep_to_act_dur_t val) +int32_t lsm6dsv_act_from_sleep_to_act_dur_set(stmdev_ctx_t *ctx, + lsm6dsv_act_from_sleep_to_act_dur_t val) { - lsm6dsv16x_inactivity_dur_t inactivity_dur; + lsm6dsv_inactivity_dur_t inactivity_dur; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_INACTIVITY_DUR, (uint8_t *)&inactivity_dur, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_INACTIVITY_DUR, (uint8_t *)&inactivity_dur, 1); if (ret == 0) { inactivity_dur.inact_dur = (uint8_t)val & 0x3U; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_INACTIVITY_DUR, (uint8_t *)&inactivity_dur, 1); + ret = lsm6dsv_write_reg(ctx, LSM6DSV_INACTIVITY_DUR, (uint8_t *)&inactivity_dur, 1); } return ret; @@ -9543,35 +8978,35 @@ int32_t lsm6dsv16x_act_from_sleep_to_act_dur_set(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_act_from_sleep_to_act_dur_get(stmdev_ctx_t *ctx, - lsm6dsv16x_act_from_sleep_to_act_dur_t *val) +int32_t lsm6dsv_act_from_sleep_to_act_dur_get(stmdev_ctx_t *ctx, + lsm6dsv_act_from_sleep_to_act_dur_t *val) { - lsm6dsv16x_inactivity_dur_t inactivity_dur; + lsm6dsv_inactivity_dur_t inactivity_dur; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_INACTIVITY_DUR, (uint8_t *)&inactivity_dur, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_INACTIVITY_DUR, (uint8_t *)&inactivity_dur, 1); if (ret != 0) { return ret; } switch (inactivity_dur.inact_dur) { - case LSM6DSV16X_SLEEP_TO_ACT_AT_1ST_SAMPLE: - *val = LSM6DSV16X_SLEEP_TO_ACT_AT_1ST_SAMPLE; + case LSM6DSV_SLEEP_TO_ACT_AT_1ST_SAMPLE: + *val = LSM6DSV_SLEEP_TO_ACT_AT_1ST_SAMPLE; break; - case LSM6DSV16X_SLEEP_TO_ACT_AT_2ND_SAMPLE: - *val = LSM6DSV16X_SLEEP_TO_ACT_AT_2ND_SAMPLE; + case LSM6DSV_SLEEP_TO_ACT_AT_2ND_SAMPLE: + *val = LSM6DSV_SLEEP_TO_ACT_AT_2ND_SAMPLE; break; - case LSM6DSV16X_SLEEP_TO_ACT_AT_3RD_SAMPLE: - *val = LSM6DSV16X_SLEEP_TO_ACT_AT_3RD_SAMPLE; + case LSM6DSV_SLEEP_TO_ACT_AT_3RD_SAMPLE: + *val = LSM6DSV_SLEEP_TO_ACT_AT_3RD_SAMPLE; break; - case LSM6DSV16X_SLEEP_TO_ACT_AT_4th_SAMPLE: - *val = LSM6DSV16X_SLEEP_TO_ACT_AT_4th_SAMPLE; + case LSM6DSV_SLEEP_TO_ACT_AT_4th_SAMPLE: + *val = LSM6DSV_SLEEP_TO_ACT_AT_4th_SAMPLE; break; default: - *val = LSM6DSV16X_SLEEP_TO_ACT_AT_1ST_SAMPLE; + *val = LSM6DSV_SLEEP_TO_ACT_AT_1ST_SAMPLE; break; } @@ -9586,17 +9021,17 @@ int32_t lsm6dsv16x_act_from_sleep_to_act_dur_get(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_act_sleep_xl_odr_set(stmdev_ctx_t *ctx, - lsm6dsv16x_act_sleep_xl_odr_t val) +int32_t lsm6dsv_act_sleep_xl_odr_set(stmdev_ctx_t *ctx, + lsm6dsv_act_sleep_xl_odr_t val) { - lsm6dsv16x_inactivity_dur_t inactivity_dur; + lsm6dsv_inactivity_dur_t inactivity_dur; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_INACTIVITY_DUR, (uint8_t *)&inactivity_dur, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_INACTIVITY_DUR, (uint8_t *)&inactivity_dur, 1); if (ret == 0) { inactivity_dur.xl_inact_odr = (uint8_t)val & 0x03U; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_INACTIVITY_DUR, (uint8_t *)&inactivity_dur, 1); + ret = lsm6dsv_write_reg(ctx, LSM6DSV_INACTIVITY_DUR, (uint8_t *)&inactivity_dur, 1); } return ret; @@ -9610,35 +9045,35 @@ int32_t lsm6dsv16x_act_sleep_xl_odr_set(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_act_sleep_xl_odr_get(stmdev_ctx_t *ctx, - lsm6dsv16x_act_sleep_xl_odr_t *val) +int32_t lsm6dsv_act_sleep_xl_odr_get(stmdev_ctx_t *ctx, + lsm6dsv_act_sleep_xl_odr_t *val) { - lsm6dsv16x_inactivity_dur_t inactivity_dur; + lsm6dsv_inactivity_dur_t inactivity_dur; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_INACTIVITY_DUR, (uint8_t *)&inactivity_dur, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_INACTIVITY_DUR, (uint8_t *)&inactivity_dur, 1); if (ret != 0) { return ret; } switch (inactivity_dur.xl_inact_odr) { - case LSM6DSV16X_1Hz875: - *val = LSM6DSV16X_1Hz875; + case LSM6DSV_1Hz875: + *val = LSM6DSV_1Hz875; break; - case LSM6DSV16X_15Hz: - *val = LSM6DSV16X_15Hz; + case LSM6DSV_15Hz: + *val = LSM6DSV_15Hz; break; - case LSM6DSV16X_30Hz: - *val = LSM6DSV16X_30Hz; + case LSM6DSV_30Hz: + *val = LSM6DSV_30Hz; break; - case LSM6DSV16X_60Hz: - *val = LSM6DSV16X_60Hz; + case LSM6DSV_60Hz: + *val = LSM6DSV_60Hz; break; default: - *val = LSM6DSV16X_1Hz875; + *val = LSM6DSV_1Hz875; break; } @@ -9653,19 +9088,19 @@ int32_t lsm6dsv16x_act_sleep_xl_odr_get(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_act_thresholds_set(stmdev_ctx_t *ctx, - lsm6dsv16x_act_thresholds_t *val) +int32_t lsm6dsv_act_thresholds_set(stmdev_ctx_t *ctx, + lsm6dsv_act_thresholds_t *val) { - lsm6dsv16x_inactivity_ths_t inactivity_ths; - lsm6dsv16x_inactivity_dur_t inactivity_dur; - lsm6dsv16x_wake_up_ths_t wake_up_ths; - lsm6dsv16x_wake_up_dur_t wake_up_dur; + lsm6dsv_inactivity_ths_t inactivity_ths; + lsm6dsv_inactivity_dur_t inactivity_dur; + lsm6dsv_wake_up_ths_t wake_up_ths; + lsm6dsv_wake_up_dur_t wake_up_dur; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_INACTIVITY_DUR, (uint8_t *)&inactivity_dur, 1); - ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_INACTIVITY_THS, (uint8_t *)&inactivity_ths, 1); - ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_WAKE_UP_THS, (uint8_t *)&wake_up_ths, 1); - ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_WAKE_UP_DUR, (uint8_t *)&wake_up_dur, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_INACTIVITY_DUR, (uint8_t *)&inactivity_dur, 1); + ret += lsm6dsv_read_reg(ctx, LSM6DSV_INACTIVITY_THS, (uint8_t *)&inactivity_ths, 1); + ret += lsm6dsv_read_reg(ctx, LSM6DSV_WAKE_UP_THS, (uint8_t *)&wake_up_ths, 1); + ret += lsm6dsv_read_reg(ctx, LSM6DSV_WAKE_UP_DUR, (uint8_t *)&wake_up_dur, 1); if (ret != 0) { return ret; } inactivity_dur.wu_inact_ths_w = val->inactivity_cfg.wu_inact_ths_w; @@ -9676,10 +9111,10 @@ int32_t lsm6dsv16x_act_thresholds_set(stmdev_ctx_t *ctx, wake_up_ths.wk_ths = val->threshold; wake_up_dur.wake_dur = val->duration; - ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_INACTIVITY_DUR, (uint8_t *)&inactivity_dur, 1); - ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_INACTIVITY_THS, (uint8_t *)&inactivity_ths, 1); - ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_WAKE_UP_THS, (uint8_t *)&wake_up_ths, 1); - ret += lsm6dsv16x_write_reg(ctx, LSM6DSV16X_WAKE_UP_DUR, (uint8_t *)&wake_up_dur, 1); + ret += lsm6dsv_write_reg(ctx, LSM6DSV_INACTIVITY_DUR, (uint8_t *)&inactivity_dur, 1); + ret += lsm6dsv_write_reg(ctx, LSM6DSV_INACTIVITY_THS, (uint8_t *)&inactivity_ths, 1); + ret += lsm6dsv_write_reg(ctx, LSM6DSV_WAKE_UP_THS, (uint8_t *)&wake_up_ths, 1); + ret += lsm6dsv_write_reg(ctx, LSM6DSV_WAKE_UP_DUR, (uint8_t *)&wake_up_dur, 1); return ret; } @@ -9692,19 +9127,19 @@ int32_t lsm6dsv16x_act_thresholds_set(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_act_thresholds_get(stmdev_ctx_t *ctx, - lsm6dsv16x_act_thresholds_t *val) +int32_t lsm6dsv_act_thresholds_get(stmdev_ctx_t *ctx, + lsm6dsv_act_thresholds_t *val) { - lsm6dsv16x_inactivity_dur_t inactivity_dur; - lsm6dsv16x_inactivity_ths_t inactivity_ths; - lsm6dsv16x_wake_up_ths_t wake_up_ths; - lsm6dsv16x_wake_up_dur_t wake_up_dur; + lsm6dsv_inactivity_dur_t inactivity_dur; + lsm6dsv_inactivity_ths_t inactivity_ths; + lsm6dsv_wake_up_ths_t wake_up_ths; + lsm6dsv_wake_up_dur_t wake_up_dur; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_INACTIVITY_DUR, (uint8_t *)&inactivity_dur, 1); - ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_INACTIVITY_THS, (uint8_t *)&inactivity_ths, 1); - ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_WAKE_UP_THS, (uint8_t *)&wake_up_ths, 1); - ret += lsm6dsv16x_read_reg(ctx, LSM6DSV16X_WAKE_UP_DUR, (uint8_t *)&wake_up_dur, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_INACTIVITY_DUR, (uint8_t *)&inactivity_dur, 1); + ret += lsm6dsv_read_reg(ctx, LSM6DSV_INACTIVITY_THS, (uint8_t *)&inactivity_ths, 1); + ret += lsm6dsv_read_reg(ctx, LSM6DSV_WAKE_UP_THS, (uint8_t *)&wake_up_ths, 1); + ret += lsm6dsv_read_reg(ctx, LSM6DSV_WAKE_UP_DUR, (uint8_t *)&wake_up_dur, 1); if (ret != 0) { return ret; } val->inactivity_cfg.wu_inact_ths_w = inactivity_dur.wu_inact_ths_w; @@ -9726,18 +9161,18 @@ int32_t lsm6dsv16x_act_thresholds_get(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_act_wkup_time_windows_set(stmdev_ctx_t *ctx, - lsm6dsv16x_act_wkup_time_windows_t val) +int32_t lsm6dsv_act_wkup_time_windows_set(stmdev_ctx_t *ctx, + lsm6dsv_act_wkup_time_windows_t val) { - lsm6dsv16x_wake_up_dur_t wake_up_dur; + lsm6dsv_wake_up_dur_t wake_up_dur; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_WAKE_UP_DUR, (uint8_t *)&wake_up_dur, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_WAKE_UP_DUR, (uint8_t *)&wake_up_dur, 1); if (ret == 0) { wake_up_dur.wake_dur = val.shock; wake_up_dur.sleep_dur = val.quiet; - ret = lsm6dsv16x_write_reg(ctx, LSM6DSV16X_WAKE_UP_DUR, (uint8_t *)&wake_up_dur, 1); + ret = lsm6dsv_write_reg(ctx, LSM6DSV_WAKE_UP_DUR, (uint8_t *)&wake_up_dur, 1); } return ret; @@ -9751,13 +9186,13 @@ int32_t lsm6dsv16x_act_wkup_time_windows_set(stmdev_ctx_t *ctx, * @retval interface status (MANDATORY: return 0 -> no Error) * */ -int32_t lsm6dsv16x_act_wkup_time_windows_get(stmdev_ctx_t *ctx, - lsm6dsv16x_act_wkup_time_windows_t *val) +int32_t lsm6dsv_act_wkup_time_windows_get(stmdev_ctx_t *ctx, + lsm6dsv_act_wkup_time_windows_t *val) { - lsm6dsv16x_wake_up_dur_t wake_up_dur; + lsm6dsv_wake_up_dur_t wake_up_dur; int32_t ret; - ret = lsm6dsv16x_read_reg(ctx, LSM6DSV16X_WAKE_UP_DUR, (uint8_t *)&wake_up_dur, 1); + ret = lsm6dsv_read_reg(ctx, LSM6DSV_WAKE_UP_DUR, (uint8_t *)&wake_up_dur, 1); if (ret != 0) { return ret; } val->shock = wake_up_dur.wake_dur; @@ -9769,4 +9204,4 @@ int32_t lsm6dsv16x_act_wkup_time_windows_get(stmdev_ctx_t *ctx, /** * @} * - */ \ No newline at end of file + */ diff --git a/lib/LSM6DSV16X/lsm6dsv16x_reg.h b/lib/LSM6DSV16X/lsm6dsv_reg.h similarity index 51% rename from lib/LSM6DSV16X/lsm6dsv16x_reg.h rename to lib/LSM6DSV16X/lsm6dsv_reg.h index 8371a427a..8359f815c 100644 --- a/lib/LSM6DSV16X/lsm6dsv16x_reg.h +++ b/lib/LSM6DSV16X/lsm6dsv_reg.h @@ -1,9 +1,9 @@ /** ****************************************************************************** - * @file lsm6dsv16x_reg.h + * @file lsm6dsv_reg.h * @author Sensors Software Solution Team * @brief This file contains all the functions prototypes for the - * lsm6dsv16x_reg.c driver. + * lsm6dsv_reg.c driver. ****************************************************************************** * @attention * @@ -19,8 +19,8 @@ */ /* Define to prevent recursive inclusion -------------------------------------*/ -#ifndef LSM6DSV16X_REGS_H -#define LSM6DSV16X_REGS_H +#ifndef LSM6DSV_REGS_H +#define LSM6DSV_REGS_H #ifdef __cplusplus extern "C" { @@ -31,7 +31,7 @@ extern "C" { #include #include -/** @addtogroup LSM6DSV16X +/** @addtogroup LSM6DSV * @{ * */ @@ -163,17 +163,17 @@ typedef struct * */ -/** @defgroup LSM6DSV16X_Infos +/** @defgroup LSM6DSV_Infos * @{ * */ /** I2C Device Address 8 bit format if SA0=0 -> D5 if SA0=1 -> D7 **/ -#define LSM6DSV16X_I2C_ADD_L 0xD5U -#define LSM6DSV16X_I2C_ADD_H 0xD7U +#define LSM6DSV_I2C_ADD_L 0xD5U +#define LSM6DSV_I2C_ADD_H 0xD7U /** Device Identification (Who am I) **/ -#define LSM6DSV16X_ID 0x70U +#define LSM6DSV_ID 0x70U /** * @} @@ -185,7 +185,7 @@ typedef struct * */ -#define LSM6DSV16X_FUNC_CFG_ACCESS 0x1U +#define LSM6DSV_FUNC_CFG_ACCESS 0x1U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -205,9 +205,9 @@ typedef struct uint8_t spi2_reset : 1; uint8_t ois_ctrl_from_ui : 1; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_func_cfg_access_t; +} lsm6dsv_func_cfg_access_t; -#define LSM6DSV16X_PIN_CTRL 0x2U +#define LSM6DSV_PIN_CTRL 0x2U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -221,9 +221,9 @@ typedef struct uint8_t ibhr_por_en : 1; uint8_t not_used0 : 5; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_pin_ctrl_t; +} lsm6dsv_pin_ctrl_t; -#define LSM6DSV16X_IF_CFG 0x3U +#define LSM6DSV_IF_CFG 0x3U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -245,9 +245,9 @@ typedef struct uint8_t not_used0 : 1; uint8_t i2c_i3c_disable : 1; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_if_cfg_t; +} lsm6dsv_if_cfg_t; -#define LSM6DSV16X_ODR_TRIG_CFG 0x6U +#define LSM6DSV_ODR_TRIG_CFG 0x6U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -255,9 +255,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t odr_trig_nodr : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_odr_trig_cfg_t; +} lsm6dsv_odr_trig_cfg_t; -#define LSM6DSV16X_FIFO_CTRL1 0x7U +#define LSM6DSV_FIFO_CTRL1 0x7U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -265,9 +265,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t wtm : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_fifo_ctrl1_t; +} lsm6dsv_fifo_ctrl1_t; -#define LSM6DSV16X_FIFO_CTRL2 0x8U +#define LSM6DSV_FIFO_CTRL2 0x8U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -287,9 +287,9 @@ typedef struct uint8_t uncompr_rate : 2; uint8_t xl_dualc_batch_from_fsm : 1; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_fifo_ctrl2_t; +} lsm6dsv_fifo_ctrl2_t; -#define LSM6DSV16X_FIFO_CTRL3 0x9U +#define LSM6DSV_FIFO_CTRL3 0x9U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -299,9 +299,9 @@ typedef struct uint8_t bdr_gy : 4; uint8_t bdr_xl : 4; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_fifo_ctrl3_t; +} lsm6dsv_fifo_ctrl3_t; -#define LSM6DSV16X_FIFO_CTRL4 0x0AU +#define LSM6DSV_FIFO_CTRL4 0x0AU typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -315,9 +315,9 @@ typedef struct uint8_t g_eis_fifo_en : 1; uint8_t fifo_mode : 3; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_fifo_ctrl4_t; +} lsm6dsv_fifo_ctrl4_t; -#define LSM6DSV16X_COUNTER_BDR_REG1 0x0BU +#define LSM6DSV_COUNTER_BDR_REG1 0x0BU typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -331,9 +331,9 @@ typedef struct uint8_t not_used0 : 3; uint8_t cnt_bdr_th : 2; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_counter_bdr_reg1_t; +} lsm6dsv_counter_bdr_reg1_t; -#define LSM6DSV16X_COUNTER_BDR_REG2 0x0CU +#define LSM6DSV_COUNTER_BDR_REG2 0x0CU typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -341,9 +341,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t cnt_bdr_th : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_counter_bdr_reg2_t; +} lsm6dsv_counter_bdr_reg2_t; -#define LSM6DSV16X_INT1_CTRL 0x0DU +#define LSM6DSV_INT1_CTRL 0x0DU typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -365,9 +365,9 @@ typedef struct uint8_t int1_drdy_g : 1; uint8_t int1_drdy_xl : 1; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_int1_ctrl_t; +} lsm6dsv_int1_ctrl_t; -#define LSM6DSV16X_INT2_CTRL 0x0EU +#define LSM6DSV_INT2_CTRL 0x0EU typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -389,9 +389,9 @@ typedef struct uint8_t int2_drdy_g : 1; uint8_t int2_drdy_xl : 1; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_int2_ctrl_t; +} lsm6dsv_int2_ctrl_t; -#define LSM6DSV16X_WHO_AM_I 0x0FU +#define LSM6DSV_WHO_AM_I 0x0FU typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -399,9 +399,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t id : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_who_am_i_t; +} lsm6dsv_who_am_i_t; -#define LSM6DSV16X_CTRL1 0x10U +#define LSM6DSV_CTRL1 0x10U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -413,9 +413,9 @@ typedef struct uint8_t op_mode_xl : 3; uint8_t odr_xl : 4; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_ctrl1_t; +} lsm6dsv_ctrl1_t; -#define LSM6DSV16X_CTRL2 0x11U +#define LSM6DSV_CTRL2 0x11U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -427,9 +427,9 @@ typedef struct uint8_t op_mode_g : 3; uint8_t odr_g : 4; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_ctrl2_t; +} lsm6dsv_ctrl2_t; -#define LSM6DSV16X_CTRL3 0x12U +#define LSM6DSV_CTRL3 0x12U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -447,9 +447,9 @@ typedef struct uint8_t not_used0 : 1; uint8_t sw_reset : 1; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_ctrl3_t; +} lsm6dsv_ctrl3_t; -#define LSM6DSV16X_CTRL4 0x13U +#define LSM6DSV_CTRL4 0x13U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -467,9 +467,9 @@ typedef struct uint8_t drdy_pulsed : 1; uint8_t int2_in_lh : 1; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_ctrl4_t; +} lsm6dsv_ctrl4_t; -#define LSM6DSV16X_CTRL5 0x14U +#define LSM6DSV_CTRL5 0x14U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -481,9 +481,9 @@ typedef struct uint8_t bus_act_sel : 2; uint8_t int_en_i3c : 1; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_ctrl5_t; +} lsm6dsv_ctrl5_t; -#define LSM6DSV16X_CTRL6 0x15U +#define LSM6DSV_CTRL6 0x15U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -495,27 +495,21 @@ typedef struct uint8_t lpf1_g_bw : 3; uint8_t fs_g : 4; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_ctrl6_t; +} lsm6dsv_ctrl6_t; -#define LSM6DSV16X_CTRL7 0x16U +#define LSM6DSV_CTRL7 0x16U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN uint8_t lpf1_g_en : 1; - uint8_t not_used0 : 3; - uint8_t ah_qvar_c_zin : 2; - uint8_t int2_drdy_ah_qvar : 1; - uint8_t ah_qvar_en : 1; + uint8_t not_used0 : 7; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t ah_qvar_en : 1; - uint8_t int2_drdy_ah_qvar : 1; - uint8_t ah_qvar_c_zin : 2; - uint8_t not_used0 : 3; + uint8_t not_used0 : 7; uint8_t lpf1_g_en : 1; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_ctrl7_t; +} lsm6dsv_ctrl7_t; -#define LSM6DSV16X_CTRL8 0x17U +#define LSM6DSV_CTRL8 0x17U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -531,9 +525,9 @@ typedef struct uint8_t not_used0 : 1; uint8_t fs_xl : 2; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_ctrl8_t; +} lsm6dsv_ctrl8_t; -#define LSM6DSV16X_CTRL9 0x18U +#define LSM6DSV_CTRL9 0x18U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -555,9 +549,9 @@ typedef struct uint8_t usr_off_w : 1; uint8_t usr_off_on_out : 1; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_ctrl9_t; +} lsm6dsv_ctrl9_t; -#define LSM6DSV16X_CTRL10 0x19U +#define LSM6DSV_CTRL10 0x19U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -573,9 +567,9 @@ typedef struct uint8_t st_g : 2; uint8_t st_xl : 2; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_ctrl10_t; +} lsm6dsv_ctrl10_t; -#define LSM6DSV16X_CTRL_STATUS 0x1AU +#define LSM6DSV_CTRL_STATUS 0x1AU typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -587,9 +581,9 @@ typedef struct uint8_t fsm_wr_ctrl_status : 1; uint8_t not_used0 : 2; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_ctrl_status_t; +} lsm6dsv_ctrl_status_t; -#define LSM6DSV16X_FIFO_STATUS1 0x1BU +#define LSM6DSV_FIFO_STATUS1 0x1BU typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -597,9 +591,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t diff_fifo : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_fifo_status1_t; +} lsm6dsv_fifo_status1_t; -#define LSM6DSV16X_FIFO_STATUS2 0x1CU +#define LSM6DSV_FIFO_STATUS2 0x1CU typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -619,9 +613,9 @@ typedef struct uint8_t not_used0 : 2; uint8_t diff_fifo : 1; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_fifo_status2_t; +} lsm6dsv_fifo_status2_t; -#define LSM6DSV16X_ALL_INT_SRC 0x1DU +#define LSM6DSV_ALL_INT_SRC 0x1DU typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -643,16 +637,16 @@ typedef struct uint8_t wu_ia : 1; uint8_t ff_ia : 1; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_all_int_src_t; +} lsm6dsv_all_int_src_t; -#define LSM6DSV16X_STATUS_REG 0x1EU +#define LSM6DSV_STATUS_REG 0x1EU typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN uint8_t xlda : 1; uint8_t gda : 1; uint8_t tda : 1; - uint8_t ah_qvarda : 1; + uint8_t not_used1 : 1; uint8_t gda_eis : 1; uint8_t ois_drdy : 1; uint8_t not_used0 : 1; @@ -662,14 +656,14 @@ typedef struct uint8_t not_used0 : 1; uint8_t ois_drdy : 1; uint8_t gda_eis : 1; - uint8_t ah_qvarda : 1; + uint8_t not_used1 : 1; uint8_t tda : 1; uint8_t gda : 1; uint8_t xlda : 1; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_status_reg_t; +} lsm6dsv_status_reg_t; -#define LSM6DSV16X_OUT_TEMP_L 0x20U +#define LSM6DSV_OUT_TEMP_L 0x20U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -677,9 +671,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t temp : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_out_temp_l_t; +} lsm6dsv_out_temp_l_t; -#define LSM6DSV16X_OUT_TEMP_H 0x21U +#define LSM6DSV_OUT_TEMP_H 0x21U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -687,9 +681,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t temp : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_out_temp_h_t; +} lsm6dsv_out_temp_h_t; -#define LSM6DSV16X_OUTX_L_G 0x22U +#define LSM6DSV_OUTX_L_G 0x22U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -697,9 +691,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t outx_g : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_outx_l_g_t; +} lsm6dsv_outx_l_g_t; -#define LSM6DSV16X_OUTX_H_G 0x23U +#define LSM6DSV_OUTX_H_G 0x23U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -707,9 +701,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t outx_g : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_outx_h_g_t; +} lsm6dsv_outx_h_g_t; -#define LSM6DSV16X_OUTY_L_G 0x24U +#define LSM6DSV_OUTY_L_G 0x24U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -717,9 +711,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t outy_g : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_outy_l_g_t; +} lsm6dsv_outy_l_g_t; -#define LSM6DSV16X_OUTY_H_G 0x25U +#define LSM6DSV_OUTY_H_G 0x25U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -727,9 +721,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t outy_g : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_outy_h_g_t; +} lsm6dsv_outy_h_g_t; -#define LSM6DSV16X_OUTZ_L_G 0x26U +#define LSM6DSV_OUTZ_L_G 0x26U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -737,9 +731,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t outz_g : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_outz_l_g_t; +} lsm6dsv_outz_l_g_t; -#define LSM6DSV16X_OUTZ_H_G 0x27U +#define LSM6DSV_OUTZ_H_G 0x27U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -747,9 +741,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t outz_g : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_outz_h_g_t; +} lsm6dsv_outz_h_g_t; -#define LSM6DSV16X_OUTX_L_A 0x28U +#define LSM6DSV_OUTX_L_A 0x28U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -757,9 +751,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t outx_a : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_outx_l_a_t; +} lsm6dsv_outx_l_a_t; -#define LSM6DSV16X_OUTX_H_A 0x29U +#define LSM6DSV_OUTX_H_A 0x29U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -767,9 +761,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t outx_a : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_outx_h_a_t; +} lsm6dsv_outx_h_a_t; -#define LSM6DSV16X_OUTY_L_A 0x2AU +#define LSM6DSV_OUTY_L_A 0x2AU typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -777,9 +771,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t outy_a : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_outy_l_a_t; +} lsm6dsv_outy_l_a_t; -#define LSM6DSV16X_OUTY_H_A 0x2BU +#define LSM6DSV_OUTY_H_A 0x2BU typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -787,9 +781,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t outy_a : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_outy_h_a_t; +} lsm6dsv_outy_h_a_t; -#define LSM6DSV16X_OUTZ_L_A 0x2CU +#define LSM6DSV_OUTZ_L_A 0x2CU typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -797,9 +791,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t outz_a : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_outz_l_a_t; +} lsm6dsv_outz_l_a_t; -#define LSM6DSV16X_OUTZ_H_A 0x2DU +#define LSM6DSV_OUTZ_H_A 0x2DU typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -807,9 +801,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t outz_a : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_outz_h_a_t; +} lsm6dsv_outz_h_a_t; -#define LSM6DSV16X_UI_OUTX_L_G_OIS_EIS 0x2EU +#define LSM6DSV_UI_OUTX_L_G_OIS_EIS 0x2EU typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -817,9 +811,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t ui_outx_g_ois_eis : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_ui_outx_l_g_ois_eis_t; +} lsm6dsv_ui_outx_l_g_ois_eis_t; -#define LSM6DSV16X_UI_OUTX_H_G_OIS_EIS 0x2FU +#define LSM6DSV_UI_OUTX_H_G_OIS_EIS 0x2FU typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -827,9 +821,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t ui_outx_g_ois_eis : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_ui_outx_h_g_ois_eis_t; +} lsm6dsv_ui_outx_h_g_ois_eis_t; -#define LSM6DSV16X_UI_OUTY_L_G_OIS_EIS 0x30U +#define LSM6DSV_UI_OUTY_L_G_OIS_EIS 0x30U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -837,9 +831,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t ui_outy_g_ois_eis : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_ui_outy_l_g_ois_eis_t; +} lsm6dsv_ui_outy_l_g_ois_eis_t; -#define LSM6DSV16X_UI_OUTY_H_G_OIS_EIS 0x31U +#define LSM6DSV_UI_OUTY_H_G_OIS_EIS 0x31U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -847,9 +841,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t ui_outy_g_ois_eis : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_ui_outy_h_g_ois_eis_t; +} lsm6dsv_ui_outy_h_g_ois_eis_t; -#define LSM6DSV16X_UI_OUTZ_L_G_OIS_EIS 0x32U +#define LSM6DSV_UI_OUTZ_L_G_OIS_EIS 0x32U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -857,9 +851,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t ui_outz_g_ois_eis : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_ui_outz_l_g_ois_eis_t; +} lsm6dsv_ui_outz_l_g_ois_eis_t; -#define LSM6DSV16X_UI_OUTZ_H_G_OIS_EIS 0x33U +#define LSM6DSV_UI_OUTZ_H_G_OIS_EIS 0x33U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -867,9 +861,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t ui_outz_g_ois_eis : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_ui_outz_h_g_ois_eis_t; +} lsm6dsv_ui_outz_h_g_ois_eis_t; -#define LSM6DSV16X_UI_OUTX_L_A_OIS_DUALC 0x34U +#define LSM6DSV_UI_OUTX_L_A_OIS_DUALC 0x34U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -877,9 +871,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t ui_outx_a_ois_dualc : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_ui_outx_l_a_ois_dualc_t; +} lsm6dsv_ui_outx_l_a_ois_dualc_t; -#define LSM6DSV16X_UI_OUTX_H_A_OIS_DUALC 0x35U +#define LSM6DSV_UI_OUTX_H_A_OIS_DUALC 0x35U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -887,9 +881,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t ui_outx_a_ois_dualc : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_ui_outx_h_a_ois_dualc_t; +} lsm6dsv_ui_outx_h_a_ois_dualc_t; -#define LSM6DSV16X_UI_OUTY_L_A_OIS_DUALC 0x36U +#define LSM6DSV_UI_OUTY_L_A_OIS_DUALC 0x36U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -897,9 +891,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t ui_outy_a_ois_dualc : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_ui_outy_l_a_ois_dualc_t; +} lsm6dsv_ui_outy_l_a_ois_dualc_t; -#define LSM6DSV16X_UI_OUTY_H_A_OIS_DUALC 0x37U +#define LSM6DSV_UI_OUTY_H_A_OIS_DUALC 0x37U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -907,9 +901,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t ui_outy_a_ois_dualc : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_ui_outy_h_a_ois_dualc_t; +} lsm6dsv_ui_outy_h_a_ois_dualc_t; -#define LSM6DSV16X_UI_OUTZ_L_A_OIS_DUALC 0x38U +#define LSM6DSV_UI_OUTZ_L_A_OIS_DUALC 0x38U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -917,9 +911,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t ui_outz_a_ois_dualc : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_ui_outz_l_a_ois_dualc_t; +} lsm6dsv_ui_outz_l_a_ois_dualc_t; -#define LSM6DSV16X_UI_OUTZ_H_A_OIS_DUALC 0x39U +#define LSM6DSV_UI_OUTZ_H_A_OIS_DUALC 0x39U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -927,29 +921,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t ui_outz_a_ois_dualc : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_ui_outz_h_a_ois_dualc_t; - -#define LSM6DSV16X_AH_QVAR_OUT_L 0x3AU -typedef struct -{ -#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t ah_qvar : 8; -#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t ah_qvar : 8; -#endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_ah_qvar_out_l_t; +} lsm6dsv_ui_outz_h_a_ois_dualc_t; -#define LSM6DSV16X_AH_QVAR_OUT_H 0x3BU -typedef struct -{ -#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t ah_qvar : 8; -#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t ah_qvar : 8; -#endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_ah_qvar_out_h_t; - -#define LSM6DSV16X_TIMESTAMP0 0x40U +#define LSM6DSV_TIMESTAMP0 0x40U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -957,9 +931,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t timestamp : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_timestamp0_t; +} lsm6dsv_timestamp0_t; -#define LSM6DSV16X_TIMESTAMP1 0x41U +#define LSM6DSV_TIMESTAMP1 0x41U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -967,9 +941,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t timestamp : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_timestamp1_t; +} lsm6dsv_timestamp1_t; -#define LSM6DSV16X_TIMESTAMP2 0x42U +#define LSM6DSV_TIMESTAMP2 0x42U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -977,9 +951,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t timestamp : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_timestamp2_t; +} lsm6dsv_timestamp2_t; -#define LSM6DSV16X_TIMESTAMP3 0x43U +#define LSM6DSV_TIMESTAMP3 0x43U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -987,9 +961,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t timestamp : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_timestamp3_t; +} lsm6dsv_timestamp3_t; -#define LSM6DSV16X_UI_STATUS_REG_OIS 0x44U +#define LSM6DSV_UI_STATUS_REG_OIS 0x44U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -1003,9 +977,9 @@ typedef struct uint8_t gda_ois : 1; uint8_t xlda_ois : 1; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_ui_status_reg_ois_t; +} lsm6dsv_ui_status_reg_ois_t; -#define LSM6DSV16X_WAKE_UP_SRC 0x45U +#define LSM6DSV_WAKE_UP_SRC 0x45U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -1027,9 +1001,9 @@ typedef struct uint8_t y_wu : 1; uint8_t z_wu : 1; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_wake_up_src_t; +} lsm6dsv_wake_up_src_t; -#define LSM6DSV16X_TAP_SRC 0x46U +#define LSM6DSV_TAP_SRC 0x46U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -1051,9 +1025,9 @@ typedef struct uint8_t y_tap : 1; uint8_t z_tap : 1; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_tap_src_t; +} lsm6dsv_tap_src_t; -#define LSM6DSV16X_D6D_SRC 0x47U +#define LSM6DSV_D6D_SRC 0x47U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -1075,9 +1049,9 @@ typedef struct uint8_t xh : 1; uint8_t xl : 1; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_d6d_src_t; +} lsm6dsv_d6d_src_t; -#define LSM6DSV16X_STATUS_MASTER_MAINPAGE 0x48U +#define LSM6DSV_STATUS_MASTER_MAINPAGE 0x48U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -1097,9 +1071,9 @@ typedef struct uint8_t not_used0 : 2; uint8_t sens_hub_endop : 1; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_status_master_mainpage_t; +} lsm6dsv_status_master_mainpage_t; -#define LSM6DSV16X_EMB_FUNC_STATUS_MAINPAGE 0x49U +#define LSM6DSV_EMB_FUNC_STATUS_MAINPAGE 0x49U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -1117,9 +1091,9 @@ typedef struct uint8_t is_step_det : 1; uint8_t not_used0 : 3; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_emb_func_status_mainpage_t; +} lsm6dsv_emb_func_status_mainpage_t; -#define LSM6DSV16X_FSM_STATUS_MAINPAGE 0x4AU +#define LSM6DSV_FSM_STATUS_MAINPAGE 0x4AU typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -1141,27 +1115,9 @@ typedef struct uint8_t is_fsm2 : 1; uint8_t is_fsm1 : 1; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_fsm_status_mainpage_t; - -#define LSM6DSV16X_MLC_STATUS_MAINPAGE 0x4BU -typedef struct -{ -#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t is_mlc1 : 1; - uint8_t is_mlc2 : 1; - uint8_t is_mlc3 : 1; - uint8_t is_mlc4 : 1; - uint8_t not_used0 : 4; -#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t not_used0 : 4; - uint8_t is_mlc4 : 1; - uint8_t is_mlc3 : 1; - uint8_t is_mlc2 : 1; - uint8_t is_mlc1 : 1; -#endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_mlc_status_mainpage_t; +} lsm6dsv_fsm_status_mainpage_t; -#define LSM6DSV16X_INTERNAL_FREQ 0x4FU +#define LSM6DSV_INTERNAL_FREQ 0x4FU typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -1169,9 +1125,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t freq_fine : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_internal_freq_t; +} lsm6dsv_internal_freq_t; -#define LSM6DSV16X_FUNCTIONS_ENABLE 0x50U +#define LSM6DSV_FUNCTIONS_ENABLE 0x50U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -1189,9 +1145,9 @@ typedef struct uint8_t not_used0 : 1; uint8_t inact_en : 2; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_functions_enable_t; +} lsm6dsv_functions_enable_t; -#define LSM6DSV16X_DEN 0x51U +#define LSM6DSV_DEN 0x51U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -1213,9 +1169,9 @@ typedef struct uint8_t den_z : 1; uint8_t den_xl_g : 1; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_den_t; +} lsm6dsv_den_t; -#define LSM6DSV16X_INACTIVITY_DUR 0x54U +#define LSM6DSV_INACTIVITY_DUR 0x54U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -1229,9 +1185,9 @@ typedef struct uint8_t xl_inact_odr : 2; uint8_t inact_dur : 2; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_inactivity_dur_t; +} lsm6dsv_inactivity_dur_t; -#define LSM6DSV16X_INACTIVITY_THS 0x55U +#define LSM6DSV_INACTIVITY_THS 0x55U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -1241,9 +1197,9 @@ typedef struct uint8_t not_used0 : 2; uint8_t inact_ths : 6; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_inactivity_ths_t; +} lsm6dsv_inactivity_ths_t; -#define LSM6DSV16X_TAP_CFG0 0x56U +#define LSM6DSV_TAP_CFG0 0x56U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -1265,9 +1221,9 @@ typedef struct uint8_t tap_z_en : 1; uint8_t lir : 1; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_tap_cfg0_t; +} lsm6dsv_tap_cfg0_t; -#define LSM6DSV16X_TAP_CFG1 0x57U +#define LSM6DSV_TAP_CFG1 0x57U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -1277,9 +1233,9 @@ typedef struct uint8_t tap_priority : 3; uint8_t tap_ths_x : 5; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_tap_cfg1_t; +} lsm6dsv_tap_cfg1_t; -#define LSM6DSV16X_TAP_CFG2 0x58U +#define LSM6DSV_TAP_CFG2 0x58U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -1289,9 +1245,9 @@ typedef struct uint8_t not_used0 : 3; uint8_t tap_ths_y : 5; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_tap_cfg2_t; +} lsm6dsv_tap_cfg2_t; -#define LSM6DSV16X_TAP_THS_6D 0x59U +#define LSM6DSV_TAP_THS_6D 0x59U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -1303,9 +1259,9 @@ typedef struct uint8_t sixd_ths : 2; uint8_t tap_ths_z : 5; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_tap_ths_6d_t; +} lsm6dsv_tap_ths_6d_t; -#define LSM6DSV16X_TAP_DUR 0x5AU +#define LSM6DSV_TAP_DUR 0x5AU typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -1317,9 +1273,9 @@ typedef struct uint8_t quiet : 2; uint8_t shock : 2; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_tap_dur_t; +} lsm6dsv_tap_dur_t; -#define LSM6DSV16X_WAKE_UP_THS 0x5BU +#define LSM6DSV_WAKE_UP_THS 0x5BU typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -1331,9 +1287,9 @@ typedef struct uint8_t usr_off_on_wu : 1; uint8_t wk_ths : 6; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_wake_up_ths_t; +} lsm6dsv_wake_up_ths_t; -#define LSM6DSV16X_WAKE_UP_DUR 0x5CU +#define LSM6DSV_WAKE_UP_DUR 0x5CU typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -1347,9 +1303,9 @@ typedef struct uint8_t not_used0 : 1; uint8_t sleep_dur : 4; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_wake_up_dur_t; +} lsm6dsv_wake_up_dur_t; -#define LSM6DSV16X_FREE_FALL 0x5DU +#define LSM6DSV_FREE_FALL 0x5DU typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -1359,9 +1315,9 @@ typedef struct uint8_t ff_dur : 5; uint8_t ff_ths : 3; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_free_fall_t; +} lsm6dsv_free_fall_t; -#define LSM6DSV16X_MD1_CFG 0x5EU +#define LSM6DSV_MD1_CFG 0x5EU typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -1383,9 +1339,9 @@ typedef struct uint8_t int1_emb_func : 1; uint8_t int1_shub : 1; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_md1_cfg_t; +} lsm6dsv_md1_cfg_t; -#define LSM6DSV16X_MD2_CFG 0x5FU +#define LSM6DSV_MD2_CFG 0x5FU typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -1407,9 +1363,9 @@ typedef struct uint8_t int2_emb_func : 1; uint8_t int2_timestamp : 1; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_md2_cfg_t; +} lsm6dsv_md2_cfg_t; -#define LSM6DSV16X_HAODR_CFG 0x62U +#define LSM6DSV_HAODR_CFG 0x62U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -1419,9 +1375,9 @@ typedef struct uint8_t not_used0 : 6; uint8_t haodr_sel : 2; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_haodr_cfg_t; +} lsm6dsv_haodr_cfg_t; -#define LSM6DSV16X_EMB_FUNC_CFG 0x63U +#define LSM6DSV_EMB_FUNC_CFG 0x63U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -1437,9 +1393,9 @@ typedef struct uint8_t emb_func_disable : 1; uint8_t not_used0 : 3; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_emb_func_cfg_t; +} lsm6dsv_emb_func_cfg_t; -#define LSM6DSV16X_UI_HANDSHAKE_CTRL 0x64U +#define LSM6DSV_UI_HANDSHAKE_CTRL 0x64U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -1451,9 +1407,9 @@ typedef struct uint8_t ui_shared_ack : 1; uint8_t ui_shared_req : 1; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_ui_handshake_ctrl_t; +} lsm6dsv_ui_handshake_ctrl_t; -#define LSM6DSV16X_UI_SPI2_SHARED_0 0x65U +#define LSM6DSV_UI_SPI2_SHARED_0 0x65U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -1461,9 +1417,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t ui_spi2_shared : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_ui_spi2_shared_0_t; +} lsm6dsv_ui_spi2_shared_0_t; -#define LSM6DSV16X_UI_SPI2_SHARED_1 0x66U +#define LSM6DSV_UI_SPI2_SHARED_1 0x66U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -1471,9 +1427,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t ui_spi2_shared : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_ui_spi2_shared_1_t; +} lsm6dsv_ui_spi2_shared_1_t; -#define LSM6DSV16X_UI_SPI2_SHARED_2 0x67U +#define LSM6DSV_UI_SPI2_SHARED_2 0x67U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -1481,9 +1437,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t ui_spi2_shared : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_ui_spi2_shared_2_t; +} lsm6dsv_ui_spi2_shared_2_t; -#define LSM6DSV16X_UI_SPI2_SHARED_3 0x68U +#define LSM6DSV_UI_SPI2_SHARED_3 0x68U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -1491,9 +1447,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t ui_spi2_shared : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_ui_spi2_shared_3_t; +} lsm6dsv_ui_spi2_shared_3_t; -#define LSM6DSV16X_UI_SPI2_SHARED_4 0x69U +#define LSM6DSV_UI_SPI2_SHARED_4 0x69U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -1501,9 +1457,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t ui_spi2_shared : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_ui_spi2_shared_4_t; +} lsm6dsv_ui_spi2_shared_4_t; -#define LSM6DSV16X_UI_SPI2_SHARED_5 0x6AU +#define LSM6DSV_UI_SPI2_SHARED_5 0x6AU typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -1511,9 +1467,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t ui_spi2_shared : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_ui_spi2_shared_5_t; +} lsm6dsv_ui_spi2_shared_5_t; -#define LSM6DSV16X_CTRL_EIS 0x6BU +#define LSM6DSV_CTRL_EIS 0x6BU typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -1529,9 +1485,9 @@ typedef struct uint8_t g_eis_on_g_ois_out_reg : 1; uint8_t fs_g_eis : 3; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_ctrl_eis_t; +} lsm6dsv_ctrl_eis_t; -#define LSM6DSV16X_UI_INT_OIS 0x6FU +#define LSM6DSV_UI_INT_OIS 0x6FU typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -1547,9 +1503,9 @@ typedef struct uint8_t st_ois_clampdis : 1; uint8_t not_used0 : 4; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_ui_int_ois_t; +} lsm6dsv_ui_int_ois_t; -#define LSM6DSV16X_UI_CTRL1_OIS 0x70U +#define LSM6DSV_UI_CTRL1_OIS 0x70U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -1567,9 +1523,9 @@ typedef struct uint8_t ois_g_en : 1; uint8_t spi2_read_en : 1; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_ui_ctrl1_ois_t; +} lsm6dsv_ui_ctrl1_ois_t; -#define LSM6DSV16X_UI_CTRL2_OIS 0x71U +#define LSM6DSV_UI_CTRL2_OIS 0x71U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -1581,9 +1537,9 @@ typedef struct uint8_t lpf1_g_ois_bw : 2; uint8_t fs_g_ois : 3; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_ui_ctrl2_ois_t; +} lsm6dsv_ui_ctrl2_ois_t; -#define LSM6DSV16X_UI_CTRL3_OIS 0x72U +#define LSM6DSV_UI_CTRL3_OIS 0x72U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -1597,9 +1553,9 @@ typedef struct uint8_t not_used0 : 1; uint8_t fs_xl_ois : 2; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_ui_ctrl3_ois_t; +} lsm6dsv_ui_ctrl3_ois_t; -#define LSM6DSV16X_X_OFS_USR 0x73U +#define LSM6DSV_X_OFS_USR 0x73U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -1607,9 +1563,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t x_ofs_usr : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_x_ofs_usr_t; +} lsm6dsv_x_ofs_usr_t; -#define LSM6DSV16X_Y_OFS_USR 0x74U +#define LSM6DSV_Y_OFS_USR 0x74U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -1617,9 +1573,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t y_ofs_usr : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_y_ofs_usr_t; +} lsm6dsv_y_ofs_usr_t; -#define LSM6DSV16X_Z_OFS_USR 0x75U +#define LSM6DSV_Z_OFS_USR 0x75U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -1627,9 +1583,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t z_ofs_usr : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_z_ofs_usr_t; +} lsm6dsv_z_ofs_usr_t; -#define LSM6DSV16X_FIFO_DATA_OUT_TAG 0x78U +#define LSM6DSV_FIFO_DATA_OUT_TAG 0x78U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -1641,9 +1597,9 @@ typedef struct uint8_t tag_cnt : 2; uint8_t not_used0 : 1; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_fifo_data_out_tag_t; +} lsm6dsv_fifo_data_out_tag_t; -#define LSM6DSV16X_FIFO_DATA_OUT_X_L 0x79U +#define LSM6DSV_FIFO_DATA_OUT_X_L 0x79U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -1651,9 +1607,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t fifo_data_out : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_fifo_data_out_x_l_t; +} lsm6dsv_fifo_data_out_x_l_t; -#define LSM6DSV16X_FIFO_DATA_OUT_X_H 0x7AU +#define LSM6DSV_FIFO_DATA_OUT_X_H 0x7AU typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -1661,9 +1617,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t fifo_data_out : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_fifo_data_out_x_h_t; +} lsm6dsv_fifo_data_out_x_h_t; -#define LSM6DSV16X_FIFO_DATA_OUT_Y_L 0x7BU +#define LSM6DSV_FIFO_DATA_OUT_Y_L 0x7BU typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -1671,9 +1627,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t fifo_data_out : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_fifo_data_out_y_l_t; +} lsm6dsv_fifo_data_out_y_l_t; -#define LSM6DSV16X_FIFO_DATA_OUT_Y_H 0x7CU +#define LSM6DSV_FIFO_DATA_OUT_Y_H 0x7CU typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -1681,9 +1637,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t fifo_data_out : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_fifo_data_out_y_h_t; +} lsm6dsv_fifo_data_out_y_h_t; -#define LSM6DSV16X_FIFO_DATA_OUT_Z_L 0x7DU +#define LSM6DSV_FIFO_DATA_OUT_Z_L 0x7DU typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -1691,9 +1647,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t fifo_data_out : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_fifo_data_out_z_l_t; +} lsm6dsv_fifo_data_out_z_l_t; -#define LSM6DSV16X_FIFO_DATA_OUT_Z_H 0x7EU +#define LSM6DSV_FIFO_DATA_OUT_Z_H 0x7EU typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -1701,7 +1657,7 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t fifo_data_out : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_fifo_data_out_z_h_t; +} lsm6dsv_fifo_data_out_z_h_t; /** * @} @@ -1713,7 +1669,7 @@ typedef struct * */ -#define LSM6DSV16X_SPI2_WHO_AM_I 0x0FU +#define LSM6DSV_SPI2_WHO_AM_I 0x0FU typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -1721,9 +1677,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t id : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_spi2_who_am_i_t; +} lsm6dsv_spi2_who_am_i_t; -#define LSM6DSV16X_SPI2_STATUS_REG_OIS 0x1EU +#define LSM6DSV_SPI2_STATUS_REG_OIS 0x1EU typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -1737,9 +1693,9 @@ typedef struct uint8_t gda : 1; uint8_t xlda : 1; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_spi2_status_reg_ois_t; +} lsm6dsv_spi2_status_reg_ois_t; -#define LSM6DSV16X_SPI2_OUT_TEMP_L 0x20U +#define LSM6DSV_SPI2_OUT_TEMP_L 0x20U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -1747,9 +1703,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t temp : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_spi2_out_temp_l_t; +} lsm6dsv_spi2_out_temp_l_t; -#define LSM6DSV16X_SPI2_OUT_TEMP_H 0x21U +#define LSM6DSV_SPI2_OUT_TEMP_H 0x21U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -1757,9 +1713,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t temp : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_spi2_out_temp_h_t; +} lsm6dsv_spi2_out_temp_h_t; -#define LSM6DSV16X_SPI2_OUTX_L_G_OIS 0x22U +#define LSM6DSV_SPI2_OUTX_L_G_OIS 0x22U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -1767,9 +1723,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t spi2_outx_g_ois : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_spi2_outx_l_g_ois_t; +} lsm6dsv_spi2_outx_l_g_ois_t; -#define LSM6DSV16X_SPI2_OUTX_H_G_OIS 0x23U +#define LSM6DSV_SPI2_OUTX_H_G_OIS 0x23U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -1777,9 +1733,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t spi2_outx_g_ois : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_spi2_outx_h_g_ois_t; +} lsm6dsv_spi2_outx_h_g_ois_t; -#define LSM6DSV16X_SPI2_OUTY_L_G_OIS 0x24U +#define LSM6DSV_SPI2_OUTY_L_G_OIS 0x24U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -1787,9 +1743,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t spi2_outy_g_ois : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_spi2_outy_l_g_ois_t; +} lsm6dsv_spi2_outy_l_g_ois_t; -#define LSM6DSV16X_SPI2_OUTY_H_G_OIS 0x25U +#define LSM6DSV_SPI2_OUTY_H_G_OIS 0x25U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -1797,9 +1753,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t spi2_outy_g_ois : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_spi2_outy_h_g_ois_t; +} lsm6dsv_spi2_outy_h_g_ois_t; -#define LSM6DSV16X_SPI2_OUTZ_L_G_OIS 0x26U +#define LSM6DSV_SPI2_OUTZ_L_G_OIS 0x26U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -1807,9 +1763,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t spi2_outz_g_ois : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_spi2_outz_l_g_ois_t; +} lsm6dsv_spi2_outz_l_g_ois_t; -#define LSM6DSV16X_SPI2_OUTZ_H_G_OIS 0x27U +#define LSM6DSV_SPI2_OUTZ_H_G_OIS 0x27U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -1817,9 +1773,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t spi2_outz_g_ois : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_spi2_outz_h_g_ois_t; +} lsm6dsv_spi2_outz_h_g_ois_t; -#define LSM6DSV16X_SPI2_OUTX_L_A_OIS 0x28U +#define LSM6DSV_SPI2_OUTX_L_A_OIS 0x28U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -1827,9 +1783,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t spi2_outx_a_ois : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_spi2_outx_l_a_ois_t; +} lsm6dsv_spi2_outx_l_a_ois_t; -#define LSM6DSV16X_SPI2_OUTX_H_A_OIS 0x29U +#define LSM6DSV_SPI2_OUTX_H_A_OIS 0x29U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -1837,9 +1793,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t spi2_outx_a_ois : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_spi2_outx_h_a_ois_t; +} lsm6dsv_spi2_outx_h_a_ois_t; -#define LSM6DSV16X_SPI2_OUTY_L_A_OIS 0x2AU +#define LSM6DSV_SPI2_OUTY_L_A_OIS 0x2AU typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -1847,9 +1803,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t spi2_outy_a_ois : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_spi2_outy_l_a_ois_t; +} lsm6dsv_spi2_outy_l_a_ois_t; -#define LSM6DSV16X_SPI2_OUTY_H_A_OIS 0x2BU +#define LSM6DSV_SPI2_OUTY_H_A_OIS 0x2BU typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -1857,9 +1813,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t spi2_outy_a_ois : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_spi2_outy_h_a_ois_t; +} lsm6dsv_spi2_outy_h_a_ois_t; -#define LSM6DSV16X_SPI2_OUTZ_L_A_OIS 0x2CU +#define LSM6DSV_SPI2_OUTZ_L_A_OIS 0x2CU typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -1867,9 +1823,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t spi2_outz_a_ois : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_spi2_outz_l_a_ois_t; +} lsm6dsv_spi2_outz_l_a_ois_t; -#define LSM6DSV16X_SPI2_OUTZ_H_A_OIS 0x2DU +#define LSM6DSV_SPI2_OUTZ_H_A_OIS 0x2DU typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -1877,9 +1833,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t spi2_outz_a_ois : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_spi2_outz_h_a_ois_t; +} lsm6dsv_spi2_outz_h_a_ois_t; -#define LSM6DSV16X_SPI2_HANDSHAKE_CTRL 0x6EU +#define LSM6DSV_SPI2_HANDSHAKE_CTRL 0x6EU typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -1891,9 +1847,9 @@ typedef struct uint8_t spi2_shared_req : 1; uint8_t spi2_shared_ack : 1; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_spi2_handshake_ctrl_t; +} lsm6dsv_spi2_handshake_ctrl_t; -#define LSM6DSV16X_SPI2_INT_OIS 0x6FU +#define LSM6DSV_SPI2_INT_OIS 0x6FU typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -1911,9 +1867,9 @@ typedef struct uint8_t st_g_ois : 2; uint8_t st_xl_ois : 2; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_spi2_int_ois_t; +} lsm6dsv_spi2_int_ois_t; -#define LSM6DSV16X_SPI2_CTRL1_OIS 0x70U +#define LSM6DSV_SPI2_CTRL1_OIS 0x70U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -1931,9 +1887,9 @@ typedef struct uint8_t ois_g_en : 1; uint8_t spi2_read_en : 1; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_spi2_ctrl1_ois_t; +} lsm6dsv_spi2_ctrl1_ois_t; -#define LSM6DSV16X_SPI2_CTRL2_OIS 0x71U +#define LSM6DSV_SPI2_CTRL2_OIS 0x71U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -1945,9 +1901,9 @@ typedef struct uint8_t lpf1_g_ois_bw : 2; uint8_t fs_g_ois : 3; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_spi2_ctrl2_ois_t; +} lsm6dsv_spi2_ctrl2_ois_t; -#define LSM6DSV16X_SPI2_CTRL3_OIS 0x72U +#define LSM6DSV_SPI2_CTRL3_OIS 0x72U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -1961,7 +1917,7 @@ typedef struct uint8_t not_used0 : 1; uint8_t fs_xl_ois : 2; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_spi2_ctrl3_ois_t; +} lsm6dsv_spi2_ctrl3_ois_t; /** * @} @@ -1973,7 +1929,7 @@ typedef struct * */ -#define LSM6DSV16X_PAGE_SEL 0x2U +#define LSM6DSV_PAGE_SEL 0x2U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -1983,9 +1939,9 @@ typedef struct uint8_t page_sel : 4; uint8_t not_used0 : 4; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_page_sel_t; +} lsm6dsv_page_sel_t; -#define LSM6DSV16X_EMB_FUNC_EN_A 0x4U +#define LSM6DSV_EMB_FUNC_EN_A 0x4U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -1995,11 +1951,9 @@ typedef struct uint8_t pedo_en : 1; uint8_t tilt_en : 1; uint8_t sign_motion_en : 1; - uint8_t not_used1 : 1; - uint8_t mlc_before_fsm_en : 1; + uint8_t not_used1 : 2; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t mlc_before_fsm_en : 1; - uint8_t not_used1 : 1; + uint8_t not_used1 : 2; uint8_t sign_motion_en : 1; uint8_t tilt_en : 1; uint8_t pedo_en : 1; @@ -2007,27 +1961,25 @@ typedef struct uint8_t sflp_game_en : 1; uint8_t not_used0 : 1; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_emb_func_en_a_t; +} lsm6dsv_emb_func_en_a_t; -#define LSM6DSV16X_EMB_FUNC_EN_B 0x5U +#define LSM6DSV_EMB_FUNC_EN_B 0x5U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN uint8_t fsm_en : 1; uint8_t not_used0 : 2; uint8_t fifo_compr_en : 1; - uint8_t mlc_en : 1; - uint8_t not_used1 : 3; + uint8_t not_used1 : 4; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t not_used1 : 3; - uint8_t mlc_en : 1; + uint8_t not_used1 : 4; uint8_t fifo_compr_en : 1; uint8_t not_used0 : 2; uint8_t fsm_en : 1; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_emb_func_en_b_t; +} lsm6dsv_emb_func_en_b_t; -#define LSM6DSV16X_EMB_FUNC_EXEC_STATUS 0x7U +#define LSM6DSV_EMB_FUNC_EXEC_STATUS 0x7U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -2039,9 +1991,9 @@ typedef struct uint8_t emb_func_exec_ovr : 1; uint8_t emb_func_endop : 1; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_emb_func_exec_status_t; +} lsm6dsv_emb_func_exec_status_t; -#define LSM6DSV16X_PAGE_ADDRESS 0x8U +#define LSM6DSV_PAGE_ADDRESS 0x8U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -2049,9 +2001,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t page_addr : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_page_address_t; +} lsm6dsv_page_address_t; -#define LSM6DSV16X_PAGE_VALUE 0x9U +#define LSM6DSV_PAGE_VALUE 0x9U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -2059,9 +2011,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t page_value : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_page_value_t; +} lsm6dsv_page_value_t; -#define LSM6DSV16X_EMB_FUNC_INT1 0x0AU +#define LSM6DSV_EMB_FUNC_INT1 0x0AU typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -2079,9 +2031,9 @@ typedef struct uint8_t int1_step_detector : 1; uint8_t not_used0 : 3; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_emb_func_int1_t; +} lsm6dsv_emb_func_int1_t; -#define LSM6DSV16X_FSM_INT1 0x0BU +#define LSM6DSV_FSM_INT1 0x0BU typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -2103,27 +2055,9 @@ typedef struct uint8_t int1_fsm2 : 1; uint8_t int1_fsm1 : 1; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_fsm_int1_t; - -#define LSM6DSV16X_MLC_INT1 0x0DU -typedef struct -{ -#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t int1_mlc1 : 1; - uint8_t int1_mlc2 : 1; - uint8_t int1_mlc3 : 1; - uint8_t int1_mlc4 : 1; - uint8_t not_used0 : 4; -#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t not_used0 : 4; - uint8_t int1_mlc4 : 1; - uint8_t int1_mlc3 : 1; - uint8_t int1_mlc2 : 1; - uint8_t int1_mlc1 : 1; -#endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_mlc_int1_t; +} lsm6dsv_fsm_int1_t; -#define LSM6DSV16X_EMB_FUNC_INT2 0x0EU +#define LSM6DSV_EMB_FUNC_INT2 0x0EU typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -2141,9 +2075,9 @@ typedef struct uint8_t int2_step_detector : 1; uint8_t not_used0 : 3; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_emb_func_int2_t; +} lsm6dsv_emb_func_int2_t; -#define LSM6DSV16X_FSM_INT2 0x0FU +#define LSM6DSV_FSM_INT2 0x0FU typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -2165,27 +2099,9 @@ typedef struct uint8_t int2_fsm2 : 1; uint8_t int2_fsm1 : 1; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_fsm_int2_t; - -#define LSM6DSV16X_MLC_INT2 0x11U -typedef struct -{ -#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t int2_mlc1 : 1; - uint8_t int2_mlc2 : 1; - uint8_t int2_mlc3 : 1; - uint8_t int2_mlc4 : 1; - uint8_t not_used0 : 4; -#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t not_used0 : 4; - uint8_t int2_mlc4 : 1; - uint8_t int2_mlc3 : 1; - uint8_t int2_mlc2 : 1; - uint8_t int2_mlc1 : 1; -#endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_mlc_int2_t; +} lsm6dsv_fsm_int2_t; -#define LSM6DSV16X_EMB_FUNC_STATUS 0x12U +#define LSM6DSV_EMB_FUNC_STATUS 0x12U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -2203,9 +2119,9 @@ typedef struct uint8_t is_step_det : 1; uint8_t not_used0 : 3; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_emb_func_status_t; +} lsm6dsv_emb_func_status_t; -#define LSM6DSV16X_FSM_STATUS 0x13U +#define LSM6DSV_FSM_STATUS 0x13U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -2227,27 +2143,9 @@ typedef struct uint8_t is_fsm2 : 1; uint8_t is_fsm1 : 1; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_fsm_status_t; - -#define LSM6DSV16X_MLC_STATUS 0x15U -typedef struct -{ -#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t is_mlc1 : 1; - uint8_t is_mlc2 : 1; - uint8_t is_mlc3 : 1; - uint8_t is_mlc4 : 1; - uint8_t not_used0 : 4; -#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t not_used0 : 4; - uint8_t is_mlc4 : 1; - uint8_t is_mlc3 : 1; - uint8_t is_mlc2 : 1; - uint8_t is_mlc1 : 1; -#endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_mlc_status_t; +} lsm6dsv_fsm_status_t; -#define LSM6DSV16X_PAGE_RW 0x17U +#define LSM6DSV_PAGE_RW 0x17U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -2261,9 +2159,9 @@ typedef struct uint8_t page_read : 1; uint8_t not_used0 : 5; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_page_rw_t; +} lsm6dsv_page_rw_t; -#define LSM6DSV16X_EMB_FUNC_FIFO_EN_A 0x44U +#define LSM6DSV_EMB_FUNC_FIFO_EN_A 0x44U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -2273,9 +2171,9 @@ typedef struct uint8_t sflp_gravity_fifo_en : 1; uint8_t sflp_gbias_fifo_en : 1; uint8_t step_counter_fifo_en : 1; - uint8_t mlc_fifo_en : 1; + uint8_t not_used2 : 1; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t mlc_fifo_en : 1; + uint8_t not_used2 : 1; uint8_t step_counter_fifo_en : 1; uint8_t sflp_gbias_fifo_en : 1; uint8_t sflp_gravity_fifo_en : 1; @@ -2283,23 +2181,9 @@ typedef struct uint8_t sflp_game_fifo_en : 1; uint8_t not_used0 : 1; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_emb_func_fifo_en_a_t; - -#define LSM6DSV16X_EMB_FUNC_FIFO_EN_B 0x45U -typedef struct -{ -#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t not_used0 : 1; - uint8_t mlc_filter_feature_fifo_en : 1; - uint8_t not_used1 : 6; -#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t not_used1 : 6; - uint8_t mlc_filter_feature_fifo_en : 1; - uint8_t not_used0 : 1; -#endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_emb_func_fifo_en_b_t; +} lsm6dsv_emb_func_fifo_en_a_t; -#define LSM6DSV16X_FSM_ENABLE 0x46U +#define LSM6DSV_FSM_ENABLE 0x46U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -2321,9 +2205,9 @@ typedef struct uint8_t fsm2_en : 1; uint8_t fsm1_en : 1; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_fsm_enable_t; +} lsm6dsv_fsm_enable_t; -#define LSM6DSV16X_FSM_LONG_COUNTER_L 0x48U +#define LSM6DSV_FSM_LONG_COUNTER_L 0x48U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -2331,9 +2215,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t fsm_lc : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_fsm_long_counter_l_t; +} lsm6dsv_fsm_long_counter_l_t; -#define LSM6DSV16X_FSM_LONG_COUNTER_H 0x49U +#define LSM6DSV_FSM_LONG_COUNTER_H 0x49U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -2341,9 +2225,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t fsm_lc : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_fsm_long_counter_h_t; +} lsm6dsv_fsm_long_counter_h_t; -#define LSM6DSV16X_INT_ACK_MASK 0x4BU +#define LSM6DSV_INT_ACK_MASK 0x4BU typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -2351,9 +2235,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t iack_mask : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_int_ack_mask_t; +} lsm6dsv_int_ack_mask_t; -#define LSM6DSV16X_FSM_OUTS1 0x4CU +#define LSM6DSV_FSM_OUTS1 0x4CU typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -2375,9 +2259,9 @@ typedef struct uint8_t fsm1_p_v : 1; uint8_t fsm1_n_v : 1; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_fsm_outs1_t; +} lsm6dsv_fsm_outs1_t; -#define LSM6DSV16X_FSM_OUTS2 0x4DU +#define LSM6DSV_FSM_OUTS2 0x4DU typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -2399,9 +2283,9 @@ typedef struct uint8_t fsm2_p_v : 1; uint8_t fsm2_n_v : 1; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_fsm_outs2_t; +} lsm6dsv_fsm_outs2_t; -#define LSM6DSV16X_FSM_OUTS3 0x4EU +#define LSM6DSV_FSM_OUTS3 0x4EU typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -2423,9 +2307,9 @@ typedef struct uint8_t fsm3_p_v : 1; uint8_t fsm3_n_v : 1; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_fsm_outs3_t; +} lsm6dsv_fsm_outs3_t; -#define LSM6DSV16X_FSM_OUTS4 0x4FU +#define LSM6DSV_FSM_OUTS4 0x4FU typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -2447,9 +2331,9 @@ typedef struct uint8_t fsm4_p_v : 1; uint8_t fsm4_n_v : 1; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_fsm_outs4_t; +} lsm6dsv_fsm_outs4_t; -#define LSM6DSV16X_FSM_OUTS5 0x50U +#define LSM6DSV_FSM_OUTS5 0x50U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -2471,9 +2355,9 @@ typedef struct uint8_t fsm5_p_v : 1; uint8_t fsm5_n_v : 1; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_fsm_outs5_t; +} lsm6dsv_fsm_outs5_t; -#define LSM6DSV16X_FSM_OUTS6 0x51U +#define LSM6DSV_FSM_OUTS6 0x51U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -2495,9 +2379,9 @@ typedef struct uint8_t fsm6_p_v : 1; uint8_t fsm6_n_v : 1; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_fsm_outs6_t; +} lsm6dsv_fsm_outs6_t; -#define LSM6DSV16X_FSM_OUTS7 0x52U +#define LSM6DSV_FSM_OUTS7 0x52U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -2519,9 +2403,9 @@ typedef struct uint8_t fsm7_p_v : 1; uint8_t fsm7_n_v : 1; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_fsm_outs7_t; +} lsm6dsv_fsm_outs7_t; -#define LSM6DSV16X_FSM_OUTS8 0x53U +#define LSM6DSV_FSM_OUTS8 0x53U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -2543,9 +2427,9 @@ typedef struct uint8_t fsm8_p_v : 1; uint8_t fsm8_n_v : 1; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_fsm_outs8_t; +} lsm6dsv_fsm_outs8_t; -#define LSM6DSV16X_SFLP_ODR 0x5EU +#define LSM6DSV_SFLP_ODR 0x5EU typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -2557,9 +2441,9 @@ typedef struct uint8_t sflp_game_odr : 3; uint8_t not_used0 : 3; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_sflp_odr_t; +} lsm6dsv_sflp_odr_t; -#define LSM6DSV16X_FSM_ODR 0x5FU +#define LSM6DSV_FSM_ODR 0x5FU typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -2571,23 +2455,9 @@ typedef struct uint8_t fsm_odr : 3; uint8_t not_used0 : 3; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_fsm_odr_t; - -#define LSM6DSV16X_MLC_ODR 0x60U -typedef struct -{ -#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t not_used0 : 4; - uint8_t mlc_odr : 3; - uint8_t not_used1 : 1; -#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t not_used1 : 1; - uint8_t mlc_odr : 3; - uint8_t not_used0 : 4; -#endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_mlc_odr_t; +} lsm6dsv_fsm_odr_t; -#define LSM6DSV16X_STEP_COUNTER_L 0x62U +#define LSM6DSV_STEP_COUNTER_L 0x62U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -2595,9 +2465,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t step : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_step_counter_l_t; +} lsm6dsv_step_counter_l_t; -#define LSM6DSV16X_STEP_COUNTER_H 0x63U +#define LSM6DSV_STEP_COUNTER_H 0x63U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -2605,9 +2475,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t step : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_step_counter_h_t; +} lsm6dsv_step_counter_h_t; -#define LSM6DSV16X_EMB_FUNC_SRC 0x64U +#define LSM6DSV_EMB_FUNC_SRC 0x64U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -2627,9 +2497,9 @@ typedef struct uint8_t stepcounter_bit_set : 1; uint8_t not_used0 : 2; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_emb_func_src_t; +} lsm6dsv_emb_func_src_t; -#define LSM6DSV16X_EMB_FUNC_INIT_A 0x66U +#define LSM6DSV_EMB_FUNC_INIT_A 0x66U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -2639,11 +2509,9 @@ typedef struct uint8_t step_det_init : 1; uint8_t tilt_init : 1; uint8_t sig_mot_init : 1; - uint8_t not_used1 : 1; - uint8_t mlc_before_fsm_init : 1; + uint8_t not_used1 : 2; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t mlc_before_fsm_init : 1; - uint8_t not_used1 : 1; + uint8_t not_used1 : 2; uint8_t sig_mot_init : 1; uint8_t tilt_init : 1; uint8_t step_det_init : 1; @@ -2651,65 +2519,23 @@ typedef struct uint8_t sflp_game_init : 1; uint8_t not_used0 : 1; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_emb_func_init_a_t; +} lsm6dsv_emb_func_init_a_t; -#define LSM6DSV16X_EMB_FUNC_INIT_B 0x67U +#define LSM6DSV_EMB_FUNC_INIT_B 0x67U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN uint8_t fsm_init : 1; uint8_t not_used0 : 2; uint8_t fifo_compr_init : 1; - uint8_t mlc_init : 1; - uint8_t not_used1 : 3; + uint8_t not_used1 : 4; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t not_used1 : 3; - uint8_t mlc_init : 1; + uint8_t not_used1 : 4; uint8_t fifo_compr_init : 1; uint8_t not_used0 : 2; uint8_t fsm_init : 1; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_emb_func_init_b_t; - -#define LSM6DSV16X_MLC1_SRC 0x70U -typedef struct -{ -#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t mlc1_src : 8; -#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t mlc1_src : 8; -#endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_mlc1_src_t; - -#define LSM6DSV16X_MLC2_SRC 0x71U -typedef struct -{ -#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t mlc2_src : 8; -#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t mlc2_src : 8; -#endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_mlc2_src_t; - -#define LSM6DSV16X_MLC3_SRC 0x72U -typedef struct -{ -#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t mlc3_src : 8; -#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t mlc3_src : 8; -#endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_mlc3_src_t; - -#define LSM6DSV16X_MLC4_SRC 0x73U -typedef struct -{ -#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t mlc4_src : 8; -#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t mlc4_src : 8; -#endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_mlc4_src_t; +} lsm6dsv_emb_func_init_b_t; /** * @} @@ -2720,9 +2546,9 @@ typedef struct * @{ * */ -#define LSM6DSV16X_EMB_ADV_PG_0 0x000U +#define LSM6DSV_EMB_ADV_PG_0 0x000U -#define LSM6DSV16X_SFLP_GAME_GBIASX_L 0x6EU +#define LSM6DSV_SFLP_GAME_GBIASX_L 0x6EU typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -2730,9 +2556,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t gbiasx : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_sflp_game_gbiasx_l_t; +} lsm6dsv_sflp_game_gbiasx_l_t; -#define LSM6DSV16X_SFLP_GAME_GBIASX_H 0x6FU +#define LSM6DSV_SFLP_GAME_GBIASX_H 0x6FU typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -2740,9 +2566,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t gbiasx : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_sflp_game_gbiasx_h_t; +} lsm6dsv_sflp_game_gbiasx_h_t; -#define LSM6DSV16X_SFLP_GAME_GBIASY_L 0x70U +#define LSM6DSV_SFLP_GAME_GBIASY_L 0x70U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -2750,9 +2576,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t gbiasy : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_sflp_game_gbiasy_l_t; +} lsm6dsv_sflp_game_gbiasy_l_t; -#define LSM6DSV16X_SFLP_GAME_GBIASY_H 0x71U +#define LSM6DSV_SFLP_GAME_GBIASY_H 0x71U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -2760,9 +2586,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t gbiasy : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_sflp_game_gbiasy_h_t; +} lsm6dsv_sflp_game_gbiasy_h_t; -#define LSM6DSV16X_SFLP_GAME_GBIASZ_L 0x72U +#define LSM6DSV_SFLP_GAME_GBIASZ_L 0x72U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -2770,9 +2596,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t gbiasz : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_sflp_game_gbiasz_l_t; +} lsm6dsv_sflp_game_gbiasz_l_t; -#define LSM6DSV16X_SFLP_GAME_GBIASZ_H 0x73U +#define LSM6DSV_SFLP_GAME_GBIASZ_H 0x73U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -2780,9 +2606,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t gbiasz : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_sflp_game_gbiasz_h_t; +} lsm6dsv_sflp_game_gbiasz_h_t; -#define LSM6DSV16X_FSM_EXT_SENSITIVITY_L 0xBAU +#define LSM6DSV_FSM_EXT_SENSITIVITY_L 0xBAU typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -2790,9 +2616,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t fsm_ext_s : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_fsm_ext_sensitivity_l_t; +} lsm6dsv_fsm_ext_sensitivity_l_t; -#define LSM6DSV16X_FSM_EXT_SENSITIVITY_H 0xBBU +#define LSM6DSV_FSM_EXT_SENSITIVITY_H 0xBBU typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -2800,9 +2626,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t fsm_ext_s : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_fsm_ext_sensitivity_h_t; +} lsm6dsv_fsm_ext_sensitivity_h_t; -#define LSM6DSV16X_FSM_EXT_OFFX_L 0xC0U +#define LSM6DSV_FSM_EXT_OFFX_L 0xC0U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -2810,9 +2636,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t fsm_ext_offx : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_fsm_ext_offx_l_t; +} lsm6dsv_fsm_ext_offx_l_t; -#define LSM6DSV16X_FSM_EXT_OFFX_H 0xC1U +#define LSM6DSV_FSM_EXT_OFFX_H 0xC1U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -2820,9 +2646,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t fsm_ext_offx : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_fsm_ext_offx_h_t; +} lsm6dsv_fsm_ext_offx_h_t; -#define LSM6DSV16X_FSM_EXT_OFFY_L 0xC2U +#define LSM6DSV_FSM_EXT_OFFY_L 0xC2U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -2830,9 +2656,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t fsm_ext_offy : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_fsm_ext_offy_l_t; +} lsm6dsv_fsm_ext_offy_l_t; -#define LSM6DSV16X_FSM_EXT_OFFY_H 0xC3U +#define LSM6DSV_FSM_EXT_OFFY_H 0xC3U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -2840,9 +2666,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t fsm_ext_offy : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_fsm_ext_offy_h_t; +} lsm6dsv_fsm_ext_offy_h_t; -#define LSM6DSV16X_FSM_EXT_OFFZ_L 0xC4U +#define LSM6DSV_FSM_EXT_OFFZ_L 0xC4U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -2850,9 +2676,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t fsm_ext_offz : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_fsm_ext_offz_l_t; +} lsm6dsv_fsm_ext_offz_l_t; -#define LSM6DSV16X_FSM_EXT_OFFZ_H 0xC5U +#define LSM6DSV_FSM_EXT_OFFZ_H 0xC5U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -2860,9 +2686,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t fsm_ext_offz : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_fsm_ext_offz_h_t; +} lsm6dsv_fsm_ext_offz_h_t; -#define LSM6DSV16X_FSM_EXT_MATRIX_XX_L 0xC6U +#define LSM6DSV_FSM_EXT_MATRIX_XX_L 0xC6U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -2870,9 +2696,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t fsm_ext_mat_xx : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_fsm_ext_matrix_xx_l_t; +} lsm6dsv_fsm_ext_matrix_xx_l_t; -#define LSM6DSV16X_FSM_EXT_MATRIX_XX_H 0xC7U +#define LSM6DSV_FSM_EXT_MATRIX_XX_H 0xC7U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -2880,9 +2706,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t fsm_ext_mat_xx : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_fsm_ext_matrix_xx_h_t; +} lsm6dsv_fsm_ext_matrix_xx_h_t; -#define LSM6DSV16X_FSM_EXT_MATRIX_XY_L 0xC8U +#define LSM6DSV_FSM_EXT_MATRIX_XY_L 0xC8U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -2890,9 +2716,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t fsm_ext_mat_xy : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_fsm_ext_matrix_xy_l_t; +} lsm6dsv_fsm_ext_matrix_xy_l_t; -#define LSM6DSV16X_FSM_EXT_MATRIX_XY_H 0xC9U +#define LSM6DSV_FSM_EXT_MATRIX_XY_H 0xC9U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -2900,9 +2726,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t fsm_ext_mat_xy : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_fsm_ext_matrix_xy_h_t; +} lsm6dsv_fsm_ext_matrix_xy_h_t; -#define LSM6DSV16X_FSM_EXT_MATRIX_XZ_L 0xCAU +#define LSM6DSV_FSM_EXT_MATRIX_XZ_L 0xCAU typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -2910,9 +2736,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t fsm_ext_mat_xz : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_fsm_ext_matrix_xz_l_t; +} lsm6dsv_fsm_ext_matrix_xz_l_t; -#define LSM6DSV16X_FSM_EXT_MATRIX_XZ_H 0xCBU +#define LSM6DSV_FSM_EXT_MATRIX_XZ_H 0xCBU typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -2920,9 +2746,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t fsm_ext_mat_xz : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_fsm_ext_matrix_xz_h_t; +} lsm6dsv_fsm_ext_matrix_xz_h_t; -#define LSM6DSV16X_FSM_EXT_MATRIX_YY_L 0xCCU +#define LSM6DSV_FSM_EXT_MATRIX_YY_L 0xCCU typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -2930,9 +2756,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t fsm_ext_mat_yy : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_fsm_ext_matrix_yy_l_t; +} lsm6dsv_fsm_ext_matrix_yy_l_t; -#define LSM6DSV16X_FSM_EXT_MATRIX_YY_H 0xCDU +#define LSM6DSV_FSM_EXT_MATRIX_YY_H 0xCDU typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -2940,9 +2766,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t fsm_ext_mat_yy : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_fsm_ext_matrix_yy_h_t; +} lsm6dsv_fsm_ext_matrix_yy_h_t; -#define LSM6DSV16X_FSM_EXT_MATRIX_YZ_L 0xCEU +#define LSM6DSV_FSM_EXT_MATRIX_YZ_L 0xCEU typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -2950,9 +2776,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t fsm_ext_mat_yz : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_fsm_ext_matrix_yz_l_t; +} lsm6dsv_fsm_ext_matrix_yz_l_t; -#define LSM6DSV16X_FSM_EXT_MATRIX_YZ_H 0xCFU +#define LSM6DSV_FSM_EXT_MATRIX_YZ_H 0xCFU typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -2960,9 +2786,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t fsm_ext_mat_yz : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_fsm_ext_matrix_yz_h_t; +} lsm6dsv_fsm_ext_matrix_yz_h_t; -#define LSM6DSV16X_FSM_EXT_MATRIX_ZZ_L 0xD0U +#define LSM6DSV_FSM_EXT_MATRIX_ZZ_L 0xD0U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -2970,9 +2796,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t fsm_ext_mat_zz : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_fsm_ext_matrix_zz_l_t; +} lsm6dsv_fsm_ext_matrix_zz_l_t; -#define LSM6DSV16X_FSM_EXT_MATRIX_ZZ_H 0xD1U +#define LSM6DSV_FSM_EXT_MATRIX_ZZ_H 0xD1U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -2980,9 +2806,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t fsm_ext_mat_zz : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_fsm_ext_matrix_zz_h_t; +} lsm6dsv_fsm_ext_matrix_zz_h_t; -#define LSM6DSV16X_EXT_CFG_A 0xD4U +#define LSM6DSV_EXT_CFG_A 0xD4U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -2996,9 +2822,9 @@ typedef struct uint8_t not_used0 : 1; uint8_t ext_z_axis : 3; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_ext_cfg_a_t; +} lsm6dsv_ext_cfg_a_t; -#define LSM6DSV16X_EXT_CFG_B 0xD5U +#define LSM6DSV_EXT_CFG_B 0xD5U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -3008,7 +2834,7 @@ typedef struct uint8_t not_used0 : 5; uint8_t ext_x_axis : 3; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_ext_cfg_b_t; +} lsm6dsv_ext_cfg_b_t; /** * @} @@ -3019,9 +2845,9 @@ typedef struct * @{ * */ -#define LSM6DSV16X_EMB_ADV_PG_1 0x100U +#define LSM6DSV_EMB_ADV_PG_1 0x100U -#define LSM6DSV16X_FSM_LC_TIMEOUT_L 0x7AU +#define LSM6DSV_FSM_LC_TIMEOUT_L 0x7AU typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -3029,9 +2855,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t fsm_lc_timeout : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_fsm_lc_timeout_l_t; +} lsm6dsv_fsm_lc_timeout_l_t; -#define LSM6DSV16X_FSM_LC_TIMEOUT_H 0x7BU +#define LSM6DSV_FSM_LC_TIMEOUT_H 0x7BU typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -3039,9 +2865,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t fsm_lc_timeout : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_fsm_lc_timeout_h_t; +} lsm6dsv_fsm_lc_timeout_h_t; -#define LSM6DSV16X_FSM_PROGRAMS 0x7CU +#define LSM6DSV_FSM_PROGRAMS 0x7CU typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -3049,9 +2875,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t fsm_n_prog : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_fsm_programs_t; +} lsm6dsv_fsm_programs_t; -#define LSM6DSV16X_FSM_START_ADD_L 0x7EU +#define LSM6DSV_FSM_START_ADD_L 0x7EU typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -3059,9 +2885,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t fsm_start : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_fsm_start_add_l_t; +} lsm6dsv_fsm_start_add_l_t; -#define LSM6DSV16X_FSM_START_ADD_H 0x7FU +#define LSM6DSV_FSM_START_ADD_H 0x7FU typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -3069,25 +2895,23 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t fsm_start : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_fsm_start_add_h_t; +} lsm6dsv_fsm_start_add_h_t; -#define LSM6DSV16X_PEDO_CMD_REG 0x83U +#define LSM6DSV_PEDO_CMD_REG 0x83U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t not_used0 : 2; - uint8_t fp_rejection_en : 1; + uint8_t not_used0 : 3; uint8_t carry_count_en : 1; uint8_t not_used1 : 4; #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t not_used1 : 4; uint8_t carry_count_en : 1; - uint8_t fp_rejection_en : 1; - uint8_t not_used0 : 2; + uint8_t not_used0 : 3; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_pedo_cmd_reg_t; +} lsm6dsv_pedo_cmd_reg_t; -#define LSM6DSV16X_PEDO_DEB_STEPS_CONF 0x84U +#define LSM6DSV_PEDO_DEB_STEPS_CONF 0x84U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -3095,9 +2919,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t deb_step : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_pedo_deb_steps_conf_t; +} lsm6dsv_pedo_deb_steps_conf_t; -#define LSM6DSV16X_PEDO_SC_DELTAT_L 0xD0U +#define LSM6DSV_PEDO_SC_DELTAT_L 0xD0U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -3105,9 +2929,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t pd_sc : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_pedo_sc_deltat_l_t; +} lsm6dsv_pedo_sc_deltat_l_t; -#define LSM6DSV16X_PEDO_SC_DELTAT_H 0xD1U +#define LSM6DSV_PEDO_SC_DELTAT_H 0xD1U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -3115,35 +2939,20 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t pd_sc : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_pedo_sc_deltat_h_t; - -#define LSM6DSV16X_MLC_EXT_SENSITIVITY_L 0xE8U -typedef struct -{ -#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t mlc_ext_s : 8; -#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t mlc_ext_s : 8; -#endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_mlc_ext_sensitivity_l_t; +} lsm6dsv_pedo_sc_deltat_h_t; -#define LSM6DSV16X_MLC_EXT_SENSITIVITY_H 0xE9U -typedef struct -{ -#if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN - uint8_t mlc_ext_s : 8; -#elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN - uint8_t mlc_ext_s : 8; -#endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_mlc_ext_sensitivity_h_t; +/** + * @} + * + */ /** @defgroup bitfields page pg2_emb_adv * @{ * */ -#define LSM6DSV16X_EMB_ADV_PG_2 0x200U +#define LSM6DSV_EMB_ADV_PG_2 0x200U -#define LSM6DSV16X_EXT_FORMAT 0x00 +#define LSM6DSV_EXT_FORMAT 0x00 typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -3155,9 +2964,9 @@ typedef struct uint8_t ext_format_sel : 1; uint8_t not_used0 : 2; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_ext_format_t; +} lsm6dsv_ext_format_t; -#define LSM6DSV16X_EXT_3BYTE_SENSITIVITY_L 0x02U +#define LSM6DSV_EXT_3BYTE_SENSITIVITY_L 0x02U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -3165,9 +2974,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t ext_3byte_s : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_ext_3byte_sensitivity_l_t; +} lsm6dsv_ext_3byte_sensitivity_l_t; -#define LSM6DSV16X_EXT_3BYTE_SENSITIVITY_H 0x03U +#define LSM6DSV_EXT_3BYTE_SENSITIVITY_H 0x03U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -3175,9 +2984,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t ext_3byte_s : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_ext_3byte_sensitivity_h_t; +} lsm6dsv_ext_3byte_sensitivity_h_t; -#define LSM6DSV16X_EXT_3BYTE_OFFSET_XL 0x06U +#define LSM6DSV_EXT_3BYTE_OFFSET_XL 0x06U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -3185,9 +2994,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t ext_3byte_off : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_ext_3byte_offset_xl_t; +} lsm6dsv_ext_3byte_offset_xl_t; -#define LSM6DSV16X_EXT_3BYTE_OFFSET_L 0x07U +#define LSM6DSV_EXT_3BYTE_OFFSET_L 0x07U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -3195,9 +3004,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t ext_3byte_off : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_ext_3byte_offset_l_t; +} lsm6dsv_ext_3byte_offset_l_t; -#define LSM6DSV16X_EXT_3BYTE_OFFSET_H 0x08U +#define LSM6DSV_EXT_3BYTE_OFFSET_H 0x08U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -3205,7 +3014,7 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t ext_3byte_off : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_ext_3byte_offset_h_t; +} lsm6dsv_ext_3byte_offset_h_t; /** * @} @@ -3217,7 +3026,7 @@ typedef struct * */ -#define LSM6DSV16X_SENSOR_HUB_1 0x2U +#define LSM6DSV_SENSOR_HUB_1 0x2U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -3225,9 +3034,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t sensorhub1 : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_sensor_hub_1_t; +} lsm6dsv_sensor_hub_1_t; -#define LSM6DSV16X_SENSOR_HUB_2 0x3U +#define LSM6DSV_SENSOR_HUB_2 0x3U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -3235,9 +3044,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t sensorhub2 : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_sensor_hub_2_t; +} lsm6dsv_sensor_hub_2_t; -#define LSM6DSV16X_SENSOR_HUB_3 0x4U +#define LSM6DSV_SENSOR_HUB_3 0x4U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -3245,9 +3054,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t sensorhub3 : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_sensor_hub_3_t; +} lsm6dsv_sensor_hub_3_t; -#define LSM6DSV16X_SENSOR_HUB_4 0x5U +#define LSM6DSV_SENSOR_HUB_4 0x5U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -3255,9 +3064,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t sensorhub4 : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_sensor_hub_4_t; +} lsm6dsv_sensor_hub_4_t; -#define LSM6DSV16X_SENSOR_HUB_5 0x6U +#define LSM6DSV_SENSOR_HUB_5 0x6U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -3265,9 +3074,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t sensorhub5 : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_sensor_hub_5_t; +} lsm6dsv_sensor_hub_5_t; -#define LSM6DSV16X_SENSOR_HUB_6 0x7U +#define LSM6DSV_SENSOR_HUB_6 0x7U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -3275,9 +3084,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t sensorhub6 : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_sensor_hub_6_t; +} lsm6dsv_sensor_hub_6_t; -#define LSM6DSV16X_SENSOR_HUB_7 0x8U +#define LSM6DSV_SENSOR_HUB_7 0x8U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -3285,9 +3094,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t sensorhub7 : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_sensor_hub_7_t; +} lsm6dsv_sensor_hub_7_t; -#define LSM6DSV16X_SENSOR_HUB_8 0x9U +#define LSM6DSV_SENSOR_HUB_8 0x9U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -3295,9 +3104,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t sensorhub8 : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_sensor_hub_8_t; +} lsm6dsv_sensor_hub_8_t; -#define LSM6DSV16X_SENSOR_HUB_9 0x0AU +#define LSM6DSV_SENSOR_HUB_9 0x0AU typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -3305,9 +3114,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t sensorhub9 : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_sensor_hub_9_t; +} lsm6dsv_sensor_hub_9_t; -#define LSM6DSV16X_SENSOR_HUB_10 0x0BU +#define LSM6DSV_SENSOR_HUB_10 0x0BU typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -3315,9 +3124,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t sensorhub10 : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_sensor_hub_10_t; +} lsm6dsv_sensor_hub_10_t; -#define LSM6DSV16X_SENSOR_HUB_11 0x0CU +#define LSM6DSV_SENSOR_HUB_11 0x0CU typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -3325,9 +3134,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t sensorhub11 : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_sensor_hub_11_t; +} lsm6dsv_sensor_hub_11_t; -#define LSM6DSV16X_SENSOR_HUB_12 0x0DU +#define LSM6DSV_SENSOR_HUB_12 0x0DU typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -3335,9 +3144,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t sensorhub12 : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_sensor_hub_12_t; +} lsm6dsv_sensor_hub_12_t; -#define LSM6DSV16X_SENSOR_HUB_13 0x0EU +#define LSM6DSV_SENSOR_HUB_13 0x0EU typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -3345,9 +3154,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t sensorhub13 : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_sensor_hub_13_t; +} lsm6dsv_sensor_hub_13_t; -#define LSM6DSV16X_SENSOR_HUB_14 0x0FU +#define LSM6DSV_SENSOR_HUB_14 0x0FU typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -3355,9 +3164,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t sensorhub14 : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_sensor_hub_14_t; +} lsm6dsv_sensor_hub_14_t; -#define LSM6DSV16X_SENSOR_HUB_15 0x10U +#define LSM6DSV_SENSOR_HUB_15 0x10U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -3365,9 +3174,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t sensorhub15 : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_sensor_hub_15_t; +} lsm6dsv_sensor_hub_15_t; -#define LSM6DSV16X_SENSOR_HUB_16 0x11U +#define LSM6DSV_SENSOR_HUB_16 0x11U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -3375,9 +3184,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t sensorhub16 : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_sensor_hub_16_t; +} lsm6dsv_sensor_hub_16_t; -#define LSM6DSV16X_SENSOR_HUB_17 0x12U +#define LSM6DSV_SENSOR_HUB_17 0x12U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -3385,9 +3194,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t sensorhub17 : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_sensor_hub_17_t; +} lsm6dsv_sensor_hub_17_t; -#define LSM6DSV16X_SENSOR_HUB_18 0x13U +#define LSM6DSV_SENSOR_HUB_18 0x13U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -3395,9 +3204,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t sensorhub18 : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_sensor_hub_18_t; +} lsm6dsv_sensor_hub_18_t; -#define LSM6DSV16X_MASTER_CONFIG 0x14U +#define LSM6DSV_MASTER_CONFIG 0x14U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -3417,9 +3226,9 @@ typedef struct uint8_t master_on : 1; uint8_t aux_sens_on : 2; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_master_config_t; +} lsm6dsv_master_config_t; -#define LSM6DSV16X_SLV0_ADD 0x15U +#define LSM6DSV_SLV0_ADD 0x15U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -3429,9 +3238,9 @@ typedef struct uint8_t slave0_add : 7; uint8_t rw_0 : 1; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_slv0_add_t; +} lsm6dsv_slv0_add_t; -#define LSM6DSV16X_SLV0_SUBADD 0x16U +#define LSM6DSV_SLV0_SUBADD 0x16U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -3439,9 +3248,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t slave0_reg : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_slv0_subadd_t; +} lsm6dsv_slv0_subadd_t; -#define LSM6DSV16X_SLV0_CONFIG 0x17U +#define LSM6DSV_SLV0_CONFIG 0x17U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -3455,9 +3264,9 @@ typedef struct uint8_t batch_ext_sens_0_en : 1; uint8_t slave0_numop : 3; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_slv0_config_t; +} lsm6dsv_slv0_config_t; -#define LSM6DSV16X_SLV1_ADD 0x18U +#define LSM6DSV_SLV1_ADD 0x18U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -3467,9 +3276,9 @@ typedef struct uint8_t slave1_add : 7; uint8_t r_1 : 1; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_slv1_add_t; +} lsm6dsv_slv1_add_t; -#define LSM6DSV16X_SLV1_SUBADD 0x19U +#define LSM6DSV_SLV1_SUBADD 0x19U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -3477,9 +3286,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t slave1_reg : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_slv1_subadd_t; +} lsm6dsv_slv1_subadd_t; -#define LSM6DSV16X_SLV1_CONFIG 0x1AU +#define LSM6DSV_SLV1_CONFIG 0x1AU typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -3491,9 +3300,9 @@ typedef struct uint8_t batch_ext_sens_1_en : 1; uint8_t slave1_numop : 3; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_slv1_config_t; +} lsm6dsv_slv1_config_t; -#define LSM6DSV16X_SLV2_ADD 0x1BU +#define LSM6DSV_SLV2_ADD 0x1BU typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -3503,9 +3312,9 @@ typedef struct uint8_t slave2_add : 7; uint8_t r_2 : 1; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_slv2_add_t; +} lsm6dsv_slv2_add_t; -#define LSM6DSV16X_SLV2_SUBADD 0x1CU +#define LSM6DSV_SLV2_SUBADD 0x1CU typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -3513,9 +3322,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t slave2_reg : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_slv2_subadd_t; +} lsm6dsv_slv2_subadd_t; -#define LSM6DSV16X_SLV2_CONFIG 0x1DU +#define LSM6DSV_SLV2_CONFIG 0x1DU typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -3527,9 +3336,9 @@ typedef struct uint8_t batch_ext_sens_2_en : 1; uint8_t slave2_numop : 3; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_slv2_config_t; +} lsm6dsv_slv2_config_t; -#define LSM6DSV16X_SLV3_ADD 0x1EU +#define LSM6DSV_SLV3_ADD 0x1EU typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -3539,9 +3348,9 @@ typedef struct uint8_t slave3_add : 7; uint8_t r_3 : 1; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_slv3_add_t; +} lsm6dsv_slv3_add_t; -#define LSM6DSV16X_SLV3_SUBADD 0x1FU +#define LSM6DSV_SLV3_SUBADD 0x1FU typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -3549,9 +3358,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t slave3_reg : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_slv3_subadd_t; +} lsm6dsv_slv3_subadd_t; -#define LSM6DSV16X_SLV3_CONFIG 0x20U +#define LSM6DSV_SLV3_CONFIG 0x20U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -3563,9 +3372,9 @@ typedef struct uint8_t batch_ext_sens_3_en : 1; uint8_t slave3_numop : 3; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_slv3_config_t; +} lsm6dsv_slv3_config_t; -#define LSM6DSV16X_DATAWRITE_SLV0 0x21U +#define LSM6DSV_DATAWRITE_SLV0 0x21U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -3573,9 +3382,9 @@ typedef struct #elif DRV_BYTE_ORDER == DRV_BIG_ENDIAN uint8_t slave0_dataw : 8; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_datawrite_slv0_t; +} lsm6dsv_datawrite_slv0_t; -#define LSM6DSV16X_STATUS_MASTER 0x22U +#define LSM6DSV_STATUS_MASTER 0x22U typedef struct { #if DRV_BYTE_ORDER == DRV_LITTLE_ENDIAN @@ -3595,7 +3404,7 @@ typedef struct uint8_t not_used0 : 2; uint8_t sens_hub_endop : 1; #endif /* DRV_BYTE_ORDER */ -} lsm6dsv16x_status_master_t; +} lsm6dsv_status_master_t; /** * @} @@ -3603,7 +3412,7 @@ typedef struct */ /** - * @defgroup LSM6DSO_Register_Union + * @defgroup LSM6DSV_Register_Union * @brief This union group all the registers having a bit-field * description. * This union is useful but it's not needed by the driver. @@ -3616,243 +3425,229 @@ typedef struct */ typedef union { - lsm6dsv16x_func_cfg_access_t func_cfg_access; - lsm6dsv16x_pin_ctrl_t pin_ctrl; - lsm6dsv16x_if_cfg_t if_cfg; - lsm6dsv16x_odr_trig_cfg_t odr_trig_cfg; - lsm6dsv16x_fifo_ctrl1_t fifo_ctrl1; - lsm6dsv16x_fifo_ctrl2_t fifo_ctrl2; - lsm6dsv16x_fifo_ctrl3_t fifo_ctrl3; - lsm6dsv16x_fifo_ctrl4_t fifo_ctrl4; - lsm6dsv16x_counter_bdr_reg1_t counter_bdr_reg1; - lsm6dsv16x_counter_bdr_reg2_t counter_bdr_reg2; - lsm6dsv16x_int1_ctrl_t int1_ctrl; - lsm6dsv16x_int2_ctrl_t int2_ctrl; - lsm6dsv16x_who_am_i_t who_am_i; - lsm6dsv16x_ctrl1_t ctrl1; - lsm6dsv16x_ctrl2_t ctrl2; - lsm6dsv16x_ctrl3_t ctrl3; - lsm6dsv16x_ctrl4_t ctrl4; - lsm6dsv16x_ctrl5_t ctrl5; - lsm6dsv16x_ctrl6_t ctrl6; - lsm6dsv16x_ctrl7_t ctrl7; - lsm6dsv16x_ctrl8_t ctrl8; - lsm6dsv16x_ctrl9_t ctrl9; - lsm6dsv16x_ctrl10_t ctrl10; - lsm6dsv16x_ctrl_status_t ctrl_status; - lsm6dsv16x_fifo_status1_t fifo_status1; - lsm6dsv16x_fifo_status2_t fifo_status2; - lsm6dsv16x_all_int_src_t all_int_src; - lsm6dsv16x_status_reg_t status_reg; - lsm6dsv16x_out_temp_l_t out_temp_l; - lsm6dsv16x_out_temp_h_t out_temp_h; - lsm6dsv16x_outx_l_g_t outx_l_g; - lsm6dsv16x_outx_h_g_t outx_h_g; - lsm6dsv16x_outy_l_g_t outy_l_g; - lsm6dsv16x_outy_h_g_t outy_h_g; - lsm6dsv16x_outz_l_g_t outz_l_g; - lsm6dsv16x_outz_h_g_t outz_h_g; - lsm6dsv16x_outx_l_a_t outx_l_a; - lsm6dsv16x_outx_h_a_t outx_h_a; - lsm6dsv16x_outy_l_a_t outy_l_a; - lsm6dsv16x_outy_h_a_t outy_h_a; - lsm6dsv16x_outz_l_a_t outz_l_a; - lsm6dsv16x_outz_h_a_t outz_h_a; - lsm6dsv16x_ui_outx_l_g_ois_eis_t ui_outx_l_g_ois_eis; - lsm6dsv16x_ui_outx_h_g_ois_eis_t ui_outx_h_g_ois_eis; - lsm6dsv16x_ui_outy_l_g_ois_eis_t ui_outy_l_g_ois_eis; - lsm6dsv16x_ui_outy_h_g_ois_eis_t ui_outy_h_g_ois_eis; - lsm6dsv16x_ui_outz_l_g_ois_eis_t ui_outz_l_g_ois_eis; - lsm6dsv16x_ui_outz_h_g_ois_eis_t ui_outz_h_g_ois_eis; - lsm6dsv16x_ui_outx_l_a_ois_dualc_t ui_outx_l_a_ois_dualc; - lsm6dsv16x_ui_outx_h_a_ois_dualc_t ui_outx_h_a_ois_dualc; - lsm6dsv16x_ui_outy_l_a_ois_dualc_t ui_outy_l_a_ois_dualc; - lsm6dsv16x_ui_outy_h_a_ois_dualc_t ui_outy_h_a_ois_dualc; - lsm6dsv16x_ui_outz_l_a_ois_dualc_t ui_outz_l_a_ois_dualc; - lsm6dsv16x_ui_outz_h_a_ois_dualc_t ui_outz_h_a_ois_dualc; - lsm6dsv16x_ah_qvar_out_l_t ah_qvar_out_l; - lsm6dsv16x_ah_qvar_out_h_t ah_qvar_out_h; - lsm6dsv16x_timestamp0_t timestamp0; - lsm6dsv16x_timestamp1_t timestamp1; - lsm6dsv16x_timestamp2_t timestamp2; - lsm6dsv16x_timestamp3_t timestamp3; - lsm6dsv16x_ui_status_reg_ois_t ui_status_reg_ois; - lsm6dsv16x_wake_up_src_t wake_up_src; - lsm6dsv16x_tap_src_t tap_src; - lsm6dsv16x_d6d_src_t d6d_src; - lsm6dsv16x_status_master_mainpage_t status_master_mainpage; - lsm6dsv16x_emb_func_status_mainpage_t emb_func_status_mainpage; - lsm6dsv16x_fsm_status_mainpage_t fsm_status_mainpage; - lsm6dsv16x_mlc_status_mainpage_t mlc_status_mainpage; - lsm6dsv16x_internal_freq_t internal_freq; - lsm6dsv16x_functions_enable_t functions_enable; - lsm6dsv16x_den_t den; - lsm6dsv16x_inactivity_dur_t inactivity_dur; - lsm6dsv16x_inactivity_ths_t inactivity_ths; - lsm6dsv16x_tap_cfg0_t tap_cfg0; - lsm6dsv16x_tap_cfg1_t tap_cfg1; - lsm6dsv16x_tap_cfg2_t tap_cfg2; - lsm6dsv16x_tap_ths_6d_t tap_ths_6d; - lsm6dsv16x_tap_dur_t tap_dur; - lsm6dsv16x_wake_up_ths_t wake_up_ths; - lsm6dsv16x_wake_up_dur_t wake_up_dur; - lsm6dsv16x_free_fall_t free_fall; - lsm6dsv16x_md1_cfg_t md1_cfg; - lsm6dsv16x_md2_cfg_t md2_cfg; - lsm6dsv16x_emb_func_cfg_t emb_func_cfg; - lsm6dsv16x_ui_handshake_ctrl_t ui_handshake_ctrl; - lsm6dsv16x_ui_spi2_shared_0_t ui_spi2_shared_0; - lsm6dsv16x_ui_spi2_shared_1_t ui_spi2_shared_1; - lsm6dsv16x_ui_spi2_shared_2_t ui_spi2_shared_2; - lsm6dsv16x_ui_spi2_shared_3_t ui_spi2_shared_3; - lsm6dsv16x_ui_spi2_shared_4_t ui_spi2_shared_4; - lsm6dsv16x_ui_spi2_shared_5_t ui_spi2_shared_5; - lsm6dsv16x_ctrl_eis_t ctrl_eis; - lsm6dsv16x_ui_int_ois_t ui_int_ois; - lsm6dsv16x_ui_ctrl1_ois_t ui_ctrl1_ois; - lsm6dsv16x_ui_ctrl2_ois_t ui_ctrl2_ois; - lsm6dsv16x_ui_ctrl3_ois_t ui_ctrl3_ois; - lsm6dsv16x_x_ofs_usr_t x_ofs_usr; - lsm6dsv16x_y_ofs_usr_t y_ofs_usr; - lsm6dsv16x_z_ofs_usr_t z_ofs_usr; - lsm6dsv16x_fifo_data_out_tag_t fifo_data_out_tag; - lsm6dsv16x_fifo_data_out_x_l_t fifo_data_out_x_l; - lsm6dsv16x_fifo_data_out_x_h_t fifo_data_out_x_h; - lsm6dsv16x_fifo_data_out_y_l_t fifo_data_out_y_l; - lsm6dsv16x_fifo_data_out_y_h_t fifo_data_out_y_h; - lsm6dsv16x_fifo_data_out_z_l_t fifo_data_out_z_l; - lsm6dsv16x_fifo_data_out_z_h_t fifo_data_out_z_h; - lsm6dsv16x_spi2_who_am_i_t spi2_who_am_i; - lsm6dsv16x_spi2_status_reg_ois_t spi2_status_reg_ois; - lsm6dsv16x_spi2_out_temp_l_t spi2_out_temp_l; - lsm6dsv16x_spi2_out_temp_h_t spi2_out_temp_h; - lsm6dsv16x_spi2_outx_l_g_ois_t spi2_outx_l_g_ois; - lsm6dsv16x_spi2_outx_h_g_ois_t spi2_outx_h_g_ois; - lsm6dsv16x_spi2_outy_l_g_ois_t spi2_outy_l_g_ois; - lsm6dsv16x_spi2_outy_h_g_ois_t spi2_outy_h_g_ois; - lsm6dsv16x_spi2_outz_l_g_ois_t spi2_outz_l_g_ois; - lsm6dsv16x_spi2_outz_h_g_ois_t spi2_outz_h_g_ois; - lsm6dsv16x_spi2_outx_l_a_ois_t spi2_outx_l_a_ois; - lsm6dsv16x_spi2_outx_h_a_ois_t spi2_outx_h_a_ois; - lsm6dsv16x_spi2_outy_l_a_ois_t spi2_outy_l_a_ois; - lsm6dsv16x_spi2_outy_h_a_ois_t spi2_outy_h_a_ois; - lsm6dsv16x_spi2_outz_l_a_ois_t spi2_outz_l_a_ois; - lsm6dsv16x_spi2_outz_h_a_ois_t spi2_outz_h_a_ois; - lsm6dsv16x_spi2_handshake_ctrl_t spi2_handshake_ctrl; - lsm6dsv16x_spi2_int_ois_t spi2_int_ois; - lsm6dsv16x_spi2_ctrl1_ois_t spi2_ctrl1_ois; - lsm6dsv16x_spi2_ctrl2_ois_t spi2_ctrl2_ois; - lsm6dsv16x_spi2_ctrl3_ois_t spi2_ctrl3_ois; - lsm6dsv16x_page_sel_t page_sel; - lsm6dsv16x_emb_func_en_a_t emb_func_en_a; - lsm6dsv16x_emb_func_en_b_t emb_func_en_b; - lsm6dsv16x_emb_func_exec_status_t emb_func_exec_status; - lsm6dsv16x_page_address_t page_address; - lsm6dsv16x_page_value_t page_value; - lsm6dsv16x_emb_func_int1_t emb_func_int1; - lsm6dsv16x_fsm_int1_t fsm_int1; - lsm6dsv16x_mlc_int1_t mlc_int1; - lsm6dsv16x_emb_func_int2_t emb_func_int2; - lsm6dsv16x_fsm_int2_t fsm_int2; - lsm6dsv16x_mlc_int2_t mlc_int2; - lsm6dsv16x_emb_func_status_t emb_func_status; - lsm6dsv16x_fsm_status_t fsm_status; - lsm6dsv16x_mlc_status_t mlc_status; - lsm6dsv16x_page_rw_t page_rw; - lsm6dsv16x_emb_func_fifo_en_a_t emb_func_fifo_en_a; - lsm6dsv16x_emb_func_fifo_en_b_t emb_func_fifo_en_b; - lsm6dsv16x_fsm_enable_t fsm_enable; - lsm6dsv16x_fsm_long_counter_l_t fsm_long_counter_l; - lsm6dsv16x_fsm_long_counter_h_t fsm_long_counter_h; - lsm6dsv16x_int_ack_mask_t int_ack_mask; - lsm6dsv16x_fsm_outs1_t fsm_outs1; - lsm6dsv16x_fsm_outs2_t fsm_outs2; - lsm6dsv16x_fsm_outs3_t fsm_outs3; - lsm6dsv16x_fsm_outs4_t fsm_outs4; - lsm6dsv16x_fsm_outs5_t fsm_outs5; - lsm6dsv16x_fsm_outs6_t fsm_outs6; - lsm6dsv16x_fsm_outs7_t fsm_outs7; - lsm6dsv16x_fsm_outs8_t fsm_outs8; - lsm6dsv16x_fsm_odr_t fsm_odr; - lsm6dsv16x_mlc_odr_t mlc_odr; - lsm6dsv16x_step_counter_l_t step_counter_l; - lsm6dsv16x_step_counter_h_t step_counter_h; - lsm6dsv16x_emb_func_src_t emb_func_src; - lsm6dsv16x_emb_func_init_a_t emb_func_init_a; - lsm6dsv16x_emb_func_init_b_t emb_func_init_b; - lsm6dsv16x_mlc1_src_t mlc1_src; - lsm6dsv16x_mlc2_src_t mlc2_src; - lsm6dsv16x_mlc3_src_t mlc3_src; - lsm6dsv16x_mlc4_src_t mlc4_src; - lsm6dsv16x_fsm_ext_sensitivity_l_t fsm_ext_sensitivity_l; - lsm6dsv16x_fsm_ext_sensitivity_h_t fsm_ext_sensitivity_h; - lsm6dsv16x_fsm_ext_offx_l_t fsm_ext_offx_l; - lsm6dsv16x_fsm_ext_offx_h_t fsm_ext_offx_h; - lsm6dsv16x_fsm_ext_offy_l_t fsm_ext_offy_l; - lsm6dsv16x_fsm_ext_offy_h_t fsm_ext_offy_h; - lsm6dsv16x_fsm_ext_offz_l_t fsm_ext_offz_l; - lsm6dsv16x_fsm_ext_offz_h_t fsm_ext_offz_h; - lsm6dsv16x_fsm_ext_matrix_xx_l_t fsm_ext_matrix_xx_l; - lsm6dsv16x_fsm_ext_matrix_xx_h_t fsm_ext_matrix_xx_h; - lsm6dsv16x_fsm_ext_matrix_xy_l_t fsm_ext_matrix_xy_l; - lsm6dsv16x_fsm_ext_matrix_xy_h_t fsm_ext_matrix_xy_h; - lsm6dsv16x_fsm_ext_matrix_xz_l_t fsm_ext_matrix_xz_l; - lsm6dsv16x_fsm_ext_matrix_xz_h_t fsm_ext_matrix_xz_h; - lsm6dsv16x_fsm_ext_matrix_yy_l_t fsm_ext_matrix_yy_l; - lsm6dsv16x_fsm_ext_matrix_yy_h_t fsm_ext_matrix_yy_h; - lsm6dsv16x_fsm_ext_matrix_yz_l_t fsm_ext_matrix_yz_l; - lsm6dsv16x_fsm_ext_matrix_yz_h_t fsm_ext_matrix_yz_h; - lsm6dsv16x_fsm_ext_matrix_zz_l_t fsm_ext_matrix_zz_l; - lsm6dsv16x_fsm_ext_matrix_zz_h_t fsm_ext_matrix_zz_h; - lsm6dsv16x_ext_cfg_a_t ext_cfg_a; - lsm6dsv16x_ext_cfg_b_t ext_cfg_b; - lsm6dsv16x_fsm_lc_timeout_l_t fsm_lc_timeout_l; - lsm6dsv16x_fsm_lc_timeout_h_t fsm_lc_timeout_h; - lsm6dsv16x_fsm_programs_t fsm_programs; - lsm6dsv16x_fsm_start_add_l_t fsm_start_add_l; - lsm6dsv16x_fsm_start_add_h_t fsm_start_add_h; - lsm6dsv16x_pedo_cmd_reg_t pedo_cmd_reg; - lsm6dsv16x_pedo_deb_steps_conf_t pedo_deb_steps_conf; - lsm6dsv16x_pedo_sc_deltat_l_t pedo_sc_deltat_l; - lsm6dsv16x_pedo_sc_deltat_h_t pedo_sc_deltat_h; - lsm6dsv16x_mlc_ext_sensitivity_l_t mlc_ext_sensitivity_l; - lsm6dsv16x_mlc_ext_sensitivity_h_t mlc_ext_sensitivity_h; - lsm6dsv16x_sensor_hub_1_t sensor_hub_1; - lsm6dsv16x_sensor_hub_2_t sensor_hub_2; - lsm6dsv16x_sensor_hub_3_t sensor_hub_3; - lsm6dsv16x_sensor_hub_4_t sensor_hub_4; - lsm6dsv16x_sensor_hub_5_t sensor_hub_5; - lsm6dsv16x_sensor_hub_6_t sensor_hub_6; - lsm6dsv16x_sensor_hub_7_t sensor_hub_7; - lsm6dsv16x_sensor_hub_8_t sensor_hub_8; - lsm6dsv16x_sensor_hub_9_t sensor_hub_9; - lsm6dsv16x_sensor_hub_10_t sensor_hub_10; - lsm6dsv16x_sensor_hub_11_t sensor_hub_11; - lsm6dsv16x_sensor_hub_12_t sensor_hub_12; - lsm6dsv16x_sensor_hub_13_t sensor_hub_13; - lsm6dsv16x_sensor_hub_14_t sensor_hub_14; - lsm6dsv16x_sensor_hub_15_t sensor_hub_15; - lsm6dsv16x_sensor_hub_16_t sensor_hub_16; - lsm6dsv16x_sensor_hub_17_t sensor_hub_17; - lsm6dsv16x_sensor_hub_18_t sensor_hub_18; - lsm6dsv16x_master_config_t master_config; - lsm6dsv16x_slv0_add_t slv0_add; - lsm6dsv16x_slv0_subadd_t slv0_subadd; - lsm6dsv16x_slv0_config_t slv0_config; - lsm6dsv16x_slv1_add_t slv1_add; - lsm6dsv16x_slv1_subadd_t slv1_subadd; - lsm6dsv16x_slv1_config_t slv1_config; - lsm6dsv16x_slv2_add_t slv2_add; - lsm6dsv16x_slv2_subadd_t slv2_subadd; - lsm6dsv16x_slv2_config_t slv2_config; - lsm6dsv16x_slv3_add_t slv3_add; - lsm6dsv16x_slv3_subadd_t slv3_subadd; - lsm6dsv16x_slv3_config_t slv3_config; - lsm6dsv16x_datawrite_slv0_t datawrite_slv0; - lsm6dsv16x_status_master_t status_master; + lsm6dsv_func_cfg_access_t func_cfg_access; + lsm6dsv_pin_ctrl_t pin_ctrl; + lsm6dsv_if_cfg_t if_cfg; + lsm6dsv_odr_trig_cfg_t odr_trig_cfg; + lsm6dsv_fifo_ctrl1_t fifo_ctrl1; + lsm6dsv_fifo_ctrl2_t fifo_ctrl2; + lsm6dsv_fifo_ctrl3_t fifo_ctrl3; + lsm6dsv_fifo_ctrl4_t fifo_ctrl4; + lsm6dsv_counter_bdr_reg1_t counter_bdr_reg1; + lsm6dsv_counter_bdr_reg2_t counter_bdr_reg2; + lsm6dsv_int1_ctrl_t int1_ctrl; + lsm6dsv_int2_ctrl_t int2_ctrl; + lsm6dsv_who_am_i_t who_am_i; + lsm6dsv_ctrl1_t ctrl1; + lsm6dsv_ctrl2_t ctrl2; + lsm6dsv_ctrl3_t ctrl3; + lsm6dsv_ctrl4_t ctrl4; + lsm6dsv_ctrl5_t ctrl5; + lsm6dsv_ctrl6_t ctrl6; + lsm6dsv_ctrl7_t ctrl7; + lsm6dsv_ctrl8_t ctrl8; + lsm6dsv_ctrl9_t ctrl9; + lsm6dsv_ctrl10_t ctrl10; + lsm6dsv_ctrl_status_t ctrl_status; + lsm6dsv_fifo_status1_t fifo_status1; + lsm6dsv_fifo_status2_t fifo_status2; + lsm6dsv_all_int_src_t all_int_src; + lsm6dsv_status_reg_t status_reg; + lsm6dsv_out_temp_l_t out_temp_l; + lsm6dsv_out_temp_h_t out_temp_h; + lsm6dsv_outx_l_g_t outx_l_g; + lsm6dsv_outx_h_g_t outx_h_g; + lsm6dsv_outy_l_g_t outy_l_g; + lsm6dsv_outy_h_g_t outy_h_g; + lsm6dsv_outz_l_g_t outz_l_g; + lsm6dsv_outz_h_g_t outz_h_g; + lsm6dsv_outx_l_a_t outx_l_a; + lsm6dsv_outx_h_a_t outx_h_a; + lsm6dsv_outy_l_a_t outy_l_a; + lsm6dsv_outy_h_a_t outy_h_a; + lsm6dsv_outz_l_a_t outz_l_a; + lsm6dsv_outz_h_a_t outz_h_a; + lsm6dsv_ui_outx_l_g_ois_eis_t ui_outx_l_g_ois_eis; + lsm6dsv_ui_outx_h_g_ois_eis_t ui_outx_h_g_ois_eis; + lsm6dsv_ui_outy_l_g_ois_eis_t ui_outy_l_g_ois_eis; + lsm6dsv_ui_outy_h_g_ois_eis_t ui_outy_h_g_ois_eis; + lsm6dsv_ui_outz_l_g_ois_eis_t ui_outz_l_g_ois_eis; + lsm6dsv_ui_outz_h_g_ois_eis_t ui_outz_h_g_ois_eis; + lsm6dsv_ui_outx_l_a_ois_dualc_t ui_outx_l_a_ois_dualc; + lsm6dsv_ui_outx_h_a_ois_dualc_t ui_outx_h_a_ois_dualc; + lsm6dsv_ui_outy_l_a_ois_dualc_t ui_outy_l_a_ois_dualc; + lsm6dsv_ui_outy_h_a_ois_dualc_t ui_outy_h_a_ois_dualc; + lsm6dsv_ui_outz_l_a_ois_dualc_t ui_outz_l_a_ois_dualc; + lsm6dsv_ui_outz_h_a_ois_dualc_t ui_outz_h_a_ois_dualc; + lsm6dsv_timestamp0_t timestamp0; + lsm6dsv_timestamp1_t timestamp1; + lsm6dsv_timestamp2_t timestamp2; + lsm6dsv_timestamp3_t timestamp3; + lsm6dsv_ui_status_reg_ois_t ui_status_reg_ois; + lsm6dsv_wake_up_src_t wake_up_src; + lsm6dsv_tap_src_t tap_src; + lsm6dsv_d6d_src_t d6d_src; + lsm6dsv_status_master_mainpage_t status_master_mainpage; + lsm6dsv_emb_func_status_mainpage_t emb_func_status_mainpage; + lsm6dsv_fsm_status_mainpage_t fsm_status_mainpage; + lsm6dsv_internal_freq_t internal_freq; + lsm6dsv_functions_enable_t functions_enable; + lsm6dsv_den_t den; + lsm6dsv_inactivity_dur_t inactivity_dur; + lsm6dsv_inactivity_ths_t inactivity_ths; + lsm6dsv_tap_cfg0_t tap_cfg0; + lsm6dsv_tap_cfg1_t tap_cfg1; + lsm6dsv_tap_cfg2_t tap_cfg2; + lsm6dsv_tap_ths_6d_t tap_ths_6d; + lsm6dsv_tap_dur_t tap_dur; + lsm6dsv_wake_up_ths_t wake_up_ths; + lsm6dsv_wake_up_dur_t wake_up_dur; + lsm6dsv_free_fall_t free_fall; + lsm6dsv_md1_cfg_t md1_cfg; + lsm6dsv_md2_cfg_t md2_cfg; + lsm6dsv_emb_func_cfg_t emb_func_cfg; + lsm6dsv_ui_handshake_ctrl_t ui_handshake_ctrl; + lsm6dsv_ui_spi2_shared_0_t ui_spi2_shared_0; + lsm6dsv_ui_spi2_shared_1_t ui_spi2_shared_1; + lsm6dsv_ui_spi2_shared_2_t ui_spi2_shared_2; + lsm6dsv_ui_spi2_shared_3_t ui_spi2_shared_3; + lsm6dsv_ui_spi2_shared_4_t ui_spi2_shared_4; + lsm6dsv_ui_spi2_shared_5_t ui_spi2_shared_5; + lsm6dsv_ctrl_eis_t ctrl_eis; + lsm6dsv_ui_int_ois_t ui_int_ois; + lsm6dsv_ui_ctrl1_ois_t ui_ctrl1_ois; + lsm6dsv_ui_ctrl2_ois_t ui_ctrl2_ois; + lsm6dsv_ui_ctrl3_ois_t ui_ctrl3_ois; + lsm6dsv_x_ofs_usr_t x_ofs_usr; + lsm6dsv_y_ofs_usr_t y_ofs_usr; + lsm6dsv_z_ofs_usr_t z_ofs_usr; + lsm6dsv_fifo_data_out_tag_t fifo_data_out_tag; + lsm6dsv_fifo_data_out_x_l_t fifo_data_out_x_l; + lsm6dsv_fifo_data_out_x_h_t fifo_data_out_x_h; + lsm6dsv_fifo_data_out_y_l_t fifo_data_out_y_l; + lsm6dsv_fifo_data_out_y_h_t fifo_data_out_y_h; + lsm6dsv_fifo_data_out_z_l_t fifo_data_out_z_l; + lsm6dsv_fifo_data_out_z_h_t fifo_data_out_z_h; + lsm6dsv_spi2_who_am_i_t spi2_who_am_i; + lsm6dsv_spi2_status_reg_ois_t spi2_status_reg_ois; + lsm6dsv_spi2_out_temp_l_t spi2_out_temp_l; + lsm6dsv_spi2_out_temp_h_t spi2_out_temp_h; + lsm6dsv_spi2_outx_l_g_ois_t spi2_outx_l_g_ois; + lsm6dsv_spi2_outx_h_g_ois_t spi2_outx_h_g_ois; + lsm6dsv_spi2_outy_l_g_ois_t spi2_outy_l_g_ois; + lsm6dsv_spi2_outy_h_g_ois_t spi2_outy_h_g_ois; + lsm6dsv_spi2_outz_l_g_ois_t spi2_outz_l_g_ois; + lsm6dsv_spi2_outz_h_g_ois_t spi2_outz_h_g_ois; + lsm6dsv_spi2_outx_l_a_ois_t spi2_outx_l_a_ois; + lsm6dsv_spi2_outx_h_a_ois_t spi2_outx_h_a_ois; + lsm6dsv_spi2_outy_l_a_ois_t spi2_outy_l_a_ois; + lsm6dsv_spi2_outy_h_a_ois_t spi2_outy_h_a_ois; + lsm6dsv_spi2_outz_l_a_ois_t spi2_outz_l_a_ois; + lsm6dsv_spi2_outz_h_a_ois_t spi2_outz_h_a_ois; + lsm6dsv_spi2_handshake_ctrl_t spi2_handshake_ctrl; + lsm6dsv_spi2_int_ois_t spi2_int_ois; + lsm6dsv_spi2_ctrl1_ois_t spi2_ctrl1_ois; + lsm6dsv_spi2_ctrl2_ois_t spi2_ctrl2_ois; + lsm6dsv_spi2_ctrl3_ois_t spi2_ctrl3_ois; + lsm6dsv_page_sel_t page_sel; + lsm6dsv_emb_func_en_a_t emb_func_en_a; + lsm6dsv_emb_func_en_b_t emb_func_en_b; + lsm6dsv_emb_func_exec_status_t emb_func_exec_status; + lsm6dsv_page_address_t page_address; + lsm6dsv_page_value_t page_value; + lsm6dsv_emb_func_int1_t emb_func_int1; + lsm6dsv_fsm_int1_t fsm_int1; + lsm6dsv_emb_func_int2_t emb_func_int2; + lsm6dsv_fsm_int2_t fsm_int2; + lsm6dsv_emb_func_status_t emb_func_status; + lsm6dsv_fsm_status_t fsm_status; + lsm6dsv_page_rw_t page_rw; + lsm6dsv_emb_func_fifo_en_a_t emb_func_fifo_en_a; + lsm6dsv_fsm_enable_t fsm_enable; + lsm6dsv_fsm_long_counter_l_t fsm_long_counter_l; + lsm6dsv_fsm_long_counter_h_t fsm_long_counter_h; + lsm6dsv_int_ack_mask_t int_ack_mask; + lsm6dsv_fsm_outs1_t fsm_outs1; + lsm6dsv_fsm_outs2_t fsm_outs2; + lsm6dsv_fsm_outs3_t fsm_outs3; + lsm6dsv_fsm_outs4_t fsm_outs4; + lsm6dsv_fsm_outs5_t fsm_outs5; + lsm6dsv_fsm_outs6_t fsm_outs6; + lsm6dsv_fsm_outs7_t fsm_outs7; + lsm6dsv_fsm_outs8_t fsm_outs8; + lsm6dsv_fsm_odr_t fsm_odr; + lsm6dsv_step_counter_l_t step_counter_l; + lsm6dsv_step_counter_h_t step_counter_h; + lsm6dsv_emb_func_src_t emb_func_src; + lsm6dsv_emb_func_init_a_t emb_func_init_a; + lsm6dsv_emb_func_init_b_t emb_func_init_b; + lsm6dsv_fsm_ext_sensitivity_l_t fsm_ext_sensitivity_l; + lsm6dsv_fsm_ext_sensitivity_h_t fsm_ext_sensitivity_h; + lsm6dsv_fsm_ext_offx_l_t fsm_ext_offx_l; + lsm6dsv_fsm_ext_offx_h_t fsm_ext_offx_h; + lsm6dsv_fsm_ext_offy_l_t fsm_ext_offy_l; + lsm6dsv_fsm_ext_offy_h_t fsm_ext_offy_h; + lsm6dsv_fsm_ext_offz_l_t fsm_ext_offz_l; + lsm6dsv_fsm_ext_offz_h_t fsm_ext_offz_h; + lsm6dsv_fsm_ext_matrix_xx_l_t fsm_ext_matrix_xx_l; + lsm6dsv_fsm_ext_matrix_xx_h_t fsm_ext_matrix_xx_h; + lsm6dsv_fsm_ext_matrix_xy_l_t fsm_ext_matrix_xy_l; + lsm6dsv_fsm_ext_matrix_xy_h_t fsm_ext_matrix_xy_h; + lsm6dsv_fsm_ext_matrix_xz_l_t fsm_ext_matrix_xz_l; + lsm6dsv_fsm_ext_matrix_xz_h_t fsm_ext_matrix_xz_h; + lsm6dsv_fsm_ext_matrix_yy_l_t fsm_ext_matrix_yy_l; + lsm6dsv_fsm_ext_matrix_yy_h_t fsm_ext_matrix_yy_h; + lsm6dsv_fsm_ext_matrix_yz_l_t fsm_ext_matrix_yz_l; + lsm6dsv_fsm_ext_matrix_yz_h_t fsm_ext_matrix_yz_h; + lsm6dsv_fsm_ext_matrix_zz_l_t fsm_ext_matrix_zz_l; + lsm6dsv_fsm_ext_matrix_zz_h_t fsm_ext_matrix_zz_h; + lsm6dsv_ext_cfg_a_t ext_cfg_a; + lsm6dsv_ext_cfg_b_t ext_cfg_b; + lsm6dsv_fsm_lc_timeout_l_t fsm_lc_timeout_l; + lsm6dsv_fsm_lc_timeout_h_t fsm_lc_timeout_h; + lsm6dsv_fsm_programs_t fsm_programs; + lsm6dsv_fsm_start_add_l_t fsm_start_add_l; + lsm6dsv_fsm_start_add_h_t fsm_start_add_h; + lsm6dsv_pedo_cmd_reg_t pedo_cmd_reg; + lsm6dsv_pedo_deb_steps_conf_t pedo_deb_steps_conf; + lsm6dsv_pedo_sc_deltat_l_t pedo_sc_deltat_l; + lsm6dsv_pedo_sc_deltat_h_t pedo_sc_deltat_h; + lsm6dsv_sensor_hub_1_t sensor_hub_1; + lsm6dsv_sensor_hub_2_t sensor_hub_2; + lsm6dsv_sensor_hub_3_t sensor_hub_3; + lsm6dsv_sensor_hub_4_t sensor_hub_4; + lsm6dsv_sensor_hub_5_t sensor_hub_5; + lsm6dsv_sensor_hub_6_t sensor_hub_6; + lsm6dsv_sensor_hub_7_t sensor_hub_7; + lsm6dsv_sensor_hub_8_t sensor_hub_8; + lsm6dsv_sensor_hub_9_t sensor_hub_9; + lsm6dsv_sensor_hub_10_t sensor_hub_10; + lsm6dsv_sensor_hub_11_t sensor_hub_11; + lsm6dsv_sensor_hub_12_t sensor_hub_12; + lsm6dsv_sensor_hub_13_t sensor_hub_13; + lsm6dsv_sensor_hub_14_t sensor_hub_14; + lsm6dsv_sensor_hub_15_t sensor_hub_15; + lsm6dsv_sensor_hub_16_t sensor_hub_16; + lsm6dsv_sensor_hub_17_t sensor_hub_17; + lsm6dsv_sensor_hub_18_t sensor_hub_18; + lsm6dsv_master_config_t master_config; + lsm6dsv_slv0_add_t slv0_add; + lsm6dsv_slv0_subadd_t slv0_subadd; + lsm6dsv_slv0_config_t slv0_config; + lsm6dsv_slv1_add_t slv1_add; + lsm6dsv_slv1_subadd_t slv1_subadd; + lsm6dsv_slv1_config_t slv1_config; + lsm6dsv_slv2_add_t slv2_add; + lsm6dsv_slv2_subadd_t slv2_subadd; + lsm6dsv_slv2_config_t slv2_config; + lsm6dsv_slv3_add_t slv3_add; + lsm6dsv_slv3_subadd_t slv3_subadd; + lsm6dsv_slv3_config_t slv3_config; + lsm6dsv_datawrite_slv0_t datawrite_slv0; + lsm6dsv_status_master_t status_master; bitwise_t bitwise; uint8_t byte; -} lsm6dsv16x_reg_t; +} lsm6dsv_reg_t; /** * @} @@ -3872,248 +3667,245 @@ typedef union * them with a custom implementation. */ -int32_t lsm6dsv16x_read_reg(stmdev_ctx_t *ctx, uint8_t reg, - uint8_t *data, - uint16_t len); -int32_t lsm6dsv16x_write_reg(stmdev_ctx_t *ctx, uint8_t reg, - uint8_t *data, - uint16_t len); - -float_t lsm6dsv16x_from_sflp_to_mg(int16_t lsb); -float_t lsm6dsv16x_from_fs2_to_mg(int16_t lsb); -float_t lsm6dsv16x_from_fs4_to_mg(int16_t lsb); -float_t lsm6dsv16x_from_fs8_to_mg(int16_t lsb); -float_t lsm6dsv16x_from_fs16_to_mg(int16_t lsb); +int32_t lsm6dsv_read_reg(stmdev_ctx_t *ctx, uint8_t reg, + uint8_t *data, + uint16_t len); +int32_t lsm6dsv_write_reg(stmdev_ctx_t *ctx, uint8_t reg, + uint8_t *data, + uint16_t len); -float_t lsm6dsv16x_from_fs125_to_mdps(int16_t lsb); -float_t lsm6dsv16x_from_fs500_to_mdps(int16_t lsb); -float_t lsm6dsv16x_from_fs250_to_mdps(int16_t lsb); -float_t lsm6dsv16x_from_fs1000_to_mdps(int16_t lsb); -float_t lsm6dsv16x_from_fs2000_to_mdps(int16_t lsb); -float_t lsm6dsv16x_from_fs4000_to_mdps(int16_t lsb); +float_t lsm6dsv_from_sflp_to_mg(int16_t lsb); +float_t lsm6dsv_from_fs2_to_mg(int16_t lsb); +float_t lsm6dsv_from_fs4_to_mg(int16_t lsb); +float_t lsm6dsv_from_fs8_to_mg(int16_t lsb); +float_t lsm6dsv_from_fs16_to_mg(int16_t lsb); -float_t lsm6dsv16x_from_lsb_to_celsius(int16_t lsb); +float_t lsm6dsv_from_fs125_to_mdps(int16_t lsb); +float_t lsm6dsv_from_fs500_to_mdps(int16_t lsb); +float_t lsm6dsv_from_fs250_to_mdps(int16_t lsb); +float_t lsm6dsv_from_fs1000_to_mdps(int16_t lsb); +float_t lsm6dsv_from_fs2000_to_mdps(int16_t lsb); +float_t lsm6dsv_from_fs4000_to_mdps(int16_t lsb); -float_t lsm6dsv16x_from_lsb_to_nsec(uint32_t lsb); +float_t lsm6dsv_from_lsb_to_celsius(int16_t lsb); -float_t lsm6dsv16x_from_lsb_to_mv(int16_t lsb); +float_t lsm6dsv_from_lsb_to_nsec(uint32_t lsb); -int32_t lsm6dsv16x_xl_offset_on_out_set(stmdev_ctx_t *ctx, uint8_t val); -int32_t lsm6dsv16x_xl_offset_on_out_get(stmdev_ctx_t *ctx, uint8_t *val); +int32_t lsm6dsv_xl_offset_on_out_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6dsv_xl_offset_on_out_get(stmdev_ctx_t *ctx, uint8_t *val); typedef struct { float_t z_mg; float_t y_mg; float_t x_mg; -} lsm6dsv16x_xl_offset_mg_t; -int32_t lsm6dsv16x_xl_offset_mg_set(stmdev_ctx_t *ctx, - lsm6dsv16x_xl_offset_mg_t val); -int32_t lsm6dsv16x_xl_offset_mg_get(stmdev_ctx_t *ctx, - lsm6dsv16x_xl_offset_mg_t *val); +} lsm6dsv_xl_offset_mg_t; +int32_t lsm6dsv_xl_offset_mg_set(stmdev_ctx_t *ctx, + lsm6dsv_xl_offset_mg_t val); +int32_t lsm6dsv_xl_offset_mg_get(stmdev_ctx_t *ctx, + lsm6dsv_xl_offset_mg_t *val); typedef enum { - LSM6DSV16X_READY = 0x0, - LSM6DSV16X_GLOBAL_RST = 0x1, - LSM6DSV16X_RESTORE_CAL_PARAM = 0x2, - LSM6DSV16X_RESTORE_CTRL_REGS = 0x4, -} lsm6dsv16x_reset_t; -int32_t lsm6dsv16x_reset_set(stmdev_ctx_t *ctx, lsm6dsv16x_reset_t val); -int32_t lsm6dsv16x_reset_get(stmdev_ctx_t *ctx, lsm6dsv16x_reset_t *val); + LSM6DSV_READY = 0x0, + LSM6DSV_GLOBAL_RST = 0x1, + LSM6DSV_RESTORE_CAL_PARAM = 0x2, + LSM6DSV_RESTORE_CTRL_REGS = 0x4, +} lsm6dsv_reset_t; +int32_t lsm6dsv_reset_set(stmdev_ctx_t *ctx, lsm6dsv_reset_t val); +int32_t lsm6dsv_reset_get(stmdev_ctx_t *ctx, lsm6dsv_reset_t *val); typedef enum { - LSM6DSV16X_MAIN_MEM_BANK = 0x0, - LSM6DSV16X_EMBED_FUNC_MEM_BANK = 0x1, - LSM6DSV16X_SENSOR_HUB_MEM_BANK = 0x2, -} lsm6dsv16x_mem_bank_t; -int32_t lsm6dsv16x_mem_bank_set(stmdev_ctx_t *ctx, lsm6dsv16x_mem_bank_t val); -int32_t lsm6dsv16x_mem_bank_get(stmdev_ctx_t *ctx, lsm6dsv16x_mem_bank_t *val); + LSM6DSV_MAIN_MEM_BANK = 0x0, + LSM6DSV_EMBED_FUNC_MEM_BANK = 0x1, + LSM6DSV_SENSOR_HUB_MEM_BANK = 0x2, +} lsm6dsv_mem_bank_t; +int32_t lsm6dsv_mem_bank_set(stmdev_ctx_t *ctx, lsm6dsv_mem_bank_t val); +int32_t lsm6dsv_mem_bank_get(stmdev_ctx_t *ctx, lsm6dsv_mem_bank_t *val); -int32_t lsm6dsv16x_device_id_get(stmdev_ctx_t *ctx, uint8_t *val); +int32_t lsm6dsv_device_id_get(stmdev_ctx_t *ctx, uint8_t *val); typedef enum { - LSM6DSV16X_ODR_OFF = 0x0, - LSM6DSV16X_ODR_AT_1Hz875 = 0x1, - LSM6DSV16X_ODR_AT_7Hz5 = 0x2, - LSM6DSV16X_ODR_AT_15Hz = 0x3, - LSM6DSV16X_ODR_AT_30Hz = 0x4, - LSM6DSV16X_ODR_AT_60Hz = 0x5, - LSM6DSV16X_ODR_AT_120Hz = 0x6, - LSM6DSV16X_ODR_AT_240Hz = 0x7, - LSM6DSV16X_ODR_AT_480Hz = 0x8, - LSM6DSV16X_ODR_AT_960Hz = 0x9, - LSM6DSV16X_ODR_AT_1920Hz = 0xA, - LSM6DSV16X_ODR_AT_3840Hz = 0xB, - LSM6DSV16X_ODR_AT_7680Hz = 0xC, - LSM6DSV16X_ODR_HA01_AT_15Hz625 = 0x13, - LSM6DSV16X_ODR_HA01_AT_31Hz25 = 0x14, - LSM6DSV16X_ODR_HA01_AT_62Hz5 = 0x15, - LSM6DSV16X_ODR_HA01_AT_125Hz = 0x16, - LSM6DSV16X_ODR_HA01_AT_250Hz = 0x17, - LSM6DSV16X_ODR_HA01_AT_500Hz = 0x18, - LSM6DSV16X_ODR_HA01_AT_1000Hz = 0x19, - LSM6DSV16X_ODR_HA01_AT_2000Hz = 0x1A, - LSM6DSV16X_ODR_HA01_AT_4000Hz = 0x1B, - LSM6DSV16X_ODR_HA01_AT_8000Hz = 0x1C, - LSM6DSV16X_ODR_HA02_AT_12Hz5 = 0x23, - LSM6DSV16X_ODR_HA02_AT_25Hz = 0x24, - LSM6DSV16X_ODR_HA02_AT_50Hz = 0x25, - LSM6DSV16X_ODR_HA02_AT_100Hz = 0x26, - LSM6DSV16X_ODR_HA02_AT_200Hz = 0x27, - LSM6DSV16X_ODR_HA02_AT_400Hz = 0x28, - LSM6DSV16X_ODR_HA02_AT_800Hz = 0x29, - LSM6DSV16X_ODR_HA02_AT_1600Hz = 0x2A, - LSM6DSV16X_ODR_HA02_AT_3200Hz = 0x2B, - LSM6DSV16X_ODR_HA02_AT_6400Hz = 0x2C, -} lsm6dsv16x_data_rate_t; -int32_t lsm6dsv16x_xl_data_rate_set(stmdev_ctx_t *ctx, - lsm6dsv16x_data_rate_t val); -int32_t lsm6dsv16x_xl_data_rate_get(stmdev_ctx_t *ctx, - lsm6dsv16x_data_rate_t *val); -int32_t lsm6dsv16x_gy_data_rate_set(stmdev_ctx_t *ctx, - lsm6dsv16x_data_rate_t val); -int32_t lsm6dsv16x_gy_data_rate_get(stmdev_ctx_t *ctx, - lsm6dsv16x_data_rate_t *val); + LSM6DSV_ODR_OFF = 0x0, + LSM6DSV_ODR_AT_1Hz875 = 0x1, + LSM6DSV_ODR_AT_7Hz5 = 0x2, + LSM6DSV_ODR_AT_15Hz = 0x3, + LSM6DSV_ODR_AT_30Hz = 0x4, + LSM6DSV_ODR_AT_60Hz = 0x5, + LSM6DSV_ODR_AT_120Hz = 0x6, + LSM6DSV_ODR_AT_240Hz = 0x7, + LSM6DSV_ODR_AT_480Hz = 0x8, + LSM6DSV_ODR_AT_960Hz = 0x9, + LSM6DSV_ODR_AT_1920Hz = 0xA, + LSM6DSV_ODR_AT_3840Hz = 0xB, + LSM6DSV_ODR_AT_7680Hz = 0xC, + LSM6DSV_ODR_HA01_AT_15Hz625 = 0x13, + LSM6DSV_ODR_HA01_AT_31Hz25 = 0x14, + LSM6DSV_ODR_HA01_AT_62Hz5 = 0x15, + LSM6DSV_ODR_HA01_AT_125Hz = 0x16, + LSM6DSV_ODR_HA01_AT_250Hz = 0x17, + LSM6DSV_ODR_HA01_AT_500Hz = 0x18, + LSM6DSV_ODR_HA01_AT_1000Hz = 0x19, + LSM6DSV_ODR_HA01_AT_2000Hz = 0x1A, + LSM6DSV_ODR_HA01_AT_4000Hz = 0x1B, + LSM6DSV_ODR_HA01_AT_8000Hz = 0x1C, + LSM6DSV_ODR_HA02_AT_12Hz5 = 0x23, + LSM6DSV_ODR_HA02_AT_25Hz = 0x24, + LSM6DSV_ODR_HA02_AT_50Hz = 0x25, + LSM6DSV_ODR_HA02_AT_100Hz = 0x26, + LSM6DSV_ODR_HA02_AT_200Hz = 0x27, + LSM6DSV_ODR_HA02_AT_400Hz = 0x28, + LSM6DSV_ODR_HA02_AT_800Hz = 0x29, + LSM6DSV_ODR_HA02_AT_1600Hz = 0x2A, + LSM6DSV_ODR_HA02_AT_3200Hz = 0x2B, + LSM6DSV_ODR_HA02_AT_6400Hz = 0x2C, +} lsm6dsv_data_rate_t; +int32_t lsm6dsv_xl_data_rate_set(stmdev_ctx_t *ctx, + lsm6dsv_data_rate_t val); +int32_t lsm6dsv_xl_data_rate_get(stmdev_ctx_t *ctx, + lsm6dsv_data_rate_t *val); +int32_t lsm6dsv_gy_data_rate_set(stmdev_ctx_t *ctx, + lsm6dsv_data_rate_t val); +int32_t lsm6dsv_gy_data_rate_get(stmdev_ctx_t *ctx, + lsm6dsv_data_rate_t *val); typedef enum { - LSM6DSV16X_XL_HIGH_PERFORMANCE_MD = 0x0, - LSM6DSV16X_XL_HIGH_ACCURACY_ODR_MD = 0x1, - LSM6DSV16X_XL_ODR_TRIGGERED_MD = 0x3, - LSM6DSV16X_XL_LOW_POWER_2_AVG_MD = 0x4, - LSM6DSV16X_XL_LOW_POWER_4_AVG_MD = 0x5, - LSM6DSV16X_XL_LOW_POWER_8_AVG_MD = 0x6, - LSM6DSV16X_XL_NORMAL_MD = 0x7, -} lsm6dsv16x_xl_mode_t; -int32_t lsm6dsv16x_xl_mode_set(stmdev_ctx_t *ctx, lsm6dsv16x_xl_mode_t val); -int32_t lsm6dsv16x_xl_mode_get(stmdev_ctx_t *ctx, lsm6dsv16x_xl_mode_t *val); + LSM6DSV_XL_HIGH_PERFORMANCE_MD = 0x0, + LSM6DSV_XL_HIGH_ACCURACY_ODR_MD = 0x1, + LSM6DSV_XL_ODR_TRIGGERED_MD = 0x3, + LSM6DSV_XL_LOW_POWER_2_AVG_MD = 0x4, + LSM6DSV_XL_LOW_POWER_4_AVG_MD = 0x5, + LSM6DSV_XL_LOW_POWER_8_AVG_MD = 0x6, + LSM6DSV_XL_NORMAL_MD = 0x7, +} lsm6dsv_xl_mode_t; +int32_t lsm6dsv_xl_mode_set(stmdev_ctx_t *ctx, lsm6dsv_xl_mode_t val); +int32_t lsm6dsv_xl_mode_get(stmdev_ctx_t *ctx, lsm6dsv_xl_mode_t *val); typedef enum { - LSM6DSV16X_GY_HIGH_PERFORMANCE_MD = 0x0, - LSM6DSV16X_GY_HIGH_ACCURACY_ODR_MD = 0x1, - LSM6DSV16X_GY_SLEEP_MD = 0x4, - LSM6DSV16X_GY_LOW_POWER_MD = 0x5, -} lsm6dsv16x_gy_mode_t; -int32_t lsm6dsv16x_gy_mode_set(stmdev_ctx_t *ctx, lsm6dsv16x_gy_mode_t val); -int32_t lsm6dsv16x_gy_mode_get(stmdev_ctx_t *ctx, lsm6dsv16x_gy_mode_t *val); + LSM6DSV_GY_HIGH_PERFORMANCE_MD = 0x0, + LSM6DSV_GY_HIGH_ACCURACY_ODR_MD = 0x1, + LSM6DSV_GY_SLEEP_MD = 0x4, + LSM6DSV_GY_LOW_POWER_MD = 0x5, +} lsm6dsv_gy_mode_t; +int32_t lsm6dsv_gy_mode_set(stmdev_ctx_t *ctx, lsm6dsv_gy_mode_t val); +int32_t lsm6dsv_gy_mode_get(stmdev_ctx_t *ctx, lsm6dsv_gy_mode_t *val); -int32_t lsm6dsv16x_auto_increment_set(stmdev_ctx_t *ctx, uint8_t val); -int32_t lsm6dsv16x_auto_increment_get(stmdev_ctx_t *ctx, uint8_t *val); +int32_t lsm6dsv_auto_increment_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6dsv_auto_increment_get(stmdev_ctx_t *ctx, uint8_t *val); -int32_t lsm6dsv16x_block_data_update_set(stmdev_ctx_t *ctx, uint8_t val); -int32_t lsm6dsv16x_block_data_update_get(stmdev_ctx_t *ctx, uint8_t *val); +int32_t lsm6dsv_block_data_update_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6dsv_block_data_update_get(stmdev_ctx_t *ctx, uint8_t *val); -int32_t lsm6dsv16x_odr_trig_cfg_set(stmdev_ctx_t *ctx, uint8_t val); -int32_t lsm6dsv16x_odr_trig_cfg_get(stmdev_ctx_t *ctx, uint8_t *val); +int32_t lsm6dsv_odr_trig_cfg_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6dsv_odr_trig_cfg_get(stmdev_ctx_t *ctx, uint8_t *val); typedef enum { - LSM6DSV16X_DRDY_LATCHED = 0x0, - LSM6DSV16X_DRDY_PULSED = 0x1, -} lsm6dsv16x_data_ready_mode_t; -int32_t lsm6dsv16x_data_ready_mode_set(stmdev_ctx_t *ctx, - lsm6dsv16x_data_ready_mode_t val); -int32_t lsm6dsv16x_data_ready_mode_get(stmdev_ctx_t *ctx, - lsm6dsv16x_data_ready_mode_t *val); + LSM6DSV_DRDY_LATCHED = 0x0, + LSM6DSV_DRDY_PULSED = 0x1, +} lsm6dsv_data_ready_mode_t; +int32_t lsm6dsv_data_ready_mode_set(stmdev_ctx_t *ctx, + lsm6dsv_data_ready_mode_t val); +int32_t lsm6dsv_data_ready_mode_get(stmdev_ctx_t *ctx, + lsm6dsv_data_ready_mode_t *val); typedef struct { uint8_t enable : 1; /* interrupt enable */ uint8_t lir : 1; /* interrupt pulsed or latched */ -} lsm6dsv16x_interrupt_mode_t; -int32_t lsm6dsv16x_interrupt_enable_set(stmdev_ctx_t *ctx, - lsm6dsv16x_interrupt_mode_t val); -int32_t lsm6dsv16x_interrupt_enable_get(stmdev_ctx_t *ctx, - lsm6dsv16x_interrupt_mode_t *val); +} lsm6dsv_interrupt_mode_t; +int32_t lsm6dsv_interrupt_enable_set(stmdev_ctx_t *ctx, + lsm6dsv_interrupt_mode_t val); +int32_t lsm6dsv_interrupt_enable_get(stmdev_ctx_t *ctx, + lsm6dsv_interrupt_mode_t *val); typedef enum { - LSM6DSV16X_125dps = 0x0, - LSM6DSV16X_250dps = 0x1, - LSM6DSV16X_500dps = 0x2, - LSM6DSV16X_1000dps = 0x3, - LSM6DSV16X_2000dps = 0x4, - LSM6DSV16X_4000dps = 0xc, -} lsm6dsv16x_gy_full_scale_t; -int32_t lsm6dsv16x_gy_full_scale_set(stmdev_ctx_t *ctx, - lsm6dsv16x_gy_full_scale_t val); -int32_t lsm6dsv16x_gy_full_scale_get(stmdev_ctx_t *ctx, - lsm6dsv16x_gy_full_scale_t *val); + LSM6DSV_125dps = 0x0, + LSM6DSV_250dps = 0x1, + LSM6DSV_500dps = 0x2, + LSM6DSV_1000dps = 0x3, + LSM6DSV_2000dps = 0x4, + LSM6DSV_4000dps = 0xc, +} lsm6dsv_gy_full_scale_t; +int32_t lsm6dsv_gy_full_scale_set(stmdev_ctx_t *ctx, + lsm6dsv_gy_full_scale_t val); +int32_t lsm6dsv_gy_full_scale_get(stmdev_ctx_t *ctx, + lsm6dsv_gy_full_scale_t *val); typedef enum { - LSM6DSV16X_2g = 0x0, - LSM6DSV16X_4g = 0x1, - LSM6DSV16X_8g = 0x2, - LSM6DSV16X_16g = 0x3, -} lsm6dsv16x_xl_full_scale_t; -int32_t lsm6dsv16x_xl_full_scale_set(stmdev_ctx_t *ctx, - lsm6dsv16x_xl_full_scale_t val); -int32_t lsm6dsv16x_xl_full_scale_get(stmdev_ctx_t *ctx, - lsm6dsv16x_xl_full_scale_t *val); + LSM6DSV_2g = 0x0, + LSM6DSV_4g = 0x1, + LSM6DSV_8g = 0x2, + LSM6DSV_16g = 0x3, +} lsm6dsv_xl_full_scale_t; +int32_t lsm6dsv_xl_full_scale_set(stmdev_ctx_t *ctx, + lsm6dsv_xl_full_scale_t val); +int32_t lsm6dsv_xl_full_scale_get(stmdev_ctx_t *ctx, + lsm6dsv_xl_full_scale_t *val); -int32_t lsm6dsv16x_xl_dual_channel_set(stmdev_ctx_t *ctx, uint8_t val); -int32_t lsm6dsv16x_xl_dual_channel_get(stmdev_ctx_t *ctx, uint8_t *val); +int32_t lsm6dsv_xl_dual_channel_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6dsv_xl_dual_channel_get(stmdev_ctx_t *ctx, uint8_t *val); typedef enum { - LSM6DSV16X_XL_ST_DISABLE = 0x0, - LSM6DSV16X_XL_ST_POSITIVE = 0x1, - LSM6DSV16X_XL_ST_NEGATIVE = 0x2, -} lsm6dsv16x_xl_self_test_t; -int32_t lsm6dsv16x_xl_self_test_set(stmdev_ctx_t *ctx, - lsm6dsv16x_xl_self_test_t val); -int32_t lsm6dsv16x_xl_self_test_get(stmdev_ctx_t *ctx, - lsm6dsv16x_xl_self_test_t *val); + LSM6DSV_XL_ST_DISABLE = 0x0, + LSM6DSV_XL_ST_POSITIVE = 0x1, + LSM6DSV_XL_ST_NEGATIVE = 0x2, +} lsm6dsv_xl_self_test_t; +int32_t lsm6dsv_xl_self_test_set(stmdev_ctx_t *ctx, + lsm6dsv_xl_self_test_t val); +int32_t lsm6dsv_xl_self_test_get(stmdev_ctx_t *ctx, + lsm6dsv_xl_self_test_t *val); typedef enum { - LSM6DSV16X_OIS_XL_ST_DISABLE = 0x0, - LSM6DSV16X_OIS_XL_ST_POSITIVE = 0x1, - LSM6DSV16X_OIS_XL_ST_NEGATIVE = 0x2, -} lsm6dsv16x_ois_xl_self_test_t; -int32_t lsm6dsv16x_ois_xl_self_test_set(stmdev_ctx_t *ctx, - lsm6dsv16x_ois_xl_self_test_t val); -int32_t lsm6dsv16x_ois_xl_self_test_get(stmdev_ctx_t *ctx, - lsm6dsv16x_ois_xl_self_test_t *val); + LSM6DSV_OIS_XL_ST_DISABLE = 0x0, + LSM6DSV_OIS_XL_ST_POSITIVE = 0x1, + LSM6DSV_OIS_XL_ST_NEGATIVE = 0x2, +} lsm6dsv_ois_xl_self_test_t; +int32_t lsm6dsv_ois_xl_self_test_set(stmdev_ctx_t *ctx, + lsm6dsv_ois_xl_self_test_t val); +int32_t lsm6dsv_ois_xl_self_test_get(stmdev_ctx_t *ctx, + lsm6dsv_ois_xl_self_test_t *val); typedef enum { - LSM6DSV16X_GY_ST_DISABLE = 0x0, - LSM6DSV16X_GY_ST_POSITIVE = 0x1, - LSM6DSV16X_GY_ST_NEGATIVE = 0x2, + LSM6DSV_GY_ST_DISABLE = 0x0, + LSM6DSV_GY_ST_POSITIVE = 0x1, + LSM6DSV_GY_ST_NEGATIVE = 0x2, -} lsm6dsv16x_gy_self_test_t; -int32_t lsm6dsv16x_gy_self_test_set(stmdev_ctx_t *ctx, - lsm6dsv16x_gy_self_test_t val); -int32_t lsm6dsv16x_gy_self_test_get(stmdev_ctx_t *ctx, - lsm6dsv16x_gy_self_test_t *val); +} lsm6dsv_gy_self_test_t; +int32_t lsm6dsv_gy_self_test_set(stmdev_ctx_t *ctx, + lsm6dsv_gy_self_test_t val); +int32_t lsm6dsv_gy_self_test_get(stmdev_ctx_t *ctx, + lsm6dsv_gy_self_test_t *val); typedef enum { - LSM6DSV16X_OIS_GY_ST_DISABLE = 0x0, - LSM6DSV16X_OIS_GY_ST_POSITIVE = 0x1, - LSM6DSV16X_OIS_GY_ST_NEGATIVE = 0x2, - LSM6DSV16X_OIS_GY_ST_CLAMP_POS = 0x5, - LSM6DSV16X_OIS_GY_ST_CLAMP_NEG = 0x6, + LSM6DSV_OIS_GY_ST_DISABLE = 0x0, + LSM6DSV_OIS_GY_ST_POSITIVE = 0x1, + LSM6DSV_OIS_GY_ST_NEGATIVE = 0x2, + LSM6DSV_OIS_GY_ST_CLAMP_POS = 0x5, + LSM6DSV_OIS_GY_ST_CLAMP_NEG = 0x6, -} lsm6dsv16x_ois_gy_self_test_t; -int32_t lsm6dsv16x_ois_gy_self_test_set(stmdev_ctx_t *ctx, - lsm6dsv16x_ois_gy_self_test_t val); -int32_t lsm6dsv16x_ois_gy_self_test_get(stmdev_ctx_t *ctx, - lsm6dsv16x_ois_gy_self_test_t *val); +} lsm6dsv_ois_gy_self_test_t; +int32_t lsm6dsv_ois_gy_self_test_set(stmdev_ctx_t *ctx, + lsm6dsv_ois_gy_self_test_t val); +int32_t lsm6dsv_ois_gy_self_test_get(stmdev_ctx_t *ctx, + lsm6dsv_ois_gy_self_test_t *val); typedef struct { uint8_t drdy_xl : 1; uint8_t drdy_gy : 1; uint8_t drdy_temp : 1; - uint8_t drdy_ah_qvar : 1; uint8_t drdy_eis : 1; uint8_t drdy_ois : 1; uint8_t gy_settling : 1; @@ -4155,10 +3947,6 @@ typedef struct uint8_t fsm6 : 1; uint8_t fsm7 : 1; uint8_t fsm8 : 1; - uint8_t mlc1 : 1; - uint8_t mlc2 : 1; - uint8_t mlc3 : 1; - uint8_t mlc4 : 1; uint8_t sh_endop : 1; uint8_t sh_slave0_nack : 1; uint8_t sh_slave1_nack : 1; @@ -4169,9 +3957,9 @@ typedef struct uint8_t fifo_full : 1; uint8_t fifo_ovr : 1; uint8_t fifo_th : 1; -} lsm6dsv16x_all_sources_t; -int32_t lsm6dsv16x_all_sources_get(stmdev_ctx_t *ctx, - lsm6dsv16x_all_sources_t *val); +} lsm6dsv_all_sources_t; +int32_t lsm6dsv_all_sources_get(stmdev_ctx_t *ctx, + lsm6dsv_all_sources_t *val); typedef struct { @@ -4192,65 +3980,63 @@ typedef struct uint8_t wakeup : 1; uint8_t freefall : 1; uint8_t sleep_change : 1; -} lsm6dsv16x_pin_int_route_t; -int32_t lsm6dsv16x_pin_int1_route_set(stmdev_ctx_t *ctx, - lsm6dsv16x_pin_int_route_t *val); -int32_t lsm6dsv16x_pin_int1_route_get(stmdev_ctx_t *ctx, - lsm6dsv16x_pin_int_route_t *val); -int32_t lsm6dsv16x_pin_int2_route_set(stmdev_ctx_t *ctx, - lsm6dsv16x_pin_int_route_t *val); -int32_t lsm6dsv16x_pin_int2_route_get(stmdev_ctx_t *ctx, - lsm6dsv16x_pin_int_route_t *val); +} lsm6dsv_pin_int_route_t; +int32_t lsm6dsv_pin_int1_route_set(stmdev_ctx_t *ctx, + lsm6dsv_pin_int_route_t *val); +int32_t lsm6dsv_pin_int1_route_get(stmdev_ctx_t *ctx, + lsm6dsv_pin_int_route_t *val); +int32_t lsm6dsv_pin_int2_route_set(stmdev_ctx_t *ctx, + lsm6dsv_pin_int_route_t *val); +int32_t lsm6dsv_pin_int2_route_get(stmdev_ctx_t *ctx, + lsm6dsv_pin_int_route_t *val); typedef struct { uint8_t drdy_xl : 1; uint8_t drdy_gy : 1; uint8_t drdy_temp : 1; -} lsm6dsv16x_data_ready_t; -int32_t lsm6dsv16x_flag_data_ready_get(stmdev_ctx_t *ctx, - lsm6dsv16x_data_ready_t *val); - -int32_t lsm6dsv16x_int_ack_mask_set(stmdev_ctx_t *ctx, uint8_t val); -int32_t lsm6dsv16x_int_ack_mask_get(stmdev_ctx_t *ctx, uint8_t *val); +} lsm6dsv_data_ready_t; +int32_t lsm6dsv_flag_data_ready_get(stmdev_ctx_t *ctx, + lsm6dsv_data_ready_t *val); -int32_t lsm6dsv16x_temperature_raw_get(stmdev_ctx_t *ctx, int16_t *val); +int32_t lsm6dsv_int_ack_mask_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6dsv_int_ack_mask_get(stmdev_ctx_t *ctx, uint8_t *val); -int32_t lsm6dsv16x_angular_rate_raw_get(stmdev_ctx_t *ctx, int16_t *val); +int32_t lsm6dsv_temperature_raw_get(stmdev_ctx_t *ctx, int16_t *val); -int32_t lsm6dsv16x_ois_angular_rate_raw_get(stmdev_ctx_t *ctx, int16_t *val); +int32_t lsm6dsv_angular_rate_raw_get(stmdev_ctx_t *ctx, int16_t *val); -int32_t lsm6dsv16x_ois_eis_angular_rate_raw_get(stmdev_ctx_t *ctx, - int16_t *val); +int32_t lsm6dsv_ois_angular_rate_raw_get(stmdev_ctx_t *ctx, int16_t *val); -int32_t lsm6dsv16x_acceleration_raw_get(stmdev_ctx_t *ctx, int16_t *val); +int32_t lsm6dsv_ois_eis_angular_rate_raw_get(stmdev_ctx_t *ctx, + int16_t *val); -int32_t lsm6dsv16x_dual_acceleration_raw_get(stmdev_ctx_t *ctx, int16_t *val); +int32_t lsm6dsv_acceleration_raw_get(stmdev_ctx_t *ctx, int16_t *val); -int32_t lsm6dsv16x_ois_dual_acceleration_raw_get(stmdev_ctx_t *ctx, - int16_t *val); +int32_t lsm6dsv_dual_acceleration_raw_get(stmdev_ctx_t *ctx, int16_t *val); -int32_t lsm6dsv16x_ah_qvar_raw_get(stmdev_ctx_t *ctx, int16_t *val); +int32_t lsm6dsv_ois_dual_acceleration_raw_get(stmdev_ctx_t *ctx, + int16_t *val); -int32_t lsm6dsv16x_odr_cal_reg_get(stmdev_ctx_t *ctx, int8_t *val); +int32_t lsm6dsv_odr_cal_reg_get(stmdev_ctx_t *ctx, int8_t *val); -int32_t lsm6dsv16x_ln_pg_write(stmdev_ctx_t *ctx, uint16_t address, - uint8_t *buf, uint8_t len); -int32_t lsm6dsv16x_ln_pg_read(stmdev_ctx_t *ctx, uint16_t address, uint8_t *buf, - uint8_t len); +int32_t lsm6dsv_ln_pg_write(stmdev_ctx_t *ctx, uint16_t address, + uint8_t *buf, uint8_t len); +int32_t lsm6dsv_ln_pg_read(stmdev_ctx_t *ctx, uint16_t address, uint8_t *buf, + uint8_t len); -int32_t lsm6dsv16x_emb_function_dbg_set(stmdev_ctx_t *ctx, uint8_t val); -int32_t lsm6dsv16x_emb_function_dbg_get(stmdev_ctx_t *ctx, uint8_t *val); +int32_t lsm6dsv_emb_function_dbg_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6dsv_emb_function_dbg_get(stmdev_ctx_t *ctx, uint8_t *val); typedef enum { - LSM6DSV16X_DEN_ACT_LOW = 0x0, - LSM6DSV16X_DEN_ACT_HIGH = 0x1, -} lsm6dsv16x_den_polarity_t; -int32_t lsm6dsv16x_den_polarity_set(stmdev_ctx_t *ctx, - lsm6dsv16x_den_polarity_t val); -int32_t lsm6dsv16x_den_polarity_get(stmdev_ctx_t *ctx, - lsm6dsv16x_den_polarity_t *val); + LSM6DSV_DEN_ACT_LOW = 0x0, + LSM6DSV_DEN_ACT_HIGH = 0x1, +} lsm6dsv_den_polarity_t; +int32_t lsm6dsv_den_polarity_set(stmdev_ctx_t *ctx, + lsm6dsv_den_polarity_t val); +int32_t lsm6dsv_den_polarity_get(stmdev_ctx_t *ctx, + lsm6dsv_den_polarity_t *val); typedef struct { @@ -4261,170 +4047,170 @@ typedef struct uint8_t den_z : 1; enum { - DEN_NOT_DEFINED = 0x00, - LEVEL_TRIGGER = 0x02, - LEVEL_LATCHED = 0x03, + LSM6DSV_DEN_NOT_DEFINED = 0x00, + LSM6DSV_LEVEL_TRIGGER = 0x02, + LSM6DSV_LEVEL_LATCHED = 0x03, } mode; -} lsm6dsv16x_den_conf_t; -int32_t lsm6dsv16x_den_conf_set(stmdev_ctx_t *ctx, lsm6dsv16x_den_conf_t val); -int32_t lsm6dsv16x_den_conf_get(stmdev_ctx_t *ctx, lsm6dsv16x_den_conf_t *val); +} lsm6dsv_den_conf_t; +int32_t lsm6dsv_den_conf_set(stmdev_ctx_t *ctx, lsm6dsv_den_conf_t val); +int32_t lsm6dsv_den_conf_get(stmdev_ctx_t *ctx, lsm6dsv_den_conf_t *val); typedef enum { - LSM6DSV16X_EIS_125dps = 0x0, - LSM6DSV16X_EIS_250dps = 0x1, - LSM6DSV16X_EIS_500dps = 0x2, - LSM6DSV16X_EIS_1000dps = 0x3, - LSM6DSV16X_EIS_2000dps = 0x4, -} lsm6dsv16x_eis_gy_full_scale_t; -int32_t lsm6dsv16x_eis_gy_full_scale_set(stmdev_ctx_t *ctx, - lsm6dsv16x_eis_gy_full_scale_t val); -int32_t lsm6dsv16x_eis_gy_full_scale_get(stmdev_ctx_t *ctx, - lsm6dsv16x_eis_gy_full_scale_t *val); + LSM6DSV_EIS_125dps = 0x0, + LSM6DSV_EIS_250dps = 0x1, + LSM6DSV_EIS_500dps = 0x2, + LSM6DSV_EIS_1000dps = 0x3, + LSM6DSV_EIS_2000dps = 0x4, +} lsm6dsv_eis_gy_full_scale_t; +int32_t lsm6dsv_eis_gy_full_scale_set(stmdev_ctx_t *ctx, + lsm6dsv_eis_gy_full_scale_t val); +int32_t lsm6dsv_eis_gy_full_scale_get(stmdev_ctx_t *ctx, + lsm6dsv_eis_gy_full_scale_t *val); -int32_t lsm6dsv16x_eis_gy_on_spi2_set(stmdev_ctx_t *ctx, uint8_t val); -int32_t lsm6dsv16x_eis_gy_on_spi2_get(stmdev_ctx_t *ctx, uint8_t *val); +int32_t lsm6dsv_eis_gy_on_spi2_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6dsv_eis_gy_on_spi2_get(stmdev_ctx_t *ctx, uint8_t *val); typedef enum { - LSM6DSV16X_EIS_ODR_OFF = 0x0, - LSM6DSV16X_EIS_1920Hz = 0x1, - LSM6DSV16X_EIS_960Hz = 0x2, -} lsm6dsv16x_gy_eis_data_rate_t; -int32_t lsm6dsv16x_gy_eis_data_rate_set(stmdev_ctx_t *ctx, - lsm6dsv16x_gy_eis_data_rate_t val); -int32_t lsm6dsv16x_gy_eis_data_rate_get(stmdev_ctx_t *ctx, - lsm6dsv16x_gy_eis_data_rate_t *val); + LSM6DSV_EIS_ODR_OFF = 0x0, + LSM6DSV_EIS_1920Hz = 0x1, + LSM6DSV_EIS_960Hz = 0x2, +} lsm6dsv_gy_eis_data_rate_t; +int32_t lsm6dsv_gy_eis_data_rate_set(stmdev_ctx_t *ctx, + lsm6dsv_gy_eis_data_rate_t val); +int32_t lsm6dsv_gy_eis_data_rate_get(stmdev_ctx_t *ctx, + lsm6dsv_gy_eis_data_rate_t *val); -int32_t lsm6dsv16x_fifo_watermark_set(stmdev_ctx_t *ctx, uint8_t val); -int32_t lsm6dsv16x_fifo_watermark_get(stmdev_ctx_t *ctx, uint8_t *val); +int32_t lsm6dsv_fifo_watermark_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6dsv_fifo_watermark_get(stmdev_ctx_t *ctx, uint8_t *val); -int32_t lsm6dsv16x_fifo_xl_dual_fsm_batch_set(stmdev_ctx_t *ctx, uint8_t val); -int32_t lsm6dsv16x_fifo_xl_dual_fsm_batch_get(stmdev_ctx_t *ctx, uint8_t *val); +int32_t lsm6dsv_fifo_xl_dual_fsm_batch_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6dsv_fifo_xl_dual_fsm_batch_get(stmdev_ctx_t *ctx, uint8_t *val); typedef enum { - LSM6DSV16X_CMP_DISABLE = 0x0, - LSM6DSV16X_CMP_8_TO_1 = 0x1, - LSM6DSV16X_CMP_16_TO_1 = 0x2, - LSM6DSV16X_CMP_32_TO_1 = 0x3, -} lsm6dsv16x_fifo_compress_algo_t; -int32_t lsm6dsv16x_fifo_compress_algo_set(stmdev_ctx_t *ctx, - lsm6dsv16x_fifo_compress_algo_t val); -int32_t lsm6dsv16x_fifo_compress_algo_get(stmdev_ctx_t *ctx, - lsm6dsv16x_fifo_compress_algo_t *val); - -int32_t lsm6dsv16x_fifo_virtual_sens_odr_chg_set(stmdev_ctx_t *ctx, + LSM6DSV_CMP_DISABLE = 0x0, + LSM6DSV_CMP_8_TO_1 = 0x1, + LSM6DSV_CMP_16_TO_1 = 0x2, + LSM6DSV_CMP_32_TO_1 = 0x3, +} lsm6dsv_fifo_compress_algo_t; +int32_t lsm6dsv_fifo_compress_algo_set(stmdev_ctx_t *ctx, + lsm6dsv_fifo_compress_algo_t val); +int32_t lsm6dsv_fifo_compress_algo_get(stmdev_ctx_t *ctx, + lsm6dsv_fifo_compress_algo_t *val); + +int32_t lsm6dsv_fifo_virtual_sens_odr_chg_set(stmdev_ctx_t *ctx, + uint8_t val); +int32_t lsm6dsv_fifo_virtual_sens_odr_chg_get(stmdev_ctx_t *ctx, + uint8_t *val); + +int32_t lsm6dsv_fifo_compress_algo_real_time_set(stmdev_ctx_t *ctx, uint8_t val); -int32_t lsm6dsv16x_fifo_virtual_sens_odr_chg_get(stmdev_ctx_t *ctx, +int32_t lsm6dsv_fifo_compress_algo_real_time_get(stmdev_ctx_t *ctx, uint8_t *val); -int32_t lsm6dsv16x_fifo_compress_algo_real_time_set(stmdev_ctx_t *ctx, - uint8_t val); -int32_t lsm6dsv16x_fifo_compress_algo_real_time_get(stmdev_ctx_t *ctx, - uint8_t *val); - -int32_t lsm6dsv16x_fifo_stop_on_wtm_set(stmdev_ctx_t *ctx, uint8_t val); -int32_t lsm6dsv16x_fifo_stop_on_wtm_get(stmdev_ctx_t *ctx, uint8_t *val); +int32_t lsm6dsv_fifo_stop_on_wtm_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6dsv_fifo_stop_on_wtm_get(stmdev_ctx_t *ctx, uint8_t *val); typedef enum { - LSM6DSV16X_XL_NOT_BATCHED = 0x0, - LSM6DSV16X_XL_BATCHED_AT_1Hz875 = 0x1, - LSM6DSV16X_XL_BATCHED_AT_7Hz5 = 0x2, - LSM6DSV16X_XL_BATCHED_AT_15Hz = 0x3, - LSM6DSV16X_XL_BATCHED_AT_30Hz = 0x4, - LSM6DSV16X_XL_BATCHED_AT_60Hz = 0x5, - LSM6DSV16X_XL_BATCHED_AT_120Hz = 0x6, - LSM6DSV16X_XL_BATCHED_AT_240Hz = 0x7, - LSM6DSV16X_XL_BATCHED_AT_480Hz = 0x8, - LSM6DSV16X_XL_BATCHED_AT_960Hz = 0x9, - LSM6DSV16X_XL_BATCHED_AT_1920Hz = 0xa, - LSM6DSV16X_XL_BATCHED_AT_3840Hz = 0xb, - LSM6DSV16X_XL_BATCHED_AT_7680Hz = 0xc, -} lsm6dsv16x_fifo_xl_batch_t; -int32_t lsm6dsv16x_fifo_xl_batch_set(stmdev_ctx_t *ctx, - lsm6dsv16x_fifo_xl_batch_t val); -int32_t lsm6dsv16x_fifo_xl_batch_get(stmdev_ctx_t *ctx, - lsm6dsv16x_fifo_xl_batch_t *val); + LSM6DSV_XL_NOT_BATCHED = 0x0, + LSM6DSV_XL_BATCHED_AT_1Hz875 = 0x1, + LSM6DSV_XL_BATCHED_AT_7Hz5 = 0x2, + LSM6DSV_XL_BATCHED_AT_15Hz = 0x3, + LSM6DSV_XL_BATCHED_AT_30Hz = 0x4, + LSM6DSV_XL_BATCHED_AT_60Hz = 0x5, + LSM6DSV_XL_BATCHED_AT_120Hz = 0x6, + LSM6DSV_XL_BATCHED_AT_240Hz = 0x7, + LSM6DSV_XL_BATCHED_AT_480Hz = 0x8, + LSM6DSV_XL_BATCHED_AT_960Hz = 0x9, + LSM6DSV_XL_BATCHED_AT_1920Hz = 0xa, + LSM6DSV_XL_BATCHED_AT_3840Hz = 0xb, + LSM6DSV_XL_BATCHED_AT_7680Hz = 0xc, +} lsm6dsv_fifo_xl_batch_t; +int32_t lsm6dsv_fifo_xl_batch_set(stmdev_ctx_t *ctx, + lsm6dsv_fifo_xl_batch_t val); +int32_t lsm6dsv_fifo_xl_batch_get(stmdev_ctx_t *ctx, + lsm6dsv_fifo_xl_batch_t *val); typedef enum { - LSM6DSV16X_GY_NOT_BATCHED = 0x0, - LSM6DSV16X_GY_BATCHED_AT_1Hz875 = 0x1, - LSM6DSV16X_GY_BATCHED_AT_7Hz5 = 0x2, - LSM6DSV16X_GY_BATCHED_AT_15Hz = 0x3, - LSM6DSV16X_GY_BATCHED_AT_30Hz = 0x4, - LSM6DSV16X_GY_BATCHED_AT_60Hz = 0x5, - LSM6DSV16X_GY_BATCHED_AT_120Hz = 0x6, - LSM6DSV16X_GY_BATCHED_AT_240Hz = 0x7, - LSM6DSV16X_GY_BATCHED_AT_480Hz = 0x8, - LSM6DSV16X_GY_BATCHED_AT_960Hz = 0x9, - LSM6DSV16X_GY_BATCHED_AT_1920Hz = 0xa, - LSM6DSV16X_GY_BATCHED_AT_3840Hz = 0xb, - LSM6DSV16X_GY_BATCHED_AT_7680Hz = 0xc, -} lsm6dsv16x_fifo_gy_batch_t; -int32_t lsm6dsv16x_fifo_gy_batch_set(stmdev_ctx_t *ctx, - lsm6dsv16x_fifo_gy_batch_t val); -int32_t lsm6dsv16x_fifo_gy_batch_get(stmdev_ctx_t *ctx, - lsm6dsv16x_fifo_gy_batch_t *val); + LSM6DSV_GY_NOT_BATCHED = 0x0, + LSM6DSV_GY_BATCHED_AT_1Hz875 = 0x1, + LSM6DSV_GY_BATCHED_AT_7Hz5 = 0x2, + LSM6DSV_GY_BATCHED_AT_15Hz = 0x3, + LSM6DSV_GY_BATCHED_AT_30Hz = 0x4, + LSM6DSV_GY_BATCHED_AT_60Hz = 0x5, + LSM6DSV_GY_BATCHED_AT_120Hz = 0x6, + LSM6DSV_GY_BATCHED_AT_240Hz = 0x7, + LSM6DSV_GY_BATCHED_AT_480Hz = 0x8, + LSM6DSV_GY_BATCHED_AT_960Hz = 0x9, + LSM6DSV_GY_BATCHED_AT_1920Hz = 0xa, + LSM6DSV_GY_BATCHED_AT_3840Hz = 0xb, + LSM6DSV_GY_BATCHED_AT_7680Hz = 0xc, +} lsm6dsv_fifo_gy_batch_t; +int32_t lsm6dsv_fifo_gy_batch_set(stmdev_ctx_t *ctx, + lsm6dsv_fifo_gy_batch_t val); +int32_t lsm6dsv_fifo_gy_batch_get(stmdev_ctx_t *ctx, + lsm6dsv_fifo_gy_batch_t *val); typedef enum { - LSM6DSV16X_BYPASS_MODE = 0x0, - LSM6DSV16X_FIFO_MODE = 0x1, - LSM6DSV16X_STREAM_WTM_TO_FULL_MODE = 0x2, - LSM6DSV16X_STREAM_TO_FIFO_MODE = 0x3, - LSM6DSV16X_BYPASS_TO_STREAM_MODE = 0x4, - LSM6DSV16X_STREAM_MODE = 0x6, - LSM6DSV16X_BYPASS_TO_FIFO_MODE = 0x7, -} lsm6dsv16x_fifo_mode_t; -int32_t lsm6dsv16x_fifo_mode_set(stmdev_ctx_t *ctx, lsm6dsv16x_fifo_mode_t val); -int32_t lsm6dsv16x_fifo_mode_get(stmdev_ctx_t *ctx, - lsm6dsv16x_fifo_mode_t *val); - -int32_t lsm6dsv16x_fifo_gy_eis_batch_set(stmdev_ctx_t *ctx, uint8_t val); -int32_t lsm6dsv16x_fifo_gy_eis_batch_get(stmdev_ctx_t *ctx, uint8_t *val); + LSM6DSV_BYPASS_MODE = 0x0, + LSM6DSV_FIFO_MODE = 0x1, + LSM6DSV_STREAM_WTM_TO_FULL_MODE = 0x2, + LSM6DSV_STREAM_TO_FIFO_MODE = 0x3, + LSM6DSV_BYPASS_TO_STREAM_MODE = 0x4, + LSM6DSV_STREAM_MODE = 0x6, + LSM6DSV_BYPASS_TO_FIFO_MODE = 0x7, +} lsm6dsv_fifo_mode_t; +int32_t lsm6dsv_fifo_mode_set(stmdev_ctx_t *ctx, lsm6dsv_fifo_mode_t val); +int32_t lsm6dsv_fifo_mode_get(stmdev_ctx_t *ctx, + lsm6dsv_fifo_mode_t *val); + +int32_t lsm6dsv_fifo_gy_eis_batch_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6dsv_fifo_gy_eis_batch_get(stmdev_ctx_t *ctx, uint8_t *val); typedef enum { - LSM6DSV16X_TEMP_NOT_BATCHED = 0x0, - LSM6DSV16X_TEMP_BATCHED_AT_1Hz875 = 0x1, - LSM6DSV16X_TEMP_BATCHED_AT_15Hz = 0x2, - LSM6DSV16X_TEMP_BATCHED_AT_60Hz = 0x3, -} lsm6dsv16x_fifo_temp_batch_t; -int32_t lsm6dsv16x_fifo_temp_batch_set(stmdev_ctx_t *ctx, - lsm6dsv16x_fifo_temp_batch_t val); -int32_t lsm6dsv16x_fifo_temp_batch_get(stmdev_ctx_t *ctx, - lsm6dsv16x_fifo_temp_batch_t *val); + LSM6DSV_TEMP_NOT_BATCHED = 0x0, + LSM6DSV_TEMP_BATCHED_AT_1Hz875 = 0x1, + LSM6DSV_TEMP_BATCHED_AT_15Hz = 0x2, + LSM6DSV_TEMP_BATCHED_AT_60Hz = 0x3, +} lsm6dsv_fifo_temp_batch_t; +int32_t lsm6dsv_fifo_temp_batch_set(stmdev_ctx_t *ctx, + lsm6dsv_fifo_temp_batch_t val); +int32_t lsm6dsv_fifo_temp_batch_get(stmdev_ctx_t *ctx, + lsm6dsv_fifo_temp_batch_t *val); typedef enum { - LSM6DSV16X_TMSTMP_NOT_BATCHED = 0x0, - LSM6DSV16X_TMSTMP_DEC_1 = 0x1, - LSM6DSV16X_TMSTMP_DEC_8 = 0x2, - LSM6DSV16X_TMSTMP_DEC_32 = 0x3, -} lsm6dsv16x_fifo_timestamp_batch_t; -int32_t lsm6dsv16x_fifo_timestamp_batch_set(stmdev_ctx_t *ctx, - lsm6dsv16x_fifo_timestamp_batch_t val); -int32_t lsm6dsv16x_fifo_timestamp_batch_get(stmdev_ctx_t *ctx, - lsm6dsv16x_fifo_timestamp_batch_t *val); - -int32_t lsm6dsv16x_fifo_batch_counter_threshold_set(stmdev_ctx_t *ctx, - uint16_t val); -int32_t lsm6dsv16x_fifo_batch_counter_threshold_get(stmdev_ctx_t *ctx, - uint16_t *val); + LSM6DSV_TMSTMP_NOT_BATCHED = 0x0, + LSM6DSV_TMSTMP_DEC_1 = 0x1, + LSM6DSV_TMSTMP_DEC_8 = 0x2, + LSM6DSV_TMSTMP_DEC_32 = 0x3, +} lsm6dsv_fifo_timestamp_batch_t; +int32_t lsm6dsv_fifo_timestamp_batch_set(stmdev_ctx_t *ctx, + lsm6dsv_fifo_timestamp_batch_t val); +int32_t lsm6dsv_fifo_timestamp_batch_get(stmdev_ctx_t *ctx, + lsm6dsv_fifo_timestamp_batch_t *val); + +int32_t lsm6dsv_fifo_batch_counter_threshold_set(stmdev_ctx_t *ctx, + uint16_t val); +int32_t lsm6dsv_fifo_batch_counter_threshold_get(stmdev_ctx_t *ctx, + uint16_t *val); typedef enum { - LSM6DSV16X_XL_BATCH_EVENT = 0x0, - LSM6DSV16X_GY_BATCH_EVENT = 0x1, - LSM6DSV16X_GY_EIS_BATCH_EVENT = 0x2, -} lsm6dsv16x_fifo_batch_cnt_event_t; -int32_t lsm6dsv16x_fifo_batch_cnt_event_set(stmdev_ctx_t *ctx, - lsm6dsv16x_fifo_batch_cnt_event_t val); -int32_t lsm6dsv16x_fifo_batch_cnt_event_get(stmdev_ctx_t *ctx, - lsm6dsv16x_fifo_batch_cnt_event_t *val); + LSM6DSV_XL_BATCH_EVENT = 0x0, + LSM6DSV_GY_BATCH_EVENT = 0x1, + LSM6DSV_GY_EIS_BATCH_EVENT = 0x2, +} lsm6dsv_fifo_batch_cnt_event_t; +int32_t lsm6dsv_fifo_batch_cnt_event_set(stmdev_ctx_t *ctx, + lsm6dsv_fifo_batch_cnt_event_t val); +int32_t lsm6dsv_fifo_batch_cnt_event_get(stmdev_ctx_t *ctx, + lsm6dsv_fifo_batch_cnt_event_t *val); typedef struct { @@ -4433,82 +4219,73 @@ typedef struct uint8_t fifo_full : 1; uint8_t fifo_ovr : 1; uint8_t fifo_th : 1; -} lsm6dsv16x_fifo_status_t; +} lsm6dsv_fifo_status_t; -int32_t lsm6dsv16x_fifo_status_get(stmdev_ctx_t *ctx, - lsm6dsv16x_fifo_status_t *val); +int32_t lsm6dsv_fifo_status_get(stmdev_ctx_t *ctx, + lsm6dsv_fifo_status_t *val); typedef struct { enum { - LSM6DSV16X_FIFO_EMPTY = 0x0, - LSM6DSV16X_GY_NC_TAG = 0x1, - LSM6DSV16X_XL_NC_TAG = 0x2, - LSM6DSV16X_TEMPERATURE_TAG = 0x3, - LSM6DSV16X_TIMESTAMP_TAG = 0x4, - LSM6DSV16X_CFG_CHANGE_TAG = 0x5, - LSM6DSV16X_XL_NC_T_2_TAG = 0x6, - LSM6DSV16X_XL_NC_T_1_TAG = 0x7, - LSM6DSV16X_XL_2XC_TAG = 0x8, - LSM6DSV16X_XL_3XC_TAG = 0x9, - LSM6DSV16X_GY_NC_T_2_TAG = 0xA, - LSM6DSV16X_GY_NC_T_1_TAG = 0xB, - LSM6DSV16X_GY_2XC_TAG = 0xC, - LSM6DSV16X_GY_3XC_TAG = 0xD, - LSM6DSV16X_SENSORHUB_SLAVE0_TAG = 0xE, - LSM6DSV16X_SENSORHUB_SLAVE1_TAG = 0xF, - LSM6DSV16X_SENSORHUB_SLAVE2_TAG = 0x10, - LSM6DSV16X_SENSORHUB_SLAVE3_TAG = 0x11, - LSM6DSV16X_STEP_COUNTER_TAG = 0x12, - LSM6DSV16X_SFLP_GAME_ROTATION_VECTOR_TAG = 0x13, - LSM6DSV16X_SFLP_GYROSCOPE_BIAS_TAG = 0x16, - LSM6DSV16X_SFLP_GRAVITY_VECTOR_TAG = 0x17, - LSM6DSV16X_SENSORHUB_NACK_TAG = 0x19, - LSM6DSV16X_MLC_RESULT_TAG = 0x1A, - LSM6DSV16X_MLC_FILTER = 0x1B, - LSM6DSV16X_MLC_FEATURE = 0x1C, - LSM6DSV16X_XL_DUAL_CORE = 0x1D, - LSM6DSV16X_GY_ENHANCED_EIS = 0x1E, + LSM6DSV_FIFO_EMPTY = 0x0, + LSM6DSV_GY_NC_TAG = 0x1, + LSM6DSV_XL_NC_TAG = 0x2, + LSM6DSV_TEMPERATURE_TAG = 0x3, + LSM6DSV_TIMESTAMP_TAG = 0x4, + LSM6DSV_CFG_CHANGE_TAG = 0x5, + LSM6DSV_XL_NC_T_2_TAG = 0x6, + LSM6DSV_XL_NC_T_1_TAG = 0x7, + LSM6DSV_XL_2XC_TAG = 0x8, + LSM6DSV_XL_3XC_TAG = 0x9, + LSM6DSV_GY_NC_T_2_TAG = 0xA, + LSM6DSV_GY_NC_T_1_TAG = 0xB, + LSM6DSV_GY_2XC_TAG = 0xC, + LSM6DSV_GY_3XC_TAG = 0xD, + LSM6DSV_SENSORHUB_SLAVE0_TAG = 0xE, + LSM6DSV_SENSORHUB_SLAVE1_TAG = 0xF, + LSM6DSV_SENSORHUB_SLAVE2_TAG = 0x10, + LSM6DSV_SENSORHUB_SLAVE3_TAG = 0x11, + LSM6DSV_STEP_COUNTER_TAG = 0x12, + LSM6DSV_SFLP_GAME_ROTATION_VECTOR_TAG = 0x13, + LSM6DSV_SFLP_GYROSCOPE_BIAS_TAG = 0x16, + LSM6DSV_SFLP_GRAVITY_VECTOR_TAG = 0x17, + LSM6DSV_SENSORHUB_NACK_TAG = 0x19, + LSM6DSV_XL_DUAL_CORE = 0x1D, + LSM6DSV_GY_ENHANCED_EIS = 0x1E, } tag; uint8_t cnt; uint8_t data[6]; -} lsm6dsv16x_fifo_out_raw_t; -int32_t lsm6dsv16x_fifo_out_raw_get(stmdev_ctx_t *ctx, - lsm6dsv16x_fifo_out_raw_t *val); - -int32_t lsm6dsv16x_fifo_stpcnt_batch_set(stmdev_ctx_t *ctx, uint8_t val); -int32_t lsm6dsv16x_fifo_stpcnt_batch_get(stmdev_ctx_t *ctx, uint8_t *val); +} lsm6dsv_fifo_out_raw_t; +int32_t lsm6dsv_fifo_out_raw_get(stmdev_ctx_t *ctx, + lsm6dsv_fifo_out_raw_t *val); -int32_t lsm6dsv16x_fifo_mlc_batch_set(stmdev_ctx_t *ctx, uint8_t val); -int32_t lsm6dsv16x_fifo_mlc_batch_get(stmdev_ctx_t *ctx, uint8_t *val); +int32_t lsm6dsv_fifo_stpcnt_batch_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6dsv_fifo_stpcnt_batch_get(stmdev_ctx_t *ctx, uint8_t *val); -int32_t lsm6dsv16x_fifo_mlc_filt_batch_set(stmdev_ctx_t *ctx, uint8_t val); -int32_t lsm6dsv16x_fifo_mlc_filt_batch_get(stmdev_ctx_t *ctx, uint8_t *val); - -int32_t lsm6dsv16x_fifo_sh_batch_slave_set(stmdev_ctx_t *ctx, uint8_t idx, uint8_t val); -int32_t lsm6dsv16x_fifo_sh_batch_slave_get(stmdev_ctx_t *ctx, uint8_t idx, uint8_t *val); +int32_t lsm6dsv_fifo_sh_batch_slave_set(stmdev_ctx_t *ctx, uint8_t idx, uint8_t val); +int32_t lsm6dsv_fifo_sh_batch_slave_get(stmdev_ctx_t *ctx, uint8_t idx, uint8_t *val); typedef struct { uint8_t game_rotation : 1; uint8_t gravity : 1; uint8_t gbias : 1; -} lsm6dsv16x_fifo_sflp_raw_t; -int32_t lsm6dsv16x_fifo_sflp_batch_set(stmdev_ctx_t *ctx, - lsm6dsv16x_fifo_sflp_raw_t val); -int32_t lsm6dsv16x_fifo_sflp_batch_get(stmdev_ctx_t *ctx, - lsm6dsv16x_fifo_sflp_raw_t *val); +} lsm6dsv_fifo_sflp_raw_t; +int32_t lsm6dsv_fifo_sflp_batch_set(stmdev_ctx_t *ctx, + lsm6dsv_fifo_sflp_raw_t val); +int32_t lsm6dsv_fifo_sflp_batch_get(stmdev_ctx_t *ctx, + lsm6dsv_fifo_sflp_raw_t *val); typedef enum { - LSM6DSV16X_AUTO = 0x0, - LSM6DSV16X_ALWAYS_ACTIVE = 0x1, -} lsm6dsv16x_filt_anti_spike_t; -int32_t lsm6dsv16x_filt_anti_spike_set(stmdev_ctx_t *ctx, - lsm6dsv16x_filt_anti_spike_t val); -int32_t lsm6dsv16x_filt_anti_spike_get(stmdev_ctx_t *ctx, - lsm6dsv16x_filt_anti_spike_t *val); + LSM6DSV_AUTO = 0x0, + LSM6DSV_ALWAYS_ACTIVE = 0x1, +} lsm6dsv_filt_anti_spike_t; +int32_t lsm6dsv_filt_anti_spike_set(stmdev_ctx_t *ctx, + lsm6dsv_filt_anti_spike_t val); +int32_t lsm6dsv_filt_anti_spike_get(stmdev_ctx_t *ctx, + lsm6dsv_filt_anti_spike_t *val); typedef struct { @@ -4516,147 +4293,147 @@ typedef struct uint8_t ois_drdy : 1; uint8_t irq_xl : 1; uint8_t irq_g : 1; -} lsm6dsv16x_filt_settling_mask_t; -int32_t lsm6dsv16x_filt_settling_mask_set(stmdev_ctx_t *ctx, - lsm6dsv16x_filt_settling_mask_t val); -int32_t lsm6dsv16x_filt_settling_mask_get(stmdev_ctx_t *ctx, - lsm6dsv16x_filt_settling_mask_t *val); +} lsm6dsv_filt_settling_mask_t; +int32_t lsm6dsv_filt_settling_mask_set(stmdev_ctx_t *ctx, + lsm6dsv_filt_settling_mask_t val); +int32_t lsm6dsv_filt_settling_mask_get(stmdev_ctx_t *ctx, + lsm6dsv_filt_settling_mask_t *val); typedef struct { uint8_t ois_drdy : 1; -} lsm6dsv16x_filt_ois_settling_mask_t; -int32_t lsm6dsv16x_filt_ois_settling_mask_set(stmdev_ctx_t *ctx, - lsm6dsv16x_filt_ois_settling_mask_t val); -int32_t lsm6dsv16x_filt_ois_settling_mask_get(stmdev_ctx_t *ctx, - lsm6dsv16x_filt_ois_settling_mask_t *val); +} lsm6dsv_filt_ois_settling_mask_t; +int32_t lsm6dsv_filt_ois_settling_mask_set(stmdev_ctx_t *ctx, + lsm6dsv_filt_ois_settling_mask_t val); +int32_t lsm6dsv_filt_ois_settling_mask_get(stmdev_ctx_t *ctx, + lsm6dsv_filt_ois_settling_mask_t *val); typedef enum { - LSM6DSV16X_GY_ULTRA_LIGHT = 0x0, - LSM6DSV16X_GY_VERY_LIGHT = 0x1, - LSM6DSV16X_GY_LIGHT = 0x2, - LSM6DSV16X_GY_MEDIUM = 0x3, - LSM6DSV16X_GY_STRONG = 0x4, - LSM6DSV16X_GY_VERY_STRONG = 0x5, - LSM6DSV16X_GY_AGGRESSIVE = 0x6, - LSM6DSV16X_GY_XTREME = 0x7, -} lsm6dsv16x_filt_gy_lp1_bandwidth_t; -int32_t lsm6dsv16x_filt_gy_lp1_bandwidth_set(stmdev_ctx_t *ctx, - lsm6dsv16x_filt_gy_lp1_bandwidth_t val); -int32_t lsm6dsv16x_filt_gy_lp1_bandwidth_get(stmdev_ctx_t *ctx, - lsm6dsv16x_filt_gy_lp1_bandwidth_t *val); - -int32_t lsm6dsv16x_filt_gy_lp1_set(stmdev_ctx_t *ctx, uint8_t val); -int32_t lsm6dsv16x_filt_gy_lp1_get(stmdev_ctx_t *ctx, uint8_t *val); + LSM6DSV_GY_ULTRA_LIGHT = 0x0, + LSM6DSV_GY_VERY_LIGHT = 0x1, + LSM6DSV_GY_LIGHT = 0x2, + LSM6DSV_GY_MEDIUM = 0x3, + LSM6DSV_GY_STRONG = 0x4, + LSM6DSV_GY_VERY_STRONG = 0x5, + LSM6DSV_GY_AGGRESSIVE = 0x6, + LSM6DSV_GY_XTREME = 0x7, +} lsm6dsv_filt_gy_lp1_bandwidth_t; +int32_t lsm6dsv_filt_gy_lp1_bandwidth_set(stmdev_ctx_t *ctx, + lsm6dsv_filt_gy_lp1_bandwidth_t val); +int32_t lsm6dsv_filt_gy_lp1_bandwidth_get(stmdev_ctx_t *ctx, + lsm6dsv_filt_gy_lp1_bandwidth_t *val); + +int32_t lsm6dsv_filt_gy_lp1_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6dsv_filt_gy_lp1_get(stmdev_ctx_t *ctx, uint8_t *val); typedef enum { - LSM6DSV16X_XL_ULTRA_LIGHT = 0x0, - LSM6DSV16X_XL_VERY_LIGHT = 0x1, - LSM6DSV16X_XL_LIGHT = 0x2, - LSM6DSV16X_XL_MEDIUM = 0x3, - LSM6DSV16X_XL_STRONG = 0x4, - LSM6DSV16X_XL_VERY_STRONG = 0x5, - LSM6DSV16X_XL_AGGRESSIVE = 0x6, - LSM6DSV16X_XL_XTREME = 0x7, -} lsm6dsv16x_filt_xl_lp2_bandwidth_t; -int32_t lsm6dsv16x_filt_xl_lp2_bandwidth_set(stmdev_ctx_t *ctx, - lsm6dsv16x_filt_xl_lp2_bandwidth_t val); -int32_t lsm6dsv16x_filt_xl_lp2_bandwidth_get(stmdev_ctx_t *ctx, - lsm6dsv16x_filt_xl_lp2_bandwidth_t *val); - -int32_t lsm6dsv16x_filt_xl_lp2_set(stmdev_ctx_t *ctx, uint8_t val); -int32_t lsm6dsv16x_filt_xl_lp2_get(stmdev_ctx_t *ctx, uint8_t *val); - -int32_t lsm6dsv16x_filt_xl_hp_set(stmdev_ctx_t *ctx, uint8_t val); -int32_t lsm6dsv16x_filt_xl_hp_get(stmdev_ctx_t *ctx, uint8_t *val); - -int32_t lsm6dsv16x_filt_xl_fast_settling_set(stmdev_ctx_t *ctx, uint8_t val); -int32_t lsm6dsv16x_filt_xl_fast_settling_get(stmdev_ctx_t *ctx, uint8_t *val); + LSM6DSV_XL_ULTRA_LIGHT = 0x0, + LSM6DSV_XL_VERY_LIGHT = 0x1, + LSM6DSV_XL_LIGHT = 0x2, + LSM6DSV_XL_MEDIUM = 0x3, + LSM6DSV_XL_STRONG = 0x4, + LSM6DSV_XL_VERY_STRONG = 0x5, + LSM6DSV_XL_AGGRESSIVE = 0x6, + LSM6DSV_XL_XTREME = 0x7, +} lsm6dsv_filt_xl_lp2_bandwidth_t; +int32_t lsm6dsv_filt_xl_lp2_bandwidth_set(stmdev_ctx_t *ctx, + lsm6dsv_filt_xl_lp2_bandwidth_t val); +int32_t lsm6dsv_filt_xl_lp2_bandwidth_get(stmdev_ctx_t *ctx, + lsm6dsv_filt_xl_lp2_bandwidth_t *val); + +int32_t lsm6dsv_filt_xl_lp2_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6dsv_filt_xl_lp2_get(stmdev_ctx_t *ctx, uint8_t *val); + +int32_t lsm6dsv_filt_xl_hp_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6dsv_filt_xl_hp_get(stmdev_ctx_t *ctx, uint8_t *val); + +int32_t lsm6dsv_filt_xl_fast_settling_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6dsv_filt_xl_fast_settling_get(stmdev_ctx_t *ctx, uint8_t *val); typedef enum { - LSM6DSV16X_HP_MD_NORMAL = 0x0, - LSM6DSV16X_HP_MD_REFERENCE = 0x1, -} lsm6dsv16x_filt_xl_hp_mode_t; -int32_t lsm6dsv16x_filt_xl_hp_mode_set(stmdev_ctx_t *ctx, - lsm6dsv16x_filt_xl_hp_mode_t val); -int32_t lsm6dsv16x_filt_xl_hp_mode_get(stmdev_ctx_t *ctx, - lsm6dsv16x_filt_xl_hp_mode_t *val); + LSM6DSV_HP_MD_NORMAL = 0x0, + LSM6DSV_HP_MD_REFERENCE = 0x1, +} lsm6dsv_filt_xl_hp_mode_t; +int32_t lsm6dsv_filt_xl_hp_mode_set(stmdev_ctx_t *ctx, + lsm6dsv_filt_xl_hp_mode_t val); +int32_t lsm6dsv_filt_xl_hp_mode_get(stmdev_ctx_t *ctx, + lsm6dsv_filt_xl_hp_mode_t *val); typedef enum { - LSM6DSV16X_WK_FEED_SLOPE = 0x0, - LSM6DSV16X_WK_FEED_HIGH_PASS = 0x1, - LSM6DSV16X_WK_FEED_LP_WITH_OFFSET = 0x2, -} lsm6dsv16x_filt_wkup_act_feed_t; -int32_t lsm6dsv16x_filt_wkup_act_feed_set(stmdev_ctx_t *ctx, - lsm6dsv16x_filt_wkup_act_feed_t val); -int32_t lsm6dsv16x_filt_wkup_act_feed_get(stmdev_ctx_t *ctx, - lsm6dsv16x_filt_wkup_act_feed_t *val); + LSM6DSV_WK_FEED_SLOPE = 0x0, + LSM6DSV_WK_FEED_HIGH_PASS = 0x1, + LSM6DSV_WK_FEED_LP_WITH_OFFSET = 0x2, +} lsm6dsv_filt_wkup_act_feed_t; +int32_t lsm6dsv_filt_wkup_act_feed_set(stmdev_ctx_t *ctx, + lsm6dsv_filt_wkup_act_feed_t val); +int32_t lsm6dsv_filt_wkup_act_feed_get(stmdev_ctx_t *ctx, + lsm6dsv_filt_wkup_act_feed_t *val); -int32_t lsm6dsv16x_mask_trigger_xl_settl_set(stmdev_ctx_t *ctx, uint8_t val); -int32_t lsm6dsv16x_mask_trigger_xl_settl_get(stmdev_ctx_t *ctx, uint8_t *val); +int32_t lsm6dsv_mask_trigger_xl_settl_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6dsv_mask_trigger_xl_settl_get(stmdev_ctx_t *ctx, uint8_t *val); typedef enum { - LSM6DSV16X_SIXD_FEED_ODR_DIV_2 = 0x0, - LSM6DSV16X_SIXD_FEED_LOW_PASS = 0x1, -} lsm6dsv16x_filt_sixd_feed_t; -int32_t lsm6dsv16x_filt_sixd_feed_set(stmdev_ctx_t *ctx, - lsm6dsv16x_filt_sixd_feed_t val); -int32_t lsm6dsv16x_filt_sixd_feed_get(stmdev_ctx_t *ctx, - lsm6dsv16x_filt_sixd_feed_t *val); + LSM6DSV_SIXD_FEED_ODR_DIV_2 = 0x0, + LSM6DSV_SIXD_FEED_LOW_PASS = 0x1, +} lsm6dsv_filt_sixd_feed_t; +int32_t lsm6dsv_filt_sixd_feed_set(stmdev_ctx_t *ctx, + lsm6dsv_filt_sixd_feed_t val); +int32_t lsm6dsv_filt_sixd_feed_get(stmdev_ctx_t *ctx, + lsm6dsv_filt_sixd_feed_t *val); typedef enum { - LSM6DSV16X_EIS_LP_NORMAL = 0x0, - LSM6DSV16X_EIS_LP_LIGHT = 0x1, -} lsm6dsv16x_filt_gy_eis_lp_bandwidth_t; -int32_t lsm6dsv16x_filt_gy_eis_lp_bandwidth_set(stmdev_ctx_t *ctx, - lsm6dsv16x_filt_gy_eis_lp_bandwidth_t val); -int32_t lsm6dsv16x_filt_gy_eis_lp_bandwidth_get(stmdev_ctx_t *ctx, - lsm6dsv16x_filt_gy_eis_lp_bandwidth_t *val); + LSM6DSV_EIS_LP_NORMAL = 0x0, + LSM6DSV_EIS_LP_LIGHT = 0x1, +} lsm6dsv_filt_gy_eis_lp_bandwidth_t; +int32_t lsm6dsv_filt_gy_eis_lp_bandwidth_set(stmdev_ctx_t *ctx, + lsm6dsv_filt_gy_eis_lp_bandwidth_t val); +int32_t lsm6dsv_filt_gy_eis_lp_bandwidth_get(stmdev_ctx_t *ctx, + lsm6dsv_filt_gy_eis_lp_bandwidth_t *val); typedef enum { - LSM6DSV16X_OIS_GY_LP_NORMAL = 0x0, - LSM6DSV16X_OIS_GY_LP_STRONG = 0x1, - LSM6DSV16X_OIS_GY_LP_AGGRESSIVE = 0x2, - LSM6DSV16X_OIS_GY_LP_LIGHT = 0x3, -} lsm6dsv16x_filt_gy_ois_lp_bandwidth_t; -int32_t lsm6dsv16x_filt_gy_ois_lp_bandwidth_set(stmdev_ctx_t *ctx, - lsm6dsv16x_filt_gy_ois_lp_bandwidth_t val); -int32_t lsm6dsv16x_filt_gy_ois_lp_bandwidth_get(stmdev_ctx_t *ctx, - lsm6dsv16x_filt_gy_ois_lp_bandwidth_t *val); + LSM6DSV_OIS_GY_LP_NORMAL = 0x0, + LSM6DSV_OIS_GY_LP_STRONG = 0x1, + LSM6DSV_OIS_GY_LP_AGGRESSIVE = 0x2, + LSM6DSV_OIS_GY_LP_LIGHT = 0x3, +} lsm6dsv_filt_gy_ois_lp_bandwidth_t; +int32_t lsm6dsv_filt_gy_ois_lp_bandwidth_set(stmdev_ctx_t *ctx, + lsm6dsv_filt_gy_ois_lp_bandwidth_t val); +int32_t lsm6dsv_filt_gy_ois_lp_bandwidth_get(stmdev_ctx_t *ctx, + lsm6dsv_filt_gy_ois_lp_bandwidth_t *val); typedef enum { - LSM6DSV16X_OIS_XL_LP_ULTRA_LIGHT = 0x0, - LSM6DSV16X_OIS_XL_LP_VERY_LIGHT = 0x1, - LSM6DSV16X_OIS_XL_LP_LIGHT = 0x2, - LSM6DSV16X_OIS_XL_LP_NORMAL = 0x3, - LSM6DSV16X_OIS_XL_LP_STRONG = 0x4, - LSM6DSV16X_OIS_XL_LP_VERY_STRONG = 0x5, - LSM6DSV16X_OIS_XL_LP_AGGRESSIVE = 0x6, - LSM6DSV16X_OIS_XL_LP_XTREME = 0x7, -} lsm6dsv16x_filt_xl_ois_lp_bandwidth_t; -int32_t lsm6dsv16x_filt_xl_ois_lp_bandwidth_set(stmdev_ctx_t *ctx, - lsm6dsv16x_filt_xl_ois_lp_bandwidth_t val); -int32_t lsm6dsv16x_filt_xl_ois_lp_bandwidth_get(stmdev_ctx_t *ctx, - lsm6dsv16x_filt_xl_ois_lp_bandwidth_t *val); + LSM6DSV_OIS_XL_LP_ULTRA_LIGHT = 0x0, + LSM6DSV_OIS_XL_LP_VERY_LIGHT = 0x1, + LSM6DSV_OIS_XL_LP_LIGHT = 0x2, + LSM6DSV_OIS_XL_LP_NORMAL = 0x3, + LSM6DSV_OIS_XL_LP_STRONG = 0x4, + LSM6DSV_OIS_XL_LP_VERY_STRONG = 0x5, + LSM6DSV_OIS_XL_LP_AGGRESSIVE = 0x6, + LSM6DSV_OIS_XL_LP_XTREME = 0x7, +} lsm6dsv_filt_xl_ois_lp_bandwidth_t; +int32_t lsm6dsv_filt_xl_ois_lp_bandwidth_set(stmdev_ctx_t *ctx, + lsm6dsv_filt_xl_ois_lp_bandwidth_t val); +int32_t lsm6dsv_filt_xl_ois_lp_bandwidth_get(stmdev_ctx_t *ctx, + lsm6dsv_filt_xl_ois_lp_bandwidth_t *val); typedef enum { - LSM6DSV16X_PROTECT_CTRL_REGS = 0x0, - LSM6DSV16X_WRITE_CTRL_REG = 0x1, -} lsm6dsv16x_fsm_permission_t; -int32_t lsm6dsv16x_fsm_permission_set(stmdev_ctx_t *ctx, - lsm6dsv16x_fsm_permission_t val); -int32_t lsm6dsv16x_fsm_permission_get(stmdev_ctx_t *ctx, - lsm6dsv16x_fsm_permission_t *val); -int32_t lsm6dsv16x_fsm_permission_status(stmdev_ctx_t *ctx, uint8_t *val); + LSM6DSV_PROTECT_CTRL_REGS = 0x0, + LSM6DSV_WRITE_CTRL_REG = 0x1, +} lsm6dsv_fsm_permission_t; +int32_t lsm6dsv_fsm_permission_set(stmdev_ctx_t *ctx, + lsm6dsv_fsm_permission_t val); +int32_t lsm6dsv_fsm_permission_get(stmdev_ctx_t *ctx, + lsm6dsv_fsm_permission_t *val); +int32_t lsm6dsv_fsm_permission_status(stmdev_ctx_t *ctx, uint8_t *val); typedef struct { @@ -4668,12 +4445,12 @@ typedef struct uint8_t fsm6_en : 1; uint8_t fsm7_en : 1; uint8_t fsm8_en : 1; -} lsm6dsv16x_fsm_mode_t; -int32_t lsm6dsv16x_fsm_mode_set(stmdev_ctx_t *ctx, lsm6dsv16x_fsm_mode_t val); -int32_t lsm6dsv16x_fsm_mode_get(stmdev_ctx_t *ctx, lsm6dsv16x_fsm_mode_t *val); +} lsm6dsv_fsm_mode_t; +int32_t lsm6dsv_fsm_mode_set(stmdev_ctx_t *ctx, lsm6dsv_fsm_mode_t val); +int32_t lsm6dsv_fsm_mode_get(stmdev_ctx_t *ctx, lsm6dsv_fsm_mode_t *val); -int32_t lsm6dsv16x_fsm_long_cnt_set(stmdev_ctx_t *ctx, uint16_t val); -int32_t lsm6dsv16x_fsm_long_cnt_get(stmdev_ctx_t *ctx, uint16_t *val); +int32_t lsm6dsv_fsm_long_cnt_set(stmdev_ctx_t *ctx, uint16_t val); +int32_t lsm6dsv_fsm_long_cnt_get(stmdev_ctx_t *ctx, uint16_t *val); typedef struct @@ -4686,39 +4463,39 @@ typedef struct uint8_t fsm_outs6; uint8_t fsm_outs7; uint8_t fsm_outs8; -} lsm6dsv16x_fsm_out_t; -int32_t lsm6dsv16x_fsm_out_get(stmdev_ctx_t *ctx, lsm6dsv16x_fsm_out_t *val); +} lsm6dsv_fsm_out_t; +int32_t lsm6dsv_fsm_out_get(stmdev_ctx_t *ctx, lsm6dsv_fsm_out_t *val); typedef enum { - LSM6DSV16X_FSM_15Hz = 0x0, - LSM6DSV16X_FSM_30Hz = 0x1, - LSM6DSV16X_FSM_60Hz = 0x2, - LSM6DSV16X_FSM_120Hz = 0x3, - LSM6DSV16X_FSM_240Hz = 0x4, - LSM6DSV16X_FSM_480Hz = 0x5, - LSM6DSV16X_FSM_960Hz = 0x6, -} lsm6dsv16x_fsm_data_rate_t; -int32_t lsm6dsv16x_fsm_data_rate_set(stmdev_ctx_t *ctx, - lsm6dsv16x_fsm_data_rate_t val); -int32_t lsm6dsv16x_fsm_data_rate_get(stmdev_ctx_t *ctx, - lsm6dsv16x_fsm_data_rate_t *val); + LSM6DSV_FSM_15Hz = 0x0, + LSM6DSV_FSM_30Hz = 0x1, + LSM6DSV_FSM_60Hz = 0x2, + LSM6DSV_FSM_120Hz = 0x3, + LSM6DSV_FSM_240Hz = 0x4, + LSM6DSV_FSM_480Hz = 0x5, + LSM6DSV_FSM_960Hz = 0x6, +} lsm6dsv_fsm_data_rate_t; +int32_t lsm6dsv_fsm_data_rate_set(stmdev_ctx_t *ctx, + lsm6dsv_fsm_data_rate_t val); +int32_t lsm6dsv_fsm_data_rate_get(stmdev_ctx_t *ctx, + lsm6dsv_fsm_data_rate_t *val); -int32_t lsm6dsv16x_fsm_ext_sens_sensitivity_set(stmdev_ctx_t *ctx, - uint16_t val); -int32_t lsm6dsv16x_fsm_ext_sens_sensitivity_get(stmdev_ctx_t *ctx, - uint16_t *val); +int32_t lsm6dsv_fsm_ext_sens_sensitivity_set(stmdev_ctx_t *ctx, + uint16_t val); +int32_t lsm6dsv_fsm_ext_sens_sensitivity_get(stmdev_ctx_t *ctx, + uint16_t *val); typedef struct { uint16_t z; uint16_t y; uint16_t x; -} lsm6dsv16x_xl_fsm_ext_sens_offset_t; -int32_t lsm6dsv16x_fsm_ext_sens_offset_set(stmdev_ctx_t *ctx, - lsm6dsv16x_xl_fsm_ext_sens_offset_t val); -int32_t lsm6dsv16x_fsm_ext_sens_offset_get(stmdev_ctx_t *ctx, - lsm6dsv16x_xl_fsm_ext_sens_offset_t *val); +} lsm6dsv_xl_fsm_ext_sens_offset_t; +int32_t lsm6dsv_fsm_ext_sens_offset_set(stmdev_ctx_t *ctx, + lsm6dsv_xl_fsm_ext_sens_offset_t val); +int32_t lsm6dsv_fsm_ext_sens_offset_get(stmdev_ctx_t *ctx, + lsm6dsv_xl_fsm_ext_sens_offset_t *val); typedef struct { @@ -4728,532 +4505,472 @@ typedef struct uint16_t yy; uint16_t yz; uint16_t zz; -} lsm6dsv16x_xl_fsm_ext_sens_matrix_t; -int32_t lsm6dsv16x_fsm_ext_sens_matrix_set(stmdev_ctx_t *ctx, - lsm6dsv16x_xl_fsm_ext_sens_matrix_t val); -int32_t lsm6dsv16x_fsm_ext_sens_matrix_get(stmdev_ctx_t *ctx, - lsm6dsv16x_xl_fsm_ext_sens_matrix_t *val); +} lsm6dsv_xl_fsm_ext_sens_matrix_t; +int32_t lsm6dsv_fsm_ext_sens_matrix_set(stmdev_ctx_t *ctx, + lsm6dsv_xl_fsm_ext_sens_matrix_t val); +int32_t lsm6dsv_fsm_ext_sens_matrix_get(stmdev_ctx_t *ctx, + lsm6dsv_xl_fsm_ext_sens_matrix_t *val); typedef enum { - LSM6DSV16X_Z_EQ_Y = 0x0, - LSM6DSV16X_Z_EQ_MIN_Y = 0x1, - LSM6DSV16X_Z_EQ_X = 0x2, - LSM6DSV16X_Z_EQ_MIN_X = 0x3, - LSM6DSV16X_Z_EQ_MIN_Z = 0x4, - LSM6DSV16X_Z_EQ_Z = 0x5, -} lsm6dsv16x_fsm_ext_sens_z_orient_t; -int32_t lsm6dsv16x_fsm_ext_sens_z_orient_set(stmdev_ctx_t *ctx, - lsm6dsv16x_fsm_ext_sens_z_orient_t val); -int32_t lsm6dsv16x_fsm_ext_sens_z_orient_get(stmdev_ctx_t *ctx, - lsm6dsv16x_fsm_ext_sens_z_orient_t *val); + LSM6DSV_Z_EQ_Y = 0x0, + LSM6DSV_Z_EQ_MIN_Y = 0x1, + LSM6DSV_Z_EQ_X = 0x2, + LSM6DSV_Z_EQ_MIN_X = 0x3, + LSM6DSV_Z_EQ_MIN_Z = 0x4, + LSM6DSV_Z_EQ_Z = 0x5, +} lsm6dsv_fsm_ext_sens_z_orient_t; +int32_t lsm6dsv_fsm_ext_sens_z_orient_set(stmdev_ctx_t *ctx, + lsm6dsv_fsm_ext_sens_z_orient_t val); +int32_t lsm6dsv_fsm_ext_sens_z_orient_get(stmdev_ctx_t *ctx, + lsm6dsv_fsm_ext_sens_z_orient_t *val); typedef enum { - LSM6DSV16X_Y_EQ_Y = 0x0, - LSM6DSV16X_Y_EQ_MIN_Y = 0x1, - LSM6DSV16X_Y_EQ_X = 0x2, - LSM6DSV16X_Y_EQ_MIN_X = 0x3, - LSM6DSV16X_Y_EQ_MIN_Z = 0x4, - LSM6DSV16X_Y_EQ_Z = 0x5, -} lsm6dsv16x_fsm_ext_sens_y_orient_t; -int32_t lsm6dsv16x_fsm_ext_sens_y_orient_set(stmdev_ctx_t *ctx, - lsm6dsv16x_fsm_ext_sens_y_orient_t val); -int32_t lsm6dsv16x_fsm_ext_sens_y_orient_get(stmdev_ctx_t *ctx, - lsm6dsv16x_fsm_ext_sens_y_orient_t *val); + LSM6DSV_Y_EQ_Y = 0x0, + LSM6DSV_Y_EQ_MIN_Y = 0x1, + LSM6DSV_Y_EQ_X = 0x2, + LSM6DSV_Y_EQ_MIN_X = 0x3, + LSM6DSV_Y_EQ_MIN_Z = 0x4, + LSM6DSV_Y_EQ_Z = 0x5, +} lsm6dsv_fsm_ext_sens_y_orient_t; +int32_t lsm6dsv_fsm_ext_sens_y_orient_set(stmdev_ctx_t *ctx, + lsm6dsv_fsm_ext_sens_y_orient_t val); +int32_t lsm6dsv_fsm_ext_sens_y_orient_get(stmdev_ctx_t *ctx, + lsm6dsv_fsm_ext_sens_y_orient_t *val); typedef enum { - LSM6DSV16X_X_EQ_Y = 0x0, - LSM6DSV16X_X_EQ_MIN_Y = 0x1, - LSM6DSV16X_X_EQ_X = 0x2, - LSM6DSV16X_X_EQ_MIN_X = 0x3, - LSM6DSV16X_X_EQ_MIN_Z = 0x4, - LSM6DSV16X_X_EQ_Z = 0x5, -} lsm6dsv16x_fsm_ext_sens_x_orient_t; -int32_t lsm6dsv16x_fsm_ext_sens_x_orient_set(stmdev_ctx_t *ctx, - lsm6dsv16x_fsm_ext_sens_x_orient_t val); -int32_t lsm6dsv16x_fsm_ext_sens_x_orient_get(stmdev_ctx_t *ctx, - lsm6dsv16x_fsm_ext_sens_x_orient_t *val); - -int32_t lsm6dsv16x_fsm_long_cnt_timeout_set(stmdev_ctx_t *ctx, uint16_t val); -int32_t lsm6dsv16x_fsm_long_cnt_timeout_get(stmdev_ctx_t *ctx, uint16_t *val); + LSM6DSV_X_EQ_Y = 0x0, + LSM6DSV_X_EQ_MIN_Y = 0x1, + LSM6DSV_X_EQ_X = 0x2, + LSM6DSV_X_EQ_MIN_X = 0x3, + LSM6DSV_X_EQ_MIN_Z = 0x4, + LSM6DSV_X_EQ_Z = 0x5, +} lsm6dsv_fsm_ext_sens_x_orient_t; +int32_t lsm6dsv_fsm_ext_sens_x_orient_set(stmdev_ctx_t *ctx, + lsm6dsv_fsm_ext_sens_x_orient_t val); +int32_t lsm6dsv_fsm_ext_sens_x_orient_get(stmdev_ctx_t *ctx, + lsm6dsv_fsm_ext_sens_x_orient_t *val); -int32_t lsm6dsv16x_fsm_number_of_programs_set(stmdev_ctx_t *ctx, uint8_t val); -int32_t lsm6dsv16x_fsm_number_of_programs_get(stmdev_ctx_t *ctx, uint8_t *val); +int32_t lsm6dsv_fsm_long_cnt_timeout_set(stmdev_ctx_t *ctx, uint16_t val); +int32_t lsm6dsv_fsm_long_cnt_timeout_get(stmdev_ctx_t *ctx, uint16_t *val); -int32_t lsm6dsv16x_fsm_start_address_set(stmdev_ctx_t *ctx, uint16_t val); -int32_t lsm6dsv16x_fsm_start_address_get(stmdev_ctx_t *ctx, uint16_t *val); +int32_t lsm6dsv_fsm_number_of_programs_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6dsv_fsm_number_of_programs_get(stmdev_ctx_t *ctx, uint8_t *val); -int32_t lsm6dsv16x_ff_time_windows_set(stmdev_ctx_t *ctx, uint8_t val); -int32_t lsm6dsv16x_ff_time_windows_get(stmdev_ctx_t *ctx, uint8_t *val); - -typedef enum -{ - LSM6DSV16X_156_mg = 0x0, - LSM6DSV16X_219_mg = 0x1, - LSM6DSV16X_250_mg = 0x2, - LSM6DSV16X_312_mg = 0x3, - LSM6DSV16X_344_mg = 0x4, - LSM6DSV16X_406_mg = 0x5, - LSM6DSV16X_469_mg = 0x6, - LSM6DSV16X_500_mg = 0x7, -} lsm6dsv16x_ff_thresholds_t; -int32_t lsm6dsv16x_ff_thresholds_set(stmdev_ctx_t *ctx, - lsm6dsv16x_ff_thresholds_t val); -int32_t lsm6dsv16x_ff_thresholds_get(stmdev_ctx_t *ctx, - lsm6dsv16x_ff_thresholds_t *val); +int32_t lsm6dsv_fsm_start_address_set(stmdev_ctx_t *ctx, uint16_t val); +int32_t lsm6dsv_fsm_start_address_get(stmdev_ctx_t *ctx, uint16_t *val); -typedef enum -{ - LSM6DSV16X_MLC_OFF = 0x0, - LSM6DSV16X_MLC_ON = 0x1, - LSM6DSV16X_MLC_ON_BEFORE_FSM = 0x2, -} lsm6dsv16x_mlc_mode_t; -int32_t lsm6dsv16x_mlc_set(stmdev_ctx_t *ctx, lsm6dsv16x_mlc_mode_t val); -int32_t lsm6dsv16x_mlc_get(stmdev_ctx_t *ctx, lsm6dsv16x_mlc_mode_t *val); +int32_t lsm6dsv_ff_time_windows_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6dsv_ff_time_windows_get(stmdev_ctx_t *ctx, uint8_t *val); typedef enum { - LSM6DSV16X_MLC_15Hz = 0x0, - LSM6DSV16X_MLC_30Hz = 0x1, - LSM6DSV16X_MLC_60Hz = 0x2, - LSM6DSV16X_MLC_120Hz = 0x3, - LSM6DSV16X_MLC_240Hz = 0x4, - LSM6DSV16X_MLC_480Hz = 0x5, - LSM6DSV16X_MLC_960Hz = 0x6, -} lsm6dsv16x_mlc_data_rate_t; -int32_t lsm6dsv16x_mlc_data_rate_set(stmdev_ctx_t *ctx, - lsm6dsv16x_mlc_data_rate_t val); -int32_t lsm6dsv16x_mlc_data_rate_get(stmdev_ctx_t *ctx, - lsm6dsv16x_mlc_data_rate_t *val); - -typedef struct -{ - uint8_t mlc1_src; - uint8_t mlc2_src; - uint8_t mlc3_src; - uint8_t mlc4_src; -} lsm6dsv16x_mlc_out_t; -int32_t lsm6dsv16x_mlc_out_get(stmdev_ctx_t *ctx, lsm6dsv16x_mlc_out_t *val); - -int32_t lsm6dsv16x_mlc_ext_sens_sensitivity_set(stmdev_ctx_t *ctx, - uint16_t val); -int32_t lsm6dsv16x_mlc_ext_sens_sensitivity_get(stmdev_ctx_t *ctx, - uint16_t *val); + LSM6DSV_156_mg = 0x0, + LSM6DSV_219_mg = 0x1, + LSM6DSV_250_mg = 0x2, + LSM6DSV_312_mg = 0x3, + LSM6DSV_344_mg = 0x4, + LSM6DSV_406_mg = 0x5, + LSM6DSV_469_mg = 0x6, + LSM6DSV_500_mg = 0x7, +} lsm6dsv_ff_thresholds_t; +int32_t lsm6dsv_ff_thresholds_set(stmdev_ctx_t *ctx, + lsm6dsv_ff_thresholds_t val); +int32_t lsm6dsv_ff_thresholds_get(stmdev_ctx_t *ctx, + lsm6dsv_ff_thresholds_t *val); typedef enum { - LSM6DSV16X_OIS_CTRL_FROM_OIS = 0x0, - LSM6DSV16X_OIS_CTRL_FROM_UI = 0x1, -} lsm6dsv16x_ois_ctrl_mode_t; -int32_t lsm6dsv16x_ois_ctrl_mode_set(stmdev_ctx_t *ctx, - lsm6dsv16x_ois_ctrl_mode_t val); -int32_t lsm6dsv16x_ois_ctrl_mode_get(stmdev_ctx_t *ctx, - lsm6dsv16x_ois_ctrl_mode_t *val); + LSM6DSV_OIS_CTRL_FROM_OIS = 0x0, + LSM6DSV_OIS_CTRL_FROM_UI = 0x1, +} lsm6dsv_ois_ctrl_mode_t; +int32_t lsm6dsv_ois_ctrl_mode_set(stmdev_ctx_t *ctx, + lsm6dsv_ois_ctrl_mode_t val); +int32_t lsm6dsv_ois_ctrl_mode_get(stmdev_ctx_t *ctx, + lsm6dsv_ois_ctrl_mode_t *val); -int32_t lsm6dsv16x_ois_reset_set(stmdev_ctx_t *ctx, int8_t val); -int32_t lsm6dsv16x_ois_reset_get(stmdev_ctx_t *ctx, int8_t *val); +int32_t lsm6dsv_ois_reset_set(stmdev_ctx_t *ctx, int8_t val); +int32_t lsm6dsv_ois_reset_get(stmdev_ctx_t *ctx, int8_t *val); -int32_t lsm6dsv16x_ois_interface_pull_up_set(stmdev_ctx_t *ctx, uint8_t val); -int32_t lsm6dsv16x_ois_interface_pull_up_get(stmdev_ctx_t *ctx, uint8_t *val); +int32_t lsm6dsv_ois_interface_pull_up_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6dsv_ois_interface_pull_up_get(stmdev_ctx_t *ctx, uint8_t *val); typedef struct { uint8_t ack : 1; uint8_t req : 1; -} lsm6dsv16x_ois_handshake_t; -int32_t lsm6dsv16x_ois_handshake_from_ui_set(stmdev_ctx_t *ctx, - lsm6dsv16x_ois_handshake_t val); -int32_t lsm6dsv16x_ois_handshake_from_ui_get(stmdev_ctx_t *ctx, - lsm6dsv16x_ois_handshake_t *val); -int32_t lsm6dsv16x_ois_handshake_from_ois_set(stmdev_ctx_t *ctx, - lsm6dsv16x_ois_handshake_t val); -int32_t lsm6dsv16x_ois_handshake_from_ois_get(stmdev_ctx_t *ctx, - lsm6dsv16x_ois_handshake_t *val); +} lsm6dsv_ois_handshake_t; +int32_t lsm6dsv_ois_handshake_from_ui_set(stmdev_ctx_t *ctx, + lsm6dsv_ois_handshake_t val); +int32_t lsm6dsv_ois_handshake_from_ui_get(stmdev_ctx_t *ctx, + lsm6dsv_ois_handshake_t *val); +int32_t lsm6dsv_ois_handshake_from_ois_set(stmdev_ctx_t *ctx, + lsm6dsv_ois_handshake_t val); +int32_t lsm6dsv_ois_handshake_from_ois_get(stmdev_ctx_t *ctx, + lsm6dsv_ois_handshake_t *val); -int32_t lsm6dsv16x_ois_shared_set(stmdev_ctx_t *ctx, uint8_t val[6]); -int32_t lsm6dsv16x_ois_shared_get(stmdev_ctx_t *ctx, uint8_t val[6]); +int32_t lsm6dsv_ois_shared_set(stmdev_ctx_t *ctx, uint8_t val[6]); +int32_t lsm6dsv_ois_shared_get(stmdev_ctx_t *ctx, uint8_t val[6]); -int32_t lsm6dsv16x_ois_on_spi2_set(stmdev_ctx_t *ctx, uint8_t val); -int32_t lsm6dsv16x_ois_on_spi2_get(stmdev_ctx_t *ctx, uint8_t *val); +int32_t lsm6dsv_ois_on_spi2_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6dsv_ois_on_spi2_get(stmdev_ctx_t *ctx, uint8_t *val); typedef struct { uint8_t gy : 1; uint8_t xl : 1; -} lsm6dsv16x_ois_chain_t; -int32_t lsm6dsv16x_ois_chain_set(stmdev_ctx_t *ctx, lsm6dsv16x_ois_chain_t val); -int32_t lsm6dsv16x_ois_chain_get(stmdev_ctx_t *ctx, - lsm6dsv16x_ois_chain_t *val); +} lsm6dsv_ois_chain_t; +int32_t lsm6dsv_ois_chain_set(stmdev_ctx_t *ctx, lsm6dsv_ois_chain_t val); +int32_t lsm6dsv_ois_chain_get(stmdev_ctx_t *ctx, + lsm6dsv_ois_chain_t *val); typedef enum { - LSM6DSV16X_OIS_125dps = 0x0, - LSM6DSV16X_OIS_250dps = 0x1, - LSM6DSV16X_OIS_500dps = 0x2, - LSM6DSV16X_OIS_1000dps = 0x3, - LSM6DSV16X_OIS_2000dps = 0x4, -} lsm6dsv16x_ois_gy_full_scale_t; -int32_t lsm6dsv16x_ois_gy_full_scale_set(stmdev_ctx_t *ctx, - lsm6dsv16x_ois_gy_full_scale_t val); -int32_t lsm6dsv16x_ois_gy_full_scale_get(stmdev_ctx_t *ctx, - lsm6dsv16x_ois_gy_full_scale_t *val); + LSM6DSV_OIS_125dps = 0x0, + LSM6DSV_OIS_250dps = 0x1, + LSM6DSV_OIS_500dps = 0x2, + LSM6DSV_OIS_1000dps = 0x3, + LSM6DSV_OIS_2000dps = 0x4, +} lsm6dsv_ois_gy_full_scale_t; +int32_t lsm6dsv_ois_gy_full_scale_set(stmdev_ctx_t *ctx, + lsm6dsv_ois_gy_full_scale_t val); +int32_t lsm6dsv_ois_gy_full_scale_get(stmdev_ctx_t *ctx, + lsm6dsv_ois_gy_full_scale_t *val); typedef enum { - LSM6DSV16X_OIS_2g = 0x0, - LSM6DSV16X_OIS_4g = 0x1, - LSM6DSV16X_OIS_8g = 0x2, - LSM6DSV16X_OIS_16g = 0x3, -} lsm6dsv16x_ois_xl_full_scale_t; -int32_t lsm6dsv16x_ois_xl_full_scale_set(stmdev_ctx_t *ctx, - lsm6dsv16x_ois_xl_full_scale_t val); -int32_t lsm6dsv16x_ois_xl_full_scale_get(stmdev_ctx_t *ctx, - lsm6dsv16x_ois_xl_full_scale_t *val); + LSM6DSV_OIS_2g = 0x0, + LSM6DSV_OIS_4g = 0x1, + LSM6DSV_OIS_8g = 0x2, + LSM6DSV_OIS_16g = 0x3, +} lsm6dsv_ois_xl_full_scale_t; +int32_t lsm6dsv_ois_xl_full_scale_set(stmdev_ctx_t *ctx, + lsm6dsv_ois_xl_full_scale_t val); +int32_t lsm6dsv_ois_xl_full_scale_get(stmdev_ctx_t *ctx, + lsm6dsv_ois_xl_full_scale_t *val); typedef enum { - LSM6DSV16X_DEG_80 = 0x0, - LSM6DSV16X_DEG_70 = 0x1, - LSM6DSV16X_DEG_60 = 0x2, - LSM6DSV16X_DEG_50 = 0x3, -} lsm6dsv16x_6d_threshold_t; -int32_t lsm6dsv16x_6d_threshold_set(stmdev_ctx_t *ctx, - lsm6dsv16x_6d_threshold_t val); -int32_t lsm6dsv16x_6d_threshold_get(stmdev_ctx_t *ctx, - lsm6dsv16x_6d_threshold_t *val); + LSM6DSV_DEG_80 = 0x0, + LSM6DSV_DEG_70 = 0x1, + LSM6DSV_DEG_60 = 0x2, + LSM6DSV_DEG_50 = 0x3, +} lsm6dsv_6d_threshold_t; +int32_t lsm6dsv_6d_threshold_set(stmdev_ctx_t *ctx, + lsm6dsv_6d_threshold_t val); +int32_t lsm6dsv_6d_threshold_get(stmdev_ctx_t *ctx, + lsm6dsv_6d_threshold_t *val); -int32_t lsm6dsv16x_4d_mode_set(stmdev_ctx_t *ctx, uint8_t val); -int32_t lsm6dsv16x_4d_mode_get(stmdev_ctx_t *ctx, uint8_t *val); - -typedef enum -{ - LSM6DSV16X_2400MOhm = 0x0, - LSM6DSV16X_730MOhm = 0x1, - LSM6DSV16X_300MOhm = 0x2, - LSM6DSV16X_255MOhm = 0x3, -} lsm6dsv16x_ah_qvar_zin_t; -int32_t lsm6dsv16x_ah_qvar_zin_set(stmdev_ctx_t *ctx, - lsm6dsv16x_ah_qvar_zin_t val); -int32_t lsm6dsv16x_ah_qvar_zin_get(stmdev_ctx_t *ctx, - lsm6dsv16x_ah_qvar_zin_t *val); - -typedef struct -{ - uint8_t ah_qvar_en : 1; -} lsm6dsv16x_ah_qvar_mode_t; -int32_t lsm6dsv16x_ah_qvar_mode_set(stmdev_ctx_t *ctx, - lsm6dsv16x_ah_qvar_mode_t val); -int32_t lsm6dsv16x_ah_qvar_mode_get(stmdev_ctx_t *ctx, - lsm6dsv16x_ah_qvar_mode_t *val); +int32_t lsm6dsv_4d_mode_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6dsv_4d_mode_get(stmdev_ctx_t *ctx, uint8_t *val); typedef enum { - LSM6DSV16X_SW_RST_DYN_ADDRESS_RST = 0x0, - LSM6DSV16X_I3C_GLOBAL_RST = 0x1, -} lsm6dsv16x_i3c_reset_mode_t; -int32_t lsm6dsv16x_i3c_reset_mode_set(stmdev_ctx_t *ctx, - lsm6dsv16x_i3c_reset_mode_t val); -int32_t lsm6dsv16x_i3c_reset_mode_get(stmdev_ctx_t *ctx, - lsm6dsv16x_i3c_reset_mode_t *val); + LSM6DSV_SW_RST_DYN_ADDRESS_RST = 0x0, + LSM6DSV_I3C_GLOBAL_RST = 0x1, +} lsm6dsv_i3c_reset_mode_t; +int32_t lsm6dsv_i3c_reset_mode_set(stmdev_ctx_t *ctx, + lsm6dsv_i3c_reset_mode_t val); +int32_t lsm6dsv_i3c_reset_mode_get(stmdev_ctx_t *ctx, + lsm6dsv_i3c_reset_mode_t *val); typedef enum { - LSM6DSV16X_IBI_2us = 0x0, - LSM6DSV16X_IBI_50us = 0x1, - LSM6DSV16X_IBI_1ms = 0x2, - LSM6DSV16X_IBI_25ms = 0x3, -} lsm6dsv16x_i3c_ibi_time_t; -int32_t lsm6dsv16x_i3c_ibi_time_set(stmdev_ctx_t *ctx, - lsm6dsv16x_i3c_ibi_time_t val); -int32_t lsm6dsv16x_i3c_ibi_time_get(stmdev_ctx_t *ctx, - lsm6dsv16x_i3c_ibi_time_t *val); + LSM6DSV_IBI_2us = 0x0, + LSM6DSV_IBI_50us = 0x1, + LSM6DSV_IBI_1ms = 0x2, + LSM6DSV_IBI_25ms = 0x3, +} lsm6dsv_i3c_ibi_time_t; +int32_t lsm6dsv_i3c_ibi_time_set(stmdev_ctx_t *ctx, + lsm6dsv_i3c_ibi_time_t val); +int32_t lsm6dsv_i3c_ibi_time_get(stmdev_ctx_t *ctx, + lsm6dsv_i3c_ibi_time_t *val); -int32_t lsm6dsv16x_sh_master_interface_pull_up_set(stmdev_ctx_t *ctx, - uint8_t val); -int32_t lsm6dsv16x_sh_master_interface_pull_up_get(stmdev_ctx_t *ctx, - uint8_t *val); +int32_t lsm6dsv_sh_master_interface_pull_up_set(stmdev_ctx_t *ctx, + uint8_t val); +int32_t lsm6dsv_sh_master_interface_pull_up_get(stmdev_ctx_t *ctx, + uint8_t *val); -int32_t lsm6dsv16x_sh_read_data_raw_get(stmdev_ctx_t *ctx, uint8_t *val, - uint8_t len); +int32_t lsm6dsv_sh_read_data_raw_get(stmdev_ctx_t *ctx, uint8_t *val, + uint8_t len); typedef enum { - LSM6DSV16X_SLV_0 = 0x0, - LSM6DSV16X_SLV_0_1 = 0x1, - LSM6DSV16X_SLV_0_1_2 = 0x2, - LSM6DSV16X_SLV_0_1_2_3 = 0x3, -} lsm6dsv16x_sh_slave_connected_t; -int32_t lsm6dsv16x_sh_slave_connected_set(stmdev_ctx_t *ctx, - lsm6dsv16x_sh_slave_connected_t val); -int32_t lsm6dsv16x_sh_slave_connected_get(stmdev_ctx_t *ctx, - lsm6dsv16x_sh_slave_connected_t *val); + LSM6DSV_SLV_0 = 0x0, + LSM6DSV_SLV_0_1 = 0x1, + LSM6DSV_SLV_0_1_2 = 0x2, + LSM6DSV_SLV_0_1_2_3 = 0x3, +} lsm6dsv_sh_slave_connected_t; +int32_t lsm6dsv_sh_slave_connected_set(stmdev_ctx_t *ctx, + lsm6dsv_sh_slave_connected_t val); +int32_t lsm6dsv_sh_slave_connected_get(stmdev_ctx_t *ctx, + lsm6dsv_sh_slave_connected_t *val); -int32_t lsm6dsv16x_sh_master_set(stmdev_ctx_t *ctx, uint8_t val); -int32_t lsm6dsv16x_sh_master_get(stmdev_ctx_t *ctx, uint8_t *val); +int32_t lsm6dsv_sh_master_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6dsv_sh_master_get(stmdev_ctx_t *ctx, uint8_t *val); -int32_t lsm6dsv16x_sh_pass_through_set(stmdev_ctx_t *ctx, uint8_t val); -int32_t lsm6dsv16x_sh_pass_through_get(stmdev_ctx_t *ctx, uint8_t *val); +int32_t lsm6dsv_sh_pass_through_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6dsv_sh_pass_through_get(stmdev_ctx_t *ctx, uint8_t *val); typedef enum { - LSM6DSV16X_SH_TRG_XL_GY_DRDY = 0x0, - LSM6DSV16X_SH_TRIG_INT2 = 0x1, -} lsm6dsv16x_sh_syncro_mode_t; -int32_t lsm6dsv16x_sh_syncro_mode_set(stmdev_ctx_t *ctx, - lsm6dsv16x_sh_syncro_mode_t val); -int32_t lsm6dsv16x_sh_syncro_mode_get(stmdev_ctx_t *ctx, - lsm6dsv16x_sh_syncro_mode_t *val); + LSM6DSV_SH_TRG_XL_GY_DRDY = 0x0, + LSM6DSV_SH_TRIG_INT2 = 0x1, +} lsm6dsv_sh_syncro_mode_t; +int32_t lsm6dsv_sh_syncro_mode_set(stmdev_ctx_t *ctx, + lsm6dsv_sh_syncro_mode_t val); +int32_t lsm6dsv_sh_syncro_mode_get(stmdev_ctx_t *ctx, + lsm6dsv_sh_syncro_mode_t *val); typedef enum { - LSM6DSV16X_EACH_SH_CYCLE = 0x0, - LSM6DSV16X_ONLY_FIRST_CYCLE = 0x1, -} lsm6dsv16x_sh_write_mode_t; -int32_t lsm6dsv16x_sh_write_mode_set(stmdev_ctx_t *ctx, - lsm6dsv16x_sh_write_mode_t val); -int32_t lsm6dsv16x_sh_write_mode_get(stmdev_ctx_t *ctx, - lsm6dsv16x_sh_write_mode_t *val); + LSM6DSV_EACH_SH_CYCLE = 0x0, + LSM6DSV_ONLY_FIRST_CYCLE = 0x1, +} lsm6dsv_sh_write_mode_t; +int32_t lsm6dsv_sh_write_mode_set(stmdev_ctx_t *ctx, + lsm6dsv_sh_write_mode_t val); +int32_t lsm6dsv_sh_write_mode_get(stmdev_ctx_t *ctx, + lsm6dsv_sh_write_mode_t *val); -int32_t lsm6dsv16x_sh_reset_set(stmdev_ctx_t *ctx, uint8_t val); -int32_t lsm6dsv16x_sh_reset_get(stmdev_ctx_t *ctx, uint8_t *val); +int32_t lsm6dsv_sh_reset_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6dsv_sh_reset_get(stmdev_ctx_t *ctx, uint8_t *val); typedef struct { uint8_t slv0_add; uint8_t slv0_subadd; uint8_t slv0_data; -} lsm6dsv16x_sh_cfg_write_t; -int32_t lsm6dsv16x_sh_cfg_write(stmdev_ctx_t *ctx, - lsm6dsv16x_sh_cfg_write_t *val); +} lsm6dsv_sh_cfg_write_t; +int32_t lsm6dsv_sh_cfg_write(stmdev_ctx_t *ctx, + lsm6dsv_sh_cfg_write_t *val); typedef enum { - LSM6DSV16X_SH_15Hz = 0x1, - LSM6DSV16X_SH_30Hz = 0x2, - LSM6DSV16X_SH_60Hz = 0x3, - LSM6DSV16X_SH_120Hz = 0x4, - LSM6DSV16X_SH_240Hz = 0x5, - LSM6DSV16X_SH_480Hz = 0x6, -} lsm6dsv16x_sh_data_rate_t; -int32_t lsm6dsv16x_sh_data_rate_set(stmdev_ctx_t *ctx, - lsm6dsv16x_sh_data_rate_t val); -int32_t lsm6dsv16x_sh_data_rate_get(stmdev_ctx_t *ctx, - lsm6dsv16x_sh_data_rate_t *val); + LSM6DSV_SH_15Hz = 0x1, + LSM6DSV_SH_30Hz = 0x2, + LSM6DSV_SH_60Hz = 0x3, + LSM6DSV_SH_120Hz = 0x4, + LSM6DSV_SH_240Hz = 0x5, + LSM6DSV_SH_480Hz = 0x6, +} lsm6dsv_sh_data_rate_t; +int32_t lsm6dsv_sh_data_rate_set(stmdev_ctx_t *ctx, + lsm6dsv_sh_data_rate_t val); +int32_t lsm6dsv_sh_data_rate_get(stmdev_ctx_t *ctx, + lsm6dsv_sh_data_rate_t *val); typedef struct { uint8_t slv_add; uint8_t slv_subadd; uint8_t slv_len; -} lsm6dsv16x_sh_cfg_read_t; -int32_t lsm6dsv16x_sh_slv_cfg_read(stmdev_ctx_t *ctx, uint8_t idx, - lsm6dsv16x_sh_cfg_read_t *val); +} lsm6dsv_sh_cfg_read_t; +int32_t lsm6dsv_sh_slv_cfg_read(stmdev_ctx_t *ctx, uint8_t idx, + lsm6dsv_sh_cfg_read_t *val); -int32_t lsm6dsv16x_sh_status_get(stmdev_ctx_t *ctx, - lsm6dsv16x_status_master_t *val); +int32_t lsm6dsv_sh_status_get(stmdev_ctx_t *ctx, + lsm6dsv_status_master_t *val); -int32_t lsm6dsv16x_ui_sdo_pull_up_set(stmdev_ctx_t *ctx, uint8_t val); -int32_t lsm6dsv16x_ui_sdo_pull_up_get(stmdev_ctx_t *ctx, uint8_t *val); +int32_t lsm6dsv_ui_sdo_pull_up_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6dsv_ui_sdo_pull_up_get(stmdev_ctx_t *ctx, uint8_t *val); typedef enum { - LSM6DSV16X_I2C_I3C_ENABLE = 0x0, - LSM6DSV16X_I2C_I3C_DISABLE = 0x1, -} lsm6dsv16x_ui_i2c_i3c_mode_t; -int32_t lsm6dsv16x_ui_i2c_i3c_mode_set(stmdev_ctx_t *ctx, - lsm6dsv16x_ui_i2c_i3c_mode_t val); -int32_t lsm6dsv16x_ui_i2c_i3c_mode_get(stmdev_ctx_t *ctx, - lsm6dsv16x_ui_i2c_i3c_mode_t *val); + LSM6DSV_I2C_I3C_ENABLE = 0x0, + LSM6DSV_I2C_I3C_DISABLE = 0x1, +} lsm6dsv_ui_i2c_i3c_mode_t; +int32_t lsm6dsv_ui_i2c_i3c_mode_set(stmdev_ctx_t *ctx, + lsm6dsv_ui_i2c_i3c_mode_t val); +int32_t lsm6dsv_ui_i2c_i3c_mode_get(stmdev_ctx_t *ctx, + lsm6dsv_ui_i2c_i3c_mode_t *val); typedef enum { - LSM6DSV16X_SPI_4_WIRE = 0x0, - LSM6DSV16X_SPI_3_WIRE = 0x1, -} lsm6dsv16x_spi_mode_t; -int32_t lsm6dsv16x_spi_mode_set(stmdev_ctx_t *ctx, lsm6dsv16x_spi_mode_t val); -int32_t lsm6dsv16x_spi_mode_get(stmdev_ctx_t *ctx, lsm6dsv16x_spi_mode_t *val); + LSM6DSV_SPI_4_WIRE = 0x0, + LSM6DSV_SPI_3_WIRE = 0x1, +} lsm6dsv_spi_mode_t; +int32_t lsm6dsv_spi_mode_set(stmdev_ctx_t *ctx, lsm6dsv_spi_mode_t val); +int32_t lsm6dsv_spi_mode_get(stmdev_ctx_t *ctx, lsm6dsv_spi_mode_t *val); -int32_t lsm6dsv16x_ui_sda_pull_up_set(stmdev_ctx_t *ctx, uint8_t val); -int32_t lsm6dsv16x_ui_sda_pull_up_get(stmdev_ctx_t *ctx, uint8_t *val); +int32_t lsm6dsv_ui_sda_pull_up_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6dsv_ui_sda_pull_up_get(stmdev_ctx_t *ctx, uint8_t *val); typedef enum { - LSM6DSV16X_SPI2_4_WIRE = 0x0, - LSM6DSV16X_SPI2_3_WIRE = 0x1, -} lsm6dsv16x_spi2_mode_t; -int32_t lsm6dsv16x_spi2_mode_set(stmdev_ctx_t *ctx, lsm6dsv16x_spi2_mode_t val); -int32_t lsm6dsv16x_spi2_mode_get(stmdev_ctx_t *ctx, - lsm6dsv16x_spi2_mode_t *val); + LSM6DSV_SPI2_4_WIRE = 0x0, + LSM6DSV_SPI2_3_WIRE = 0x1, +} lsm6dsv_spi2_mode_t; +int32_t lsm6dsv_spi2_mode_set(stmdev_ctx_t *ctx, lsm6dsv_spi2_mode_t val); +int32_t lsm6dsv_spi2_mode_get(stmdev_ctx_t *ctx, + lsm6dsv_spi2_mode_t *val); -int32_t lsm6dsv16x_sigmot_mode_set(stmdev_ctx_t *ctx, uint8_t val); -int32_t lsm6dsv16x_sigmot_mode_get(stmdev_ctx_t *ctx, uint8_t *val); +int32_t lsm6dsv_sigmot_mode_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6dsv_sigmot_mode_get(stmdev_ctx_t *ctx, uint8_t *val); typedef struct { uint8_t step_counter_enable : 1; - uint8_t false_step_rej : 1; -} lsm6dsv16x_stpcnt_mode_t; -int32_t lsm6dsv16x_stpcnt_mode_set(stmdev_ctx_t *ctx, - lsm6dsv16x_stpcnt_mode_t val); -int32_t lsm6dsv16x_stpcnt_mode_get(stmdev_ctx_t *ctx, - lsm6dsv16x_stpcnt_mode_t *val); +} lsm6dsv_stpcnt_mode_t; +int32_t lsm6dsv_stpcnt_mode_set(stmdev_ctx_t *ctx, + lsm6dsv_stpcnt_mode_t val); +int32_t lsm6dsv_stpcnt_mode_get(stmdev_ctx_t *ctx, + lsm6dsv_stpcnt_mode_t *val); -int32_t lsm6dsv16x_stpcnt_steps_get(stmdev_ctx_t *ctx, uint16_t *val); +int32_t lsm6dsv_stpcnt_steps_get(stmdev_ctx_t *ctx, uint16_t *val); -int32_t lsm6dsv16x_stpcnt_rst_step_set(stmdev_ctx_t *ctx, uint8_t val); -int32_t lsm6dsv16x_stpcnt_rst_step_get(stmdev_ctx_t *ctx, uint8_t *val); +int32_t lsm6dsv_stpcnt_rst_step_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6dsv_stpcnt_rst_step_get(stmdev_ctx_t *ctx, uint8_t *val); -int32_t lsm6dsv16x_stpcnt_debounce_set(stmdev_ctx_t *ctx, uint8_t val); -int32_t lsm6dsv16x_stpcnt_debounce_get(stmdev_ctx_t *ctx, uint8_t *val); +int32_t lsm6dsv_stpcnt_debounce_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6dsv_stpcnt_debounce_get(stmdev_ctx_t *ctx, uint8_t *val); -int32_t lsm6dsv16x_stpcnt_period_set(stmdev_ctx_t *ctx, uint16_t val); -int32_t lsm6dsv16x_stpcnt_period_get(stmdev_ctx_t *ctx, uint16_t *val); +int32_t lsm6dsv_stpcnt_period_set(stmdev_ctx_t *ctx, uint16_t val); +int32_t lsm6dsv_stpcnt_period_get(stmdev_ctx_t *ctx, uint16_t *val); -int32_t lsm6dsv16x_sflp_game_rotation_set(stmdev_ctx_t *ctx, uint8_t val); -int32_t lsm6dsv16x_sflp_game_rotation_get(stmdev_ctx_t *ctx, uint8_t *val); +int32_t lsm6dsv_sflp_game_rotation_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6dsv_sflp_game_rotation_get(stmdev_ctx_t *ctx, uint8_t *val); typedef struct { float_t gbias_x; /* dps */ float_t gbias_y; /* dps */ float_t gbias_z; /* dps */ -} lsm6dsv16x_sflp_gbias_t; -int32_t lsm6dsv16x_sflp_game_gbias_set(stmdev_ctx_t *ctx, - lsm6dsv16x_sflp_gbias_t *val); +} lsm6dsv_sflp_gbias_t; +int32_t lsm6dsv_sflp_game_gbias_set(stmdev_ctx_t *ctx, + lsm6dsv_sflp_gbias_t *val); typedef enum { - LSM6DSV16X_SFLP_15Hz = 0x0, - LSM6DSV16X_SFLP_30Hz = 0x1, - LSM6DSV16X_SFLP_60Hz = 0x2, - LSM6DSV16X_SFLP_120Hz = 0x3, - LSM6DSV16X_SFLP_240Hz = 0x4, - LSM6DSV16X_SFLP_480Hz = 0x5, -} lsm6dsv16x_sflp_data_rate_t; -int32_t lsm6dsv16x_sflp_data_rate_set(stmdev_ctx_t *ctx, - lsm6dsv16x_sflp_data_rate_t val); -int32_t lsm6dsv16x_sflp_data_rate_get(stmdev_ctx_t *ctx, - lsm6dsv16x_sflp_data_rate_t *val); + LSM6DSV_SFLP_15Hz = 0x0, + LSM6DSV_SFLP_30Hz = 0x1, + LSM6DSV_SFLP_60Hz = 0x2, + LSM6DSV_SFLP_120Hz = 0x3, + LSM6DSV_SFLP_240Hz = 0x4, + LSM6DSV_SFLP_480Hz = 0x5, +} lsm6dsv_sflp_data_rate_t; +int32_t lsm6dsv_sflp_data_rate_set(stmdev_ctx_t *ctx, + lsm6dsv_sflp_data_rate_t val); +int32_t lsm6dsv_sflp_data_rate_get(stmdev_ctx_t *ctx, + lsm6dsv_sflp_data_rate_t *val); typedef struct { uint8_t tap_x_en : 1; uint8_t tap_y_en : 1; uint8_t tap_z_en : 1; -} lsm6dsv16x_tap_detection_t; -int32_t lsm6dsv16x_tap_detection_set(stmdev_ctx_t *ctx, - lsm6dsv16x_tap_detection_t val); -int32_t lsm6dsv16x_tap_detection_get(stmdev_ctx_t *ctx, - lsm6dsv16x_tap_detection_t *val); +} lsm6dsv_tap_detection_t; +int32_t lsm6dsv_tap_detection_set(stmdev_ctx_t *ctx, + lsm6dsv_tap_detection_t val); +int32_t lsm6dsv_tap_detection_get(stmdev_ctx_t *ctx, + lsm6dsv_tap_detection_t *val); typedef struct { uint8_t x : 5; uint8_t y : 5; uint8_t z : 5; -} lsm6dsv16x_tap_thresholds_t; -int32_t lsm6dsv16x_tap_thresholds_set(stmdev_ctx_t *ctx, - lsm6dsv16x_tap_thresholds_t val); -int32_t lsm6dsv16x_tap_thresholds_get(stmdev_ctx_t *ctx, - lsm6dsv16x_tap_thresholds_t *val); +} lsm6dsv_tap_thresholds_t; +int32_t lsm6dsv_tap_thresholds_set(stmdev_ctx_t *ctx, + lsm6dsv_tap_thresholds_t val); +int32_t lsm6dsv_tap_thresholds_get(stmdev_ctx_t *ctx, + lsm6dsv_tap_thresholds_t *val); typedef enum { - LSM6DSV16X_XYZ = 0x0, - LSM6DSV16X_YXZ = 0x1, - LSM6DSV16X_XZY = 0x2, - LSM6DSV16X_ZYX = 0x3, - LSM6DSV16X_YZX = 0x5, - LSM6DSV16X_ZXY = 0x6, -} lsm6dsv16x_tap_axis_priority_t; -int32_t lsm6dsv16x_tap_axis_priority_set(stmdev_ctx_t *ctx, - lsm6dsv16x_tap_axis_priority_t val); -int32_t lsm6dsv16x_tap_axis_priority_get(stmdev_ctx_t *ctx, - lsm6dsv16x_tap_axis_priority_t *val); + LSM6DSV_XYZ = 0x0, + LSM6DSV_YXZ = 0x1, + LSM6DSV_XZY = 0x2, + LSM6DSV_ZYX = 0x3, + LSM6DSV_YZX = 0x5, + LSM6DSV_ZXY = 0x6, +} lsm6dsv_tap_axis_priority_t; +int32_t lsm6dsv_tap_axis_priority_set(stmdev_ctx_t *ctx, + lsm6dsv_tap_axis_priority_t val); +int32_t lsm6dsv_tap_axis_priority_get(stmdev_ctx_t *ctx, + lsm6dsv_tap_axis_priority_t *val); typedef struct { uint8_t shock : 2; uint8_t quiet : 2; uint8_t tap_gap : 4; -} lsm6dsv16x_tap_time_windows_t; -int32_t lsm6dsv16x_tap_time_windows_set(stmdev_ctx_t *ctx, - lsm6dsv16x_tap_time_windows_t val); -int32_t lsm6dsv16x_tap_time_windows_get(stmdev_ctx_t *ctx, - lsm6dsv16x_tap_time_windows_t *val); +} lsm6dsv_tap_time_windows_t; +int32_t lsm6dsv_tap_time_windows_set(stmdev_ctx_t *ctx, + lsm6dsv_tap_time_windows_t val); +int32_t lsm6dsv_tap_time_windows_get(stmdev_ctx_t *ctx, + lsm6dsv_tap_time_windows_t *val); typedef enum { - LSM6DSV16X_ONLY_SINGLE = 0x0, - LSM6DSV16X_BOTH_SINGLE_DOUBLE = 0x1, -} lsm6dsv16x_tap_mode_t; -int32_t lsm6dsv16x_tap_mode_set(stmdev_ctx_t *ctx, lsm6dsv16x_tap_mode_t val); -int32_t lsm6dsv16x_tap_mode_get(stmdev_ctx_t *ctx, lsm6dsv16x_tap_mode_t *val); + LSM6DSV_ONLY_SINGLE = 0x0, + LSM6DSV_BOTH_SINGLE_DOUBLE = 0x1, +} lsm6dsv_tap_mode_t; +int32_t lsm6dsv_tap_mode_set(stmdev_ctx_t *ctx, lsm6dsv_tap_mode_t val); +int32_t lsm6dsv_tap_mode_get(stmdev_ctx_t *ctx, lsm6dsv_tap_mode_t *val); -int32_t lsm6dsv16x_tilt_mode_set(stmdev_ctx_t *ctx, uint8_t val); -int32_t lsm6dsv16x_tilt_mode_get(stmdev_ctx_t *ctx, uint8_t *val); +int32_t lsm6dsv_tilt_mode_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6dsv_tilt_mode_get(stmdev_ctx_t *ctx, uint8_t *val); -int32_t lsm6dsv16x_timestamp_raw_get(stmdev_ctx_t *ctx, uint32_t *val); +int32_t lsm6dsv_timestamp_raw_get(stmdev_ctx_t *ctx, uint32_t *val); -int32_t lsm6dsv16x_timestamp_set(stmdev_ctx_t *ctx, uint8_t val); -int32_t lsm6dsv16x_timestamp_get(stmdev_ctx_t *ctx, uint8_t *val); +int32_t lsm6dsv_timestamp_set(stmdev_ctx_t *ctx, uint8_t val); +int32_t lsm6dsv_timestamp_get(stmdev_ctx_t *ctx, uint8_t *val); typedef enum { - LSM6DSV16X_XL_AND_GY_NOT_AFFECTED = 0x0, - LSM6DSV16X_XL_LOW_POWER_GY_NOT_AFFECTED = 0x1, - LSM6DSV16X_XL_LOW_POWER_GY_SLEEP = 0x2, - LSM6DSV16X_XL_LOW_POWER_GY_POWER_DOWN = 0x3, -} lsm6dsv16x_act_mode_t; -int32_t lsm6dsv16x_act_mode_set(stmdev_ctx_t *ctx, lsm6dsv16x_act_mode_t val); -int32_t lsm6dsv16x_act_mode_get(stmdev_ctx_t *ctx, lsm6dsv16x_act_mode_t *val); + LSM6DSV_XL_AND_GY_NOT_AFFECTED = 0x0, + LSM6DSV_XL_LOW_POWER_GY_NOT_AFFECTED = 0x1, + LSM6DSV_XL_LOW_POWER_GY_SLEEP = 0x2, + LSM6DSV_XL_LOW_POWER_GY_POWER_DOWN = 0x3, +} lsm6dsv_act_mode_t; +int32_t lsm6dsv_act_mode_set(stmdev_ctx_t *ctx, lsm6dsv_act_mode_t val); +int32_t lsm6dsv_act_mode_get(stmdev_ctx_t *ctx, lsm6dsv_act_mode_t *val); typedef enum { - LSM6DSV16X_SLEEP_TO_ACT_AT_1ST_SAMPLE = 0x0, - LSM6DSV16X_SLEEP_TO_ACT_AT_2ND_SAMPLE = 0x1, - LSM6DSV16X_SLEEP_TO_ACT_AT_3RD_SAMPLE = 0x2, - LSM6DSV16X_SLEEP_TO_ACT_AT_4th_SAMPLE = 0x3, -} lsm6dsv16x_act_from_sleep_to_act_dur_t; -int32_t lsm6dsv16x_act_from_sleep_to_act_dur_set(stmdev_ctx_t *ctx, - lsm6dsv16x_act_from_sleep_to_act_dur_t val); -int32_t lsm6dsv16x_act_from_sleep_to_act_dur_get(stmdev_ctx_t *ctx, - lsm6dsv16x_act_from_sleep_to_act_dur_t *val); + LSM6DSV_SLEEP_TO_ACT_AT_1ST_SAMPLE = 0x0, + LSM6DSV_SLEEP_TO_ACT_AT_2ND_SAMPLE = 0x1, + LSM6DSV_SLEEP_TO_ACT_AT_3RD_SAMPLE = 0x2, + LSM6DSV_SLEEP_TO_ACT_AT_4th_SAMPLE = 0x3, +} lsm6dsv_act_from_sleep_to_act_dur_t; +int32_t lsm6dsv_act_from_sleep_to_act_dur_set(stmdev_ctx_t *ctx, + lsm6dsv_act_from_sleep_to_act_dur_t val); +int32_t lsm6dsv_act_from_sleep_to_act_dur_get(stmdev_ctx_t *ctx, + lsm6dsv_act_from_sleep_to_act_dur_t *val); typedef enum { - LSM6DSV16X_1Hz875 = 0x0, - LSM6DSV16X_15Hz = 0x1, - LSM6DSV16X_30Hz = 0x2, - LSM6DSV16X_60Hz = 0x3, -} lsm6dsv16x_act_sleep_xl_odr_t; -int32_t lsm6dsv16x_act_sleep_xl_odr_set(stmdev_ctx_t *ctx, - lsm6dsv16x_act_sleep_xl_odr_t val); -int32_t lsm6dsv16x_act_sleep_xl_odr_get(stmdev_ctx_t *ctx, - lsm6dsv16x_act_sleep_xl_odr_t *val); + LSM6DSV_1Hz875 = 0x0, + LSM6DSV_15Hz = 0x1, + LSM6DSV_30Hz = 0x2, + LSM6DSV_60Hz = 0x3, +} lsm6dsv_act_sleep_xl_odr_t; +int32_t lsm6dsv_act_sleep_xl_odr_set(stmdev_ctx_t *ctx, + lsm6dsv_act_sleep_xl_odr_t val); +int32_t lsm6dsv_act_sleep_xl_odr_get(stmdev_ctx_t *ctx, + lsm6dsv_act_sleep_xl_odr_t *val); typedef struct { - lsm6dsv16x_inactivity_dur_t inactivity_cfg; + lsm6dsv_inactivity_dur_t inactivity_cfg; uint8_t inactivity_ths; uint8_t threshold; uint8_t duration; -} lsm6dsv16x_act_thresholds_t; -int32_t lsm6dsv16x_act_thresholds_set(stmdev_ctx_t *ctx, - lsm6dsv16x_act_thresholds_t *val); -int32_t lsm6dsv16x_act_thresholds_get(stmdev_ctx_t *ctx, - lsm6dsv16x_act_thresholds_t *val); +} lsm6dsv_act_thresholds_t; +int32_t lsm6dsv_act_thresholds_set(stmdev_ctx_t *ctx, + lsm6dsv_act_thresholds_t *val); +int32_t lsm6dsv_act_thresholds_get(stmdev_ctx_t *ctx, + lsm6dsv_act_thresholds_t *val); typedef struct { uint8_t shock : 2; uint8_t quiet : 4; -} lsm6dsv16x_act_wkup_time_windows_t; -int32_t lsm6dsv16x_act_wkup_time_windows_set(stmdev_ctx_t *ctx, - lsm6dsv16x_act_wkup_time_windows_t val); -int32_t lsm6dsv16x_act_wkup_time_windows_get(stmdev_ctx_t *ctx, - lsm6dsv16x_act_wkup_time_windows_t *val); +} lsm6dsv_act_wkup_time_windows_t; +int32_t lsm6dsv_act_wkup_time_windows_set(stmdev_ctx_t *ctx, + lsm6dsv_act_wkup_time_windows_t val); +int32_t lsm6dsv_act_wkup_time_windows_get(stmdev_ctx_t *ctx, + lsm6dsv_act_wkup_time_windows_t *val); /** * @} @@ -5264,6 +4981,6 @@ int32_t lsm6dsv16x_act_wkup_time_windows_get(stmdev_ctx_t *ctx, } #endif -#endif /*LSM6DSV16X_DRIVER_H */ +#endif /*LSM6DSV_DRIVER_H */ /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ diff --git a/src/sensors/SensorManager.cpp b/src/sensors/SensorManager.cpp index c90e74413..095a554b7 100644 --- a/src/sensors/SensorManager.cpp +++ b/src/sensors/SensorManager.cpp @@ -29,7 +29,7 @@ #include "mpu6050sensor.h" #include "bmi160sensor.h" #include "icm20948sensor.h" -#include "lsm6dsvsensor.h" +#include "lsm6dsvSensor.h" #include "ErroneousSensor.h" #include "sensoraddresses.h" #include "GlobalVars.h" diff --git a/src/sensors/lsm6dsvsensor.cpp b/src/sensors/lsm6dsvSensor.cpp similarity index 87% rename from src/sensors/lsm6dsvsensor.cpp rename to src/sensors/lsm6dsvSensor.cpp index 8a1bdb7df..d48ce67e7 100644 --- a/src/sensors/lsm6dsvsensor.cpp +++ b/src/sensors/lsm6dsvSensor.cpp @@ -21,11 +21,11 @@ THE SOFTWARE. */ -#include "sensors/lsm6dsvsensor.h" +#include "sensors/lsm6dsvSensor.h" #include "GlobalVars.h" #include "customConversions.h" -#include "lsm6dsvsensor.h" +#include "lsm6dsvSensor.h" #include "utils.h" @@ -56,7 +56,7 @@ LSM6DSVSensor::LSM6DSVSensor( void LSM6DSVSensor::motionSetup() { uint8_t deviceId = 0; - if (imu.ReadID(&deviceId) == LSM6DSV16X_ERROR) { + if (imu.ReadID(&deviceId) == LSM6DSV_ERROR) { m_Logger.fatal( "The %s at 0x%02x returned an error when reading the device ID of: 0x%02x", getIMUNameByType(sensorType), @@ -64,31 +64,31 @@ void LSM6DSVSensor::motionSetup() { deviceId ); ledManager.pattern(50, 50, 200); - status = LSM6DSV16X_ERROR; + status = LSM6DSV_ERROR; return; } - if (deviceId != LSM6DSV16X_ID) { + if (deviceId != LSM6DSV_ID) { m_Logger.fatal( "The %s at 0x%02x returned an invalid device ID of: 0x%02x when it should " "have been: 0x%02x", getIMUNameByType(sensorType), addr, deviceId, - LSM6DSV16X_ID + LSM6DSV_ID ); ledManager.pattern(50, 50, 200); return; } - status |= imu.Enable_6D_Orientation(LSM6DSV16X_INT2_PIN); + status |= imu.Enable_6D_Orientation(LSM6DSV_INT2_PIN); uint8_t isFaceDown; // TODO: IMU rotation could be different (IMU upside down then isFaceUp) status |= imu.Get_6D_Orientation_ZL(&isFaceDown); #ifndef LSM6DSV_NO_SELF_TEST_ON_FACEDOWN if (isFaceDown) { - if (runSelfTest() != LSM6DSV16X_OK) { + if (runSelfTest() != LSM6DSV_OK) { m_Logger.fatal( "The %s at 0x%02x returned an error during the self test " "(maybe it wasn't on a flat surface?)", @@ -96,7 +96,7 @@ void LSM6DSVSensor::motionSetup() { addr ); ledManager.pattern(50, 50, 200); - status = LSM6DSV16X_ERROR; + status = LSM6DSV_ERROR; return; } } @@ -107,7 +107,7 @@ void LSM6DSVSensor::motionSetup() { status |= imu.begin(); // Restore defaults - status |= imu.Reset_Set(LSM6DSV16X_RESET_CTRL_REGS); + status |= imu.Reset_Set(LSM6DSV_RESET_CTRL_REGS); // Enable Block Data Update status |= imu.Enable_Block_Data_Update(); @@ -118,9 +118,8 @@ void LSM6DSVSensor::motionSetup() { status |= imu.Set_G_FS(LSM6DSV_GYRO_MAX); // Set data rate - status |= imu.Set_X_ODR(LSM6DSV_GYRO_ACCEL_RATE, LSM6DSV16X_ACC_HIGH_PERFORMANCE_MODE); - status |= imu.Set_G_ODR(LSM6DSV_GYRO_ACCEL_RATE, LSM6DSV16X_GYRO_HIGH_PERFORMANCE_MODE); - + status |= imu.Set_X_ODR(LSM6DSV_GYRO_ACCEL_RATE, LSM6DSV_ACC_HIGH_PERFORMANCE_MODE); + status |= imu.Set_G_ODR(LSM6DSV_GYRO_ACCEL_RATE, LSM6DSV_GYRO_HIGH_PERFORMANCE_MODE); status |= imu.FIFO_Set_X_BDR(LSM6DSV_FIFO_DATA_RATE); #if (LSM6DSV_FUSION_SOURCE == LSM6DSV_FUSION_ESP) @@ -128,7 +127,7 @@ void LSM6DSVSensor::motionSetup() { #endif status |= imu.FIFO_Set_Timestamp_Decimation( - lsm6dsv16x_fifo_timestamp_batch_t::LSM6DSV16X_TMSTMP_DEC_1 + lsm6dsv_fifo_timestamp_batch_t::LSM6DSV_TMSTMP_DEC_1 ); // Enable IMU @@ -136,14 +135,14 @@ void LSM6DSVSensor::motionSetup() { status |= imu.Enable_X(); status |= imu.Enable_G(); - // status |= imu.Set_X_Filter_Mode(1, LSM6DSV16X_XL_LIGHT); - // status |= imu.Set_G_Filter_Mode(1, LSM6DSV16X_XL_LIGHT); + // status |= imu.Set_X_Filter_Mode(1, LSM6DSV_XL_LIGHT); + // status |= imu.Set_G_Filter_Mode(1, LSM6DSV_XL_LIGHT); - // status |= imu.Set_X_Filter_Mode(0, LSM6DSV16X_XL_ULTRA_LIGHT); - // status |= imu.Set_G_Filter_Mode(0, LSM6DSV16X_XL_ULTRA_LIGHT); + // status |= imu.Set_X_Filter_Mode(0, LSM6DSV_XL_ULTRA_LIGHT); + // status |= imu.Set_G_Filter_Mode(0, LSM6DSV_XL_ULTRA_LIGHT); // Set FIFO mode to "continuous", so old data gets thrown away - status |= imu.FIFO_Set_Mode(LSM6DSV16X_STREAM_MODE); + status |= imu.FIFO_Set_Mode(LSM6DSV_STREAM_MODE); #if (LSM6DSV_FUSION_SOURCE == LSM6DSV_FUSION_ONBOARD) status |= imu.FIFO_Set_SFLP_Batch(true, true, false); @@ -164,15 +163,12 @@ void LSM6DSVSensor::motionSetup() { status |= imu.Disable_6D_Orientation(); - // status |= imu.beginPreconfigured(); - - #ifdef LSM6DSV_INTERRUPT attachInterrupt(m_IntPin, interruptHandler, RISING); - status |= imu.Enable_Single_Tap_Detection(LSM6DSV16X_INT1_PIN); // Tap recommends an interrupt + status |= imu.Enable_Single_Tap_Detection(LSM6DSV_INT1_PIN); // Tap recommends an interrupt #else - status |= imu.Enable_Single_Tap_Detection(LSM6DSV16X_INT2_PIN); //Just poll to see if an event happened jank but works + status |= imu.Enable_Single_Tap_Detection(LSM6DSV_INT2_PIN); //Just poll to see if an event happened jank but works #endif // LSM6DSV_INTERRUPT status |= imu.Set_Tap_Threshold(LSM6DSV_TAP_THRESHOLD); @@ -181,7 +177,7 @@ void LSM6DSVSensor::motionSetup() { status |= imu.FIFO_Reset(); - if (status != LSM6DSV16X_OK) { + if (status != LSM6DSV_OK) { m_Logger.fatal( "Errors occured enabling imu features on %s at address 0x%02x", getIMUNameByType(sensorType), @@ -205,7 +201,7 @@ constexpr uint8_t fifoFramSize = 4; // X BDR, (G BDR || Game), Gravity, Timestam void LSM6DSVSensor::motionLoop() { #ifdef LSM6DSV_INTERRUPT if (imuEvent) { - LSM6DSV16X_Event_Status_t eventStatus; + LSM6DSV_Event_Status_t eventStatus; status |= imu.Get_X_Event_Status(&eventStatus); if (eventStatus.TapStatus) { @@ -226,15 +222,15 @@ void LSM6DSVSensor::motionLoop() { lastTempRead = millis(); int16_t rawTemp; - if (imu.Get_Temperature_Raw(&rawTemp) != LSM6DSV16X_OK) { + if (imu.Get_Temperature_Raw(&rawTemp) != LSM6DSV_OK) { m_Logger.error( "Error getting temperature on %s at address 0x%02x", getIMUNameByType(sensorType), addr ); - status = LSM6DSV16X_ERROR; + status = LSM6DSV_ERROR; } else { - float actualTemp = lsm6dsv16x_from_lsb_to_celsius(rawTemp); + float actualTemp = lsm6dsv_from_lsb_to_celsius(rawTemp); if (fabsf(actualTemp - temperature) > 0.01f) { temperature = actualTemp; newTemperature = true; @@ -254,13 +250,13 @@ void LSM6DSVSensor::motionLoop() { } uint16_t fifo_samples = 0; - if (imu.FIFO_Get_Num_Samples(&fifo_samples) == LSM6DSV16X_ERROR) { + if (imu.FIFO_Get_Num_Samples(&fifo_samples) == LSM6DSV_ERROR) { m_Logger.error( "Error getting number of samples in FIFO on %s at address 0x%02x", getIMUNameByType(sensorType), addr ); - status = LSM6DSV16X_ERROR; + status = LSM6DSV_ERROR; return; } @@ -288,7 +284,7 @@ void LSM6DSVSensor::motionLoop() { sfusion.update6D( rawAcceleration, rawGyro, - lsm6dsv16x_from_lsb_to_nsec(currentDataTime - previousDataTime) * 1e-9 + lsm6dsv_from_lsb_to_nsec(currentDataTime - previousDataTime) * 1e-9 ); newRawAcceleration = false; newRawGyro = false; @@ -302,7 +298,7 @@ void LSM6DSVSensor::motionLoop() { rotation.y, rotation.z, currentDataTime, - lsm6dsv16x_from_lsb_to_nsec(currentDataTime - previousDataTime) * 1e-9, + lsm6dsv_from_lsb_to_nsec(currentDataTime - previousDataTime) * 1e-9, millis(), rawGyro[0], rawGyro[1], @@ -315,7 +311,7 @@ void LSM6DSVSensor::motionLoop() { SensorStatus LSM6DSVSensor::getSensorState() { return isWorking() - ? (status == LSM6DSV16X_OK ? SensorStatus::SENSOR_OK + ? (status == LSM6DSV_OK ? SensorStatus::SENSOR_OK : SensorStatus::SENSOR_ERROR) : SensorStatus::SENSOR_OFFLINE; } @@ -335,15 +331,15 @@ Quat LSM6DSVSensor::fusedRotationToQuaternion(float x, float y, float z) { return Quat(x, y, z, w); } -LSM6DSV16XStatusTypeDef LSM6DSVSensor::runSelfTest() { +LSM6DSVStatusTypeDef LSM6DSVSensor::runSelfTest() { m_Logger.info( "%s Self Test started on address: 0x%02x", getIMUNameByType(sensorType), addr ); - if (imu.Test_IMU(LSM6DSV16X_XL_ST_NEGATIVE, LSM6DSV16X_GY_ST_NEGATIVE) == LSM6DSV16X_ERROR) { - return LSM6DSV16X_ERROR; + if (imu.Test_IMU(LSM6DSV_XL_ST_NEGATIVE, LSM6DSV_GY_ST_NEGATIVE) == LSM6DSV_ERROR) { + return LSM6DSV_ERROR; } m_Logger.info( "%s Self Test Passed on address: 0x%02x", @@ -351,7 +347,7 @@ LSM6DSV16XStatusTypeDef LSM6DSVSensor::runSelfTest() { addr ); - return LSM6DSV16X_OK; + return LSM6DSV_OK; } void LSM6DSVSensor::sendData() { @@ -389,22 +385,22 @@ void LSM6DSVSensor::sendData() { } } -LSM6DSV16XStatusTypeDef LSM6DSVSensor::readFifo(uint16_t fifo_samples) { +LSM6DSVStatusTypeDef LSM6DSVSensor::readFifo(uint16_t fifo_samples) { for (uint16_t i = 0; i < fifo_samples; i++) { uint8_t tag; - if (imu.FIFO_Get_Tag(&tag) != LSM6DSV16X_OK) { + if (imu.FIFO_Get_Tag(&tag) != LSM6DSV_OK) { m_Logger.error( "Failed to get FIFO data tag on %s at address 0x%02x", getIMUNameByType(sensorType), addr ); - return LSM6DSV16X_ERROR; + return LSM6DSV_ERROR; } switch (tag) { - case lsm6dsv16x_fifo_out_raw_t::LSM6DSV16X_TIMESTAMP_TAG: { + case lsm6dsv_fifo_out_raw_t::LSM6DSV_TIMESTAMP_TAG: { if (i % fifoFramSize != 0) { - return LSM6DSV16X_OK; // If we are not requesting a full data set + return LSM6DSV_OK; // If we are not requesting a full data set // then stop reading } previousDataTime = currentDataTime; @@ -414,7 +410,7 @@ LSM6DSV16XStatusTypeDef LSM6DSVSensor::readFifo(uint16_t fifo_samples) { getIMUNameByType(sensorType), addr ); - return LSM6DSV16X_ERROR; + return LSM6DSV_ERROR; } // newTemperature = false; @@ -426,16 +422,16 @@ LSM6DSV16XStatusTypeDef LSM6DSVSensor::readFifo(uint16_t fifo_samples) { break; } - case lsm6dsv16x_fifo_out_raw_t::LSM6DSV16X_XL_NC_TAG: { // accel + case lsm6dsv_fifo_out_raw_t::LSM6DSV_XL_NC_TAG: { // accel int32_t intAcceleration[3]; - if (imu.FIFO_Get_X_Axes(intAcceleration) != LSM6DSV16X_OK) { + if (imu.FIFO_Get_X_Axes(intAcceleration) != LSM6DSV_OK) { m_Logger.error( "Failed to get accelerometer data on %s at address 0x%02x", getIMUNameByType(sensorType), addr ); - return LSM6DSV16X_ERROR; + return LSM6DSV_ERROR; } rawAcceleration[0] = (intAcceleration[0] / mgPerG); rawAcceleration[1] = (intAcceleration[1] / mgPerG); @@ -445,15 +441,15 @@ LSM6DSV16XStatusTypeDef LSM6DSVSensor::readFifo(uint16_t fifo_samples) { break; } - case lsm6dsv16x_fifo_out_raw_t::LSM6DSV16X_SFLP_GRAVITY_VECTOR_TAG: { + case lsm6dsv_fifo_out_raw_t::LSM6DSV_SFLP_GRAVITY_VECTOR_TAG: { float gravityVector[3]; - if (imu.FIFO_Get_Gravity_Vector(gravityVector) != LSM6DSV16X_OK) { + if (imu.FIFO_Get_Gravity_Vector(gravityVector) != LSM6DSV_OK) { m_Logger.error( "Failed to get gravity vector on %s at address 0x%02x", getIMUNameByType(sensorType), addr ); - return LSM6DSV16X_ERROR; + return LSM6DSV_ERROR; } gravityVector[0] /= mgPerG; gravityVector[1] /= mgPerG; @@ -465,15 +461,15 @@ LSM6DSV16XStatusTypeDef LSM6DSVSensor::readFifo(uint16_t fifo_samples) { } #if (LSM6DSV_FUSION_SOURCE == LSM6DSV_FUSION_ESP) - case lsm6dsv16x_fifo_out_raw_t::LSM6DSV16X_GY_NC_TAG: { + case lsm6dsv_fifo_out_raw_t::LSM6DSV_GY_NC_TAG: { int32_t angularVelocity[3]; - if (imu.FIFO_Get_G_Axes(angularVelocity) != LSM6DSV16X_OK) { + if (imu.FIFO_Get_G_Axes(angularVelocity) != LSM6DSV_OK) { m_Logger.error( "Failed to get accelerometer data on %s at address 0x%02x", getIMUNameByType(sensorType), addr ); - return LSM6DSV16X_ERROR; + return LSM6DSV_ERROR; } rawGyro[0] = (float)angularVelocity[0] / mdpsPerDps; @@ -502,15 +498,15 @@ LSM6DSV16XStatusTypeDef LSM6DSVSensor::readFifo(uint16_t fifo_samples) { #endif #if (LSM6DSV_FUSION_SOURCE == LSM6DSV_FUSION_ONBOARD) - case lsm6dsv16x_fifo_out_raw_t::LSM6DSV16X_SFLP_GAME_ROTATION_VECTOR_TAG: { + case lsm6dsv_fifo_out_raw_t::LSM6DSV_SFLP_GAME_ROTATION_VECTOR_TAG: { float fusedGameRotation[3]; - if (imu.FIFO_Get_Game_Vector(fusedGameRotation) != LSM6DSV16X_OK) { + if (imu.FIFO_Get_Game_Vector(fusedGameRotation) != LSM6DSV_OK) { m_Logger.error( "Failed to get game rotation vector on %s at address 0x%02x", getIMUNameByType(sensorType), addr ); - return LSM6DSV16X_ERROR; + return LSM6DSV_ERROR; } fusedRotation = fusedRotationToQuaternion( @@ -531,40 +527,40 @@ LSM6DSV16XStatusTypeDef LSM6DSVSensor::readFifo(uint16_t fifo_samples) { default: { // We don't use the data so remove from fifo uint8_t data[6]; - if (imu.FIFO_Get_Data(data) != LSM6DSV16X_OK) { + if (imu.FIFO_Get_Data(data) != LSM6DSV_OK) { m_Logger.error( "Failed to get unwanted data on %s at address 0x%02x", getIMUNameByType(sensorType), addr ); - return LSM6DSV16X_ERROR; + return LSM6DSV_ERROR; } } } } - return LSM6DSV16X_OK; + return LSM6DSV_OK; } #if (LSM6DSV_FUSION_SOURCE == LSM6DSV_FUSION_ESP) // Used for calibration (Blocking) -LSM6DSV16XStatusTypeDef LSM6DSVSensor::readNextFifoFrame() { +LSM6DSVStatusTypeDef LSM6DSVSensor::readNextFifoFrame() { uint16_t fifo_samples = 0; while (fifo_samples < fifoFramSize) { - if (imu.FIFO_Get_Num_Samples(&fifo_samples) == LSM6DSV16X_ERROR) { + if (imu.FIFO_Get_Num_Samples(&fifo_samples) == LSM6DSV_ERROR) { m_Logger.error( "Error getting number of samples in FIFO on %s at address 0x%02x", getIMUNameByType(sensorType), addr ); - return LSM6DSV16X_ERROR; + return LSM6DSV_ERROR; } } return readFifo(fifoFramSize); } -LSM6DSV16XStatusTypeDef LSM6DSVSensor::loadIMUCalibration() { +LSM6DSVStatusTypeDef LSM6DSVSensor::loadIMUCalibration() { SlimeVR::Configuration::CalibrationConfig sensorCalibration = configuration.getCalibration(sensorId); // If no compatible calibration data is found, the calibration data will just be zero-ed out switch (sensorCalibration.type) { @@ -590,9 +586,9 @@ LSM6DSV16XStatusTypeDef LSM6DSVSensor::loadIMUCalibration() { m_Calibration.A_off[2] ); status |= imu.Enable_X_User_Offset(); - return (LSM6DSV16XStatusTypeDef)status; + return (LSM6DSVStatusTypeDef)status; #endif - return LSM6DSV16X_OK; + return LSM6DSV_OK; } #ifdef LSM6DSV_ACCEL_OFFSET_CAL @@ -637,13 +633,13 @@ void LSM6DSVSensor::calibrateAccel() { imu.FIFO_Reset(); while (true) { uint16_t fifo_samples = 0; - if (imu.FIFO_Get_Num_Samples(&fifo_samples) == LSM6DSV16X_ERROR) { + if (imu.FIFO_Get_Num_Samples(&fifo_samples) == LSM6DSV_ERROR) { m_Logger.error( "Error getting number of samples in FIFO on %s at address 0x%02x", getIMUNameByType(sensorType), addr ); - status = LSM6DSV16X_ERROR; + status = LSM6DSV_ERROR; return; } @@ -662,7 +658,7 @@ void LSM6DSVSensor::calibrateAccel() { sfusion.updateAcc( rawAcceleration, - lsm6dsv16x_from_lsb_to_nsec(currentDataTime - previousDataTime) * 1e-9 + lsm6dsv_from_lsb_to_nsec(currentDataTime - previousDataTime) * 1e-9 ); newRawAcceleration = false; @@ -821,7 +817,7 @@ void LSM6DSVSensor::calibrateGyroSensitivity() { m_Calibration.G_sensitivity[1] = 1.0f; m_Calibration.G_sensitivity[2] = 1.0f; - imu.Enable_6D_Orientation(LSM6DSV16X_INT2_PIN); + imu.Enable_6D_Orientation(LSM6DSV_INT2_PIN); Vector3 rawRotationInit; Vector3 rawRotationFinal; uint8_t count = 0; @@ -1018,7 +1014,7 @@ void LSM6DSVSensor::apply6DToRestDetection() { sfusion.update6D( rawAcceleration, rawGyro, - lsm6dsv16x_from_lsb_to_nsec(currentDataTime - previousDataTime) * 1e-9 //seconds + lsm6dsv_from_lsb_to_nsec(currentDataTime - previousDataTime) * 1e-9 //seconds ); } newRawAcceleration = false; diff --git a/src/sensors/lsm6dsvsensor.h b/src/sensors/lsm6dsvSensor.h similarity index 94% rename from src/sensors/lsm6dsvsensor.h rename to src/sensors/lsm6dsvSensor.h index 1bdf68cbf..517ff86a0 100644 --- a/src/sensors/lsm6dsvsensor.h +++ b/src/sensors/lsm6dsvSensor.h @@ -24,7 +24,7 @@ #ifndef SENSORS_LSM6DSV_H #define SENSORS_LSM6DSV_H -#include "LSM6DSV16X.h" +#include "LSM6DSV.h" #include "sensor.h" #include "SensorFusion.h" #include "magneto1.4.h" @@ -123,10 +123,10 @@ class LSM6DSVSensor : public Sensor { private: Quat fusedRotationToQuaternion(float x, float y, float z); - LSM6DSV16XStatusTypeDef runSelfTest(); - LSM6DSV16XStatusTypeDef readFifo(uint16_t fifo_samples); + LSM6DSVStatusTypeDef runSelfTest(); + LSM6DSVStatusTypeDef readFifo(uint16_t fifo_samples); - LSM6DSV16X imu; + LSM6DSV imu; uint8_t m_IntPin; uint8_t tap = 0; int8_t status = 0; @@ -140,8 +140,8 @@ class LSM6DSVSensor : public Sensor { uint32_t currentDataTime = 0; #if (LSM6DSV_FUSION_SOURCE == LSM6DSV_FUSION_ESP) - LSM6DSV16XStatusTypeDef readNextFifoFrame(); - LSM6DSV16XStatusTypeDef loadIMUCalibration(); + LSM6DSVStatusTypeDef readNextFifoFrame(); + LSM6DSVStatusTypeDef loadIMUCalibration(); void apply6DToRestDetection(); void waitForRest(); void waitForMovement(); From d2881c86e51689894d0cb3df16842bb178800fa7 Mon Sep 17 00:00:00 2001 From: wigwagwent Date: Sun, 29 Oct 2023 12:03:18 -0700 Subject: [PATCH 45/60] Update gyro sens cal --- src/sensors/lsm6dsvSensor.cpp | 180 +++++++++++++++++++++------------- src/sensors/lsm6dsvSensor.h | 2 +- 2 files changed, 113 insertions(+), 69 deletions(-) diff --git a/src/sensors/lsm6dsvSensor.cpp b/src/sensors/lsm6dsvSensor.cpp index d48ce67e7..94d47ad35 100644 --- a/src/sensors/lsm6dsvSensor.cpp +++ b/src/sensors/lsm6dsvSensor.cpp @@ -804,7 +804,7 @@ void LSM6DSVSensor::calibrateGyro() { #endif -#ifdef LSM6DSV_GYRO_SENSITIVITY_CAL //Redo based on the bmi implementation +#ifdef LSM6DSV_GYRO_SENSITIVITY_CAL void LSM6DSVSensor::calibrateGyroSensitivity() { m_Logger.info( "Gyro sensitivity calibration started on sensor #%d of type %s at address 0x%02x", @@ -812,115 +812,182 @@ void LSM6DSVSensor::calibrateGyroSensitivity() { getIMUNameByType(sensorType), addr ); + float oldSensCal[3]; + oldSensCal[0] = m_Calibration.G_sensitivity[0]; + oldSensCal[1] = m_Calibration.G_sensitivity[1]; + oldSensCal[2] = m_Calibration.G_sensitivity[2]; m_Calibration.G_sensitivity[0] = 1.0f; m_Calibration.G_sensitivity[1] = 1.0f; m_Calibration.G_sensitivity[2] = 1.0f; imu.Enable_6D_Orientation(LSM6DSV_INT2_PIN); - Vector3 rawRotationInit; + + Quat rawRotationInit; Vector3 rawRotationFinal; uint8_t count = 0; float gyroCount[3]; //Use this to determine the axis spun float calculatedScale[3] = {1.0f, 1.0f, 1.0f}; + unsigned long prevLedTime = millis(); + constexpr uint16_t ledFlashDuration = 600; + ledManager.off(); m_Logger.info(""); m_Logger.info( - "\tStep 1: Move the tracker to a corner or edge that you can get it in the " + " Step 0: Let the tracker sit, the light will flash when you should reorient the tracker" + ); + m_Logger.info( + " Step 1: Move the tracker to a corner or edge that aligns the tracker to the " "same position every time" ); - m_Logger.info("\tStep 2: Let the tracker rest until the light turns on"); m_Logger.info( - "\tStep 3: Rotate the tracker about one axis %d full rotations and align with the " - "previous edge.", + " NOTE: You might also want to unplug the USB so it doesn't affect spins" + ); + m_Logger.info( + " Step 2: Let the tracker rest until the solid light turns on, you might need to hold it" + ); + m_Logger.info( + " against a wall depending on the case and orientation" + ); + m_Logger.info( + " Step 3: Rotate the tracker in the yaw axis for %d full rotations and align " + "it with the previous edge ", LSM6DSV_GYRO_SENSITIVITY_SPINS ); - m_Logger.info("\t\tNOTE: the light will turn off after you start moving it"); + m_Logger.info( + " NOTE: The yaw axis is the direction of looking left or right with your " + "head, perpendicular to gravity" + ); + m_Logger.info(" NOTE: The light will turn off after you start moving it"); m_Logger.info( - "\tStep 4: Repeat step 1 - 3 in the other 2 axis. When the light is on you should move it" + " Step 4: Wait for the flashing light then rotate the tracker 90 degrees " + " to a new axis and " ); + m_Logger.info(" align with an edge. Repeat steps 2 and 3"); + + m_Logger.info( + " Step 5: Wait for the flashing light then rotate the tracker 90 degrees so the last " + "axis is up and" + ); + m_Logger.info(" aligned with an edge. Repeat steps 2 and 3"); + + imu.FIFO_Reset(); + delayMicroseconds(100); + while (!sfusion.getRestDetected()) //Wait for rest + { + readNextFifoFrame(); + } + - waitForRest(); while (count < 3) { ledManager.on(); - waitForMovement(); //position tracker + prevLedTime = millis(); + m_Logger.info("Move the tracker to a new axis then let sit"); + while (sfusion.getRestDetected()) + { + unsigned long now = millis(); + if (now - ledFlashDuration > prevLedTime) { + //ledManager.toggle(); + ledManager.on(); + prevLedTime = millis(); + } + if (sfusion.getRestDetected() && sfusion.isUpdated()) { + rawRotationInit = sfusion.getQuaternionQuat(); + sfusion.clearUpdated(); + } + readNextFifoFrame(); + } ledManager.off(); - waitForRest(); + while (!sfusion.getRestDetected()) + { + readNextFifoFrame(); + } ledManager.on(); // The user should rotate m_Logger.info("Rotate the tracker %d times", LSM6DSV_GYRO_SENSITIVITY_SPINS); gyroCount[0] = 0.0f; gyroCount[1] = 0.0f; gyroCount[2] = 0.0f; - rawRotationInit = sfusion.getQuaternionQuat().get_euler() * dpsPerRad; - waitForMovement(); + rawRotationInit = sfusion.getQuaternionQuat(); + while (sfusion.getRestDetected()) + { + if (sfusion.getRestDetected() && sfusion.isUpdated()) { + rawRotationInit = sfusion.getQuaternionQuat(); + sfusion.clearUpdated(); + } + readNextFifoFrame(); + } ledManager.off(); - - - while (!sfusion.getRestDetected()) { //wait for rest + while (!sfusion.getRestDetected()) { readNextFifoFrame(); gyroCount[0] += rawGyro[0]; gyroCount[1] += rawGyro[1]; gyroCount[2] += rawGyro[2]; - apply6DToRestDetection(); } - uint8_t isUp; - rawRotationFinal = sfusion.getQuaternionQuat().get_euler() * dpsPerRad; + rawRotationFinal = (sfusion.getQuaternionQuat() * rawRotationInit.inverse()).get_euler() * dpsPerRad; if (abs(gyroCount[0]) > abs(gyroCount[1]) && abs(gyroCount[0]) > abs(gyroCount[2])) { //Spun in X imu.Get_6D_Orientation_XH(&isUp); if((!isUp && gyroCount[0] > 0) || (isUp && gyroCount[0] < 0)) { - calculatedScale[0] = (1.0 / (1.0 - ((rawRotationInit.y - rawRotationFinal.y)/(360.0f * LSM6DSV_GYRO_SENSITIVITY_SPINS)))); - m_Logger.info("X, Diff: %f", rawRotationInit.y - rawRotationFinal.y); + calculatedScale[0] = (1.0 / (1.0 - ((-rawRotationFinal.y)/(360.0f * LSM6DSV_GYRO_SENSITIVITY_SPINS)))); + m_Logger.info("X, Diff: %f", -rawRotationFinal.y); } else { - calculatedScale[0] = (1.0 / (1.0 - ((rawRotationFinal.y - rawRotationInit.y)/(360.0f * LSM6DSV_GYRO_SENSITIVITY_SPINS)))); - m_Logger.info("-X, Diff: %f", rawRotationFinal.y - rawRotationInit.y); + calculatedScale[0] = (1.0 / (1.0 - ((rawRotationFinal.y)/(360.0f * LSM6DSV_GYRO_SENSITIVITY_SPINS)))); + m_Logger.info("-X, Diff: %f", rawRotationFinal.y); } } else if (abs(gyroCount[1]) > abs(gyroCount[0]) && abs(gyroCount[1]) > abs(gyroCount[2])) { //Spun in Y imu.Get_6D_Orientation_YH(&isUp); if((isUp && gyroCount[1] > 0) || (!isUp && gyroCount[1] < 0)) { - calculatedScale[1] = (1.0 / (1.0 - ((rawRotationInit.y - rawRotationFinal.y)/(360.0f * LSM6DSV_GYRO_SENSITIVITY_SPINS)))); - m_Logger.info("Y, Diff: %f", rawRotationInit.y - rawRotationFinal.y); + calculatedScale[1] = (1.0 / (1.0 - ((-rawRotationFinal.y)/(360.0f * LSM6DSV_GYRO_SENSITIVITY_SPINS)))); + m_Logger.info("Y, Diff: %f", -rawRotationFinal.y); } else { - calculatedScale[1] = (1.0 / (1.0 - ((rawRotationFinal.y - rawRotationInit.y)/(360.0f * LSM6DSV_GYRO_SENSITIVITY_SPINS)))); - m_Logger.info("-Y, Diff: %f", rawRotationFinal.y - rawRotationInit.y); + calculatedScale[1] = (1.0 / (1.0 - ((rawRotationFinal.y)/(360.0f * LSM6DSV_GYRO_SENSITIVITY_SPINS)))); + m_Logger.info("-Y, Diff: %f", rawRotationFinal.y); } } else if (abs(gyroCount[2]) > abs(gyroCount[0]) && abs(gyroCount[2]) > abs(gyroCount[1])) { //Spun in Z imu.Get_6D_Orientation_ZH(&isUp); if((isUp && gyroCount[2] > 0) || (!isUp && gyroCount[2] < 0)) { - calculatedScale[2] = (1.0 / (1.0 - ((rawRotationInit.y - rawRotationFinal.y)/(360.0f * LSM6DSV_GYRO_SENSITIVITY_SPINS)))); - m_Logger.info("Z, Diff: %f", rawRotationInit.y - rawRotationFinal.y); + calculatedScale[2] = (1.0 / (1.0 - ((-rawRotationFinal.y)/(360.0f * LSM6DSV_GYRO_SENSITIVITY_SPINS)))); + m_Logger.info("Z, Diff: %f", -rawRotationFinal.y); } else { - calculatedScale[2] = (1.0 / (1.0 - ((rawRotationFinal.y - rawRotationInit.y)/(360.0f * LSM6DSV_GYRO_SENSITIVITY_SPINS)))); - m_Logger.info("-Z, Diff: %f", rawRotationFinal.y - rawRotationInit.y); + calculatedScale[2] = (1.0 / (1.0 - ((rawRotationInit.y)/(360.0f * LSM6DSV_GYRO_SENSITIVITY_SPINS)))); + m_Logger.info("-Z, Diff: %f", rawRotationInit.y); } } count++; } - m_Calibration.G_sensitivity[0] = calculatedScale[0]; - m_Calibration.G_sensitivity[1] = calculatedScale[1]; - m_Calibration.G_sensitivity[2] = calculatedScale[2]; - saveCalibration(); - m_Logger.debug( - "Gyro Sensitivity calibration results: %f %f %f", - calculatedScale[0], - calculatedScale[1], - calculatedScale[2] - ); + if (calculatedScale[0] != 1.0f) { + m_Calibration.G_sensitivity[0] = calculatedScale[0]; + } else { + m_Calibration.G_sensitivity[0] = oldSensCal[0]; + } + if (calculatedScale[1] != 1.0f) { + m_Calibration.G_sensitivity[1] = calculatedScale[1]; + } else { + m_Calibration.G_sensitivity[1] = oldSensCal[1]; + } + + if (calculatedScale[2] != 1.0f) { + m_Calibration.G_sensitivity[2] = calculatedScale[2]; + } else { + m_Calibration.G_sensitivity[2] = oldSensCal[2]; + } + + saveCalibration(); imu.Disable_6D_Orientation(); lastData = millis(); @@ -928,13 +995,13 @@ void LSM6DSVSensor::calibrateGyroSensitivity() { #ifdef DEBUG_SENSOR m_Logger.trace( "Gyro Sensitivity calibration results: %f %f %f", - calculatedScale[0], - calculatedScale[1], - calculatedScale[2] + m_Calibration.G_sensitivity[0], + m_Calibration.G_sensitivity[1], + m_Calibration.G_sensitivity[2] ); #endif } -#endif +#endif // LSM6DSV_GYRO_SENSITIVITY_CAL void LSM6DSVSensor::startCalibration(int calibrationType) { m_Logger.info("Flip right side up in the next 5 seconds to start calibration."); @@ -1009,27 +1076,4 @@ void LSM6DSVSensor::saveCalibration() { configuration.save(); } -void LSM6DSVSensor::apply6DToRestDetection() { - if (newRawGyro && newRawAcceleration) { - sfusion.update6D( - rawAcceleration, - rawGyro, - lsm6dsv_from_lsb_to_nsec(currentDataTime - previousDataTime) * 1e-9 //seconds - ); - } - newRawAcceleration = false; -} - -void LSM6DSVSensor::waitForRest() { - while (!sfusion.getRestDetected()) { - readNextFifoFrame(); - apply6DToRestDetection(); - } -} -void LSM6DSVSensor::waitForMovement() { - while (sfusion.getRestDetected()) { - readNextFifoFrame(); - apply6DToRestDetection(); - } -} #endif // (LSM6DSV_FUSION_SOURCE == LSM6DSV_FUSION_ESP) diff --git a/src/sensors/lsm6dsvSensor.h b/src/sensors/lsm6dsvSensor.h index 517ff86a0..fed4a07a0 100644 --- a/src/sensors/lsm6dsvSensor.h +++ b/src/sensors/lsm6dsvSensor.h @@ -69,7 +69,7 @@ -#define LSM6DSV_INTERRUPT //recommended for tap detect +#define LSM6DSV_INTERRUPT //recommended not required // #define LSM6DSV_NO_SELF_TEST_ON_FACEDOWN From 4c6d8dfd85682ff2b5498306f05a3dc8dc7a5873 Mon Sep 17 00:00:00 2001 From: wigwagwent Date: Sun, 29 Oct 2023 12:10:04 -0700 Subject: [PATCH 46/60] Change defualt to lsm fusion, and fix build --- src/sensors/lsm6dsvSensor.cpp | 5 ++++- src/sensors/lsm6dsvSensor.h | 5 +---- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/sensors/lsm6dsvSensor.cpp b/src/sensors/lsm6dsvSensor.cpp index 94d47ad35..410e2c899 100644 --- a/src/sensors/lsm6dsvSensor.cpp +++ b/src/sensors/lsm6dsvSensor.cpp @@ -769,7 +769,10 @@ void LSM6DSVSensor::calibrateGyro() { delay(2000); imu.FIFO_Reset(); - waitForRest(); + while (!sfusion.getRestDetected()) //Wait for rest + { + readNextFifoFrame(); + } uint16_t count = 0; while (count < calibrationSamples) { readNextFifoFrame(); diff --git a/src/sensors/lsm6dsvSensor.h b/src/sensors/lsm6dsvSensor.h index fed4a07a0..c51320e89 100644 --- a/src/sensors/lsm6dsvSensor.h +++ b/src/sensors/lsm6dsvSensor.h @@ -77,7 +77,7 @@ #define LSM6DSV_FUSION_ESP 0 #define LSM6DSV_FUSION_ONBOARD 1 -#define LSM6DSV_FUSION_SOURCE LSM6DSV_FUSION_ESP +#define LSM6DSV_FUSION_SOURCE LSM6DSV_FUSION_ONBOARD @@ -142,9 +142,6 @@ class LSM6DSVSensor : public Sensor { #if (LSM6DSV_FUSION_SOURCE == LSM6DSV_FUSION_ESP) LSM6DSVStatusTypeDef readNextFifoFrame(); LSM6DSVStatusTypeDef loadIMUCalibration(); - void apply6DToRestDetection(); - void waitForRest(); - void waitForMovement(); void saveCalibration(); SlimeVR::Configuration::LSM6DSVCalibrationConfig m_Calibration; SlimeVR::Sensors::SensorFusionRestDetect sfusion; From 7642ac43b3f88d9e212d09e2b5e7575356402ce9 Mon Sep 17 00:00:00 2001 From: wigwagwent Date: Sat, 4 Nov 2023 10:42:34 -0700 Subject: [PATCH 47/60] Rename last folder --- lib/{LSM6DSV16X => LSM6DSV}/LSM6DSV.cpp | 0 lib/{LSM6DSV16X => LSM6DSV}/LSM6DSV.h | 0 lib/{LSM6DSV16X => LSM6DSV}/lsm6dsv_reg.c | 0 lib/{LSM6DSV16X => LSM6DSV}/lsm6dsv_reg.h | 0 4 files changed, 0 insertions(+), 0 deletions(-) rename lib/{LSM6DSV16X => LSM6DSV}/LSM6DSV.cpp (100%) rename lib/{LSM6DSV16X => LSM6DSV}/LSM6DSV.h (100%) rename lib/{LSM6DSV16X => LSM6DSV}/lsm6dsv_reg.c (100%) rename lib/{LSM6DSV16X => LSM6DSV}/lsm6dsv_reg.h (100%) diff --git a/lib/LSM6DSV16X/LSM6DSV.cpp b/lib/LSM6DSV/LSM6DSV.cpp similarity index 100% rename from lib/LSM6DSV16X/LSM6DSV.cpp rename to lib/LSM6DSV/LSM6DSV.cpp diff --git a/lib/LSM6DSV16X/LSM6DSV.h b/lib/LSM6DSV/LSM6DSV.h similarity index 100% rename from lib/LSM6DSV16X/LSM6DSV.h rename to lib/LSM6DSV/LSM6DSV.h diff --git a/lib/LSM6DSV16X/lsm6dsv_reg.c b/lib/LSM6DSV/lsm6dsv_reg.c similarity index 100% rename from lib/LSM6DSV16X/lsm6dsv_reg.c rename to lib/LSM6DSV/lsm6dsv_reg.c diff --git a/lib/LSM6DSV16X/lsm6dsv_reg.h b/lib/LSM6DSV/lsm6dsv_reg.h similarity index 100% rename from lib/LSM6DSV16X/lsm6dsv_reg.h rename to lib/LSM6DSV/lsm6dsv_reg.h From c02bcee493e6be69e5db856a7279a178b184e50c Mon Sep 17 00:00:00 2001 From: wigwagwent Date: Sat, 4 Nov 2023 13:24:35 -0700 Subject: [PATCH 48/60] ESP fusion is weird again --- lib/LSM6DSV/LSM6DSV.h | 3 - lib/math/customConversions.cpp | 45 ------ lib/math/customConversions.h | 43 ------ src/sensors/lsm6dsvSensor.cpp | 244 +++++++++++++-------------------- src/sensors/lsm6dsvSensor.h | 6 +- 5 files changed, 102 insertions(+), 239 deletions(-) delete mode 100644 lib/math/customConversions.cpp delete mode 100644 lib/math/customConversions.h diff --git a/lib/LSM6DSV/LSM6DSV.h b/lib/LSM6DSV/LSM6DSV.h index 5cab610e0..0b5cf200e 100644 --- a/lib/LSM6DSV/LSM6DSV.h +++ b/lib/LSM6DSV/LSM6DSV.h @@ -68,8 +68,6 @@ #define LSM6DSV_MIN_ST_LIMIT_mdps 150000.0f #define LSM6DSV_MAX_ST_LIMIT_mdps 700000.0f -#define LSM6DSV_QVAR_GAIN 78.000f - #define LSM6DSV_ACC_USR_OFF_W_HIGH_LSB (float)(pow(2, -6)) #define LSM6DSV_ACC_USR_OFF_W_LOW_LSB (float)(pow(2, -10)) #define LSM6DSV_ACC_USR_OFF_W_HIGH_MAX LSM6DSV_ACC_USR_OFF_W_HIGH_LSB * INT8_MAX @@ -148,7 +146,6 @@ class LSM6DSV { LSM6DSV(SPIClass *spi, int cs_pin, uint32_t spi_speed = 2000000); LSM6DSVStatusTypeDef begin(); - LSM6DSVStatusTypeDef beginPreconfigured(); LSM6DSVStatusTypeDef end(); LSM6DSVStatusTypeDef ReadID(uint8_t *Id); diff --git a/lib/math/customConversions.cpp b/lib/math/customConversions.cpp deleted file mode 100644 index 925e6e11b..000000000 --- a/lib/math/customConversions.cpp +++ /dev/null @@ -1,45 +0,0 @@ -/* - SlimeVR Code is placed under the MIT license - Copyright (c) 2023 Eiren Rain & SlimeVR contributors - - Permission is hereby granted, free of charge, to any person obtaining a copy - of this software and associated documentation files (the "Software"), to deal - in the Software without restriction, including without limitation the rights - to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - copies of the Software, and to permit persons to whom the Software is - furnished to do so, subject to the following conditions: - - The above copyright notice and this permission notice shall be included in - all copies or substantial portions of the Software. - - THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - THE SOFTWARE. -*/ - -#include "customConversions.h" - -//Convert 2 bytes in f16 format to a float -//f16: SEEE EEDD DDDD DDDD -//f32: SEEE EEEE EDDD DDDD DDDD DDDD DDDD DDDD -float Conversions::convertBytesToFloat(uint8_t dataLow, uint8_t dataHigh) { - uint16_t combinedData = ((uint16_t)dataHigh << 8 | dataLow); - uint32_t dataHolder = ((uint32_t)(combinedData & DATA_MASK_F16) << 13); - dataHolder |= ((uint32_t)(combinedData & SIGN_BIT_F16) << 16); - uint8_t exponent = ((combinedData & EXPONENT_MASK_F16) >> 10); - if (!exponent) //subnormal - return 0.0F; - if (exponent == 0b00011111) //nan - exponent == 0xff; - else - exponent += 127 - 15; - dataHolder |= (((uint32_t)exponent << 23)); - float dataFloat = 0.0F; - memcpy(&dataFloat, &dataHolder, 4); - // float dataFloat = reinterpret_cast(dataHolder); - return dataFloat; -} \ No newline at end of file diff --git a/lib/math/customConversions.h b/lib/math/customConversions.h deleted file mode 100644 index 9eae2b7ef..000000000 --- a/lib/math/customConversions.h +++ /dev/null @@ -1,43 +0,0 @@ -/* - SlimeVR Code is placed under the MIT license - Copyright (c) 2023 Eiren Rain & SlimeVR contributors - - Permission is hereby granted, free of charge, to any person obtaining a copy - of this software and associated documentation files (the "Software"), to deal - in the Software without restriction, including without limitation the rights - to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - copies of the Software, and to permit persons to whom the Software is - furnished to do so, subject to the following conditions: - - The above copyright notice and this permission notice shall be included in - all copies or substantial portions of the Software. - - THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - THE SOFTWARE. -*/ - -#include "Arduino.h" - -#define SIGN_BIT_F16 0x8000 -#define EXPONENT_MASK_F16 0x7C00 -#define DATA_MASK_F16 0x03FF - - -#ifndef _CUSTOM_CONVERSIONS_H_ -#define _CUSTOM_CONVERSIONS_H_ - - - -namespace Conversions { - float convertBytesToFloat(uint8_t dataLow, uint8_t dataHigh); -} - - - -#endif - diff --git a/src/sensors/lsm6dsvSensor.cpp b/src/sensors/lsm6dsvSensor.cpp index 410e2c899..69a0ebfb6 100644 --- a/src/sensors/lsm6dsvSensor.cpp +++ b/src/sensors/lsm6dsvSensor.cpp @@ -22,9 +22,7 @@ */ #include "sensors/lsm6dsvSensor.h" - #include "GlobalVars.h" -#include "customConversions.h" #include "lsm6dsvSensor.h" #include "utils.h" @@ -153,13 +151,23 @@ void LSM6DSVSensor::motionSetup() { #if (LSM6DSV_FUSION_SOURCE == LSM6DSV_FUSION_ESP) - loadIMUCalibration(); - // Calibration if (isFaceDown) { startCalibration(0); // can not calibrate onboard fusion + } else { + loadIMUCalibration(); } + +#ifdef LSM6DSV_ACCEL_OFFSET_CAL + int8_t status = 0; + status |= imu.Set_X_User_Offset( + m_Calibration.A_off[0], + m_Calibration.A_off[1], + m_Calibration.A_off[2] + ); + status |= imu.Enable_X_User_Offset(); #endif +#endif //LSM6DSV_FUSION_SOURCE == LSM6DSV_FUSION_ESP status |= imu.Disable_6D_Orientation(); @@ -191,6 +199,7 @@ void LSM6DSVSensor::motionSetup() { working = true; configured = true; lastTempRead = millis(); + printCalibration(); } constexpr float mgPerG = 1000.0f; @@ -557,7 +566,16 @@ LSM6DSVStatusTypeDef LSM6DSVSensor::readNextFifoFrame() { return LSM6DSV_ERROR; } } - return readFifo(fifoFramSize); + LSM6DSVStatusTypeDef result = readFifo(fifoFramSize); + + if (newRawAcceleration && newRawGyro) { + sfusion.update6D( + rawAcceleration, + rawGyro, + (lsm6dsv_from_lsb_to_nsec(currentDataTime - previousDataTime) * 1e-9) + ); + } + return result; } LSM6DSVStatusTypeDef LSM6DSVSensor::loadIMUCalibration() { @@ -577,19 +595,59 @@ LSM6DSVStatusTypeDef LSM6DSVSensor::loadIMUCalibration() { m_Logger.warn("Incompatible calibration data found for sensor %d, ignoring...", sensorId); m_Logger.info("Calibration is advised"); } + return LSM6DSV_OK; +} -#ifdef LSM6DSV_ACCEL_OFFSET_CAL - int8_t status = 0; - status |= imu.Set_X_User_Offset( - m_Calibration.A_off[0], - m_Calibration.A_off[1], - m_Calibration.A_off[2] +#ifdef LSM6DSV_GYRO_OFFSET_CAL +void LSM6DSVSensor::calibrateGyro() { + m_Logger.info( + "Gyro offset calibration started on sensor #%d of type %s at address 0x%02x", + getSensorId(), + getIMUNameByType(sensorType), + addr ); - status |= imu.Enable_X_User_Offset(); - return (LSM6DSVStatusTypeDef)status; -#endif - return LSM6DSV_OK; + ledManager.on(); + constexpr uint16_t calibrationSamples = 300; + // Reset values + float tempGxyz[3] = {0, 0, 0}; + + m_Calibration.G_off[0] = 0.0f; + m_Calibration.G_off[1] = 0.0f; + m_Calibration.G_off[2] = 0.0f; + + + // Wait for sensor to calm down before calibration + m_Logger.info("Put down the device and wait for baseline gyro reading calibration"); + delay(2000); + + imu.FIFO_Reset(); + + uint16_t count = 0; + while (count < calibrationSamples) { + printf("Gyro Sample"); + readNextFifoFrame(); + if (newRawGyro) { + tempGxyz[0] += rawGyro[0]; + tempGxyz[1] += rawGyro[1]; + tempGxyz[2] += rawGyro[2]; + count++; + } + } + ledManager.off(); + m_Calibration.G_off[0] = tempGxyz[0] / calibrationSamples; + m_Calibration.G_off[1] = tempGxyz[1] / calibrationSamples; + m_Calibration.G_off[2] = tempGxyz[2] / calibrationSamples; + +#ifdef DEBUG_SENSOR + m_Logger.trace( + "Gyro calibration results: %f %f %f", + m_Calibration.G_off[0], + m_Calibration.G_off[1], + m_Calibration.G_off[2] + ); +#endif //DEBUG_SENSOR } +#endif //LSM6DSV_GYRO_OFFSET_CAL #ifdef LSM6DSV_ACCEL_OFFSET_CAL void LSM6DSVSensor::calibrateAccel() { @@ -600,7 +658,6 @@ void LSM6DSVSensor::calibrateAccel() { addr ); MagnetoCalibration* magneto = new MagnetoCalibration(); - imu.Disable_X_User_Offset(); m_Logger.info( "Put the device into 6 unique orientations (all sides), leave it still and do " @@ -717,94 +774,18 @@ void LSM6DSVSensor::calibrateAccel() { m_Calibration.A_off[0] = A_BAinv[0][0]; m_Calibration.A_off[1] = A_BAinv[0][1]; m_Calibration.A_off[2] = A_BAinv[0][2]; - saveCalibration(); - imu.Set_X_User_Offset( - m_Calibration.A_off[0], - m_Calibration.A_off[1], - m_Calibration.A_off[2] - ); - lastData = millis(); #ifdef DEBUG_SENSOR m_Logger.trace( "Accel calibration results: %f %f %f", - A_BAinv[0][0], - A_BAinv[0][1], - A_BAinv[0][2] - ); -#endif -} -#endif - -#ifdef LSM6DSV_GYRO_OFFSET_CAL -void LSM6DSVSensor::calibrateGyro() { - m_Logger.info( - "Gyro offset calibration started on sensor #%d of type %s at address 0x%02x", - getSensorId(), - getIMUNameByType(sensorType), - addr - ); - ledManager.on(); - constexpr uint16_t calibrationSamples = 300; - // Reset values - float tempGxyz[3] = {0, 0, 0}; - - m_Calibration.G_off[0] = 0.0f; - m_Calibration.G_off[1] = 0.0f; - m_Calibration.G_off[2] = 0.0f; - - float tempGsensitivity[3]; - tempGsensitivity[0] = m_Calibration.G_sensitivity[0]; - tempGsensitivity[1] = m_Calibration.G_sensitivity[1]; - tempGsensitivity[2] = m_Calibration.G_sensitivity[2]; - - m_Calibration.G_sensitivity[0] = 1.0f; - m_Calibration.G_sensitivity[1] = 1.0f; - m_Calibration.G_sensitivity[2] = 1.0f; - - - // Wait for sensor to calm down before calibration - m_Logger.info("Put down the device and wait for baseline gyro reading calibration"); - delay(2000); - - imu.FIFO_Reset(); - while (!sfusion.getRestDetected()) //Wait for rest - { - readNextFifoFrame(); - } - uint16_t count = 0; - while (count < calibrationSamples) { - readNextFifoFrame(); - if (newRawGyro) { - tempGxyz[0] += rawGyro[0]; - tempGxyz[1] += rawGyro[1]; - tempGxyz[2] += rawGyro[2]; - count++; - } - } - ledManager.off(); - m_Calibration.G_off[0] = tempGxyz[0] / calibrationSamples; - m_Calibration.G_off[1] = tempGxyz[1] / calibrationSamples; - m_Calibration.G_off[2] = tempGxyz[2] / calibrationSamples; - - m_Calibration.G_sensitivity[0] = tempGsensitivity[0]; - m_Calibration.G_sensitivity[1] = tempGsensitivity[1]; - m_Calibration.G_sensitivity[2] = tempGsensitivity[2]; - saveCalibration(); - lastData = millis(); - - -#ifdef DEBUG_SENSOR - m_Logger.trace( - "Gyro calibration results: %f %f %f", - tempGxyz[0], - tempGxyz[1], - tempGxyz[2] + m_Calibration.A_off[0], + m_Calibration.A_off[1], + m_Calibration.A_off[2] ); -#endif +#endif //DEBUG_SENSOR } -#endif +#endif //LSM6DSV_ACCEL_OFFSET_CAL #ifdef LSM6DSV_GYRO_SENSITIVITY_CAL @@ -815,16 +796,6 @@ void LSM6DSVSensor::calibrateGyroSensitivity() { getIMUNameByType(sensorType), addr ); - float oldSensCal[3]; - oldSensCal[0] = m_Calibration.G_sensitivity[0]; - oldSensCal[1] = m_Calibration.G_sensitivity[1]; - oldSensCal[2] = m_Calibration.G_sensitivity[2]; - - m_Calibration.G_sensitivity[0] = 1.0f; - m_Calibration.G_sensitivity[1] = 1.0f; - m_Calibration.G_sensitivity[2] = 1.0f; - - imu.Enable_6D_Orientation(LSM6DSV_INT2_PIN); Quat rawRotationInit; Vector3 rawRotationFinal; @@ -883,7 +854,6 @@ void LSM6DSVSensor::calibrateGyroSensitivity() { { readNextFifoFrame(); } - while (count < 3) { ledManager.on(); @@ -972,28 +942,9 @@ void LSM6DSVSensor::calibrateGyroSensitivity() { count++; } - if (calculatedScale[0] != 1.0f) { - m_Calibration.G_sensitivity[0] = calculatedScale[0]; - } else { - m_Calibration.G_sensitivity[0] = oldSensCal[0]; - } - - if (calculatedScale[1] != 1.0f) { - m_Calibration.G_sensitivity[1] = calculatedScale[1]; - } else { - m_Calibration.G_sensitivity[1] = oldSensCal[1]; - } - - if (calculatedScale[2] != 1.0f) { - m_Calibration.G_sensitivity[2] = calculatedScale[2]; - } else { - m_Calibration.G_sensitivity[2] = oldSensCal[2]; - } - - saveCalibration(); - imu.Disable_6D_Orientation(); - - lastData = millis(); + m_Calibration.G_sensitivity[0] = calculatedScale[0]; + m_Calibration.G_sensitivity[1] = calculatedScale[1]; + m_Calibration.G_sensitivity[2] = calculatedScale[2]; #ifdef DEBUG_SENSOR m_Logger.trace( @@ -1002,7 +953,7 @@ void LSM6DSVSensor::calibrateGyroSensitivity() { m_Calibration.G_sensitivity[1], m_Calibration.G_sensitivity[2] ); -#endif +#endif //DEBUG_SENSOR } #endif // LSM6DSV_GYRO_SENSITIVITY_CAL @@ -1013,17 +964,13 @@ void LSM6DSVSensor::startCalibration(int calibrationType) { imu.Get_6D_Orientation_ZH(&isFaceUp); if (!isFaceUp) { - m_Logger.info("Flip the tracker over now for gyro sensitivity calibration"); - delay(5000); - imu.Get_6D_Orientation_ZH(&isFaceUp); - - if (!isFaceUp) { - return; - } - calibrateGyroSensitivity(); + loadIMUCalibration(); return; } + m_Calibration.G_sensitivity[0] = 1.0f; + m_Calibration.G_sensitivity[1] = 1.0f; + m_Calibration.G_sensitivity[2] = 1.0f; #ifdef LSM6DSV_GYRO_OFFSET_CAL calibrateGyro(); @@ -1032,20 +979,27 @@ void LSM6DSVSensor::startCalibration(int calibrationType) { #ifdef LSM6DSV_ACCEL_OFFSET_CAL calibrateAccel(); #endif + +#ifdef LSM6DSV_GYRO_SENSITIVITY_CAL + calibrateGyroSensitivity(); +#endif + + saveCalibration(); + m_Logger.info("Calibration finished, enjoy"); } void LSM6DSVSensor::printCalibration() { m_Logger.info("Sensor #%d Calibration Data", getSensorId()); m_Logger.info(" Accel Offset Calibration Values for %s on 0x%02x", getIMUNameByType(sensorType), addr); - m_Logger.info(" X Scale: %f", m_Calibration.A_off[0]); - m_Logger.info(" Y Scale: %f", m_Calibration.A_off[1]); - m_Logger.info(" Z Scale: %f", m_Calibration.A_off[2]); + m_Logger.info(" X Offset: %f", m_Calibration.A_off[0]); + m_Logger.info(" Y Offset: %f", m_Calibration.A_off[1]); + m_Logger.info(" Z Offset: %f", m_Calibration.A_off[2]); m_Logger.info(" Gyro Offset Calibration Values for %s on 0x%02x", getIMUNameByType(sensorType), addr); - m_Logger.info(" X Scale: %f", m_Calibration.G_off[0]); - m_Logger.info(" Y Scale: %f", m_Calibration.G_off[1]); - m_Logger.info(" Z Scale: %f", m_Calibration.G_off[2]); + m_Logger.info(" X Offset: %f", m_Calibration.G_off[0]); + m_Logger.info(" Y Offset: %f", m_Calibration.G_off[1]); + m_Logger.info(" Z Offset: %f", m_Calibration.G_off[2]); m_Logger.info(" Gyro Sensitivity Calibration Values for %s on 0x%02x", getIMUNameByType(sensorType), addr); m_Logger.info(" X Scale: %f", m_Calibration.G_sensitivity[0]); diff --git a/src/sensors/lsm6dsvSensor.h b/src/sensors/lsm6dsvSensor.h index c51320e89..2f5e6927d 100644 --- a/src/sensors/lsm6dsvSensor.h +++ b/src/sensors/lsm6dsvSensor.h @@ -88,7 +88,7 @@ #endif #ifdef LSM6DSV_GYRO_SENSITIVITY_CAL -#define LSM6DSV_GYRO_SENSITIVITY_SPINS 2 +#define LSM6DSV_GYRO_SENSITIVITY_SPINS 1 #endif @@ -114,8 +114,8 @@ class LSM6DSVSensor : public Sensor { #if (LSM6DSV_FUSION_SOURCE == LSM6DSV_FUSION_ESP) void startCalibration(int calibrationType) override final; - void calibrateAccel() override final; void calibrateGyro() override final; + void calibrateAccel() override final; void calibrateGyroSensitivity() override final; void printCalibration() override final; void resetCalibration() override final; @@ -143,7 +143,7 @@ class LSM6DSVSensor : public Sensor { LSM6DSVStatusTypeDef readNextFifoFrame(); LSM6DSVStatusTypeDef loadIMUCalibration(); void saveCalibration(); - SlimeVR::Configuration::LSM6DSVCalibrationConfig m_Calibration; + SlimeVR::Configuration::LSM6DSVCalibrationConfig m_Calibration = {}; SlimeVR::Sensors::SensorFusionRestDetect sfusion; float rawGyro[3]; bool newRawGyro = false; From 2c9722629902837a8c8a6695e66f7d311953945b Mon Sep 17 00:00:00 2001 From: wigwagwent Date: Sat, 4 Nov 2023 14:06:59 -0700 Subject: [PATCH 49/60] Fix build --- src/sensors/SensorFusionRestDetect.cpp | 5 ----- src/sensors/lsm6dsvSensor.cpp | 12 ++++++++++-- 2 files changed, 10 insertions(+), 7 deletions(-) diff --git a/src/sensors/SensorFusionRestDetect.cpp b/src/sensors/SensorFusionRestDetect.cpp index 53978b7cb..1fa959b19 100644 --- a/src/sensors/SensorFusionRestDetect.cpp +++ b/src/sensors/SensorFusionRestDetect.cpp @@ -35,10 +35,5 @@ namespace SlimeVR return vqf.getRestDetected(); #endif } - - void SensorFusionRestDetect::updateRestDetectionParameters(RestDetectionParams newParams) - { - restDetection.updateRestDetectionParameters(newParams); - } } } diff --git a/src/sensors/lsm6dsvSensor.cpp b/src/sensors/lsm6dsvSensor.cpp index 69a0ebfb6..f1045e14d 100644 --- a/src/sensors/lsm6dsvSensor.cpp +++ b/src/sensors/lsm6dsvSensor.cpp @@ -286,7 +286,11 @@ void LSM6DSVSensor::motionLoop() { } lastData = millis(); - sfusion.getLinearAcc(acceleration); + sensor_real_t linAcceleration[3]; + sfusion.getLinearAcc(linAcceleration); + acceleration.x = linAcceleration[0]; + acceleration.y = linAcceleration[1]; + acceleration.z = linAcceleration[2]; newAcceleration = true; if (newRawAcceleration && newRawGyro) { @@ -464,7 +468,11 @@ LSM6DSVStatusTypeDef LSM6DSVSensor::readFifo(uint16_t fifo_samples) { gravityVector[1] /= mgPerG; gravityVector[2] /= mgPerG; - SlimeVR::Sensors::SensorFusion::calcLinearAcc(rawAcceleration, gravityVector, acceleration); + sensor_real_t linAcceleration[3]; + SlimeVR::Sensors::SensorFusion::calcLinearAcc(rawAcceleration, gravityVector, linAcceleration); + acceleration.x = linAcceleration[0]; + acceleration.y = linAcceleration[1]; + acceleration.z = linAcceleration[2]; newAcceleration = true; break; } From e1a5bbe249e4cd70f2159ad453716d697ec92686 Mon Sep 17 00:00:00 2001 From: wigwagwent Date: Thu, 30 Nov 2023 19:26:13 -0700 Subject: [PATCH 50/60] Remove debugging code --- src/motionprocessing/RestDetection.h | 16 +---- src/sensors/SensorFusionRestDetect.cpp | 7 -- src/sensors/SensorFusionRestDetect.h | 2 - src/sensors/lsm6dsvSensor.cpp | 1 - src/sensors/sensor.cpp | 42 ------------ src/sensors/sensor.h | 33 +--------- src/serial/serialcommands.cpp | 90 -------------------------- 7 files changed, 3 insertions(+), 188 deletions(-) diff --git a/src/motionprocessing/RestDetection.h b/src/motionprocessing/RestDetection.h index 8d8ae1a9c..c0215baff 100644 --- a/src/motionprocessing/RestDetection.h +++ b/src/motionprocessing/RestDetection.h @@ -18,11 +18,11 @@ #define NaN std::numeric_limits::quiet_NaN() struct RestDetectionParams { - sensor_real_t biasClip; //dps + sensor_real_t biasClip; sensor_real_t biasSigmaRest; uint32_t restMinTimeMicros; sensor_real_t restFilterTau; - sensor_real_t restThGyr; //dps + sensor_real_t restThGyr; sensor_real_t restThAcc; RestDetectionParams(): biasClip(2.0f), @@ -50,13 +50,6 @@ class RestDetection { setup(); } - void updateRestDetectionParameters(RestDetectionParams newParams) { - params = newParams; -#ifndef REST_DETECTION_DISABLE_LPF - resetState(); -#endif // REST_DETECTION_DISABLE_LPF - } - #ifndef REST_DETECTION_DISABLE_LPF void filterInitialState(sensor_real_t x0, const double b[3], const double a[2], double out[]) { @@ -192,11 +185,6 @@ class RestDetection { #endif } - void update6D(uint32_t dtMicros, sensor_real_t acc[3], sensor_real_t gyr[3]) { - updateAcc(dtMicros, acc); - updateGyr(dtMicros, gyr); //gyro doesn't set restDection to true only false - } - bool getRestDetected() { return restDetected; } diff --git a/src/sensors/SensorFusionRestDetect.cpp b/src/sensors/SensorFusionRestDetect.cpp index 1fa959b19..db82e4829 100644 --- a/src/sensors/SensorFusionRestDetect.cpp +++ b/src/sensors/SensorFusionRestDetect.cpp @@ -18,13 +18,6 @@ namespace SlimeVR restDetection.updateGyr(deltat * 1e6, Gxyz); SensorFusion::updateGyro(Gxyz, deltat); } - - void SensorFusionRestDetect::update6D(sensor_real_t Axyz[3], sensor_real_t Gxyz[3], sensor_real_t deltat) - { - if (deltat < 0) deltat = accTs; //They need to be the same - restDetection.update6D(deltat * 1e6, Axyz, Gxyz); - SensorFusion::update6D(Axyz, Gxyz, deltat); - } #endif bool SensorFusionRestDetect::getRestDetected() diff --git a/src/sensors/SensorFusionRestDetect.h b/src/sensors/SensorFusionRestDetect.h index 2042c92d7..e38d351c0 100644 --- a/src/sensors/SensorFusionRestDetect.h +++ b/src/sensors/SensorFusionRestDetect.h @@ -41,8 +41,6 @@ namespace SlimeVR #if !SENSOR_FUSION_WITH_RESTDETECT void updateAcc(sensor_real_t Axyz[3], sensor_real_t deltat); void updateGyro(sensor_real_t Gxyz[3], sensor_real_t deltat); - void update6D(sensor_real_t Axyz[3], sensor_real_t Gxyz[3], sensor_real_t deltat); - void updateRestDetectionParameters(RestDetectionParams newParams); #endif protected: #if !SENSOR_FUSION_WITH_RESTDETECT diff --git a/src/sensors/lsm6dsvSensor.cpp b/src/sensors/lsm6dsvSensor.cpp index f1045e14d..e73bfd6ba 100644 --- a/src/sensors/lsm6dsvSensor.cpp +++ b/src/sensors/lsm6dsvSensor.cpp @@ -199,7 +199,6 @@ void LSM6DSVSensor::motionSetup() { working = true; configured = true; lastTempRead = millis(); - printCalibration(); } constexpr float mgPerG = 1000.0f; diff --git a/src/sensors/sensor.cpp b/src/sensors/sensor.cpp index 0212011b0..8ee6349b3 100644 --- a/src/sensors/sensor.cpp +++ b/src/sensors/sensor.cpp @@ -67,24 +67,6 @@ void Sensor::printDebugTemperatureCalibrationState() { printTemperatureCalibrati void Sensor::saveTemperatureCalibration() { printTemperatureCalibrationUnsupported(); }; void Sensor::resetTemperatureCalibrationState() { printTemperatureCalibrationUnsupported(); }; -void Sensor::printCalibrationUnsupported(CalibrationType type) { - m_Logger.warn( - "Calibration of type %s is not supported for IMU %s, sensor # %d, skipping", - getCalibrationNameByType(type), - getIMUNameByType(sensorType), - getSensorId() - ); -} -void Sensor::printCalibration() { printCalibrationUnsupported(CalibrationType::CALIBRATION_TYPE_ALL); }; -void Sensor::resetCalibration() { printCalibrationUnsupported(CalibrationType::CALIBRATION_TYPE_ALL); }; -void Sensor::startCalibration(int type) { printCalibrationUnsupported((CalibrationType)type); }; -void Sensor::calibrateAccel() { printCalibrationUnsupported(CalibrationType::CALIBRATION_TYPE_ACCEL); }; -void Sensor::calibrateGyro() { printCalibrationUnsupported(CalibrationType::CALIBRATION_TYPE_GYRO); }; -void Sensor::calibrateMag() { printCalibrationUnsupported(CalibrationType::CALIBRATION_TYPE_MAG); }; -void Sensor::calibrateTemp() { printCalibrationUnsupported(CalibrationType::CALIBRATION_TYPE_TEMP); }; -void Sensor::calibrateGyroSensitivity() { printCalibrationUnsupported(CalibrationType::CALIBRATION_TYPE_GYRO_SENSITIVITY); }; - - const char * getIMUNameByType(int imuType) { switch(imuType) { case IMU_MPU9250: @@ -112,27 +94,3 @@ const char * getIMUNameByType(int imuType) { } return "Unknown"; } - -const char * getCalibrationNameByType(CalibrationType calibrationType) { - switch(calibrationType) { - case CalibrationType::CALIBRATION_TYPE_NONE: - return "None"; - case CalibrationType::CALIBRATION_TYPE_ALL: - return "All"; - case CalibrationType::CALIBRATION_TYPE_ACCEL: - return "ACCEL"; - case CalibrationType::CALIBRATION_TYPE_GYRO: - return "GYRO"; - case CalibrationType::CALIBRATION_TYPE_MAG: - return "MAG"; - case CalibrationType::CALIBRATION_TYPE_6DOF: - return "6DOF"; - case CalibrationType::CALIBRATION_TYPE_9DOF: - return "9DOF"; - case CalibrationType::CALIBRATION_TYPE_TEMP: - return "Temp"; - case CalibrationType::CALIBRATION_TYPE_GYRO_SENSITIVITY: - return "Gyro Sensitivity"; - } - return "Unknown"; -} diff --git a/src/sensors/sensor.h b/src/sensors/sensor.h index f0bb1cb37..da6ce90b4 100644 --- a/src/sensors/sensor.h +++ b/src/sensors/sensor.h @@ -41,28 +41,6 @@ enum class SensorStatus : uint8_t { SENSOR_ERROR = 2 }; -enum class CalibrationType: uint8_t { - CALIBRATION_TYPE_NONE = 0, - CALIBRATION_TYPE_ALL = 1, - CALIBRATION_TYPE_ACCEL = 2, - CALIBRATION_TYPE_GYRO = 3, - CALIBRATION_TYPE_MAG = 4, - CALIBRATION_TYPE_6DOF = 5, - CALIBRATION_TYPE_9DOF = 6, - - CALIBRATION_TYPE_TEMP = 7, - CALIBRATION_TYPE_GYRO_SENSITIVITY = 8 -}; - -enum class CalibrationStatus : uint8_t { - CALIBRATION_STATUS_CALIBRATED = 0, - CALIBRATION_STATUS_FULLY_CALIBRATED = 1, //Like temp cal, not required but possible - CALIBRATION_STATUS_REQUIRED = 2, - CALIBRATION_STATUS_RECOMMENED = 3, //If temp cal is enabled then recommend - CALIBRATION_STATUS_IN_PROGRESS = 4, - CALIBRATION_STATUS_ERROR = 5 -}; - class Sensor { public: @@ -82,19 +60,12 @@ class Sensor virtual void sendData(); virtual void setAccelerationReady(); virtual void setFusedRotationReady(); - virtual void startCalibration(int calibrationType); - virtual void calibrateAccel(); - virtual void calibrateGyro(); - virtual void calibrateMag(); - virtual void calibrateTemp(); - virtual void calibrateGyroSensitivity(); + virtual void startCalibration(int calibrationType){}; virtual SensorStatus getSensorState(); virtual void printTemperatureCalibrationState(); virtual void printDebugTemperatureCalibrationState(); virtual void resetTemperatureCalibrationState(); virtual void saveTemperatureCalibration(); - virtual void printCalibration(); - virtual void resetCalibration(); bool isWorking() { return working; }; @@ -142,10 +113,8 @@ class Sensor private: void printTemperatureCalibrationUnsupported(); - void printCalibrationUnsupported(CalibrationType calibrationType); }; const char * getIMUNameByType(int imuType); -const char * getCalibrationNameByType(CalibrationType calibrationType); #endif // SLIMEVR_SENSOR_H_ diff --git a/src/serial/serialcommands.cpp b/src/serial/serialcommands.cpp index b604d4f3d..a85469502 100644 --- a/src/serial/serialcommands.cpp +++ b/src/serial/serialcommands.cpp @@ -323,102 +323,12 @@ namespace SerialCommands { logger.info(" Temperature calibration config saves automatically when calibration percent is at 100%"); } - void cmdCalibration(CmdParser* parser) { - if (parser->getParamCount() > 1) { - if (parser->equalCmdParam(1, "GET")) { - for (auto sensor : sensorManager.getSensors()) { - sensor->printCalibration(); - } - return; - } else if (parser->equalCmdParam(1, "RESET")) { - for (auto sensor : sensorManager.getSensors()) { - if (parser->getParamCount() > 2) { - if (sensor->getSensorId() == std::atoi(parser->getCmdParam(2))) { - sensor->resetCalibration(); - return; - } - } else { - - sensor->resetCalibration(); - } - } - if (parser->getParamCount() > 2) { - logger.warn("Sensor with Id %d does not exist, nothing reset", std::atoi(parser->getCmdParam(2))); - } - return; - } else if (parser->equalCmdParam(1, "START") && parser->getParamCount() > 3) { - for (auto sensor : sensorManager.getSensors()) { - if (sensor->getSensorId() == std::atoi(parser->getCmdParam(2))) { - switch ((CalibrationType)std::atoi(parser->getCmdParam(3))) - { - case CalibrationType::CALIBRATION_TYPE_NONE: - return; - case CalibrationType::CALIBRATION_TYPE_ALL: - sensor->calibrateGyro(); - sensor->calibrateAccel(); - sensor->calibrateMag(); - sensor->calibrateTemp(); - sensor->calibrateGyroSensitivity(); - return; - case CalibrationType::CALIBRATION_TYPE_ACCEL: - sensor->calibrateAccel(); - return; - case CalibrationType::CALIBRATION_TYPE_GYRO: - sensor->calibrateGyro(); - return; - case CalibrationType::CALIBRATION_TYPE_MAG: - sensor->calibrateMag(); - return; - case CalibrationType::CALIBRATION_TYPE_6DOF: - sensor->calibrateGyro(); - sensor->calibrateAccel(); - return; - case CalibrationType::CALIBRATION_TYPE_9DOF: - sensor->calibrateGyro(); - sensor->calibrateAccel(); - sensor->calibrateMag(); - return; - case CalibrationType::CALIBRATION_TYPE_TEMP: - sensor->calibrateTemp(); - return; - case CalibrationType::CALIBRATION_TYPE_GYRO_SENSITIVITY: - sensor->calibrateGyroSensitivity(); - return; - - default: - logger.warn( - "The calibration type of %d can not be called from serial", - (CalibrationType)std::atoi(parser->getCmdParam(3)) - ); - return; - } - } - } - return; - } - } - logger.info("Usage:"); - logger.info(" CALIBRATION GET: Prints out current stored calibration values"); - logger.info(" CALIBRATION RESET: Resets the calibration values to the default"); - logger.info(" CALIBRATION RESET \"Sensor#\": Resets the calibration values on sensor # (0 or 1) to the default"); - logger.info(" CALIBRATION START \"Sensor#\" \"Type$$\": Starts the calibration of type $$ on sensor # (0 or 1)"); - logger.info(" 1 - All"); - logger.info(" 2 - Accel"); - logger.info(" 3 - Gyro"); - logger.info(" 4 - Mag"); - logger.info(" 5 - 6DOF"); - logger.info(" 6 - 9DOF"); - logger.info(" 7 - Temp"); - logger.info(" 8 - Gyro Sensitivity"); - } - void setUp() { cmdCallbacks.addCmd("SET", &cmdSet); cmdCallbacks.addCmd("GET", &cmdGet); cmdCallbacks.addCmd("FRST", &cmdFactoryReset); cmdCallbacks.addCmd("REBOOT", &cmdReboot); cmdCallbacks.addCmd("TCAL", &cmdTemperatureCalibration); - cmdCallbacks.addCmd("CALIBRATION", &cmdCalibration); } void update() { From 6a0b47253fd013db6271d6a287642a5f76123d64 Mon Sep 17 00:00:00 2001 From: wigwagwent Date: Thu, 30 Nov 2023 21:20:13 -0700 Subject: [PATCH 51/60] revert because changes made in merge #305 --- src/sensors/SensorFusion.cpp | 6 +++--- src/sensors/SensorFusionRestDetect.cpp | 4 ++-- src/sensors/bmi160sensor.cpp | 2 +- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/src/sensors/SensorFusion.cpp b/src/sensors/SensorFusion.cpp index 6621e2446..e36380a40 100644 --- a/src/sensors/SensorFusion.cpp +++ b/src/sensors/SensorFusion.cpp @@ -173,9 +173,9 @@ namespace SlimeVR void SensorFusion::calcLinearAcc(const sensor_real_t accin[3], const sensor_real_t gravVec[3], sensor_real_t accout[3]) { - accout[0] = (accin[0] - gravVec[0]) * CONST_EARTH_GRAVITY; - accout[1] = (accin[1] - gravVec[1]) * CONST_EARTH_GRAVITY; - accout[2] = (accin[2] - gravVec[2]) * CONST_EARTH_GRAVITY; + accout[0] = accin[0] - gravVec[0] * CONST_EARTH_GRAVITY; + accout[1] = accin[1] - gravVec[1] * CONST_EARTH_GRAVITY; + accout[2] = accin[2] - gravVec[2] * CONST_EARTH_GRAVITY; } } } diff --git a/src/sensors/SensorFusionRestDetect.cpp b/src/sensors/SensorFusionRestDetect.cpp index db82e4829..2e6630d37 100644 --- a/src/sensors/SensorFusionRestDetect.cpp +++ b/src/sensors/SensorFusionRestDetect.cpp @@ -8,14 +8,14 @@ namespace SlimeVR void SensorFusionRestDetect::updateAcc(sensor_real_t Axyz[3], sensor_real_t deltat) { if (deltat < 0) deltat = accTs; - restDetection.updateAcc(deltat * 1e6, Axyz); + restDetection.updateAcc(deltat, Axyz); SensorFusion::updateAcc(Axyz, deltat); } void SensorFusionRestDetect::updateGyro(sensor_real_t Gxyz[3], sensor_real_t deltat) { if (deltat < 0) deltat = gyrTs; - restDetection.updateGyr(deltat * 1e6, Gxyz); + restDetection.updateGyr(deltat, Gxyz); SensorFusion::updateGyro(Gxyz, deltat); } #endif diff --git a/src/sensors/bmi160sensor.cpp b/src/sensors/bmi160sensor.cpp index 04fa85093..a94036ff8 100644 --- a/src/sensors/bmi160sensor.cpp +++ b/src/sensors/bmi160sensor.cpp @@ -548,7 +548,7 @@ void BMI160Sensor::onAccelRawSample(uint32_t dtMicros, int16_t x, int16_t y, int lastAxyz[1] = Axyz[1]; lastAxyz[2] = Axyz[2]; - sfusion.updateAcc(Axyz, (double)dtMicros * 1.0e-6); + sfusion.updateAcc(Axyz, dtMicros); optimistic_yield(100); } From bbf517acd6108bbeb8cef70a8d1ad2660326648a Mon Sep 17 00:00:00 2001 From: wigwagwent Date: Thu, 30 Nov 2023 21:46:09 -0700 Subject: [PATCH 52/60] Code cleanup and make configuration easier --- src/sensors/lsm6dsvSensor.cpp | 24 +++--------------- src/sensors/lsm6dsvSensor.h | 46 ++++++++++++++++++----------------- 2 files changed, 27 insertions(+), 43 deletions(-) diff --git a/src/sensors/lsm6dsvSensor.cpp b/src/sensors/lsm6dsvSensor.cpp index e73bfd6ba..c1ebed207 100644 --- a/src/sensors/lsm6dsvSensor.cpp +++ b/src/sensors/lsm6dsvSensor.cpp @@ -268,8 +268,8 @@ void LSM6DSVSensor::motionLoop() { return; } - // Group all the data together //set the watermark level for nrf sleep - // TODO: add the interupt code to this + // Group all the data together + // TODO: add the interupt watermark code to this if (fifo_samples < fifoFramSize) { return; @@ -708,7 +708,7 @@ void LSM6DSVSensor::calibrateAccel() { } - // Group all the data together //set the watermark level here for nrf sleep + // Group all the data together if (fifo_samples < fifoFramSize) { continue; } @@ -996,24 +996,6 @@ void LSM6DSVSensor::startCalibration(int calibrationType) { m_Logger.info("Calibration finished, enjoy"); } -void LSM6DSVSensor::printCalibration() { - m_Logger.info("Sensor #%d Calibration Data", getSensorId()); - m_Logger.info(" Accel Offset Calibration Values for %s on 0x%02x", getIMUNameByType(sensorType), addr); - m_Logger.info(" X Offset: %f", m_Calibration.A_off[0]); - m_Logger.info(" Y Offset: %f", m_Calibration.A_off[1]); - m_Logger.info(" Z Offset: %f", m_Calibration.A_off[2]); - - m_Logger.info(" Gyro Offset Calibration Values for %s on 0x%02x", getIMUNameByType(sensorType), addr); - m_Logger.info(" X Offset: %f", m_Calibration.G_off[0]); - m_Logger.info(" Y Offset: %f", m_Calibration.G_off[1]); - m_Logger.info(" Z Offset: %f", m_Calibration.G_off[2]); - - m_Logger.info(" Gyro Sensitivity Calibration Values for %s on 0x%02x", getIMUNameByType(sensorType), addr); - m_Logger.info(" X Scale: %f", m_Calibration.G_sensitivity[0]); - m_Logger.info(" Y Scale: %f", m_Calibration.G_sensitivity[1]); - m_Logger.info(" Z Scale: %f", m_Calibration.G_sensitivity[2]); -} - void LSM6DSVSensor::resetCalibration() { m_Logger.info("Sensor #%d Calibration Reset", getSensorId()); m_Calibration.A_off[0] = 0.0f; diff --git a/src/sensors/lsm6dsvSensor.h b/src/sensors/lsm6dsvSensor.h index 2f5e6927d..d89361251 100644 --- a/src/sensors/lsm6dsvSensor.h +++ b/src/sensors/lsm6dsvSensor.h @@ -30,6 +30,14 @@ #include "magneto1.4.h" #include "SensorFusionRestDetect.h" +#define LSM6DSV_FUSION_ESP 0 +#define LSM6DSV_FUSION_ONBOARD 1 + +// ############################################# +// ######## Begin user configuration ########### +// ############################################# + +// #### IMU Reading Speed #### #ifndef LSM6DSV_ACCEL_MAX #define LSM6DSV_ACCEL_MAX 16 #endif @@ -46,7 +54,7 @@ #define LSM6DSV_GYRO_ACCEL_RATE 7680.0f #endif -//#ifndef LSM6DSV_FIFO_TEMP_DATA_RATE //We should use this instead +//#ifndef LSM6DSV_FIFO_TEMP_DATA_RATE //TODO: We should use this instead //#define LSM6DSV_FIFO_TEMP_DATA_RATE 1.875f //#endif @@ -54,7 +62,7 @@ #define LSM6DSV_TEMP_READ_INTERVAL 1 #endif - +// #### IMU Tap Detection #### #ifndef LSM6DSV_TAP_THRESHOLD #define LSM6DSV_TAP_THRESHOLD 5 //0-32 #endif @@ -67,18 +75,21 @@ #define LSM6DSV_TAP_QUITE_TIME 3 //0-3 #endif - - -#define LSM6DSV_INTERRUPT //recommended not required +// #### General IMU Settings #### +#define LSM6DSV_INTERRUPT // interupt recommended but not required // #define LSM6DSV_NO_SELF_TEST_ON_FACEDOWN +#define LSM6DSV_FUSION_SOURCE LSM6DSV_FUSION_ONBOARD // LSM6DSV_FUSION_ESP or LSM6DSV_FUSION_ONBOARD +// #### IMU Calibration #### +#ifndef LSM6DSV_GYRO_SENSITIVITY_SPINS +#define LSM6DSV_GYRO_SENSITIVITY_SPINS 1 // Only used if LSM6DSV_FUSION_SOURCE == LSM6DSV_FUSION_ESP +#endif -#define LSM6DSV_FUSION_ESP 0 -#define LSM6DSV_FUSION_ONBOARD 1 - -#define LSM6DSV_FUSION_SOURCE LSM6DSV_FUSION_ONBOARD +// ############################################# +// ######### End user configuration ############ +// ############################################# #if (LSM6DSV_FUSION_SOURCE == LSM6DSV_FUSION_ESP) @@ -87,14 +98,6 @@ #define LSM6DSV_GYRO_SENSITIVITY_CAL #endif -#ifdef LSM6DSV_GYRO_SENSITIVITY_CAL -#define LSM6DSV_GYRO_SENSITIVITY_SPINS 1 -#endif - - - - - class LSM6DSVSensor : public Sensor { public: LSM6DSVSensor( @@ -114,11 +117,10 @@ class LSM6DSVSensor : public Sensor { #if (LSM6DSV_FUSION_SOURCE == LSM6DSV_FUSION_ESP) void startCalibration(int calibrationType) override final; - void calibrateGyro() override final; - void calibrateAccel() override final; - void calibrateGyroSensitivity() override final; - void printCalibration() override final; - void resetCalibration() override final; + void calibrateGyro(); + void calibrateAccel(); + void calibrateGyroSensitivity(); + void resetCalibration(); #endif private: From 6f8d40e72e961c1c6c1f48852f9b0ab522e06f03 Mon Sep 17 00:00:00 2001 From: wigwagwent Date: Sat, 2 Dec 2023 09:42:08 -0700 Subject: [PATCH 53/60] Update Readme to add lsm --- README.md | 32 +++++++++++++++++++++++++++++--- 1 file changed, 29 insertions(+), 3 deletions(-) diff --git a/README.md b/README.md index f112996c4..8a5d67537 100644 --- a/README.md +++ b/README.md @@ -34,8 +34,15 @@ The following IMUs and their corresponding `IMU` values are supported by the fir * ICM-20948 (IMU_ICM20948) * Using fusion in internal DMP for 6Dof or 9DoF, 9DoF mode requires good magnetic environment. * Comment out `USE_6DOF` in `debug.h` for 9DoF mode. + * Experimental support!\ +* LSM6DSV (IMU_ICM20948) + * Supports the LSM6DSV, LSM6DSV16X, LSM6DSV16B, LSM6DSV16XB + * Using fusion in internal DMP for 6Dof or support for VQF fusion on the esp using accelerometer and gyroscope data. + * **See Sensor calibration** below for info on calibrating this sensor in VQF fusion mode. + * Calibration for DMP is to let the tracker sit for a few seconds on hard surface. * Experimental support! + Firmware can work with both ESP8266 and ESP32. Please edit `defines.h` and set your pinout properly according to how you connected the IMU. ## Sensor calibration @@ -50,9 +57,9 @@ Firmware can work with both ESP8266 and ESP32. Please edit `defines.h` and set y * LED will turn off when calibration is complete * You don't have to calibrate next time you power it on, calibration values will be saved for the next use -### BMI160 +### BMI160 / LSMS6DSV - If you have any problems with this procedure, connect the device via USB and open the serial console to check for any warnings or errors that may indicate hardware problems. + If you have any problems with this procedure, connect the device via USB and open the serial console to check for any warnings or errors that may indicate hardware problems. The serial console will also display the calibration steps as they are needed. - **Step 0: Power up with the chip facing down.** Or press the reset/reboot button. @@ -77,7 +84,26 @@ Firmware can work with both ESP8266 and ESP32. Please edit `defines.h` and set y Rotate the device 90 or 180 degrees in any direction. It should be on a different side each time. Continue to rotate until all 6 sides have been recorded. - The last position has a long flash when recorded, indicating exit from calibration mode. + The last position has a long flash when recorded, indicating exit from calibration mode. (BMI160 Only) + +- **Step 3: Gyroscope Sensitivity Calibration** (LSM6DSV Only, See #281) + + > Step 0: Let the tracker sit, the light will flash when you should reorient the tracker. + + > Step 1: Move the tracker to a corner or edge that aligns the tracker to the same position every time. + > NOTE: You might also want to unplug the USB so it doesn't affect spins. + + > Step 2: Let the tracker rest until the solid light turns on, you might need to hold it against a wall depending on the case and orientation. + + > Step 3: Rotate the tracker in the yaw axis for **2** full rotations and align it with the previous edge. + > NOTE: The yaw axis is the direction of looking left or right with your head, perpendicular to gravity. + > NOTE: The light will turn off after you start moving it. + + > Step 4: Wait for the flashing light then rotate the tracker 90 degrees to a new axis and align with an edge. Repeat steps 2 and 3. + + > Step 5: Wait for the flashing light then rotate the tracker 90 degrees so the last axis is up and aligned with an edge. Repeat steps 2 and 3. + + After letting sit the last time the calibration is complete and the tracker will flash 2 times. #### Additional info for BMI160 - For best results, **calibrate when the trackers are warmed up** - put them on for a few minutes, From af9d7d80069d813feedc63d95122026ee88a1bcd Mon Sep 17 00:00:00 2001 From: wigwagwent Date: Sat, 2 Dec 2023 13:41:56 -0700 Subject: [PATCH 54/60] update lsm lib from 16x --- README.md | 2 +- lib/LSM6DSV/LSM6DSV.cpp | 1640 +++++++++++++++++++++++++-------------- lib/LSM6DSV/LSM6DSV.h | 119 ++- 3 files changed, 1129 insertions(+), 632 deletions(-) diff --git a/README.md b/README.md index 8a5d67537..14d33c2b6 100644 --- a/README.md +++ b/README.md @@ -86,7 +86,7 @@ Firmware can work with both ESP8266 and ESP32. Please edit `defines.h` and set y The last position has a long flash when recorded, indicating exit from calibration mode. (BMI160 Only) -- **Step 3: Gyroscope Sensitivity Calibration** (LSM6DSV Only, See #281) +- **Step 3: Gyroscope Sensitivity Calibration** (LSM6DSV Only, See Merge Request SlimeVR#281) > Step 0: Let the tracker sit, the light will flash when you should reorient the tracker. diff --git a/lib/LSM6DSV/LSM6DSV.cpp b/lib/LSM6DSV/LSM6DSV.cpp index 578435314..1469c18dc 100644 --- a/lib/LSM6DSV/LSM6DSV.cpp +++ b/lib/LSM6DSV/LSM6DSV.cpp @@ -1,8 +1,8 @@ /** ****************************************************************************** * @file LSM6DSV.cpp - * @author SRA - * @version V1.5.1 + * @author STMicroelectronics + * @version V1.0.0 * @date July 2022 * @brief Implementation of a LSM6DSV inertial measurement sensor. ****************************************************************************** @@ -42,7 +42,6 @@ /* Class Implementation ------------------------------------------------------*/ - /** Constructor * @param i2c object of an helper class which handles the I2C peripheral * @param address the address of the component's instance @@ -67,6 +66,7 @@ LSM6DSV::LSM6DSV(SPIClass *spi, int cs_pin, uint32_t spi_speed) : dev_spi(spi), { reg_ctx.write_reg = LSM6DSV_io_write; reg_ctx.read_reg = LSM6DSV_io_read; + reg_ctx.mdelay = LSM6DSV_sleep; reg_ctx.handle = (void *)this; dev_i2c = NULL; acc_is_enabled = 0L; @@ -97,8 +97,7 @@ LSM6DSVStatusTypeDef LSM6DSV::begin() } /* FIFO mode selection */ - fifo_mode = LSM6DSV_BYPASS_MODE; - if (lsm6dsv_fifo_mode_set(®_ctx, fifo_mode) != LSM6DSV_OK) { + if (lsm6dsv_fifo_mode_set(®_ctx, LSM6DSV_BYPASS_MODE) != LSM6DSV_OK) { return LSM6DSV_ERROR; } @@ -111,8 +110,7 @@ LSM6DSVStatusTypeDef LSM6DSV::begin() } /* Full scale selection. */ - acc_fs = LSM6DSV_2g; - if (lsm6dsv_xl_full_scale_set(®_ctx, acc_fs) != LSM6DSV_OK) { + if (lsm6dsv_xl_full_scale_set(®_ctx, LSM6DSV_2g) != LSM6DSV_OK) { return LSM6DSV_ERROR; } @@ -125,8 +123,7 @@ LSM6DSVStatusTypeDef LSM6DSV::begin() } /* Full scale selection. */ - gyro_fs = LSM6DSV_2000dps; - if (lsm6dsv_gy_full_scale_set(®_ctx, gyro_fs) != LSM6DSV_OK) { + if (lsm6dsv_gy_full_scale_set(®_ctx, LSM6DSV_2000dps) != LSM6DSV_OK) { return LSM6DSV_ERROR; } @@ -241,6 +238,11 @@ LSM6DSVStatusTypeDef LSM6DSV::Get_X_Sensitivity(float *Sensitivity) return LSM6DSV_OK; } +/** + * @brief Get the LSM6DSV accelerometer sensor from type lsm6dsv_xl_full_scale_t + * @param full_scale enum with type lsm6dsv_xl_full_scale_t to be converted + * @retval sensitivity as float + */ float LSM6DSV::Convert_X_Sensitivity(lsm6dsv_xl_full_scale_t full_scale) { float Sensitivity = 0.0f; /* Store the Sensitivity based on actual full scale. */ @@ -599,25 +601,6 @@ LSM6DSVStatusTypeDef LSM6DSV::Get_X_AxesRaw(int16_t *Value) return LSM6DSV_OK; } -/** - * @brief Get the LSM6DSV accelerometer sensor raw axes when avaiable (Blocking) - * @param Value pointer where the raw values of the axes are written - * @retval 0 in case of success, an error code otherwise - */ -LSM6DSVStatusTypeDef LSM6DSV::Get_X_AxesRaw_When_Aval(int16_t *Value) { - lsm6dsv_data_ready_t drdy; - do { - if (lsm6dsv_flag_data_ready_get(®_ctx, &drdy) != LSM6DSV_OK) { - return LSM6DSV_ERROR; - } - } while (!drdy.drdy_xl); - - if (Get_X_AxesRaw(Value) != LSM6DSV_OK) { - return LSM6DSV_ERROR; - } - return LSM6DSV_OK; -} - /** * @brief Get the LSM6DSV ACC data ready bit value * @param Status the status of data ready bit @@ -790,297 +773,91 @@ LSM6DSVStatusTypeDef LSM6DSV::Set_X_Filter_Mode(uint8_t LowHighPassFlag, uint8_t } /** - * @brief Runs the ST specified accelerometer and gyroscope self test - * @param XTestType LSM6DSV_XL_ST_DISABLE = 0x0, LSM6DSV_XL_ST_POSITIVE = 0x1, LSM6DSV_XL_ST_NEGATIVE = 0x2 - * @param GTestType LSM6DSV_GY_ST_DISABLE = 0x0, LSM6DSV_GY_ST_POSITIVE = 0x1, LSM6DSV_GY_ST_NEGATIVE = 0x2 + * @brief Enable acceleration offset + * @param enable the acceleration offset * @retval 0 in case of success, an error code otherwise */ -LSM6DSVStatusTypeDef LSM6DSV::Test_IMU(uint8_t XTestType, uint8_t GTestType) +LSM6DSVStatusTypeDef LSM6DSV::Enable_X_User_Offset() { - uint8_t whoamI; - - if (ReadID(&whoamI) != LSM6DSV_OK) { - return LSM6DSV_ERROR; - } - - if (whoamI != LSM6DSV_ID) { + lsm6dsv_ctrl9_t ctrl9; + if (lsm6dsv_read_reg(®_ctx, LSM6DSV_CTRL9, (uint8_t *)&ctrl9, 1) != LSM6DSV_OK) { return LSM6DSV_ERROR; } + ctrl9.usr_off_on_out = PROPERTY_ENABLE; - if (Test_X_IMU(XTestType) != LSM6DSV_OK) { - return LSM6DSV_ERROR; - } - - if (Test_G_IMU(GTestType) != LSM6DSV_OK) { + if (lsm6dsv_write_reg(®_ctx, LSM6DSV_CTRL9, (uint8_t *)&ctrl9, 1) != LSM6DSV_OK) { return LSM6DSV_ERROR; } - printf("IMU Test OK\n"); - return LSM6DSV_OK; + return LSM6DSV_OK; } /** - * @brief Runs the ST specified accelerometer self test - * @param TestType LSM6DSV_XL_ST_DISABLE = 0x0, LSM6DSV_XL_ST_POSITIVE = 0x1, LSM6DSV_XL_ST_NEGATIVE = 0x2 + * @brief Disable acceleration offset * @retval 0 in case of success, an error code otherwise */ -LSM6DSVStatusTypeDef LSM6DSV::Test_X_IMU(uint8_t TestType) +LSM6DSVStatusTypeDef LSM6DSV::Disable_X_User_Offset() { - int16_t data_raw[3]; - float val_st_off[3]; - float val_st_on[3]; - float test_val[3]; - - if (Reset_Set(LSM6DSV_RESTORE_CTRL_REGS) != LSM6DSV_OK) { - return LSM6DSV_ERROR; - } - - if (lsm6dsv_block_data_update_set(®_ctx, PROPERTY_ENABLE) != LSM6DSV_OK) { - return LSM6DSV_ERROR; - } - - /* - * Accelerometer Self Test - */ - if (lsm6dsv_xl_data_rate_set(®_ctx, LSM6DSV_ODR_AT_60Hz) != LSM6DSV_OK) { - return LSM6DSV_ERROR; - } - - if (lsm6dsv_xl_full_scale_set(®_ctx, LSM6DSV_4g) != LSM6DSV_OK) { - return LSM6DSV_ERROR; - } - delay(100); //Wait for Accelerometer to stabalize; - memset(val_st_off, 0x00, 3 * sizeof(float)); - memset(val_st_on, 0x00, 3 * sizeof(float)); - - /*Ignore First Data*/ - if (Get_X_AxesRaw_When_Aval(data_raw) != LSM6DSV_OK) { - return LSM6DSV_ERROR; - } - - for (uint8_t i = 0; i < 5; i++) { - if (Get_X_AxesRaw_When_Aval(data_raw) != LSM6DSV_OK) { - return LSM6DSV_ERROR; - } - - /*Average the data in each axis*/ - for (uint8_t j = 0; j < 3; j++) { - val_st_off[j] += lsm6dsv_from_fs4_to_mg(data_raw[j]); - } - } - - /* Calculate the mg average values */ - for (uint8_t i = 0; i < 3; i++) { - val_st_off[i] /= 5.0f; - } - - if (lsm6dsv_xl_self_test_set(®_ctx, (lsm6dsv_xl_self_test_t)TestType) != LSM6DSV_OK) { - return LSM6DSV_ERROR; - } - delay(100); - - /*Ignore First Data*/ - if (Get_X_AxesRaw_When_Aval(data_raw) != LSM6DSV_OK) { - return LSM6DSV_ERROR; - } - - for (uint8_t i = 0; i < 5; i++) { - if (Get_X_AxesRaw_When_Aval(data_raw) != LSM6DSV_OK) { - return LSM6DSV_ERROR; - } - - /*Average the data in each axis*/ - for (uint8_t j = 0; j < 3; j++) { - val_st_on[j] += lsm6dsv_from_fs4_to_mg(data_raw[j]); - } - } - - /* Calculate the mg average values */ - for (uint8_t i = 0; i < 3; i++) { - val_st_on[i] /= 5.0f; - } - - /* Calculate the mg values for self test */ - for (uint8_t i = 0; i < 3; i++) { - test_val[i] = fabs((val_st_on[i] - val_st_off[i])); - } - - for (uint8_t i = 0; i < 3; i++) { - if (( LSM6DSV_MIN_ST_LIMIT_mg > test_val[i] ) || ( test_val[i] > LSM6DSV_MAX_ST_LIMIT_mg)) - return LSM6DSV_ERROR; - } - - if (lsm6dsv_xl_self_test_set(®_ctx, LSM6DSV_XL_ST_DISABLE) != LSM6DSV_OK) { + lsm6dsv_ctrl9_t ctrl9; + if (lsm6dsv_read_reg(®_ctx, LSM6DSV_CTRL9, (uint8_t *)&ctrl9, 1) != LSM6DSV_OK) { return LSM6DSV_ERROR; } + ctrl9.usr_off_on_out = PROPERTY_DISABLE; - if (lsm6dsv_xl_data_rate_set(®_ctx, LSM6DSV_ODR_OFF) != LSM6DSV_OK) { + if (lsm6dsv_write_reg(®_ctx, LSM6DSV_CTRL9, (uint8_t *)&ctrl9, 1) != LSM6DSV_OK) { return LSM6DSV_ERROR; } return LSM6DSV_OK; } /** - * @brief Runs the ST specified self test on the acceleration axis of the IMU - * @param TestType LSM6DSV_GY_ST_DISABLE = 0x0, LSM6DSV_GY_ST_POSITIVE = 0x1, LSM6DSV_GY_ST_NEGATIVE = 0x2 + * @brief Set the offset values for the acceleration + * @param x the acceleration offset in the x axis to be set + * @param y the acceleration offset in the y axis to be set + * @param z the acceleration offset in the z axis to be set * @retval 0 in case of success, an error code otherwise */ -LSM6DSVStatusTypeDef LSM6DSV::Test_G_IMU(uint8_t TestType = LSM6DSV_GY_ST_POSITIVE) +LSM6DSVStatusTypeDef LSM6DSV::Set_X_User_Offset(float x, float y, float z) { - int16_t data_raw[3]; - float test_val[3]; - - if (Reset_Set(LSM6DSV_RESTORE_CTRL_REGS) != LSM6DSV_OK) { - return LSM6DSV_ERROR; - } - - if (lsm6dsv_block_data_update_set(®_ctx, PROPERTY_ENABLE) != LSM6DSV_OK) { - return LSM6DSV_ERROR; - } - - /* - * Gyroscope Self Test - */ - - if (lsm6dsv_gy_data_rate_set(®_ctx, LSM6DSV_ODR_AT_240Hz) != LSM6DSV_OK) { - return LSM6DSV_ERROR; - } - - if (lsm6dsv_gy_full_scale_set(®_ctx, LSM6DSV_2000dps) != LSM6DSV_OK) { - return LSM6DSV_ERROR; - } - - delay(100); - - /*Ignore First Data*/ - if (Get_G_AxesRaw_When_Aval(data_raw) != LSM6DSV_OK) { - return LSM6DSV_ERROR; - } - - float val_st_off[3] = {0}; - float val_st_on[3] = {0}; - - for (uint8_t i = 0; i < 5; i++) { - if (Get_G_AxesRaw_When_Aval(data_raw) != LSM6DSV_OK) { - return LSM6DSV_ERROR; - } - - /*Average the data in each axis*/ - for (uint8_t j = 0; j < 3; j++) { - val_st_off[j] += lsm6dsv_from_fs2000_to_mdps(data_raw[j]); - } - } - - /* Calculate the mg average values */ - for (uint8_t i = 0; i < 3; i++) { - val_st_off[i] /= 5.0f; - } - - if (lsm6dsv_gy_self_test_set(®_ctx, (lsm6dsv_gy_self_test_t)TestType) != LSM6DSV_OK) { - return LSM6DSV_ERROR; - } - delay(100); - - /*Ignore First Data*/ - if (Get_G_AxesRaw_When_Aval(data_raw) != LSM6DSV_OK) { + lsm6dsv_ctrl9_t ctrl9; + if (lsm6dsv_read_reg(®_ctx, LSM6DSV_CTRL9, (uint8_t *)&ctrl9, 1) != LSM6DSV_OK) { return LSM6DSV_ERROR; } - - for (uint8_t i = 0; i < 5; i++) { - if (Get_G_AxesRaw_When_Aval(data_raw) != LSM6DSV_OK) { - return LSM6DSV_ERROR; - } - - /*Average the data in each axis*/ - for (uint8_t j = 0; j < 3; j++) { - val_st_on[j] += lsm6dsv_from_fs2000_to_mdps(data_raw[j]); - } - } - - /* Calculate the mg average values */ - for (uint8_t i = 0; i < 3; i++) { - val_st_on[i] /= 5.0f; - } + int8_t xyz[3]; - /* Calculate the mg values for self test */ - for (uint8_t i = 0; i < 3; i++) { - test_val[i] = fabs((val_st_on[i] - val_st_off[i])); + if ( // about +- 2 G's for high and +- 0.124 G's for low + (x <= LSM6DSV_ACC_USR_OFF_W_LOW_MAX && x >= -LSM6DSV_ACC_USR_OFF_W_LOW_MAX) && + (y <= LSM6DSV_ACC_USR_OFF_W_LOW_MAX && y >= -LSM6DSV_ACC_USR_OFF_W_LOW_MAX) && + (z <= LSM6DSV_ACC_USR_OFF_W_LOW_MAX && z >= -LSM6DSV_ACC_USR_OFF_W_LOW_MAX)) { // Then we are under the low requirements + xyz[0] = (int8_t)(x / LSM6DSV_ACC_USR_OFF_W_LOW_LSB); + xyz[1] = (int8_t)(y / LSM6DSV_ACC_USR_OFF_W_LOW_LSB); + xyz[2] = (int8_t)(z / LSM6DSV_ACC_USR_OFF_W_LOW_LSB); + ctrl9.usr_off_w = false; //(0: 2^-10 g/LSB; 1: 2^-6 g/LSB) } - /* Check self test limit */ - for (uint8_t i = 0; i < 3; i++) { - if (( LSM6DSV_MIN_ST_LIMIT_mdps > test_val[i] ) || - ( test_val[i] > LSM6DSV_MAX_ST_LIMIT_mdps)) { - return LSM6DSV_ERROR; - } + else if ( // about +- 2 G's for high and +- 0.124 G's for low + (x <= LSM6DSV_ACC_USR_OFF_W_HIGH_MAX && x >= -LSM6DSV_ACC_USR_OFF_W_HIGH_MAX) && + (y <= LSM6DSV_ACC_USR_OFF_W_HIGH_MAX && y >= -LSM6DSV_ACC_USR_OFF_W_HIGH_MAX) && + (z <= LSM6DSV_ACC_USR_OFF_W_HIGH_MAX && z >= -LSM6DSV_ACC_USR_OFF_W_HIGH_MAX)) { // Then we are under the high requirements + xyz[0] = (int8_t)(x / LSM6DSV_ACC_USR_OFF_W_HIGH_LSB); + xyz[1] = (int8_t)(y / LSM6DSV_ACC_USR_OFF_W_HIGH_LSB); + xyz[2] = (int8_t)(z / LSM6DSV_ACC_USR_OFF_W_HIGH_LSB); + ctrl9.usr_off_w = true; //(0: 2^-10 g/LSB; 1: 2^-6 g/LSB) } - - - if (lsm6dsv_gy_self_test_set(®_ctx, LSM6DSV_GY_ST_DISABLE) != LSM6DSV_OK) { - return LSM6DSV_ERROR; + else { + return LSM6DSV_ERROR; // Value too big } - if (lsm6dsv_xl_data_rate_set(®_ctx, LSM6DSV_ODR_OFF) != LSM6DSV_OK) { + if (lsm6dsv_write_reg(®_ctx, LSM6DSV_CTRL9, (uint8_t *)&ctrl9, 1) != LSM6DSV_OK) { return LSM6DSV_ERROR; } - return LSM6DSV_OK; -} - -LSM6DSVStatusTypeDef LSM6DSV::Get_T_ODR(float *Odr) { - lsm6dsv_fifo_ctrl4_t ctrl4; - - if (lsm6dsv_read_reg(®_ctx, LSM6DSV_FIFO_CTRL4, (uint8_t *)&ctrl4, 1) != LSM6DSV_OK) { + // convert from float in G's to what it wants. Signed byte + if (lsm6dsv_write_reg(®_ctx, LSM6DSV_X_OFS_USR, (uint8_t *)&xyz, 3) != LSM6DSV_OK) { return LSM6DSV_ERROR; } - - switch (ctrl4.odr_t_batch) - { - case LSM6DSV_TEMP_NOT_BATCHED: - *Odr = 0; - break; - case LSM6DSV_TEMP_BATCHED_AT_1Hz875: - *Odr = 1.875f; - break; - case LSM6DSV_TEMP_BATCHED_AT_15Hz: - *Odr = 15; - break; - case LSM6DSV_TEMP_BATCHED_AT_60Hz: - *Odr = 60; - break; - default: - break; - } - return LSM6DSV_OK; } - - -LSM6DSVStatusTypeDef LSM6DSV::Set_T_ODR(float Odr) { - lsm6dsv_fifo_ctrl4_t ctrl4; - - if (lsm6dsv_read_reg(®_ctx, LSM6DSV_FIFO_CTRL4, (uint8_t *)&ctrl4, 1) != LSM6DSV_OK) { - return LSM6DSV_ERROR; - } - - if (Odr == 0.0F) { - ctrl4.odr_t_batch = LSM6DSV_TEMP_NOT_BATCHED; - } else if (Odr <= 1.875F) { - ctrl4.odr_t_batch = LSM6DSV_TEMP_BATCHED_AT_1Hz875; - } else if (Odr <= 15.0F) { - ctrl4.odr_t_batch = LSM6DSV_TEMP_BATCHED_AT_15Hz; - } else { - ctrl4.odr_t_batch = LSM6DSV_TEMP_BATCHED_AT_60Hz; - } - - return (LSM6DSVStatusTypeDef)lsm6dsv_write_reg( - ®_ctx, - LSM6DSV_FIFO_CTRL4, - (uint8_t *)&ctrl4, - 1 - ); -} - /** * @brief Enable 6D orientation detection * @param IntPin interrupt pin line to be used @@ -2227,6 +2004,7 @@ LSM6DSVStatusTypeDef LSM6DSV::Enable_Pedometer(LSM6DSV_SensorIntPin_t IntPin) /* Enable pedometer algorithm. */ mode.step_counter_enable = PROPERTY_ENABLE; + mode.false_step_rej = PROPERTY_DISABLE; /* Turn on embedded features */ if (lsm6dsv_stpcnt_mode_set(®_ctx, mode) != LSM6DSV_OK) { @@ -2331,6 +2109,7 @@ LSM6DSVStatusTypeDef LSM6DSV::Disable_Pedometer() /* Enable pedometer algorithm. */ mode.step_counter_enable = PROPERTY_DISABLE; + mode.false_step_rej = PROPERTY_DISABLE; /* Turn off embedded features */ if (lsm6dsv_stpcnt_mode_set(®_ctx, mode) != LSM6DSV_OK) { @@ -2626,8 +2405,12 @@ LSM6DSVStatusTypeDef LSM6DSV::Disable_Tilt_Detection() return ret; } - -LSM6DSVStatusTypeDef LSM6DSV::FIFO_Reset() { +/** + * @brief Resets the fifo to an empty state + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSVStatusTypeDef LSM6DSV::FIFO_Reset() +{ if (lsm6dsv_fifo_mode_set(®_ctx, LSM6DSV_BYPASS_MODE) != LSM6DSV_OK) { return LSM6DSV_ERROR; } @@ -2646,11 +2429,13 @@ LSM6DSVStatusTypeDef LSM6DSV::FIFO_Reset() { */ LSM6DSVStatusTypeDef LSM6DSV::FIFO_Get_Num_Samples(uint16_t *NumSamples) { - lsm6dsv_fifo_status_t fifo_status; - LSM6DSVStatusTypeDef result = - (LSM6DSVStatusTypeDef) lsm6dsv_fifo_status_get(®_ctx, &fifo_status); - *NumSamples = fifo_status.fifo_level; - return result; + lsm6dsv_fifo_status_t status; + if (lsm6dsv_fifo_status_get(®_ctx, &status) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + *NumSamples = status.fifo_level; + + return LSM6DSV_OK; } /** @@ -2770,7 +2555,7 @@ LSM6DSVStatusTypeDef LSM6DSV::FIFO_Set_Mode(uint8_t Mode) newMode = LSM6DSV_BYPASS_TO_FIFO_MODE; break; default: - return LSM6DSV_ERROR; + return LSM6DSV_ERROR; } fifo_mode = newMode; if (lsm6dsv_fifo_mode_set(®_ctx, fifo_mode) != LSM6DSV_OK) { @@ -2900,7 +2685,6 @@ LSM6DSVStatusTypeDef LSM6DSV::FIFO_Get_G_Axes(int32_t *AngularVelocity) /** * @brief Set the LSM6DSV FIFO gyro BDR value - * @param Bdr FIFO gyro BDR value * @retval 0 in case of success, an error code otherwise */ @@ -2925,89 +2709,116 @@ LSM6DSVStatusTypeDef LSM6DSV::FIFO_Set_G_BDR(float Bdr) return (LSM6DSVStatusTypeDef) lsm6dsv_fifo_gy_batch_set(®_ctx, new_bdr); } -LSM6DSVStatusTypeDef LSM6DSV::FIFO_Set_SFLP_Batch(bool GameRotation, bool Gravity, bool gBias) +/** + * @brief Get the LSM6DSV FIFO status + * @param Status pointer for FIFO status + * @retval 0 in case of success, an error code otherwise +*/ +LSM6DSVStatusTypeDef LSM6DSV::FIFO_Get_Status(lsm6dsv_fifo_status_t *Status) { - lsm6dsv_fifo_sflp_raw_t fifo_sflp; - fifo_sflp.game_rotation = GameRotation; - fifo_sflp.gravity = Gravity; - fifo_sflp.gbias = gBias; - return (LSM6DSVStatusTypeDef)lsm6dsv_fifo_sflp_batch_set(®_ctx, fifo_sflp); + return (LSM6DSVStatusTypeDef)lsm6dsv_fifo_status_get(®_ctx, Status); } -LSM6DSVStatusTypeDef LSM6DSV::FIFO_Get_Status(lsm6dsv_fifo_status_t * Status) +/** + * @brief Get the Rotation Vector values + * @param rvec pointer where the Rotation Vector values are written + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSVStatusTypeDef LSM6DSV::FIFO_Get_Rotation_Vector(float *rvec) { - return (LSM6DSVStatusTypeDef) lsm6dsv_fifo_status_get(®_ctx, Status); + lsm6dsv_axis3bit16_t data_raw; + + if (FIFO_Get_Data(data_raw.u8bit) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + sflp2q(rvec, (uint16_t *)&data_raw.i16bit[0]); + + return LSM6DSV_OK; } -LSM6DSVStatusTypeDef LSM6DSV::FIFO_Get_Gravity_Vector(float * data) +/** + * @brief Get the Gravity Vector values + * @param gvec pointer where the Gravity Vector values are written + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSVStatusTypeDef LSM6DSV::FIFO_Get_Gravity_Vector(float *gvec) { - uint16_t raw_data[6]; - if (FIFO_Get_Data((uint8_t *)raw_data) != LSM6DSV_OK) { - return LSM6DSV_ERROR; - } + lsm6dsv_axis3bit16_t data_raw; - data[0] = lsm6dsv_from_sflp_to_mg(raw_data[0]); - data[1] = lsm6dsv_from_sflp_to_mg(raw_data[1]); - data[2] = lsm6dsv_from_sflp_to_mg(raw_data[2]); - return LSM6DSV_OK; + if (FIFO_Get_Data(data_raw.u8bit) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + gvec[0] = lsm6dsv_from_sflp_to_mg(data_raw.i16bit[0]); + gvec[1] = lsm6dsv_from_sflp_to_mg(data_raw.i16bit[1]); + gvec[2] = lsm6dsv_from_sflp_to_mg(data_raw.i16bit[2]); + + return LSM6DSV_OK; } -LSM6DSVStatusTypeDef LSM6DSV::FIFO_Get_Game_Vector(float * data) +/** + * @brief Get the Gravity Bias values + * @param gbias pointer where the Gravity Bias values are written + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSVStatusTypeDef LSM6DSV::FIFO_Get_Gyroscope_Bias(float *gbias) { - uint16_t raw_data[3]; - if (FIFO_Get_Data((uint8_t *)raw_data) != LSM6DSV_OK) { - return LSM6DSV_ERROR; - } + lsm6dsv_axis3bit16_t data_raw; - union bits_to_float { - uint32_t bits; - float value; - }; - - bits_to_float float_bits[3]; - float_bits[0].bits = Half_Bits_To_Float_Bits(raw_data[0]); - float_bits[1].bits = Half_Bits_To_Float_Bits(raw_data[1]); - float_bits[2].bits = Half_Bits_To_Float_Bits(raw_data[2]); - data[0] = float_bits[0].value; - data[1] = float_bits[1].value; - data[2] = float_bits[2].value; - return LSM6DSV_OK; + if (FIFO_Get_Data(data_raw.u8bit) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + gbias[0] = lsm6dsv_from_fs125_to_mdps(data_raw.i16bit[0]); + gbias[1] = lsm6dsv_from_fs125_to_mdps(data_raw.i16bit[1]); + gbias[2] = lsm6dsv_from_fs125_to_mdps(data_raw.i16bit[2]); + + return LSM6DSV_OK; } -LSM6DSVStatusTypeDef LSM6DSV::FIFO_Get_GBias_Vector(float * data) +/** + * @brief Enable the LSM6DSV FIFO Timestamp + * @retval 0 in case of success, an error code otherwise +*/ +LSM6DSVStatusTypeDef LSM6DSV::FIFO_Enable_Timestamp() { - uint16_t raw_data[3]; - if (FIFO_Get_Data((uint8_t *)raw_data) != LSM6DSV_OK) { - return LSM6DSV_ERROR; - } - data[0] = lsm6dsv_from_fs125_to_mdps(raw_data[0]); - data[1] = lsm6dsv_from_fs125_to_mdps(raw_data[1]); - data[2] = lsm6dsv_from_fs125_to_mdps(raw_data[2]); - return LSM6DSV_OK; + return (LSM6DSVStatusTypeDef)lsm6dsv_timestamp_set(®_ctx, PROPERTY_ENABLE); } -LSM6DSVStatusTypeDef LSM6DSV::FIFO_Get_Timestamp(uint32_t *timestamp) +/** + * @brief Disable the LSM6DSV FIFO Timestamp + * @retval 0 in case of success, an error code otherwise +*/ +LSM6DSVStatusTypeDef LSM6DSV::FIFO_Disable_Timestamp() { - uint32_t raw_data[2]; //first is timestamp second is half full of meta data - if (FIFO_Get_Data((uint8_t *)raw_data) != LSM6DSV_OK) { - return LSM6DSV_ERROR; - } - *timestamp = raw_data[0]; - return LSM6DSV_OK; + return (LSM6DSVStatusTypeDef)lsm6dsv_timestamp_set(®_ctx, PROPERTY_DISABLE); } +/** + * @brief Set the decimation of timestamp for how often FIFO will be updated with timestamp value + * @param decimation FIFO Timestamp Decimation + * @retval 0 in case of success, an error code otherwise + */ LSM6DSVStatusTypeDef LSM6DSV::FIFO_Set_Timestamp_Decimation(uint8_t decimation) { return (LSM6DSVStatusTypeDef)lsm6dsv_fifo_timestamp_batch_set(®_ctx, (lsm6dsv_fifo_timestamp_batch_t)decimation); - } -LSM6DSVStatusTypeDef LSM6DSV::Enable_Timestamp() +/** + * @brief Get the LSM6DSV FIFO Timestamp + * @param timestamp FIFO Timestamp + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSVStatusTypeDef LSM6DSV::FIFO_Get_Timestamp(uint32_t *timestamp) { - return (LSM6DSVStatusTypeDef)lsm6dsv_timestamp_set(®_ctx, 1); + uint32_t raw_data[2]; //first is timestamp second is half full of meta data + if (FIFO_Get_Data((uint8_t *)raw_data) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + *timestamp = raw_data[0]; + return LSM6DSV_OK; } - /** * @brief Enable the LSM6DSV gyroscope sensor * @retval 0 in case of success, an error code otherwise @@ -3073,12 +2884,11 @@ LSM6DSVStatusTypeDef LSM6DSV::Get_G_Sensitivity(float *Sensitivity) if (*Sensitivity == 0.0f) { return LSM6DSV_ERROR; } - return LSM6DSV_OK; } - -float LSM6DSV::Convert_G_Sensitivity(lsm6dsv_gy_full_scale_t full_scale) { +float LSM6DSV::Convert_G_Sensitivity(lsm6dsv_gy_full_scale_t full_scale) +{ float Sensitivity = 0.0f; /* Store the sensitivity based on actual full scale. */ switch (full_scale) { @@ -3376,25 +3186,6 @@ LSM6DSVStatusTypeDef LSM6DSV::Get_G_AxesRaw(int16_t *Value) return LSM6DSV_OK; } -/** - * @brief Get the LSM6DSV gyroscope sensor raw axes when data available (Blocking) - * @param Value pointer where the raw values of the axes are written - * @retval 0 in case of success, an error code otherwise - */ -LSM6DSVStatusTypeDef LSM6DSV::Get_G_AxesRaw_When_Aval(int16_t *Value) { - lsm6dsv_data_ready_t drdy; - do { - if (lsm6dsv_flag_data_ready_get(®_ctx, &drdy) != LSM6DSV_OK) { - return LSM6DSV_ERROR; - } - } while (!drdy.drdy_gy); - - if (Get_G_AxesRaw(Value) != LSM6DSV_OK) { - return LSM6DSV_ERROR; - } - return LSM6DSV_OK; -} - /** * @brief Get the LSM6DSV gyroscope sensor axes * @param AngularRate pointer where the values of the axes are written @@ -3447,10 +3238,10 @@ LSM6DSVStatusTypeDef LSM6DSV::Get_G_DRDY_Status(uint8_t *Status) */ LSM6DSVStatusTypeDef LSM6DSV::Set_G_Power_Mode(uint8_t PowerMode) { - return (LSM6DSVStatusTypeDef)lsm6dsv_gy_mode_set( - ®_ctx, - (lsm6dsv_gy_mode_t)PowerMode - ); + if (lsm6dsv_gy_mode_set(®_ctx, (lsm6dsv_gy_mode_t)PowerMode) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + return LSM6DSV_OK; } /** @@ -3483,272 +3274,985 @@ LSM6DSVStatusTypeDef LSM6DSV::Set_G_Filter_Mode(uint8_t LowHighPassFlag, uint8_t return LSM6DSV_OK; } -LSM6DSVStatusTypeDef LSM6DSV::Set_G_Bias(float x, float y, float z) -{ - lsm6dsv_sflp_gbias_t gbias; - gbias.gbias_x = x; - gbias.gbias_y = y; - gbias.gbias_z = z; - return (LSM6DSVStatusTypeDef)lsm6dsv_sflp_game_gbias_set( - ®_ctx, - &gbias - ); -} - -LSM6DSVStatusTypeDef LSM6DSV::Set_SFLP_ODR(float odr) { - lsm6dsv_sflp_data_rate_t rate = odr <= 15 ? LSM6DSV_SFLP_15Hz - : odr <= 30 ? LSM6DSV_SFLP_30Hz - : odr <= 60 ? LSM6DSV_SFLP_60Hz - : odr <= 120 ? LSM6DSV_SFLP_120Hz - : odr <= 240 ? LSM6DSV_SFLP_240Hz - : LSM6DSV_SFLP_480Hz; - - return (LSM6DSVStatusTypeDef)lsm6dsv_sflp_data_rate_set( - ®_ctx, - rate - ); -} - - /** - * @brief Get the LSM6DSV register value - * @param Reg address to be read - * @param Data pointer where the value is written + * @brief Get the LSM6DSV temperature sensor output data rate + * @param Odr pointer where the output data rate is written * @retval 0 in case of success, an error code otherwise */ -LSM6DSVStatusTypeDef LSM6DSV::Read_Reg(uint8_t Reg, uint8_t *Data) +LSM6DSVStatusTypeDef LSM6DSV::Get_Temp_ODR(float *Odr) { - if (lsm6dsv_read_reg(®_ctx, Reg, Data, 1) != LSM6DSV_OK) { + lsm6dsv_fifo_ctrl4_t ctrl4; + + if (lsm6dsv_read_reg(®_ctx, LSM6DSV_FIFO_CTRL4, (uint8_t *)&ctrl4, 1) != LSM6DSV_OK) { return LSM6DSV_ERROR; } + switch (ctrl4.odr_t_batch) { + case LSM6DSV_TEMP_NOT_BATCHED: + *Odr = 0; + break; + case LSM6DSV_TEMP_BATCHED_AT_1Hz875: + *Odr = 1.875f; + break; + case LSM6DSV_TEMP_BATCHED_AT_15Hz: + *Odr = 15; + break; + case LSM6DSV_TEMP_BATCHED_AT_60Hz: + *Odr = 60; + break; + default: + break; + } + return LSM6DSV_OK; } /** - * @brief Set the LSM6DSV register value - * @param Reg address to be written - * @param Data value to be written + * @brief Set the LSM6DSV temperature sensor output data rate + * @param Odr the output data rate value to be set * @retval 0 in case of success, an error code otherwise */ -LSM6DSVStatusTypeDef LSM6DSV::Write_Reg(uint8_t Reg, uint8_t Data) +LSM6DSVStatusTypeDef LSM6DSV::Set_Temp_ODR(float Odr) { - if (lsm6dsv_write_reg(®_ctx, Reg, &Data, 1) != LSM6DSV_OK) { + lsm6dsv_fifo_ctrl4_t ctrl4; + + if (lsm6dsv_read_reg(®_ctx, LSM6DSV_FIFO_CTRL4, (uint8_t *)&ctrl4, 1) != LSM6DSV_OK) { return LSM6DSV_ERROR; } - return LSM6DSV_OK; -} + if (Odr == 0.0F) { + ctrl4.odr_t_batch = LSM6DSV_TEMP_NOT_BATCHED; + } + else if (Odr <= 1.875F) { + ctrl4.odr_t_batch = LSM6DSV_TEMP_BATCHED_AT_1Hz875; + } + else if (Odr <= 15.0F) { + ctrl4.odr_t_batch = LSM6DSV_TEMP_BATCHED_AT_15Hz; + } + else { + ctrl4.odr_t_batch = LSM6DSV_TEMP_BATCHED_AT_60Hz; + } -int32_t LSM6DSV_io_write(void *handle, uint8_t WriteAddr, const uint8_t *pBuffer, uint16_t nBytesToWrite) -{ - return ((LSM6DSV *)handle)->IO_Write(pBuffer, WriteAddr, nBytesToWrite); + return (LSM6DSVStatusTypeDef)lsm6dsv_write_reg( + ®_ctx, + LSM6DSV_FIFO_CTRL4, + (uint8_t *)&ctrl4, + 1); } -int32_t LSM6DSV_io_read(void *handle, uint8_t ReadAddr, uint8_t *pBuffer, uint16_t nBytesToRead) +/** + * @brief Returns the raw temperature value from the imu + * @param value pointer where the temperature value is written + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSVStatusTypeDef LSM6DSV::Get_Temp_Raw(int16_t *value) { - return ((LSM6DSV *)handle)->IO_Read(pBuffer, ReadAddr, nBytesToRead); -} - -void LSM6DSV_sleep(uint32_t ms) { - delay(ms); + return (LSM6DSVStatusTypeDef)lsm6dsv_temperature_raw_get(®_ctx, value); } -LSM6DSVStatusTypeDef LSM6DSV::Reset_Set(uint8_t flags) +/** + * @brief Runs the ST specified accelerometer and gyroscope self test + * @param XTestType LSM6DSV_XL_ST_DISABLE = 0x0, LSM6DSV_XL_ST_POSITIVE = 0x1, LSM6DSV_XL_ST_NEGATIVE = 0x2 + * @param GTestType LSM6DSV_GY_ST_DISABLE = 0x0, LSM6DSV_GY_ST_POSITIVE = 0x1, LSM6DSV_GY_ST_NEGATIVE = 0x2 + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSVStatusTypeDef LSM6DSV::Test_IMU(uint8_t XTestType, uint8_t GTestType) { - if (lsm6dsv_reset_set(®_ctx, (lsm6dsv_reset_t)flags) != LSM6DSV_OK) { - return LSM6DSV_ERROR; - } + uint8_t whoamI; - lsm6dsv_reset_t rst; - do { - if (lsm6dsv_reset_get(®_ctx, &rst) != LSM6DSV_OK) { - return LSM6DSV_ERROR; - } - } while (rst != LSM6DSV_READY); - return LSM6DSV_OK; -} + if (ReadID(&whoamI) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } -LSM6DSVStatusTypeDef LSM6DSV::Enable_SFLP_Block(bool enable) -{ - return (LSM6DSVStatusTypeDef)lsm6dsv_sflp_game_rotation_set( - ®_ctx, - enable ? PROPERTY_ENABLE : PROPERTY_DISABLE - ); -} + if (whoamI != LSM6DSV_ID) { + return LSM6DSV_ERROR; + } -LSM6DSVStatusTypeDef LSM6DSV::Enable_Block_Data_Update(bool enable) -{ - return (LSM6DSVStatusTypeDef)lsm6dsv_block_data_update_set( - ®_ctx, - enable ? PROPERTY_ENABLE : PROPERTY_DISABLE - ); + if (Test_X_IMU(XTestType) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + if (Test_G_IMU(GTestType) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + return LSM6DSV_OK; } /** - * @brief Enable register address automatically incremented during a multiple byte - access with a serial interface. + * @brief Runs the ST specified accelerometer self test + * @param TestType LSM6DSV_XL_ST_DISABLE = 0x0, LSM6DSV_XL_ST_POSITIVE = 0x1, LSM6DSV_XL_ST_NEGATIVE = 0x2 * @retval 0 in case of success, an error code otherwise */ -LSM6DSVStatusTypeDef LSM6DSV::Set_Auto_Increment(bool enable) +LSM6DSVStatusTypeDef LSM6DSV::Test_X_IMU(uint8_t TestType) { - return (LSM6DSVStatusTypeDef)lsm6dsv_auto_increment_set( - ®_ctx, - enable ? PROPERTY_ENABLE : PROPERTY_DISABLE - ); -} + int16_t data_raw[3]; + float val_st_off[3]; + float val_st_on[3]; + float test_val[3]; -LSM6DSVStatusTypeDef LSM6DSV::Get_Temperature_Raw(int16_t * value) -{ - return (LSM6DSVStatusTypeDef)lsm6dsv_temperature_raw_get( - ®_ctx, - value - ); -} + if (Device_Reset(LSM6DSV_RESET_CTRL_REGS) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } -LSM6DSVStatusTypeDef LSM6DSV::Is_New_Temperature_Data(bool * available) -{ - lsm6dsv_status_reg_t status; - if (lsm6dsv_read_reg(®_ctx, LSM6DSV_STATUS_REG, (uint8_t *)&status, 1) != LSM6DSV_OK) { - return LSM6DSV_ERROR; - } + if (lsm6dsv_block_data_update_set(®_ctx, PROPERTY_ENABLE) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } - *available = status.tda; - return LSM6DSV_OK; -} + /* + * Accelerometer Self Test + */ + if (lsm6dsv_xl_data_rate_set(®_ctx, LSM6DSV_ODR_AT_60Hz) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } -LSM6DSVStatusTypeDef LSM6DSV::Set_SFLP_GBIAS(float x, float y, float z) { - lsm6dsv_sflp_gbias_t val = {0, 0, 0}; - val.gbias_x = x; - val.gbias_y = y; - val.gbias_z = z; - if(lsm6dsv_sflp_game_gbias_set(®_ctx, &val) != LSM6DSV_OK) { - return LSM6DSV_ERROR; - } - return LSM6DSV_OK; -} + if (lsm6dsv_xl_full_scale_set(®_ctx, LSM6DSV_4g) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + delay(100); // Wait for Accelerometer to stabalize; + memset(val_st_off, 0x00, 3 * sizeof(float)); + memset(val_st_on, 0x00, 3 * sizeof(float)); -LSM6DSVStatusTypeDef LSM6DSV::Enable_X_User_Offset() { - lsm6dsv_ctrl9_t ctrl9; - if (lsm6dsv_read_reg(®_ctx, LSM6DSV_CTRL9, (uint8_t *)&ctrl9, 1) != LSM6DSV_OK) { - return LSM6DSV_ERROR; + /*Ignore First Data*/ + if (Get_X_AxesRaw_When_Aval(data_raw) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + for (uint8_t i = 0; i < 5; i++) { + if (Get_X_AxesRaw_When_Aval(data_raw) != LSM6DSV_OK) { + return LSM6DSV_ERROR; } - ctrl9.usr_off_on_out = true; //1 On, 0 Off - if (lsm6dsv_write_reg(®_ctx, LSM6DSV_CTRL9, (uint8_t *)&ctrl9, 1) != LSM6DSV_OK) { - return LSM6DSV_ERROR; + /*Average the data in each axis*/ + for (uint8_t j = 0; j < 3; j++) { + val_st_off[j] += lsm6dsv_from_fs4_to_mg(data_raw[j]); } + } - // lsm6dsv_wake_up_ths_t wake_up_ths; - // if (lsm6dsv_read_reg(®_ctx, LSM6DSV_CTRL9, (uint8_t *)&wake_up_ths, 1) != LSM6DSV_OK) { - // return LSM6DSV_ERROR; - //} - //wake_up_ths.usr_off_on_wu = true; //1 On, 0 Off + /* Calculate the mg average values */ + for (uint8_t i = 0; i < 3; i++) { + val_st_off[i] /= 5.0f; + } - //if (lsm6dsv_write_reg(®_ctx, LSM6DSV_CTRL9, (uint8_t *)&wake_up_ths, 1) != LSM6DSV_OK) { - // return LSM6DSV_ERROR; - //} + if (lsm6dsv_xl_self_test_set(®_ctx, (lsm6dsv_xl_self_test_t)TestType) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + delay(100); - return LSM6DSV_OK; -} + /*Ignore First Data*/ + if (Get_X_AxesRaw_When_Aval(data_raw) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } -LSM6DSVStatusTypeDef LSM6DSV::Disable_X_User_Offset() { - lsm6dsv_ctrl9_t ctrl9; - if (lsm6dsv_read_reg(®_ctx, LSM6DSV_CTRL9, (uint8_t *)&ctrl9, 1) != LSM6DSV_OK) { - return LSM6DSV_ERROR; + for (uint8_t i = 0; i < 5; i++) { + if (Get_X_AxesRaw_When_Aval(data_raw) != LSM6DSV_OK) { + return LSM6DSV_ERROR; } - ctrl9.usr_off_on_out = false; //1 On, 0 Off - if (lsm6dsv_write_reg(®_ctx, LSM6DSV_CTRL9, (uint8_t *)&ctrl9, 1) != LSM6DSV_OK) { - return LSM6DSV_ERROR; + /*Average the data in each axis*/ + for (uint8_t j = 0; j < 3; j++) { + val_st_on[j] += lsm6dsv_from_fs4_to_mg(data_raw[j]); } + } + + /* Calculate the mg average values */ + for (uint8_t i = 0; i < 3; i++) { + val_st_on[i] /= 5.0f; + } + + /* Calculate the mg values for self test */ + for (uint8_t i = 0; i < 3; i++) { + test_val[i] = fabs((val_st_on[i] - val_st_off[i])); + } - // lsm6dsv_wake_up_ths_t wake_up_ths; - // if (lsm6dsv_read_reg(®_ctx, LSM6DSV_CTRL9, (uint8_t *)&wake_up_ths, 1) != LSM6DSV_OK) { - // return LSM6DSV_ERROR; - //} - //wake_up_ths.usr_off_on_wu = false; //1 On, 0 Off + for (uint8_t i = 0; i < 3; i++) { + if ((LSM6DSV_MIN_ST_LIMIT_mg > test_val[i]) || (test_val[i] > LSM6DSV_MAX_ST_LIMIT_mg)) + return LSM6DSV_ERROR; + } - //if (lsm6dsv_write_reg(®_ctx, LSM6DSV_CTRL9, (uint8_t *)&wake_up_ths, 1) != LSM6DSV_OK) { - // return LSM6DSV_ERROR; - //} + if (lsm6dsv_xl_self_test_set(®_ctx, LSM6DSV_XL_ST_DISABLE) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } - return LSM6DSV_OK; + if (lsm6dsv_xl_data_rate_set(®_ctx, LSM6DSV_ODR_OFF) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + return LSM6DSV_OK; } -LSM6DSVStatusTypeDef LSM6DSV::Set_X_User_Offset(float x, float y, float z) { - lsm6dsv_ctrl9_t ctrl9; - if (lsm6dsv_read_reg(®_ctx, LSM6DSV_CTRL9, (uint8_t *)&ctrl9, 1) != LSM6DSV_OK) { - return LSM6DSV_ERROR; +/** + * @brief Get the LSM6DSV accelerometer sensor raw axes when avaiable (Blocking) + * @param Value pointer where the raw values of the axes are written + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSVStatusTypeDef LSM6DSV::Get_X_AxesRaw_When_Aval(int16_t *Value) +{ + lsm6dsv_data_ready_t drdy; + do { + if (lsm6dsv_flag_data_ready_get(®_ctx, &drdy) != LSM6DSV_OK) { + return LSM6DSV_ERROR; } - int8_t xyz[3]; + } while (!drdy.drdy_xl); - if ( // about +- 2 G's for high and +- 0.124 G's for low - (x <= LSM6DSV_ACC_USR_OFF_W_LOW_MAX && x >= -LSM6DSV_ACC_USR_OFF_W_LOW_MAX) && - (y <= LSM6DSV_ACC_USR_OFF_W_LOW_MAX && y >= -LSM6DSV_ACC_USR_OFF_W_LOW_MAX) && - (z <= LSM6DSV_ACC_USR_OFF_W_LOW_MAX && z >= -LSM6DSV_ACC_USR_OFF_W_LOW_MAX) - ) { //Then we are under the low requirements - xyz[0] = (int8_t)(x / LSM6DSV_ACC_USR_OFF_W_LOW_LSB); - xyz[1] = (int8_t)(y / LSM6DSV_ACC_USR_OFF_W_LOW_LSB); - xyz[2] = (int8_t)(z / LSM6DSV_ACC_USR_OFF_W_LOW_LSB); - ctrl9.usr_off_w = false; //(0: 2^-10 g/LSB; 1: 2^-6 g/LSB) + if (Get_X_AxesRaw(Value) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + return LSM6DSV_OK; +} + +/** + * @brief Runs the ST specified self test on the acceleration axis of the IMU + * @param TestType LSM6DSV_GY_ST_DISABLE = 0x0, LSM6DSV_GY_ST_POSITIVE = 0x1, LSM6DSV_GY_ST_NEGATIVE = 0x2 + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSVStatusTypeDef LSM6DSV::Test_G_IMU(uint8_t TestType = LSM6DSV_GY_ST_POSITIVE) +{ + int16_t data_raw[3]; + float test_val[3]; + + if (Device_Reset(LSM6DSV_RESET_CTRL_REGS) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + if (lsm6dsv_block_data_update_set(®_ctx, PROPERTY_ENABLE) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + /* + * Gyroscope Self Test + */ + + if (lsm6dsv_gy_data_rate_set(®_ctx, LSM6DSV_ODR_AT_240Hz) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + if (lsm6dsv_gy_full_scale_set(®_ctx, LSM6DSV_2000dps) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + delay(100); + + /*Ignore First Data*/ + if (Get_G_AxesRaw_When_Aval(data_raw) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + float val_st_off[3] = {0}; + float val_st_on[3] = {0}; + + for (uint8_t i = 0; i < 5; i++) { + if (Get_G_AxesRaw_When_Aval(data_raw) != LSM6DSV_OK) { + return LSM6DSV_ERROR; } - else if ( // about +- 2 G's for high and +- 0.124 G's for low - (x <= LSM6DSV_ACC_USR_OFF_W_HIGH_MAX && x >= -LSM6DSV_ACC_USR_OFF_W_HIGH_MAX) && - (y <= LSM6DSV_ACC_USR_OFF_W_HIGH_MAX && y >= -LSM6DSV_ACC_USR_OFF_W_HIGH_MAX) && - (z <= LSM6DSV_ACC_USR_OFF_W_HIGH_MAX && z >= -LSM6DSV_ACC_USR_OFF_W_HIGH_MAX) - ) { //Then we are under the high requirements - xyz[0] = (int8_t)(x / LSM6DSV_ACC_USR_OFF_W_HIGH_LSB); - xyz[1] = (int8_t)(y / LSM6DSV_ACC_USR_OFF_W_HIGH_LSB); - xyz[2] = (int8_t)(z / LSM6DSV_ACC_USR_OFF_W_HIGH_LSB); - ctrl9.usr_off_w = true; //(0: 2^-10 g/LSB; 1: 2^-6 g/LSB) - } else { - return LSM6DSV_ERROR; //Value too big + /*Average the data in each axis*/ + for (uint8_t j = 0; j < 3; j++) { + val_st_off[j] += lsm6dsv_from_fs2000_to_mdps(data_raw[j]); } + } + + /* Calculate the mg average values */ + for (uint8_t i = 0; i < 3; i++) { + val_st_off[i] /= 5.0f; + } + if (lsm6dsv_gy_self_test_set(®_ctx, (lsm6dsv_gy_self_test_t)TestType) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + delay(100); - if (lsm6dsv_write_reg(®_ctx, LSM6DSV_CTRL9, (uint8_t *)&ctrl9, 1) != LSM6DSV_OK) { - return LSM6DSV_ERROR; + /*Ignore First Data*/ + if (Get_G_AxesRaw_When_Aval(data_raw) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + for (uint8_t i = 0; i < 5; i++) { + if (Get_G_AxesRaw_When_Aval(data_raw) != LSM6DSV_OK) { + return LSM6DSV_ERROR; } + /*Average the data in each axis*/ + for (uint8_t j = 0; j < 3; j++) { + val_st_on[j] += lsm6dsv_from_fs2000_to_mdps(data_raw[j]); + } + } - //convert from float in G's to what it wants. Signed byte - printf("x: %d, y: %d, z: %d", xyz[0], xyz[1], xyz[2]); - if (lsm6dsv_write_reg(®_ctx, LSM6DSV_X_OFS_USR, (uint8_t*)&xyz, 3) != LSM6DSV_OK) { - return LSM6DSV_ERROR; + /* Calculate the mg average values */ + for (uint8_t i = 0; i < 3; i++) { + val_st_on[i] /= 5.0f; + } + + /* Calculate the mg values for self test */ + for (uint8_t i = 0; i < 3; i++) { + test_val[i] = fabs((val_st_on[i] - val_st_off[i])); + } + + /* Check self test limit */ + for (uint8_t i = 0; i < 3; i++) { + if ((LSM6DSV_MIN_ST_LIMIT_mdps > test_val[i]) || (test_val[i] > LSM6DSV_MAX_ST_LIMIT_mdps)) { + return LSM6DSV_ERROR; } - return LSM6DSV_OK; + } + + if (lsm6dsv_gy_self_test_set(®_ctx, LSM6DSV_GY_ST_DISABLE) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + if (lsm6dsv_xl_data_rate_set(®_ctx, LSM6DSV_ODR_OFF) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + return LSM6DSV_OK; +} + +/** + * @brief Get the LSM6DSV gyroscope sensor raw axes when data available (Blocking) + * @param Value pointer where the raw values of the axes are written + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSVStatusTypeDef LSM6DSV::Get_G_AxesRaw_When_Aval(int16_t *Value) +{ + lsm6dsv_data_ready_t drdy; + do { + if (lsm6dsv_flag_data_ready_get(®_ctx, &drdy) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + } while (!drdy.drdy_gy); + + if (Get_G_AxesRaw(Value) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + return LSM6DSV_OK; +} + +/** + * @brief Enable the LSM6DSV QVAR feature + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSVStatusTypeDef LSM6DSV::QVAR_Enable() +{ + lsm6dsv_ctrl7_t ctrl7; + + if (lsm6dsv_read_reg(®_ctx, LSM6DSV_CTRL7, (uint8_t *)&ctrl7, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + ctrl7.ah_qvar_en = 1; + ctrl7.int2_drdy_ah_qvar = 1; + + if (lsm6dsv_write_reg(®_ctx, LSM6DSV_CTRL7, (uint8_t *)&ctrl7, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + return LSM6DSV_OK; +} + +/** + * @brief Disable the LSM6DSV QVAR feature + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSVStatusTypeDef LSM6DSV::QVAR_Disable() +{ + lsm6dsv_ctrl7_t ctrl7; + + if (lsm6dsv_read_reg(®_ctx, LSM6DSV_CTRL7, (uint8_t *)&ctrl7, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + ctrl7.ah_qvar_en = 0; + ctrl7.int2_drdy_ah_qvar = 0; + + if (lsm6dsv_write_reg(®_ctx, LSM6DSV_CTRL7, (uint8_t *)&ctrl7, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + return LSM6DSV_OK; +} + +/** + * @brief Read LSM6DSV QVAR output data + * @param Data pointer where the value is written + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSVStatusTypeDef LSM6DSV::QVAR_GetData(float *Data) +{ + lsm6dsv_axis1bit16_t data_raw; + (void)memset(data_raw.u8bit, 0x00, sizeof(int16_t)); + + if (lsm6dsv_ah_qvar_raw_get(®_ctx, &data_raw.i16bit) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + *Data = ((float)data_raw.i16bit) / LSM6DSV_QVAR_GAIN; + return LSM6DSV_OK; +} + +/** + * @brief Get LSM6DSV QVAR equivalent input impedance + * @param val pointer where the value is written + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSVStatusTypeDef LSM6DSV::QVAR_GetImpedance(uint16_t *val) +{ + LSM6DSVStatusTypeDef ret = LSM6DSV_OK; + lsm6dsv_ah_qvar_zin_t imp; + + if (lsm6dsv_ah_qvar_zin_get(®_ctx, &imp) != LSM6DSV_OK) { + ret = LSM6DSV_ERROR; + } + switch (imp) { + case LSM6DSV_2400MOhm: + *val = 2400; + break; + case LSM6DSV_730MOhm: + *val = 730; + break; + case LSM6DSV_300MOhm: + *val = 300; + break; + case LSM6DSV_255MOhm: + *val = 255; + break; + default: + ret = LSM6DSV_ERROR; + break; + } + + return ret; } -uint32_t LSM6DSV::Half_Bits_To_Float_Bits(uint16_t h) -{ - uint16_t h_exp, h_sig; - uint32_t f_sgn, f_exp, f_sig; - - h_exp = (h&0x7c00u); - f_sgn = ((uint32_t)h&0x8000u) << 16; - switch (h_exp) { - case 0x0000u: /* 0 or subnormal */ - h_sig = (h&0x03ffu); - /* Signed zero */ - if (h_sig == 0) { - return f_sgn; - } - /* Subnormal */ - h_sig <<= 1; - while ((h_sig&0x0400u) == 0) { - h_sig <<= 1; - h_exp++; - } - f_exp = ((uint32_t)(127 - 15 - h_exp)) << 23; - f_sig = ((uint32_t)(h_sig&0x03ffu)) << 13; - return f_sgn + f_exp + f_sig; - case 0x7c00u: /* inf or NaN */ - /* All-ones exponent and a copy of the significand */ - return f_sgn + 0x7f800000u + (((uint32_t)(h&0x03ffu)) << 13); - default: /* normalized */ - /* Just need to adjust the exponent and shift */ - return f_sgn + (((uint32_t)(h&0x7fffu) + 0x1c000u) << 13); +/** + * @brief Set LSM6DSV QVAR equivalent input impedance + * @param val impedance in MOhm (2400MOhm, 730MOhm, 300MOhm, 255MOhm) + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSVStatusTypeDef LSM6DSV::QVAR_SetImpedance(uint16_t val) +{ + LSM6DSVStatusTypeDef ret = LSM6DSV_OK; + lsm6dsv_ah_qvar_zin_t imp; + switch (val) { + case 2400: + imp = LSM6DSV_2400MOhm; + break; + case 730: + imp = LSM6DSV_730MOhm; + break; + case 300: + imp = LSM6DSV_300MOhm; + break; + case 255: + imp = LSM6DSV_255MOhm; + break; + default: + ret = LSM6DSV_ERROR; + break; + } + if (ret != LSM6DSV_ERROR) { + if (lsm6dsv_ah_qvar_zin_set(®_ctx, imp) != LSM6DSV_OK) { + ret = LSM6DSV_ERROR; } + } + return ret; } + +/** + * @brief Read LSM6DSV QVAR status + * @param val pointer where the value is written + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSVStatusTypeDef LSM6DSV::QVAR_GetStatus(uint8_t *val) +{ + lsm6dsv_status_reg_t status; + + if (lsm6dsv_read_reg(®_ctx, LSM6DSV_STATUS_REG, (uint8_t *)&status, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + *val = status.ah_qvarda; + + return LSM6DSV_OK; +} + +/** + * @brief Get MLC status + * @param status pointer where the MLC status is written + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSVStatusTypeDef LSM6DSV::Get_MLC_Status(lsm6dsv_mlc_status_mainpage_t *status) +{ + if (lsm6dsv_read_reg(®_ctx, LSM6DSV_MLC_STATUS_MAINPAGE, (uint8_t *)status, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + return LSM6DSV_OK; +} + +/** + * @brief Get MLC output + * @param output pointer where the MLC output is written + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSVStatusTypeDef LSM6DSV::Get_MLC_Output(lsm6dsv_mlc_out_t *output) +{ + if (lsm6dsv_mlc_out_get(®_ctx, output) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + return LSM6DSV_OK; +} + +/** + * @brief Enable Rotation Vector SFLP feature + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSVStatusTypeDef LSM6DSV::Enable_Rotation_Vector() +{ + lsm6dsv_fifo_sflp_raw_t fifo_sflp; + + if (lsm6dsv_fifo_sflp_batch_get(®_ctx, &fifo_sflp)) { + return LSM6DSV_ERROR; + } + /* Enable Rotation Vector SFLP feature */ + fifo_sflp.game_rotation = 1; + + if (lsm6dsv_fifo_sflp_batch_set(®_ctx, fifo_sflp)) { + return LSM6DSV_ERROR; + } + + + /* Enable SFLP low power */ + if (lsm6dsv_sflp_game_rotation_set(®_ctx, PROPERTY_ENABLE)) { + return LSM6DSV_ERROR; + } + return LSM6DSV_OK; +} + +/** + * @brief Disable Rotation Vector SFLP feature + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSVStatusTypeDef LSM6DSV::Disable_Rotation_Vector() +{ + lsm6dsv_fifo_sflp_raw_t fifo_sflp; + + if (lsm6dsv_fifo_sflp_batch_get(®_ctx, &fifo_sflp)) { + return LSM6DSV_ERROR; + } + /* Disable Rotation Vector SFLP feature */ + fifo_sflp.game_rotation = 0; + + if (lsm6dsv_fifo_sflp_batch_set(®_ctx, fifo_sflp)) { + return LSM6DSV_ERROR; + } + + /* Check if all SFLP features are disabled */ + if (!fifo_sflp.game_rotation && !fifo_sflp.gravity && !fifo_sflp.gbias) { + /* Disable SFLP */ + if (lsm6dsv_sflp_game_rotation_set(®_ctx, PROPERTY_DISABLE)) { + return LSM6DSV_ERROR; + } + } + return LSM6DSV_OK; +} + +/** + * @brief Enable Gravity Vector SFLP feature + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSVStatusTypeDef LSM6DSV::Enable_Gravity_Vector() +{ + lsm6dsv_fifo_sflp_raw_t fifo_sflp; + + + if (lsm6dsv_fifo_sflp_batch_get(®_ctx, &fifo_sflp)) { + return LSM6DSV_ERROR; + } + /* Enable Gravity Vector SFLP feature */ + fifo_sflp.gravity = 1; + + if (lsm6dsv_fifo_sflp_batch_set(®_ctx, fifo_sflp)) { + return LSM6DSV_ERROR; + } + + + /* Enable SFLP low power */ + if (lsm6dsv_sflp_game_rotation_set(®_ctx, PROPERTY_ENABLE)) { + return LSM6DSV_ERROR; + } + return LSM6DSV_OK; +} + +/** + * @brief Disable Gravity Vector SFLP feature + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSVStatusTypeDef LSM6DSV::Disable_Gravity_Vector() +{ + lsm6dsv_fifo_sflp_raw_t fifo_sflp; + + /* Set full scale */ + if (lsm6dsv_xl_full_scale_set(®_ctx, LSM6DSV_4g)) { + return LSM6DSV_ERROR; + } + if (lsm6dsv_gy_full_scale_set(®_ctx, LSM6DSV_2000dps)) { + return LSM6DSV_ERROR; + } + + if (lsm6dsv_fifo_sflp_batch_get(®_ctx, &fifo_sflp)) { + return LSM6DSV_ERROR; + } + /* Disable Gravity Vector SFLP feature */ + fifo_sflp.gravity = 0; + + if (lsm6dsv_fifo_sflp_batch_set(®_ctx, fifo_sflp)) { + return LSM6DSV_ERROR; + } + + /* Check if all SFLP features are disabled */ + if (!fifo_sflp.game_rotation && !fifo_sflp.gravity && !fifo_sflp.gbias) { + /* Disable SFLP */ + if (lsm6dsv_sflp_game_rotation_set(®_ctx, PROPERTY_DISABLE)) { + return LSM6DSV_ERROR; + } + } + return LSM6DSV_OK; +} + +/** + * @brief Enable Gyroscope Bias SFLP feature + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSVStatusTypeDef LSM6DSV::Enable_Gyroscope_Bias() +{ + lsm6dsv_fifo_sflp_raw_t fifo_sflp; + + if (lsm6dsv_fifo_sflp_batch_get(®_ctx, &fifo_sflp)) { + return LSM6DSV_ERROR; + } + /* Enable Gyroscope Bias SFLP feature */ + fifo_sflp.gbias = 1; + + if (lsm6dsv_fifo_sflp_batch_set(®_ctx, fifo_sflp)) { + return LSM6DSV_ERROR; + } + + /* Enable SFLP low power */ + if (lsm6dsv_sflp_game_rotation_set(®_ctx, PROPERTY_ENABLE)) { + return LSM6DSV_ERROR; + } + return LSM6DSV_OK; +} + +/** + * @brief Disable Gyroscope Bias SFLP feature + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSVStatusTypeDef LSM6DSV::Disable_Gyroscope_Bias() +{ + lsm6dsv_fifo_sflp_raw_t fifo_sflp; + + + if (lsm6dsv_fifo_sflp_batch_get(®_ctx, &fifo_sflp)) { + return LSM6DSV_ERROR; + } + /* Disable Gyroscope Bias SFLP feature */ + fifo_sflp.gbias = 0; + + if (lsm6dsv_fifo_sflp_batch_set(®_ctx, fifo_sflp)) { + return LSM6DSV_ERROR; + } + + /* Check if all SFLP features are disabled */ + if (!fifo_sflp.game_rotation && !fifo_sflp.gravity && !fifo_sflp.gbias) { + /* Disable SFLP */ + if (lsm6dsv_sflp_game_rotation_set(®_ctx, PROPERTY_DISABLE)) { + return LSM6DSV_ERROR; + } + } + return LSM6DSV_OK; +} + +/** + * @brief Bulk set SFLP feature + * @param GameRotation enable/disable Game Rotation Vector SFLP feature + * @param Gravity enable/disable Gravity Vector SFLP feature + * @param gBias enable/disable Gyroscope Bias SFLP feature + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSVStatusTypeDef LSM6DSV::Set_SFLP_Batch(bool GameRotation, bool Gravity, bool gBias) +{ + lsm6dsv_fifo_sflp_raw_t fifo_sflp; + fifo_sflp.game_rotation = GameRotation; + fifo_sflp.gravity = Gravity; + fifo_sflp.gbias = gBias; + if (lsm6dsv_fifo_sflp_batch_set(®_ctx, fifo_sflp)) { + return LSM6DSV_ERROR; + } + + /* Check if all SFLP features are disabled */ + if (!fifo_sflp.game_rotation && !fifo_sflp.gravity && !fifo_sflp.gbias) { + /* Disable SFLP */ + if (lsm6dsv_sflp_game_rotation_set(®_ctx, PROPERTY_DISABLE)) { + return LSM6DSV_ERROR; + } + } + else { + /* Enable SFLP low power */ + if (lsm6dsv_sflp_game_rotation_set(®_ctx, PROPERTY_ENABLE)) { + return LSM6DSV_ERROR; + } + } + return LSM6DSV_OK; +} + + +/** + * @brief Set the LSM6DSV sensor fusion low power (sflp) output data rate + * @param Odr the output data rate value to be set + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSVStatusTypeDef LSM6DSV::Set_SFLP_ODR(float odr) +{ + lsm6dsv_sflp_data_rate_t rate = odr <= 15 ? LSM6DSV_SFLP_15Hz + : odr <= 30 ? LSM6DSV_SFLP_30Hz + : odr <= 60 ? LSM6DSV_SFLP_60Hz + : odr <= 120 ? LSM6DSV_SFLP_120Hz + : odr <= 240 ? LSM6DSV_SFLP_240Hz + : LSM6DSV_SFLP_480Hz; + + return (LSM6DSVStatusTypeDef)lsm6dsv_sflp_data_rate_set(®_ctx, rate); +} + +/** + * @brief Set the LSM6DSV sflp G bias + * @param x the gyro bias in the x axis to be set + * @param y the gyro bias in the y axis to be set + * @param z the gyro bias in the z axis to be set + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSVStatusTypeDef LSM6DSV::Set_SFLP_GBIAS(float x, float y, float z) +{ + lsm6dsv_sflp_gbias_t val = {0, 0, 0}; + val.gbias_x = x; + val.gbias_y = y; + val.gbias_z = z; + if (lsm6dsv_sflp_game_gbias_set(®_ctx, &val) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + return LSM6DSV_OK; +} + +/** + * @brief Reset SFLP + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSVStatusTypeDef LSM6DSV::Reset_SFLP(void) +{ + lsm6dsv_emb_func_init_a_t emb_func_init_a; + + if (lsm6dsv_mem_bank_set(®_ctx, LSM6DSV_EMBED_FUNC_MEM_BANK) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + if (lsm6dsv_read_reg(®_ctx, LSM6DSV_EMB_FUNC_INIT_A, (uint8_t *)&emb_func_init_a, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + emb_func_init_a.sflp_game_init = 1; + + if (lsm6dsv_write_reg(®_ctx, LSM6DSV_EMB_FUNC_INIT_A, (uint8_t *)&emb_func_init_a, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + if (lsm6dsv_mem_bank_set(®_ctx, LSM6DSV_MAIN_MEM_BANK) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + return LSM6DSV_OK; +} + +/** + * @brief Enable the LSM6DSV block update feature + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSVStatusTypeDef LSM6DSV::Enable_Block_Data_Update() +{ + return (LSM6DSVStatusTypeDef)lsm6dsv_block_data_update_set(®_ctx, PROPERTY_ENABLE); +} + +/** + * @brief Disable the LSM6DSV block update feature + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSVStatusTypeDef LSM6DSV::Disable_Block_Data_Update() +{ + return (LSM6DSVStatusTypeDef)lsm6dsv_block_data_update_set(®_ctx, PROPERTY_DISABLE); +} + +/** + * @brief Enable register address automatically incremented during a multiple byte access with a serial interface. + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSVStatusTypeDef LSM6DSV::Enable_Auto_Increment() +{ + return (LSM6DSVStatusTypeDef)lsm6dsv_auto_increment_set(®_ctx, PROPERTY_ENABLE); +} + +/** + * @brief Disable register address automatically incremented during a multiple byte access with a serial interface. + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSVStatusTypeDef LSM6DSV::Disable_Auto_Increment() +{ + return (LSM6DSVStatusTypeDef)lsm6dsv_auto_increment_set(®_ctx, PROPERTY_DISABLE); +} + +/** + * @brief Preform a full reset of the LSM6DSV + * @param flags lsm6dsv_reset_t flags for what to reset + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSVStatusTypeDef LSM6DSV::Device_Reset(LSM6DSV_Reset_t flags) +{ + if (lsm6dsv_reset_set(®_ctx, (lsm6dsv_reset_t)flags) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + lsm6dsv_reset_t rst; + do { + if (lsm6dsv_reset_get(®_ctx, &rst) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + } while (rst != LSM6DSV_READY); + return LSM6DSV_OK; +} + +/** + * @brief Get the LSM6DSV register value + * @param Reg address to be read + * @param Data pointer where the value is written + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSVStatusTypeDef LSM6DSV::Read_Reg(uint8_t Reg, uint8_t *Data) +{ + if (lsm6dsv_read_reg(®_ctx, Reg, Data, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + return LSM6DSV_OK; +} + +/** + * @brief Set the LSM6DSV register value + * @param Reg address to be written + * @param Data value to be written + * @retval 0 in case of success, an error code otherwise + */ +LSM6DSVStatusTypeDef LSM6DSV::Write_Reg(uint8_t Reg, uint8_t Data) +{ + if (lsm6dsv_write_reg(®_ctx, Reg, &Data, 1) != LSM6DSV_OK) { + return LSM6DSV_ERROR; + } + + return LSM6DSV_OK; +} + +int32_t LSM6DSV_io_write(void *handle, uint8_t WriteAddr, const uint8_t *pBuffer, uint16_t nBytesToWrite) +{ + return ((LSM6DSV *)handle)->IO_Write(pBuffer, WriteAddr, nBytesToWrite); +} + +int32_t LSM6DSV_io_read(void *handle, uint8_t ReadAddr, uint8_t *pBuffer, uint16_t nBytesToRead) +{ + return ((LSM6DSV *)handle)->IO_Read(pBuffer, ReadAddr, nBytesToRead); +} + +void LSM6DSV_sleep(uint32_t ms) { + delay(ms); +} + +/** + * @brief Converts uint16_t half-precision number into a uint32_t single-precision number. + * @param h half-precision number + * @param f pointer where the result of the conversion is written + * @retval 0 + */ +LSM6DSVStatusTypeDef LSM6DSV::npy_halfbits_to_floatbits(uint16_t h, uint32_t *f) +{ + uint16_t h_exp, h_sig; + uint32_t f_sgn, f_exp, f_sig; + + h_exp = (h & 0x7c00u); + f_sgn = ((uint32_t)h & 0x8000u) << 16; + switch (h_exp) { + case 0x0000u: /* 0 or subnormal */ + h_sig = (h & 0x03ffu); + /* Signed zero */ + if (h_sig == 0) { + *f = f_sgn; + return LSM6DSV_OK; + } + /* Subnormal */ + h_sig <<= 1; + while ((h_sig & 0x0400u) == 0) { + h_sig <<= 1; + h_exp++; + } + f_exp = ((uint32_t)(127 - 15 - h_exp)) << 23; + f_sig = ((uint32_t)(h_sig & 0x03ffu)) << 13; + *f = f_sgn + f_exp + f_sig; + return LSM6DSV_OK; + case 0x7c00u: /* inf or NaN */ + /* All-ones exponent and a copy of the significand */ + *f = f_sgn + 0x7f800000u + (((uint32_t)(h & 0x03ffu)) << 13); + return LSM6DSV_OK; + default: /* normalized */ + /* Just need to adjust the exponent and shift */ + *f = f_sgn + (((uint32_t)(h & 0x7fffu) + 0x1c000u) << 13); + return LSM6DSV_OK; + } +} + +/** + * @brief Converts uint16_t half-precision number into a float single-precision number. + * @param h half-precision number + * @param f pointer where the result of the conversion is written + * @retval 0 + */ +LSM6DSVStatusTypeDef LSM6DSV::npy_half_to_float(uint16_t h, float *f) +{ + union { + float ret; + uint32_t retbits; + } conv; + npy_halfbits_to_floatbits(h, &conv.retbits); + *f = conv.ret; + return LSM6DSV_OK; +} + +/** + * @brief Compute quaternions. + * @param quat results of the computation + * @param sflp raw value of the quaternions + * @retval 0 + */ +LSM6DSVStatusTypeDef LSM6DSV::sflp2q(float quat[4], uint16_t sflp[3]) +{ + float sumsq = 0; + + npy_half_to_float(sflp[0], &quat[0]); + npy_half_to_float(sflp[1], &quat[1]); + npy_half_to_float(sflp[2], &quat[2]); + + for (uint8_t i = 0; i < 3; i++) { + sumsq += quat[i] * quat[i]; + } + + if (sumsq > 1.0f) { + float n = sqrtf(sumsq); + quat[0] /= n; + quat[1] /= n; + quat[2] /= n; + sumsq = 1.0f; + } + + quat[3] = sqrtf(1.0f - sumsq); + return LSM6DSV_OK; +} \ No newline at end of file diff --git a/lib/LSM6DSV/LSM6DSV.h b/lib/LSM6DSV/LSM6DSV.h index 0b5cf200e..7cda9a112 100644 --- a/lib/LSM6DSV/LSM6DSV.h +++ b/lib/LSM6DSV/LSM6DSV.h @@ -1,8 +1,8 @@ /** ****************************************************************************** * @file LSM6DSV.h - * @author SRA - * @version V1.5.1 + * @author STMicroelectronics + * @version V1.0.0 * @date July 2022 * @brief Abstract Class of a LSM6DSV inertial measurement sensor. ****************************************************************************** @@ -50,6 +50,12 @@ /* Defines -------------------------------------------------------------------*/ +/* For compatibility with ESP32 platforms */ +#ifdef ESP32 + #ifndef MSBFIRST + #define MSBFIRST SPI_MSBFIRST + #endif +#endif #define LSM6DSV_ACC_SENSITIVITY_FS_2G 0.061f #define LSM6DSV_ACC_SENSITIVITY_FS_4G 0.122f @@ -73,12 +79,6 @@ #define LSM6DSV_ACC_USR_OFF_W_HIGH_MAX LSM6DSV_ACC_USR_OFF_W_HIGH_LSB * INT8_MAX #define LSM6DSV_ACC_USR_OFF_W_LOW_MAX LSM6DSV_ACC_USR_OFF_W_LOW_LSB * INT8_MAX - - - -//#define ENABLE_SPI -//#define I2C_LIB_DEBUG - /* Typedefs ------------------------------------------------------------------*/ typedef enum { @@ -112,11 +112,11 @@ typedef union { uint8_t u8bit[2]; } lsm6dsv_axis1bit16_t; -enum LSM6DSV_Reset { +typedef enum { LSM6DSV_RESET_GLOBAL = 0x1, LSM6DSV_RESET_CAL_PARAM = 0x2, LSM6DSV_RESET_CTRL_REGS = 0x4, -}; +} LSM6DSV_Reset_t; typedef enum { LSM6DSV_ACC_HIGH_PERFORMANCE_MODE, @@ -151,20 +151,19 @@ class LSM6DSV { LSM6DSVStatusTypeDef Enable_X(); LSM6DSVStatusTypeDef Disable_X(); - LSM6DSVStatusTypeDef Enable_X_User_Offset(); - LSM6DSVStatusTypeDef Disable_X_User_Offset(); LSM6DSVStatusTypeDef Get_X_Sensitivity(float *Sensitivity); LSM6DSVStatusTypeDef Get_X_ODR(float *Odr); LSM6DSVStatusTypeDef Set_X_ODR(float Odr, LSM6DSV_ACC_Operating_Mode_t Mode = LSM6DSV_ACC_HIGH_PERFORMANCE_MODE); LSM6DSVStatusTypeDef Get_X_FS(int32_t *FullScale); LSM6DSVStatusTypeDef Set_X_FS(int32_t FullScale); LSM6DSVStatusTypeDef Get_X_AxesRaw(int16_t *Value); - LSM6DSVStatusTypeDef Get_X_AxesRaw_When_Aval(int16_t *Value); LSM6DSVStatusTypeDef Get_X_Axes(int32_t *Acceleration); LSM6DSVStatusTypeDef Get_X_DRDY_Status(uint8_t *Status); LSM6DSVStatusTypeDef Get_X_Event_Status(LSM6DSV_Event_Status_t *Status); LSM6DSVStatusTypeDef Set_X_Power_Mode(uint8_t PowerMode); LSM6DSVStatusTypeDef Set_X_Filter_Mode(uint8_t LowHighPassFlag, uint8_t FilterMode); + LSM6DSVStatusTypeDef Enable_X_User_Offset(); + LSM6DSVStatusTypeDef Disable_X_User_Offset(); LSM6DSVStatusTypeDef Set_X_User_Offset(float x, float y, float z); LSM6DSVStatusTypeDef Enable_G(); @@ -175,24 +174,19 @@ class LSM6DSV { LSM6DSVStatusTypeDef Get_G_FS(int32_t *FullScale); LSM6DSVStatusTypeDef Set_G_FS(int32_t FullScale); LSM6DSVStatusTypeDef Get_G_AxesRaw(int16_t *Value); - LSM6DSVStatusTypeDef Get_G_AxesRaw_When_Aval(int16_t *Value); LSM6DSVStatusTypeDef Get_G_Axes(int32_t *AngularRate); LSM6DSVStatusTypeDef Get_G_DRDY_Status(uint8_t *Status); LSM6DSVStatusTypeDef Set_G_Power_Mode(uint8_t PowerMode); LSM6DSVStatusTypeDef Set_G_Filter_Mode(uint8_t LowHighPassFlag, uint8_t FilterMode); - LSM6DSVStatusTypeDef Set_G_Bias(float x, float y, float z); + + LSM6DSVStatusTypeDef Get_Temp_ODR(float *Odr); + LSM6DSVStatusTypeDef Set_Temp_ODR(float Odr); + LSM6DSVStatusTypeDef Get_Temp_Raw(int16_t *value); LSM6DSVStatusTypeDef Test_IMU(uint8_t XTestType, uint8_t GTestType); LSM6DSVStatusTypeDef Test_X_IMU(uint8_t TestType); LSM6DSVStatusTypeDef Test_G_IMU(uint8_t TestType); - LSM6DSVStatusTypeDef Get_T_ODR(float *Odr); - LSM6DSVStatusTypeDef Set_T_ODR(float Odr); - - LSM6DSVStatusTypeDef Enable_SFLP_Block(bool enable = true); - LSM6DSVStatusTypeDef Set_SFLP_ODR(float Odr); - LSM6DSVStatusTypeDef Set_SFLP_GBIAS(float x, float y, float z); - LSM6DSVStatusTypeDef Enable_6D_Orientation(LSM6DSV_SensorIntPin_t IntPin); LSM6DSVStatusTypeDef Disable_6D_Orientation(); LSM6DSVStatusTypeDef Set_6D_Orientation_Threshold(uint8_t Threshold); @@ -230,7 +224,6 @@ class LSM6DSV { LSM6DSVStatusTypeDef Enable_Tilt_Detection(LSM6DSV_SensorIntPin_t IntPin); LSM6DSVStatusTypeDef Disable_Tilt_Detection(); - LSM6DSVStatusTypeDef FIFO_Reset(); LSM6DSVStatusTypeDef FIFO_Get_Num_Samples(uint16_t *NumSamples); LSM6DSVStatusTypeDef FIFO_Get_Full_Status(uint8_t *Status); LSM6DSVStatusTypeDef FIFO_Set_INT1_FIFO_Full(uint8_t Status); @@ -244,27 +237,45 @@ class LSM6DSV { LSM6DSVStatusTypeDef FIFO_Set_X_BDR(float Bdr); LSM6DSVStatusTypeDef FIFO_Get_G_Axes(int32_t *AngularVelocity); LSM6DSVStatusTypeDef FIFO_Set_G_BDR(float Bdr); - LSM6DSVStatusTypeDef FIFO_Set_SFLP_Batch(bool GameRotation, bool Gravity, bool gBias); LSM6DSVStatusTypeDef FIFO_Get_Status(lsm6dsv_fifo_status_t *Status); - LSM6DSVStatusTypeDef FIFO_Get_Gravity_Vector(float *data); - LSM6DSVStatusTypeDef FIFO_Get_Game_Vector(float *data); - LSM6DSVStatusTypeDef FIFO_Get_GBias_Vector(float *data); - LSM6DSVStatusTypeDef FIFO_Get_Timestamp(uint32_t *timestamp); + LSM6DSVStatusTypeDef FIFO_Get_Rotation_Vector(float *rvec); + LSM6DSVStatusTypeDef FIFO_Get_Gravity_Vector(float *gvec); + LSM6DSVStatusTypeDef FIFO_Get_Gyroscope_Bias(float *gbias); + LSM6DSVStatusTypeDef FIFO_Enable_Timestamp(); + LSM6DSVStatusTypeDef FIFO_Disable_Timestamp(); LSM6DSVStatusTypeDef FIFO_Set_Timestamp_Decimation(uint8_t decimation); - LSM6DSVStatusTypeDef Enable_Timestamp(); + LSM6DSVStatusTypeDef FIFO_Get_Timestamp(uint32_t *timestamp); + LSM6DSVStatusTypeDef FIFO_Reset(); + + LSM6DSVStatusTypeDef QVAR_Enable(); + LSM6DSVStatusTypeDef QVAR_Disable(); + LSM6DSVStatusTypeDef QVAR_GetStatus(uint8_t *val); + LSM6DSVStatusTypeDef QVAR_GetImpedance(uint16_t *val); + LSM6DSVStatusTypeDef QVAR_SetImpedance(uint16_t val); + LSM6DSVStatusTypeDef QVAR_GetData(float *Data); + + LSM6DSVStatusTypeDef Get_MLC_Status(lsm6dsv_mlc_status_mainpage_t *status); + LSM6DSVStatusTypeDef Get_MLC_Output(lsm6dsv_mlc_out_t *output); + + LSM6DSVStatusTypeDef Enable_Rotation_Vector(); + LSM6DSVStatusTypeDef Disable_Rotation_Vector(); + LSM6DSVStatusTypeDef Enable_Gravity_Vector(); + LSM6DSVStatusTypeDef Disable_Gravity_Vector(); + LSM6DSVStatusTypeDef Enable_Gyroscope_Bias(); + LSM6DSVStatusTypeDef Disable_Gyroscope_Bias(); + LSM6DSVStatusTypeDef Set_SFLP_Batch(bool GameRotation, bool Gravity, bool gBias); + LSM6DSVStatusTypeDef Set_SFLP_ODR(float Odr); + LSM6DSVStatusTypeDef Set_SFLP_GBIAS(float x, float y, float z); + LSM6DSVStatusTypeDef Reset_SFLP(); LSM6DSVStatusTypeDef Read_Reg(uint8_t Reg, uint8_t *Data); LSM6DSVStatusTypeDef Write_Reg(uint8_t Reg, uint8_t Data); - LSM6DSVStatusTypeDef Reset_Set(uint8_t flags); - - LSM6DSVStatusTypeDef Enable_Block_Data_Update(bool enable = true); - LSM6DSVStatusTypeDef Set_Auto_Increment(bool enable); - - LSM6DSVStatusTypeDef Get_Temperature_Raw(int16_t *value); - LSM6DSVStatusTypeDef Is_New_Temperature_Data(bool *available); - - uint32_t Half_Bits_To_Float_Bits(uint16_t half_bits); + LSM6DSVStatusTypeDef Enable_Block_Data_Update(); + LSM6DSVStatusTypeDef Disable_Block_Data_Update(); + LSM6DSVStatusTypeDef Enable_Auto_Increment(); + LSM6DSVStatusTypeDef Disable_Auto_Increment(); + LSM6DSVStatusTypeDef Device_Reset(LSM6DSV_Reset_t flags = LSM6DSV_RESET_GLOBAL); /** * @brief Utility function to read data. @@ -275,13 +286,9 @@ class LSM6DSV { */ uint8_t IO_Read(uint8_t *pBuffer, uint8_t RegisterAddr, uint16_t NumByteToRead) { -#ifdef ENABLE_SPI if (dev_spi) { -//#ifdef esp32 - dev_spi->beginTransaction(SPISettings(spi_speed, SPI_MSBFIRST, SPI_MODE3)); -//#else dev_spi->beginTransaction(SPISettings(spi_speed, MSBFIRST, SPI_MODE3)); -//#endif + digitalWrite(cs_pin, LOW); /* Write Reg Address */ @@ -297,12 +304,8 @@ class LSM6DSV { return 0; } -#endif if (dev_i2c) { -#ifdef I2C_LIB_DEBUG - printf("\n\n[LSM LIB] Read register: 0x%02x, Byte Count: %d bytes", RegisterAddr, NumByteToRead); -#endif dev_i2c->beginTransmission(((uint8_t)(((address) >> 1) & 0x7F))); dev_i2c->write(RegisterAddr); dev_i2c->endTransmission(false); @@ -312,9 +315,6 @@ class LSM6DSV { int i = 0; while (dev_i2c->available()) { pBuffer[i] = dev_i2c->read(); -#ifdef I2C_LIB_DEBUG - printf("\n[LSM LIB] Register Read: 0x%02x, Data: 0x%02x", RegisterAddr + i, pBuffer[i]); -#endif i++; } @@ -333,13 +333,9 @@ class LSM6DSV { */ uint8_t IO_Write(const uint8_t *pBuffer, uint8_t RegisterAddr, uint16_t NumByteToWrite) { -#ifdef ENABLE_SPI if (dev_spi) { -//#ifdef esp32 - dev_spi->beginTransaction(SPISettings(spi_speed, SPI_MSBFIRST, SPI_MODE3)); -//#else dev_spi->beginTransaction(SPISettings(spi_speed, MSBFIRST, SPI_MODE3)); -//#endif + digitalWrite(cs_pin, LOW); /* Write Reg Address */ @@ -355,19 +351,12 @@ class LSM6DSV { return 0; } -#endif if (dev_i2c) { -#ifdef I2C_LIB_DEBUG - printf("\n\n[LSM LIB] Write register: 0x%02x, Byte Count: %d bytes", RegisterAddr, NumByteToWrite); -#endif dev_i2c->beginTransmission(((uint8_t)(((address) >> 1) & 0x7F))); dev_i2c->write(RegisterAddr); for (uint16_t i = 0 ; i < NumByteToWrite ; i++) { -#ifdef I2C_LIB_DEBUG - printf("\n[LSM LIB] Register Wrote: 0x%02x, Data: 0x%02x", RegisterAddr + i, pBuffer[i]); -#endif dev_i2c->write(pBuffer[i]); } @@ -384,6 +373,11 @@ class LSM6DSV { LSM6DSVStatusTypeDef Set_X_ODR_When_Disabled(float Odr); LSM6DSVStatusTypeDef Set_G_ODR_When_Enabled(float Odr); LSM6DSVStatusTypeDef Set_G_ODR_When_Disabled(float Odr); + LSM6DSVStatusTypeDef Get_X_AxesRaw_When_Aval(int16_t *Value); + LSM6DSVStatusTypeDef Get_G_AxesRaw_When_Aval(int16_t *Value); + LSM6DSVStatusTypeDef npy_halfbits_to_floatbits(uint16_t h, uint32_t *f); + LSM6DSVStatusTypeDef npy_half_to_float(uint16_t h, float *f); + LSM6DSVStatusTypeDef sflp2q(float quat[4], uint16_t sflp[3]); float Convert_X_Sensitivity(lsm6dsv_xl_full_scale_t full_scale); float Convert_G_Sensitivity(lsm6dsv_gy_full_scale_t full_scale); @@ -402,7 +396,6 @@ class LSM6DSV { lsm6dsv_xl_full_scale_t acc_fs; lsm6dsv_gy_full_scale_t gyro_fs; lsm6dsv_fifo_mode_t fifo_mode; - uint8_t acc_is_enabled; uint8_t gyro_is_enabled; uint8_t initialized; From 10a7a279136dc441e2e595388a9e7fb334779929 Mon Sep 17 00:00:00 2001 From: wigwagwent Date: Sat, 2 Dec 2023 14:02:06 -0700 Subject: [PATCH 55/60] Fix onboard fusion after update --- lib/LSM6DSV/LSM6DSV.cpp | 178 ---------------------------------- lib/LSM6DSV/LSM6DSV.h | 10 -- src/sensors/lsm6dsvSensor.cpp | 37 ++----- 3 files changed, 8 insertions(+), 217 deletions(-) diff --git a/lib/LSM6DSV/LSM6DSV.cpp b/lib/LSM6DSV/LSM6DSV.cpp index 1469c18dc..a2a099fd8 100644 --- a/lib/LSM6DSV/LSM6DSV.cpp +++ b/lib/LSM6DSV/LSM6DSV.cpp @@ -2004,7 +2004,6 @@ LSM6DSVStatusTypeDef LSM6DSV::Enable_Pedometer(LSM6DSV_SensorIntPin_t IntPin) /* Enable pedometer algorithm. */ mode.step_counter_enable = PROPERTY_ENABLE; - mode.false_step_rej = PROPERTY_DISABLE; /* Turn on embedded features */ if (lsm6dsv_stpcnt_mode_set(®_ctx, mode) != LSM6DSV_OK) { @@ -2109,7 +2108,6 @@ LSM6DSVStatusTypeDef LSM6DSV::Disable_Pedometer() /* Enable pedometer algorithm. */ mode.step_counter_enable = PROPERTY_DISABLE; - mode.false_step_rej = PROPERTY_DISABLE; /* Turn off embedded features */ if (lsm6dsv_stpcnt_mode_set(®_ctx, mode) != LSM6DSV_OK) { @@ -3623,182 +3621,6 @@ LSM6DSVStatusTypeDef LSM6DSV::Get_G_AxesRaw_When_Aval(int16_t *Value) return LSM6DSV_OK; } -/** - * @brief Enable the LSM6DSV QVAR feature - * @retval 0 in case of success, an error code otherwise - */ -LSM6DSVStatusTypeDef LSM6DSV::QVAR_Enable() -{ - lsm6dsv_ctrl7_t ctrl7; - - if (lsm6dsv_read_reg(®_ctx, LSM6DSV_CTRL7, (uint8_t *)&ctrl7, 1) != LSM6DSV_OK) { - return LSM6DSV_ERROR; - } - - ctrl7.ah_qvar_en = 1; - ctrl7.int2_drdy_ah_qvar = 1; - - if (lsm6dsv_write_reg(®_ctx, LSM6DSV_CTRL7, (uint8_t *)&ctrl7, 1) != LSM6DSV_OK) { - return LSM6DSV_ERROR; - } - - return LSM6DSV_OK; -} - -/** - * @brief Disable the LSM6DSV QVAR feature - * @retval 0 in case of success, an error code otherwise - */ -LSM6DSVStatusTypeDef LSM6DSV::QVAR_Disable() -{ - lsm6dsv_ctrl7_t ctrl7; - - if (lsm6dsv_read_reg(®_ctx, LSM6DSV_CTRL7, (uint8_t *)&ctrl7, 1) != LSM6DSV_OK) { - return LSM6DSV_ERROR; - } - - ctrl7.ah_qvar_en = 0; - ctrl7.int2_drdy_ah_qvar = 0; - - if (lsm6dsv_write_reg(®_ctx, LSM6DSV_CTRL7, (uint8_t *)&ctrl7, 1) != LSM6DSV_OK) { - return LSM6DSV_ERROR; - } - - return LSM6DSV_OK; -} - -/** - * @brief Read LSM6DSV QVAR output data - * @param Data pointer where the value is written - * @retval 0 in case of success, an error code otherwise - */ -LSM6DSVStatusTypeDef LSM6DSV::QVAR_GetData(float *Data) -{ - lsm6dsv_axis1bit16_t data_raw; - (void)memset(data_raw.u8bit, 0x00, sizeof(int16_t)); - - if (lsm6dsv_ah_qvar_raw_get(®_ctx, &data_raw.i16bit) != LSM6DSV_OK) { - return LSM6DSV_ERROR; - } - - *Data = ((float)data_raw.i16bit) / LSM6DSV_QVAR_GAIN; - return LSM6DSV_OK; -} - -/** - * @brief Get LSM6DSV QVAR equivalent input impedance - * @param val pointer where the value is written - * @retval 0 in case of success, an error code otherwise - */ -LSM6DSVStatusTypeDef LSM6DSV::QVAR_GetImpedance(uint16_t *val) -{ - LSM6DSVStatusTypeDef ret = LSM6DSV_OK; - lsm6dsv_ah_qvar_zin_t imp; - - if (lsm6dsv_ah_qvar_zin_get(®_ctx, &imp) != LSM6DSV_OK) { - ret = LSM6DSV_ERROR; - } - switch (imp) { - case LSM6DSV_2400MOhm: - *val = 2400; - break; - case LSM6DSV_730MOhm: - *val = 730; - break; - case LSM6DSV_300MOhm: - *val = 300; - break; - case LSM6DSV_255MOhm: - *val = 255; - break; - default: - ret = LSM6DSV_ERROR; - break; - } - - return ret; -} - -/** - * @brief Set LSM6DSV QVAR equivalent input impedance - * @param val impedance in MOhm (2400MOhm, 730MOhm, 300MOhm, 255MOhm) - * @retval 0 in case of success, an error code otherwise - */ -LSM6DSVStatusTypeDef LSM6DSV::QVAR_SetImpedance(uint16_t val) -{ - LSM6DSVStatusTypeDef ret = LSM6DSV_OK; - lsm6dsv_ah_qvar_zin_t imp; - switch (val) { - case 2400: - imp = LSM6DSV_2400MOhm; - break; - case 730: - imp = LSM6DSV_730MOhm; - break; - case 300: - imp = LSM6DSV_300MOhm; - break; - case 255: - imp = LSM6DSV_255MOhm; - break; - default: - ret = LSM6DSV_ERROR; - break; - } - if (ret != LSM6DSV_ERROR) { - if (lsm6dsv_ah_qvar_zin_set(®_ctx, imp) != LSM6DSV_OK) { - ret = LSM6DSV_ERROR; - } - } - return ret; -} - -/** - * @brief Read LSM6DSV QVAR status - * @param val pointer where the value is written - * @retval 0 in case of success, an error code otherwise - */ -LSM6DSVStatusTypeDef LSM6DSV::QVAR_GetStatus(uint8_t *val) -{ - lsm6dsv_status_reg_t status; - - if (lsm6dsv_read_reg(®_ctx, LSM6DSV_STATUS_REG, (uint8_t *)&status, 1) != LSM6DSV_OK) { - return LSM6DSV_ERROR; - } - - *val = status.ah_qvarda; - - return LSM6DSV_OK; -} - -/** - * @brief Get MLC status - * @param status pointer where the MLC status is written - * @retval 0 in case of success, an error code otherwise - */ -LSM6DSVStatusTypeDef LSM6DSV::Get_MLC_Status(lsm6dsv_mlc_status_mainpage_t *status) -{ - if (lsm6dsv_read_reg(®_ctx, LSM6DSV_MLC_STATUS_MAINPAGE, (uint8_t *)status, 1) != LSM6DSV_OK) { - return LSM6DSV_ERROR; - } - - return LSM6DSV_OK; -} - -/** - * @brief Get MLC output - * @param output pointer where the MLC output is written - * @retval 0 in case of success, an error code otherwise - */ -LSM6DSVStatusTypeDef LSM6DSV::Get_MLC_Output(lsm6dsv_mlc_out_t *output) -{ - if (lsm6dsv_mlc_out_get(®_ctx, output) != LSM6DSV_OK) { - return LSM6DSV_ERROR; - } - - return LSM6DSV_OK; -} - /** * @brief Enable Rotation Vector SFLP feature * @retval 0 in case of success, an error code otherwise diff --git a/lib/LSM6DSV/LSM6DSV.h b/lib/LSM6DSV/LSM6DSV.h index 7cda9a112..db519f2eb 100644 --- a/lib/LSM6DSV/LSM6DSV.h +++ b/lib/LSM6DSV/LSM6DSV.h @@ -247,16 +247,6 @@ class LSM6DSV { LSM6DSVStatusTypeDef FIFO_Get_Timestamp(uint32_t *timestamp); LSM6DSVStatusTypeDef FIFO_Reset(); - LSM6DSVStatusTypeDef QVAR_Enable(); - LSM6DSVStatusTypeDef QVAR_Disable(); - LSM6DSVStatusTypeDef QVAR_GetStatus(uint8_t *val); - LSM6DSVStatusTypeDef QVAR_GetImpedance(uint16_t *val); - LSM6DSVStatusTypeDef QVAR_SetImpedance(uint16_t val); - LSM6DSVStatusTypeDef QVAR_GetData(float *Data); - - LSM6DSVStatusTypeDef Get_MLC_Status(lsm6dsv_mlc_status_mainpage_t *status); - LSM6DSVStatusTypeDef Get_MLC_Output(lsm6dsv_mlc_out_t *output); - LSM6DSVStatusTypeDef Enable_Rotation_Vector(); LSM6DSVStatusTypeDef Disable_Rotation_Vector(); LSM6DSVStatusTypeDef Enable_Gravity_Vector(); diff --git a/src/sensors/lsm6dsvSensor.cpp b/src/sensors/lsm6dsvSensor.cpp index c1ebed207..06ae6a68a 100644 --- a/src/sensors/lsm6dsvSensor.cpp +++ b/src/sensors/lsm6dsvSensor.cpp @@ -105,11 +105,11 @@ void LSM6DSVSensor::motionSetup() { status |= imu.begin(); // Restore defaults - status |= imu.Reset_Set(LSM6DSV_RESET_CTRL_REGS); + status |= imu.Device_Reset(LSM6DSV_RESET_CTRL_REGS); // Enable Block Data Update status |= imu.Enable_Block_Data_Update(); - status |= imu.Set_Auto_Increment(true); + status |= imu.Enable_Auto_Increment(); // Set maximums status |= imu.Set_X_FS(LSM6DSV_ACCEL_MAX); @@ -129,7 +129,7 @@ void LSM6DSVSensor::motionSetup() { ); // Enable IMU - status |= imu.Enable_Timestamp(); + status |= imu.FIFO_Enable_Timestamp(); status |= imu.Enable_X(); status |= imu.Enable_G(); @@ -143,9 +143,8 @@ void LSM6DSVSensor::motionSetup() { status |= imu.FIFO_Set_Mode(LSM6DSV_STREAM_MODE); #if (LSM6DSV_FUSION_SOURCE == LSM6DSV_FUSION_ONBOARD) - status |= imu.FIFO_Set_SFLP_Batch(true, true, false); status |= imu.Set_SFLP_ODR(LSM6DSV_FIFO_DATA_RATE); - status |= imu.Enable_SFLP_Block(); + status |= imu.Set_SFLP_Batch(true, true, false); #endif @@ -230,7 +229,7 @@ void LSM6DSVSensor::motionLoop() { lastTempRead = millis(); int16_t rawTemp; - if (imu.Get_Temperature_Raw(&rawTemp) != LSM6DSV_OK) { + if (imu.Get_Temp_Raw(&rawTemp) != LSM6DSV_OK) { m_Logger.error( "Error getting temperature on %s at address 0x%02x", getIMUNameByType(sensorType), @@ -328,21 +327,6 @@ SensorStatus LSM6DSVSensor::getSensorState() { : SensorStatus::SENSOR_OFFLINE; } -Quat LSM6DSVSensor::fusedRotationToQuaternion(float x, float y, float z) { - float length2 = x * x + y * y + z * z; - - if (length2 > 1) { - float length = sqrt(length2); - x /= length; - y /= length; - z /= length; - length2 = 1; - } - - float w = sqrt(1 - length2); - return Quat(x, y, z, w); -} - LSM6DSVStatusTypeDef LSM6DSVSensor::runSelfTest() { m_Logger.info( "%s Self Test started on address: 0x%02x", @@ -515,8 +499,8 @@ LSM6DSVStatusTypeDef LSM6DSVSensor::readFifo(uint16_t fifo_samples) { #if (LSM6DSV_FUSION_SOURCE == LSM6DSV_FUSION_ONBOARD) case lsm6dsv_fifo_out_raw_t::LSM6DSV_SFLP_GAME_ROTATION_VECTOR_TAG: { - float fusedGameRotation[3]; - if (imu.FIFO_Get_Game_Vector(fusedGameRotation) != LSM6DSV_OK) { + float quat[4]; + if (imu.FIFO_Get_Rotation_Vector(quat) != LSM6DSV_OK) { m_Logger.error( "Failed to get game rotation vector on %s at address 0x%02x", getIMUNameByType(sensorType), @@ -525,12 +509,7 @@ LSM6DSVStatusTypeDef LSM6DSVSensor::readFifo(uint16_t fifo_samples) { return LSM6DSV_ERROR; } - fusedRotation = fusedRotationToQuaternion( - fusedGameRotation[0], - fusedGameRotation[1], - fusedGameRotation[2] - ) - * sensorOffset; + fusedRotation = Quat(quat[0], quat[1], quat[2], quat[3]) * sensorOffset; if (ENABLE_INSPECTION || !OPTIMIZE_UPDATES || !lastFusedRotationSent.equalsWithEpsilon(fusedRotation)) { newFusedRotation = true; From 8029bce197f9c2c28bb7fad17d8c83737e27793c Mon Sep 17 00:00:00 2001 From: wigwagwent Date: Sat, 2 Dec 2023 14:37:24 -0700 Subject: [PATCH 56/60] fix formatting --- src/sensors/lsm6dsvSensor.cpp | 307 +++++++++++++++++++--------------- src/sensors/lsm6dsvSensor.h | 53 +++--- 2 files changed, 193 insertions(+), 167 deletions(-) diff --git a/src/sensors/lsm6dsvSensor.cpp b/src/sensors/lsm6dsvSensor.cpp index 06ae6a68a..1ead324a7 100644 --- a/src/sensors/lsm6dsvSensor.cpp +++ b/src/sensors/lsm6dsvSensor.cpp @@ -22,16 +22,15 @@ */ #include "sensors/lsm6dsvSensor.h" + #include "GlobalVars.h" #include "lsm6dsvSensor.h" #include "utils.h" - #ifdef LSM6DSV_INTERRUPT volatile bool imuEvent = false; void IRAM_ATTR interruptHandler() { imuEvent = true; } -#endif - +#endif // LSM6DSV_INTERRUPT LSM6DSVSensor::LSM6DSVSensor( uint8_t id, @@ -49,7 +48,7 @@ LSM6DSVSensor::LSM6DSVSensor( 1.0f / LSM6DSV_FIFO_DATA_RATE, 1.0f / LSM6DSV_FIFO_DATA_RATE, -1.0f ) -#endif +#endif //LSM6DSV_FUSION_SOURCE == LSM6DSV_FUSION_ESP {}; void LSM6DSVSensor::motionSetup() { @@ -82,7 +81,7 @@ void LSM6DSVSensor::motionSetup() { status |= imu.Enable_6D_Orientation(LSM6DSV_INT2_PIN); uint8_t isFaceDown; // TODO: IMU rotation could be different (IMU upside down then isFaceUp) - status |= imu.Get_6D_Orientation_ZL(&isFaceDown); + status |= imu.Get_6D_Orientation_ZL(&isFaceDown); #ifndef LSM6DSV_NO_SELF_TEST_ON_FACEDOWN if (isFaceDown) { @@ -105,7 +104,7 @@ void LSM6DSVSensor::motionSetup() { status |= imu.begin(); // Restore defaults - status |= imu.Device_Reset(LSM6DSV_RESET_CTRL_REGS); + status |= imu.Device_Reset(LSM6DSV_RESET_GLOBAL); // Enable Block Data Update status |= imu.Enable_Block_Data_Update(); @@ -122,7 +121,7 @@ void LSM6DSVSensor::motionSetup() { #if (LSM6DSV_FUSION_SOURCE == LSM6DSV_FUSION_ESP) status |= imu.FIFO_Set_G_BDR(LSM6DSV_FIFO_DATA_RATE); -#endif +#endif // LSM6DSV_FUSION_SOURCE == LSM6DSV_FUSION_ESP status |= imu.FIFO_Set_Timestamp_Decimation( lsm6dsv_fifo_timestamp_batch_t::LSM6DSV_TMSTMP_DEC_1 @@ -145,9 +144,7 @@ void LSM6DSVSensor::motionSetup() { #if (LSM6DSV_FUSION_SOURCE == LSM6DSV_FUSION_ONBOARD) status |= imu.Set_SFLP_ODR(LSM6DSV_FIFO_DATA_RATE); status |= imu.Set_SFLP_Batch(true, true, false); -#endif - - +#endif // LSM6DSV_FUSION_SOURCE == LSM6DSV_FUSION_ONBOARD #if (LSM6DSV_FUSION_SOURCE == LSM6DSV_FUSION_ESP) // Calibration @@ -165,17 +162,18 @@ void LSM6DSVSensor::motionSetup() { m_Calibration.A_off[2] ); status |= imu.Enable_X_User_Offset(); -#endif -#endif //LSM6DSV_FUSION_SOURCE == LSM6DSV_FUSION_ESP +#endif // LSM6DSV_ACCEL_OFFSET_CAL +#endif // LSM6DSV_FUSION_SOURCE == LSM6DSV_FUSION_ESP status |= imu.Disable_6D_Orientation(); - #ifdef LSM6DSV_INTERRUPT attachInterrupt(m_IntPin, interruptHandler, RISING); - status |= imu.Enable_Single_Tap_Detection(LSM6DSV_INT1_PIN); // Tap recommends an interrupt + status |= imu.Enable_Single_Tap_Detection(LSM6DSV_INT1_PIN + ); // Tap recommends an interrupt #else - status |= imu.Enable_Single_Tap_Detection(LSM6DSV_INT2_PIN); //Just poll to see if an event happened jank but works + status |= imu.Enable_Single_Tap_Detection(LSM6DSV_INT2_PIN + ); // Just poll to see if an event happened jank but works #endif // LSM6DSV_INTERRUPT status |= imu.Set_Tap_Threshold(LSM6DSV_TAP_THRESHOLD); @@ -183,7 +181,7 @@ void LSM6DSVSensor::motionSetup() { status |= imu.Set_Tap_Quiet_Time(LSM6DSV_TAP_QUITE_TIME); status |= imu.FIFO_Reset(); - + if (status != LSM6DSV_OK) { m_Logger.fatal( "Errors occured enabling imu features on %s at address 0x%02x", @@ -203,7 +201,7 @@ void LSM6DSVSensor::motionSetup() { constexpr float mgPerG = 1000.0f; constexpr float mdpsPerDps = 1000.0f; constexpr float dpsPerRad = 57.295779578552f; -constexpr uint8_t fifoFramSize = 4; // X BDR, (G BDR || Game), Gravity, Timestamp +constexpr uint8_t fifoFramSize = 4; // X BDR, (G BDR || Game), Gravity, Timestamp void LSM6DSVSensor::motionLoop() { #ifdef LSM6DSV_INTERRUPT @@ -223,7 +221,7 @@ void LSM6DSVSensor::motionLoop() { if (eventStatus.TapStatus) { tap++; } -#endif +#endif // LSM6DSV_INTERRUPT if (millis() - lastTempRead > LSM6DSV_TEMP_READ_INTERVAL * 1000) { lastTempRead = millis(); @@ -269,7 +267,7 @@ void LSM6DSVSensor::motionLoop() { // Group all the data together // TODO: add the interupt watermark code to this - + if (fifo_samples < fifoFramSize) { return; } @@ -278,7 +276,8 @@ void LSM6DSVSensor::motionLoop() { #if (LSM6DSV_FUSION_SOURCE == LSM6DSV_FUSION_ESP) fusedRotation = sfusion.getQuaternionQuat() * sensorOffset; - if (ENABLE_INSPECTION || !OPTIMIZE_UPDATES || !lastFusedRotationSent.equalsWithEpsilon(fusedRotation)) { + if (ENABLE_INSPECTION || !OPTIMIZE_UPDATES + || !lastFusedRotationSent.equalsWithEpsilon(fusedRotation)) { newFusedRotation = true; lastFusedRotationSent = fusedRotation; } @@ -300,11 +299,12 @@ void LSM6DSVSensor::motionLoop() { newRawAcceleration = false; newRawGyro = false; } -#endif +#endif // LSM6DSV_FUSION_SOURCE == LSM6DSV_FUSION_ESP #ifdef DEBUG_SENSOR Vector3 rotation = lastFusedRotationSent.get_euler() * dpsPerRad; - m_Logger.trace(",%f,%f,%f,%d,%f,%d,%f,%f,%f,%f,%f,%f", + m_Logger.trace( + ",%f,%f,%f,%d,%f,%d,%f,%f,%f,%f,%f,%f", rotation.x, rotation.y, rotation.z, @@ -316,14 +316,15 @@ void LSM6DSVSensor::motionLoop() { rawGyro[2], rawAcceleration[0], rawAcceleration[1], - rawAcceleration[2]); -#endif // DEBUG_SENSOR + rawAcceleration[2] + ); +#endif // DEBUG_SENSOR } SensorStatus LSM6DSVSensor::getSensorState() { return isWorking() ? (status == LSM6DSV_OK ? SensorStatus::SENSOR_OK - : SensorStatus::SENSOR_ERROR) + : SensorStatus::SENSOR_ERROR) : SensorStatus::SENSOR_OFFLINE; } @@ -369,8 +370,7 @@ void LSM6DSVSensor::sendData() { } #endif // SEND_ACCELERATION - if (tap != 0) - { + if (tap != 0) { networkConnection.sendSensorTap(sensorId, tap); tap = 0; } @@ -397,7 +397,7 @@ LSM6DSVStatusTypeDef LSM6DSVSensor::readFifo(uint16_t fifo_samples) { case lsm6dsv_fifo_out_raw_t::LSM6DSV_TIMESTAMP_TAG: { if (i % fifoFramSize != 0) { return LSM6DSV_OK; // If we are not requesting a full data set - // then stop reading + // then stop reading } previousDataTime = currentDataTime; if (imu.FIFO_Get_Timestamp(¤tDataTime)) { @@ -414,7 +414,7 @@ LSM6DSVStatusTypeDef LSM6DSVSensor::readFifo(uint16_t fifo_samples) { #if (LSM6DSV_FUSION_SOURCE == LSM6DSV_FUSION_ESP) newRawGyro = false; -#endif +#endif // LSM6DSV_FUSION_SOURCE == LSM6DSV_FUSION_ESP break; } @@ -452,7 +452,11 @@ LSM6DSVStatusTypeDef LSM6DSVSensor::readFifo(uint16_t fifo_samples) { gravityVector[2] /= mgPerG; sensor_real_t linAcceleration[3]; - SlimeVR::Sensors::SensorFusion::calcLinearAcc(rawAcceleration, gravityVector, linAcceleration); + SlimeVR::Sensors::SensorFusion::calcLinearAcc( + rawAcceleration, + gravityVector, + linAcceleration + ); acceleration.x = linAcceleration[0]; acceleration.y = linAcceleration[1]; acceleration.z = linAcceleration[2]; @@ -471,31 +475,31 @@ LSM6DSVStatusTypeDef LSM6DSVSensor::readFifo(uint16_t fifo_samples) { ); return LSM6DSV_ERROR; } - + rawGyro[0] = (float)angularVelocity[0] / mdpsPerDps; rawGyro[1] = (float)angularVelocity[1] / mdpsPerDps; - rawGyro[2] = (float)angularVelocity[2] / mdpsPerDps; + rawGyro[2] = (float)angularVelocity[2] / mdpsPerDps; // convert to rads/s rawGyro[0] /= dpsPerRad; rawGyro[1] /= dpsPerRad; rawGyro[2] /= dpsPerRad; -#ifdef LSM6DSV_GYRO_OFFSET_CAL +#ifdef LSM6DSV_GYRO_OFFSET_CAL rawGyro[0] -= m_Calibration.G_off[0]; rawGyro[1] -= m_Calibration.G_off[1]; rawGyro[2] -= m_Calibration.G_off[2]; -#endif +#endif // LSM6DSV_GYRO_OFFSET_CAL #ifdef LSM6DSV_GYRO_SCALE_CAL rawGyro[0] *= m_Calibration.G_sensitivity[0]; rawGyro[1] *= m_Calibration.G_sensitivity[1]; rawGyro[2] *= m_Calibration.G_sensitivity[2]; -#endif +#endif // LSM6DSV_GYRO_SCALE_CAL newRawGyro = true; break; } -#endif +#endif // LSM6DSV_FUSION_SOURCE == LSM6DSV_FUSION_ESP #if (LSM6DSV_FUSION_SOURCE == LSM6DSV_FUSION_ONBOARD) case lsm6dsv_fifo_out_raw_t::LSM6DSV_SFLP_GAME_ROTATION_VECTOR_TAG: { @@ -518,7 +522,7 @@ LSM6DSVStatusTypeDef LSM6DSVSensor::readFifo(uint16_t fifo_samples) { lastData = millis(); break; } -#endif +#endif // LSM6DSV_FUSION_SOURCE == LSM6DSV_FUSION_ONBOARD default: { // We don't use the data so remove from fifo uint8_t data[6]; @@ -536,20 +540,18 @@ LSM6DSVStatusTypeDef LSM6DSVSensor::readFifo(uint16_t fifo_samples) { return LSM6DSV_OK; } - - #if (LSM6DSV_FUSION_SOURCE == LSM6DSV_FUSION_ESP) // Used for calibration (Blocking) LSM6DSVStatusTypeDef LSM6DSVSensor::readNextFifoFrame() { uint16_t fifo_samples = 0; while (fifo_samples < fifoFramSize) { if (imu.FIFO_Get_Num_Samples(&fifo_samples) == LSM6DSV_ERROR) { - m_Logger.error( - "Error getting number of samples in FIFO on %s at address 0x%02x", - getIMUNameByType(sensorType), - addr - ); - return LSM6DSV_ERROR; + m_Logger.error( + "Error getting number of samples in FIFO on %s at address 0x%02x", + getIMUNameByType(sensorType), + addr + ); + return LSM6DSV_ERROR; } } LSM6DSVStatusTypeDef result = readFifo(fifoFramSize); @@ -565,22 +567,30 @@ LSM6DSVStatusTypeDef LSM6DSVSensor::readNextFifoFrame() { } LSM6DSVStatusTypeDef LSM6DSVSensor::loadIMUCalibration() { - SlimeVR::Configuration::CalibrationConfig sensorCalibration = configuration.getCalibration(sensorId); - // If no compatible calibration data is found, the calibration data will just be zero-ed out - switch (sensorCalibration.type) { - case SlimeVR::Configuration::CalibrationConfigType::LSM6DSV: - m_Calibration = sensorCalibration.data.lsm6dsv; - break; - - case SlimeVR::Configuration::CalibrationConfigType::NONE: - m_Logger.warn("No calibration data found for sensor %d, ignoring...", sensorId); - m_Logger.info("Calibration is advised"); - break; - - default: - m_Logger.warn("Incompatible calibration data found for sensor %d, ignoring...", sensorId); - m_Logger.info("Calibration is advised"); - } + SlimeVR::Configuration::CalibrationConfig sensorCalibration + = configuration.getCalibration(sensorId); + // If no compatible calibration data is found, the calibration data will just be + // zero-ed out + switch (sensorCalibration.type) { + case SlimeVR::Configuration::CalibrationConfigType::LSM6DSV: + m_Calibration = sensorCalibration.data.lsm6dsv; + break; + + case SlimeVR::Configuration::CalibrationConfigType::NONE: + m_Logger.warn( + "No calibration data found for sensor %d, ignoring...", + sensorId + ); + m_Logger.info("Calibration is advised"); + break; + + default: + m_Logger.warn( + "Incompatible calibration data found for sensor %d, ignoring...", + sensorId + ); + m_Logger.info("Calibration is advised"); + } return LSM6DSV_OK; } @@ -596,11 +606,10 @@ void LSM6DSVSensor::calibrateGyro() { constexpr uint16_t calibrationSamples = 300; // Reset values float tempGxyz[3] = {0, 0, 0}; - + m_Calibration.G_off[0] = 0.0f; m_Calibration.G_off[1] = 0.0f; m_Calibration.G_off[2] = 0.0f; - // Wait for sensor to calm down before calibration m_Logger.info("Put down the device and wait for baseline gyro reading calibration"); @@ -631,9 +640,9 @@ void LSM6DSVSensor::calibrateGyro() { m_Calibration.G_off[1], m_Calibration.G_off[2] ); -#endif //DEBUG_SENSOR +#endif // DEBUG_SENSOR } -#endif //LSM6DSV_GYRO_OFFSET_CAL +#endif // LSM6DSV_GYRO_OFFSET_CAL #ifdef LSM6DSV_ACCEL_OFFSET_CAL void LSM6DSVSensor::calibrateAccel() { @@ -685,7 +694,6 @@ void LSM6DSVSensor::calibrateAccel() { status = LSM6DSV_ERROR; return; } - // Group all the data together if (fifo_samples < fifoFramSize) { @@ -761,7 +769,6 @@ void LSM6DSVSensor::calibrateAccel() { m_Calibration.A_off[1] = A_BAinv[0][1]; m_Calibration.A_off[2] = A_BAinv[0][2]; - #ifdef DEBUG_SENSOR m_Logger.trace( "Accel calibration results: %f %f %f", @@ -769,15 +776,15 @@ void LSM6DSVSensor::calibrateAccel() { m_Calibration.A_off[1], m_Calibration.A_off[2] ); -#endif //DEBUG_SENSOR +#endif // DEBUG_SENSOR } -#endif //LSM6DSV_ACCEL_OFFSET_CAL - +#endif // LSM6DSV_ACCEL_OFFSET_CAL #ifdef LSM6DSV_GYRO_SENSITIVITY_CAL void LSM6DSVSensor::calibrateGyroSensitivity() { m_Logger.info( - "Gyro sensitivity calibration started on sensor #%d of type %s at address 0x%02x", + "Gyro sensitivity calibration started on sensor #%d of type %s at address " + "0x%02x", getSensorId(), getIMUNameByType(sensorType), addr @@ -786,17 +793,18 @@ void LSM6DSVSensor::calibrateGyroSensitivity() { Quat rawRotationInit; Vector3 rawRotationFinal; uint8_t count = 0; - float gyroCount[3]; //Use this to determine the axis spun + float gyroCount[3]; // Use this to determine the axis spun float calculatedScale[3] = {1.0f, 1.0f, 1.0f}; - unsigned long prevLedTime = millis(); - constexpr uint16_t ledFlashDuration = 600; + unsigned long prevLedTime = millis(); + constexpr uint16_t ledFlashDuration = 600; ledManager.off(); m_Logger.info(""); m_Logger.info( - " Step 0: Let the tracker sit, the light will flash when you should reorient the tracker" + " Step 0: Let the tracker sit, the light will flash when you should reorient " + "the tracker" ); m_Logger.info( " Step 1: Move the tracker to a corner or edge that aligns the tracker to the " @@ -806,11 +814,10 @@ void LSM6DSVSensor::calibrateGyroSensitivity() { " NOTE: You might also want to unplug the USB so it doesn't affect spins" ); m_Logger.info( - " Step 2: Let the tracker rest until the solid light turns on, you might need to hold it" - ); - m_Logger.info( - " against a wall depending on the case and orientation" + " Step 2: Let the tracker rest until the solid light turns on, you might need " + "to hold it" ); + m_Logger.info(" against a wall depending on the case and orientation"); m_Logger.info( " Step 3: Rotate the tracker in the yaw axis for %d full rotations and align " "it with the previous edge ", @@ -826,59 +833,57 @@ void LSM6DSVSensor::calibrateGyroSensitivity() { " Step 4: Wait for the flashing light then rotate the tracker 90 degrees " " to a new axis and " ); - m_Logger.info(" align with an edge. Repeat steps 2 and 3"); + m_Logger.info(" align with an edge. Repeat steps 2 and 3"); m_Logger.info( - " Step 5: Wait for the flashing light then rotate the tracker 90 degrees so the last " + " Step 5: Wait for the flashing light then rotate the tracker 90 degrees so " + "the last " "axis is up and" ); - m_Logger.info(" aligned with an edge. Repeat steps 2 and 3"); + m_Logger.info(" aligned with an edge. Repeat steps 2 and 3"); imu.FIFO_Reset(); - delayMicroseconds(100); - while (!sfusion.getRestDetected()) //Wait for rest - { - readNextFifoFrame(); - } + delayMicroseconds(100); + while (!sfusion.getRestDetected()) // Wait for rest + { + readNextFifoFrame(); + } while (count < 3) { ledManager.on(); - prevLedTime = millis(); - m_Logger.info("Move the tracker to a new axis then let sit"); - while (sfusion.getRestDetected()) - { - unsigned long now = millis(); - if (now - ledFlashDuration > prevLedTime) { - //ledManager.toggle(); + prevLedTime = millis(); + m_Logger.info("Move the tracker to a new axis then let sit"); + while (sfusion.getRestDetected()) { + unsigned long now = millis(); + if (now - ledFlashDuration > prevLedTime) { + // ledManager.toggle(); ledManager.on(); - prevLedTime = millis(); - } - if (sfusion.getRestDetected() && sfusion.isUpdated()) { - rawRotationInit = sfusion.getQuaternionQuat(); - sfusion.clearUpdated(); - } - readNextFifoFrame(); - } + prevLedTime = millis(); + } + if (sfusion.getRestDetected() && sfusion.isUpdated()) { + rawRotationInit = sfusion.getQuaternionQuat(); + sfusion.clearUpdated(); + } + readNextFifoFrame(); + } ledManager.off(); - while (!sfusion.getRestDetected()) - { - readNextFifoFrame(); - } + while (!sfusion.getRestDetected()) { + readNextFifoFrame(); + } ledManager.on(); // The user should rotate m_Logger.info("Rotate the tracker %d times", LSM6DSV_GYRO_SENSITIVITY_SPINS); gyroCount[0] = 0.0f; gyroCount[1] = 0.0f; gyroCount[2] = 0.0f; - rawRotationInit = sfusion.getQuaternionQuat(); - while (sfusion.getRestDetected()) - { - if (sfusion.getRestDetected() && sfusion.isUpdated()) { - rawRotationInit = sfusion.getQuaternionQuat(); - sfusion.clearUpdated(); - } - readNextFifoFrame(); - } + rawRotationInit = sfusion.getQuaternionQuat(); + while (sfusion.getRestDetected()) { + if (sfusion.getRestDetected() && sfusion.isUpdated()) { + rawRotationInit = sfusion.getQuaternionQuat(); + sfusion.clearUpdated(); + } + readNextFifoFrame(); + } ledManager.off(); while (!sfusion.getRestDetected()) { @@ -888,48 +893,72 @@ void LSM6DSVSensor::calibrateGyroSensitivity() { gyroCount[2] += rawGyro[2]; } uint8_t isUp; - rawRotationFinal = (sfusion.getQuaternionQuat() * rawRotationInit.inverse()).get_euler() * dpsPerRad; + rawRotationFinal + = (sfusion.getQuaternionQuat() * rawRotationInit.inverse()).get_euler() + * dpsPerRad; - if (abs(gyroCount[0]) > abs(gyroCount[1]) && abs(gyroCount[0]) > abs(gyroCount[2])) { //Spun in X + if (abs(gyroCount[0]) > abs(gyroCount[1]) + && abs(gyroCount[0]) > abs(gyroCount[2])) { // Spun in X imu.Get_6D_Orientation_XH(&isUp); - if((!isUp && gyroCount[0] > 0) || (isUp && gyroCount[0] < 0)) { - calculatedScale[0] = (1.0 / (1.0 - ((-rawRotationFinal.y)/(360.0f * LSM6DSV_GYRO_SENSITIVITY_SPINS)))); + if ((!isUp && gyroCount[0] > 0) || (isUp && gyroCount[0] < 0)) { + calculatedScale[0] + = (1.0 + / (1.0 + - ((-rawRotationFinal.y) + / (360.0f * LSM6DSV_GYRO_SENSITIVITY_SPINS)))); m_Logger.info("X, Diff: %f", -rawRotationFinal.y); - } - else { - calculatedScale[0] = (1.0 / (1.0 - ((rawRotationFinal.y)/(360.0f * LSM6DSV_GYRO_SENSITIVITY_SPINS)))); + } else { + calculatedScale[0] + = (1.0 + / (1.0 + - ((rawRotationFinal.y) + / (360.0f * LSM6DSV_GYRO_SENSITIVITY_SPINS)))); m_Logger.info("-X, Diff: %f", rawRotationFinal.y); } } - else if (abs(gyroCount[1]) > abs(gyroCount[0]) && abs(gyroCount[1]) > abs(gyroCount[2])) { //Spun in Y + else if (abs(gyroCount[1]) > abs(gyroCount[0]) && abs(gyroCount[1]) > abs(gyroCount[2])) { // Spun in Y imu.Get_6D_Orientation_YH(&isUp); - if((isUp && gyroCount[1] > 0) || (!isUp && gyroCount[1] < 0)) { - calculatedScale[1] = (1.0 / (1.0 - ((-rawRotationFinal.y)/(360.0f * LSM6DSV_GYRO_SENSITIVITY_SPINS)))); + if ((isUp && gyroCount[1] > 0) || (!isUp && gyroCount[1] < 0)) { + calculatedScale[1] + = (1.0 + / (1.0 + - ((-rawRotationFinal.y) + / (360.0f * LSM6DSV_GYRO_SENSITIVITY_SPINS)))); m_Logger.info("Y, Diff: %f", -rawRotationFinal.y); - } - else { - calculatedScale[1] = (1.0 / (1.0 - ((rawRotationFinal.y)/(360.0f * LSM6DSV_GYRO_SENSITIVITY_SPINS)))); + } else { + calculatedScale[1] + = (1.0 + / (1.0 + - ((rawRotationFinal.y) + / (360.0f * LSM6DSV_GYRO_SENSITIVITY_SPINS)))); m_Logger.info("-Y, Diff: %f", rawRotationFinal.y); } } - else if (abs(gyroCount[2]) > abs(gyroCount[0]) && abs(gyroCount[2]) > abs(gyroCount[1])) { //Spun in Z + else if (abs(gyroCount[2]) > abs(gyroCount[0]) && abs(gyroCount[2]) > abs(gyroCount[1])) { // Spun in Z imu.Get_6D_Orientation_ZH(&isUp); - if((isUp && gyroCount[2] > 0) || (!isUp && gyroCount[2] < 0)) { - calculatedScale[2] = (1.0 / (1.0 - ((-rawRotationFinal.y)/(360.0f * LSM6DSV_GYRO_SENSITIVITY_SPINS)))); + if ((isUp && gyroCount[2] > 0) || (!isUp && gyroCount[2] < 0)) { + calculatedScale[2] + = (1.0 + / (1.0 + - ((-rawRotationFinal.y) + / (360.0f * LSM6DSV_GYRO_SENSITIVITY_SPINS)))); m_Logger.info("Z, Diff: %f", -rawRotationFinal.y); - } - else { - calculatedScale[2] = (1.0 / (1.0 - ((rawRotationInit.y)/(360.0f * LSM6DSV_GYRO_SENSITIVITY_SPINS)))); + } else { + calculatedScale[2] + = (1.0 + / (1.0 + - ((rawRotationInit.y) + / (360.0f * LSM6DSV_GYRO_SENSITIVITY_SPINS)))); m_Logger.info("-Z, Diff: %f", rawRotationInit.y); } } count++; } - m_Calibration.G_sensitivity[0] = calculatedScale[0]; - m_Calibration.G_sensitivity[1] = calculatedScale[1]; + m_Calibration.G_sensitivity[0] = calculatedScale[0]; + m_Calibration.G_sensitivity[1] = calculatedScale[1]; m_Calibration.G_sensitivity[2] = calculatedScale[2]; #ifdef DEBUG_SENSOR @@ -939,9 +968,9 @@ void LSM6DSVSensor::calibrateGyroSensitivity() { m_Calibration.G_sensitivity[1], m_Calibration.G_sensitivity[2] ); -#endif //DEBUG_SENSOR +#endif // DEBUG_SENSOR } -#endif // LSM6DSV_GYRO_SENSITIVITY_CAL +#endif // LSM6DSV_GYRO_SENSITIVITY_CAL void LSM6DSVSensor::startCalibration(int calibrationType) { m_Logger.info("Flip right side up in the next 5 seconds to start calibration."); @@ -1001,4 +1030,4 @@ void LSM6DSVSensor::saveCalibration() { configuration.save(); } -#endif // (LSM6DSV_FUSION_SOURCE == LSM6DSV_FUSION_ESP) +#endif // (LSM6DSV_FUSION_SOURCE == LSM6DSV_FUSION_ESP) diff --git a/src/sensors/lsm6dsvSensor.h b/src/sensors/lsm6dsvSensor.h index d89361251..3b3f89fcf 100644 --- a/src/sensors/lsm6dsvSensor.h +++ b/src/sensors/lsm6dsvSensor.h @@ -25,10 +25,10 @@ #define SENSORS_LSM6DSV_H #include "LSM6DSV.h" -#include "sensor.h" #include "SensorFusion.h" -#include "magneto1.4.h" #include "SensorFusionRestDetect.h" +#include "magneto1.4.h" +#include "sensor.h" #define LSM6DSV_FUSION_ESP 0 #define LSM6DSV_FUSION_ONBOARD 1 @@ -40,63 +40,60 @@ // #### IMU Reading Speed #### #ifndef LSM6DSV_ACCEL_MAX #define LSM6DSV_ACCEL_MAX 16 -#endif +#endif // LSM6DSV_ACCEL_MAX #ifndef LSM6DSV_GYRO_MAX #define LSM6DSV_GYRO_MAX 2000 -#endif +#endif // LSM6DSV_GYRO_MAX #ifndef LSM6DSV_FIFO_DATA_RATE #define LSM6DSV_FIFO_DATA_RATE 120 -#endif +#endif // LSM6DSV_FIFO_DATA_RATE #ifndef LSM6DSV_GYRO_ACCEL_RATE #define LSM6DSV_GYRO_ACCEL_RATE 7680.0f -#endif +#endif // LSM6DSV_GYRO_ACCEL_RATE -//#ifndef LSM6DSV_FIFO_TEMP_DATA_RATE //TODO: We should use this instead -//#define LSM6DSV_FIFO_TEMP_DATA_RATE 1.875f -//#endif +// #ifndef LSM6DSV_FIFO_TEMP_DATA_RATE //TODO: We should use this instead +// #define LSM6DSV_FIFO_TEMP_DATA_RATE 1.875f +// #endif #ifndef LSM6DSV_TEMP_READ_INTERVAL #define LSM6DSV_TEMP_READ_INTERVAL 1 -#endif +#endif // LSM6DSV_TEMP_READ_INTERVAL // #### IMU Tap Detection #### #ifndef LSM6DSV_TAP_THRESHOLD -#define LSM6DSV_TAP_THRESHOLD 5 //0-32 -#endif +#define LSM6DSV_TAP_THRESHOLD 5 // 0-32 +#endif // LSM6DSV_TAP_THRESHOLD #ifndef LSM6DSV_TAP_SHOCK_TIME -#define LSM6DSV_TAP_SHOCK_TIME 3 //0-3 -#endif +#define LSM6DSV_TAP_SHOCK_TIME 3 // 0-3 +#endif // LSM6DSV_TAP_SHOCK_TIME #ifndef LSM6DSV_TAP_QUITE_TIME -#define LSM6DSV_TAP_QUITE_TIME 3 //0-3 -#endif +#define LSM6DSV_TAP_QUITE_TIME 3 // 0-3 +#endif // LSM6DSV_TAP_QUITE_TIME // #### General IMU Settings #### #define LSM6DSV_INTERRUPT // interupt recommended but not required // #define LSM6DSV_NO_SELF_TEST_ON_FACEDOWN -#define LSM6DSV_FUSION_SOURCE LSM6DSV_FUSION_ONBOARD // LSM6DSV_FUSION_ESP or LSM6DSV_FUSION_ONBOARD - +#define LSM6DSV_FUSION_SOURCE LSM6DSV_FUSION_ESP // LSM6DSV_FUSION_ESP or LSM6DSV_FUSION_ONBOARD // #### IMU Calibration #### #ifndef LSM6DSV_GYRO_SENSITIVITY_SPINS -#define LSM6DSV_GYRO_SENSITIVITY_SPINS 1 // Only used if LSM6DSV_FUSION_SOURCE == LSM6DSV_FUSION_ESP -#endif - +#define LSM6DSV_GYRO_SENSITIVITY_SPINS 2 // Only used if LSM6DSV_FUSION_SOURCE == LSM6DSV_FUSION_ESP +#endif // LSM6DSV_GYRO_SENSITIVITY_SPINS // ############################################# // ######### End user configuration ############ // ############################################# - #if (LSM6DSV_FUSION_SOURCE == LSM6DSV_FUSION_ESP) #define LSM6DSV_GYRO_OFFSET_CAL #define LSM6DSV_ACCEL_OFFSET_CAL #define LSM6DSV_GYRO_SENSITIVITY_CAL -#endif +#endif // LSM6DSV_FUSION_SOURCE == LSM6DSV_FUSION_ESP class LSM6DSVSensor : public Sensor { public: @@ -120,9 +117,9 @@ class LSM6DSVSensor : public Sensor { void calibrateGyro(); void calibrateAccel(); void calibrateGyroSensitivity(); - void resetCalibration(); -#endif - + void resetCalibration(); +#endif // LSM6DSV_FUSION_SOURCE == LSM6DSV_FUSION_ESP + private: Quat fusedRotationToQuaternion(float x, float y, float z); LSM6DSVStatusTypeDef runSelfTest(); @@ -149,7 +146,7 @@ class LSM6DSVSensor : public Sensor { SlimeVR::Sensors::SensorFusionRestDetect sfusion; float rawGyro[3]; bool newRawGyro = false; -#endif +#endif // LSM6DSV_FUSION_SOURCE == LSM6DSV_FUSION_ESP }; -#endif +#endif // SENSORS_LSM6DSV_H From 15f544ba1ff07bd2567cb966201c802506167b62 Mon Sep 17 00:00:00 2001 From: wigwagwent Date: Sat, 2 Dec 2023 17:35:02 -0700 Subject: [PATCH 57/60] Fix calibration --- src/sensors/lsm6dsvSensor.cpp | 57 +++++++++++------------------------ src/sensors/lsm6dsvSensor.h | 2 -- 2 files changed, 17 insertions(+), 42 deletions(-) diff --git a/src/sensors/lsm6dsvSensor.cpp b/src/sensors/lsm6dsvSensor.cpp index 1ead324a7..e6bcf2bd7 100644 --- a/src/sensors/lsm6dsvSensor.cpp +++ b/src/sensors/lsm6dsvSensor.cpp @@ -101,14 +101,9 @@ void LSM6DSVSensor::motionSetup() { m_Logger.info("Connected to %s on 0x%02x", getIMUNameByType(sensorType), addr); - status |= imu.begin(); - - // Restore defaults status |= imu.Device_Reset(LSM6DSV_RESET_GLOBAL); - // Enable Block Data Update - status |= imu.Enable_Block_Data_Update(); - status |= imu.Enable_Auto_Increment(); + status |= imu.begin(); // Set maximums status |= imu.Set_X_FS(LSM6DSV_ACCEL_MAX); @@ -132,6 +127,8 @@ void LSM6DSVSensor::motionSetup() { status |= imu.Enable_X(); status |= imu.Enable_G(); + status |= imu.Enable_6D_Orientation(LSM6DSV_INT2_PIN); + // status |= imu.Set_X_Filter_Mode(1, LSM6DSV_XL_LIGHT); // status |= imu.Set_G_Filter_Mode(1, LSM6DSV_XL_LIGHT); @@ -169,11 +166,9 @@ void LSM6DSVSensor::motionSetup() { #ifdef LSM6DSV_INTERRUPT attachInterrupt(m_IntPin, interruptHandler, RISING); - status |= imu.Enable_Single_Tap_Detection(LSM6DSV_INT1_PIN - ); // Tap recommends an interrupt + status |= imu.Enable_Single_Tap_Detection(LSM6DSV_INT1_PIN); // Tap recommends an interrupt #else - status |= imu.Enable_Single_Tap_Detection(LSM6DSV_INT2_PIN - ); // Just poll to see if an event happened jank but works + status |= imu.Enable_Single_Tap_Detection(LSM6DSV_INT2_PIN); // Just poll to see if an event happened jank but works #endif // LSM6DSV_INTERRUPT status |= imu.Set_Tap_Threshold(LSM6DSV_TAP_THRESHOLD); @@ -201,7 +196,7 @@ void LSM6DSVSensor::motionSetup() { constexpr float mgPerG = 1000.0f; constexpr float mdpsPerDps = 1000.0f; constexpr float dpsPerRad = 57.295779578552f; -constexpr uint8_t fifoFramSize = 4; // X BDR, (G BDR || Game), Gravity, Timestamp +constexpr uint8_t fifoFrameSize = 4; // X BDR, (G BDR || Rotation), Gravity, Timestamp void LSM6DSVSensor::motionLoop() { #ifdef LSM6DSV_INTERRUPT @@ -268,7 +263,7 @@ void LSM6DSVSensor::motionLoop() { // Group all the data together // TODO: add the interupt watermark code to this - if (fifo_samples < fifoFramSize) { + if (fifo_samples < fifoFrameSize) { return; } @@ -395,7 +390,7 @@ LSM6DSVStatusTypeDef LSM6DSVSensor::readFifo(uint16_t fifo_samples) { switch (tag) { case lsm6dsv_fifo_out_raw_t::LSM6DSV_TIMESTAMP_TAG: { - if (i % fifoFramSize != 0) { + if (i % fifoFrameSize != 0) { return LSM6DSV_OK; // If we are not requesting a full data set // then stop reading } @@ -490,11 +485,11 @@ LSM6DSVStatusTypeDef LSM6DSVSensor::readFifo(uint16_t fifo_samples) { rawGyro[1] -= m_Calibration.G_off[1]; rawGyro[2] -= m_Calibration.G_off[2]; #endif // LSM6DSV_GYRO_OFFSET_CAL -#ifdef LSM6DSV_GYRO_SCALE_CAL +#ifdef LSM6DSV_GYRO_SENSITIVITY_CAL rawGyro[0] *= m_Calibration.G_sensitivity[0]; rawGyro[1] *= m_Calibration.G_sensitivity[1]; rawGyro[2] *= m_Calibration.G_sensitivity[2]; -#endif // LSM6DSV_GYRO_SCALE_CAL +#endif // LSM6DSV_GYRO_SENSITIVITY_CAL newRawGyro = true; break; @@ -544,7 +539,7 @@ LSM6DSVStatusTypeDef LSM6DSVSensor::readFifo(uint16_t fifo_samples) { // Used for calibration (Blocking) LSM6DSVStatusTypeDef LSM6DSVSensor::readNextFifoFrame() { uint16_t fifo_samples = 0; - while (fifo_samples < fifoFramSize) { + while (fifo_samples < fifoFrameSize) { if (imu.FIFO_Get_Num_Samples(&fifo_samples) == LSM6DSV_ERROR) { m_Logger.error( "Error getting number of samples in FIFO on %s at address 0x%02x", @@ -554,7 +549,7 @@ LSM6DSVStatusTypeDef LSM6DSVSensor::readNextFifoFrame() { return LSM6DSV_ERROR; } } - LSM6DSVStatusTypeDef result = readFifo(fifoFramSize); + LSM6DSVStatusTypeDef result = readFifo(fifoFrameSize); if (newRawAcceleration && newRawGyro) { sfusion.update6D( @@ -567,8 +562,7 @@ LSM6DSVStatusTypeDef LSM6DSVSensor::readNextFifoFrame() { } LSM6DSVStatusTypeDef LSM6DSVSensor::loadIMUCalibration() { - SlimeVR::Configuration::CalibrationConfig sensorCalibration - = configuration.getCalibration(sensorId); + SlimeVR::Configuration::CalibrationConfig sensorCalibration = configuration.getCalibration(sensorId); // If no compatible calibration data is found, the calibration data will just be // zero-ed out switch (sensorCalibration.type) { @@ -619,7 +613,6 @@ void LSM6DSVSensor::calibrateGyro() { uint16_t count = 0; while (count < calibrationSamples) { - printf("Gyro Sample"); readNextFifoFrame(); if (newRawGyro) { tempGxyz[0] += rawGyro[0]; @@ -696,7 +689,7 @@ void LSM6DSVSensor::calibrateAccel() { } // Group all the data together - if (fifo_samples < fifoFramSize) { + if (fifo_samples < fifoFrameSize) { continue; } @@ -975,10 +968,10 @@ void LSM6DSVSensor::calibrateGyroSensitivity() { void LSM6DSVSensor::startCalibration(int calibrationType) { m_Logger.info("Flip right side up in the next 5 seconds to start calibration."); delay(5000); - uint8_t isFaceUp; - imu.Get_6D_Orientation_ZH(&isFaceUp); + uint8_t isFaceDown; + imu.Get_6D_Orientation_ZL(&isFaceDown); - if (!isFaceUp) { + if (isFaceDown) { loadIMUCalibration(); return; } @@ -1004,22 +997,6 @@ void LSM6DSVSensor::startCalibration(int calibrationType) { m_Logger.info("Calibration finished, enjoy"); } -void LSM6DSVSensor::resetCalibration() { - m_Logger.info("Sensor #%d Calibration Reset", getSensorId()); - m_Calibration.A_off[0] = 0.0f; - m_Calibration.A_off[1] = 0.0f; - m_Calibration.A_off[2] = 0.0f; - - m_Calibration.G_off[0] = 0.0f; - m_Calibration.G_off[1] = 0.0f; - m_Calibration.G_off[2] = 0.0f; - - m_Calibration.G_sensitivity[0] = 1.0f; - m_Calibration.G_sensitivity[1] = 1.0f; - m_Calibration.G_sensitivity[2] = 1.0f; - saveCalibration(); -} - void LSM6DSVSensor::saveCalibration() { m_Logger.debug("Saving the calibration data"); diff --git a/src/sensors/lsm6dsvSensor.h b/src/sensors/lsm6dsvSensor.h index 3b3f89fcf..ac518ac4c 100644 --- a/src/sensors/lsm6dsvSensor.h +++ b/src/sensors/lsm6dsvSensor.h @@ -117,11 +117,9 @@ class LSM6DSVSensor : public Sensor { void calibrateGyro(); void calibrateAccel(); void calibrateGyroSensitivity(); - void resetCalibration(); #endif // LSM6DSV_FUSION_SOURCE == LSM6DSV_FUSION_ESP private: - Quat fusedRotationToQuaternion(float x, float y, float z); LSM6DSVStatusTypeDef runSelfTest(); LSM6DSVStatusTypeDef readFifo(uint16_t fifo_samples); From fada70f27c4b7b01ca10799752787e4e9bac5274 Mon Sep 17 00:00:00 2001 From: wigwagwent Date: Sat, 2 Dec 2023 19:13:18 -0700 Subject: [PATCH 58/60] Default to builtin fusion --- src/sensors/lsm6dsvSensor.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/sensors/lsm6dsvSensor.h b/src/sensors/lsm6dsvSensor.h index ac518ac4c..3ed114782 100644 --- a/src/sensors/lsm6dsvSensor.h +++ b/src/sensors/lsm6dsvSensor.h @@ -78,7 +78,7 @@ // #### General IMU Settings #### #define LSM6DSV_INTERRUPT // interupt recommended but not required // #define LSM6DSV_NO_SELF_TEST_ON_FACEDOWN -#define LSM6DSV_FUSION_SOURCE LSM6DSV_FUSION_ESP // LSM6DSV_FUSION_ESP or LSM6DSV_FUSION_ONBOARD +#define LSM6DSV_FUSION_SOURCE LSM6DSV_FUSION_ONBOARD // LSM6DSV_FUSION_ESP or LSM6DSV_FUSION_ONBOARD // #### IMU Calibration #### #ifndef LSM6DSV_GYRO_SENSITIVITY_SPINS From 21be6243ac2474cfe8bf3d6d53e352d80e9cad97 Mon Sep 17 00:00:00 2001 From: wigwagwent Date: Tue, 12 Dec 2023 08:40:07 -0700 Subject: [PATCH 59/60] Add low pass filter for less jitter --- src/sensors/lsm6dsvSensor.cpp | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/src/sensors/lsm6dsvSensor.cpp b/src/sensors/lsm6dsvSensor.cpp index e6bcf2bd7..d74f854f5 100644 --- a/src/sensors/lsm6dsvSensor.cpp +++ b/src/sensors/lsm6dsvSensor.cpp @@ -129,11 +129,8 @@ void LSM6DSVSensor::motionSetup() { status |= imu.Enable_6D_Orientation(LSM6DSV_INT2_PIN); - // status |= imu.Set_X_Filter_Mode(1, LSM6DSV_XL_LIGHT); - // status |= imu.Set_G_Filter_Mode(1, LSM6DSV_XL_LIGHT); - - // status |= imu.Set_X_Filter_Mode(0, LSM6DSV_XL_ULTRA_LIGHT); - // status |= imu.Set_G_Filter_Mode(0, LSM6DSV_XL_ULTRA_LIGHT); + status |= imu.Set_X_Filter_Mode(0, LSM6DSV_XL_LIGHT); + status |= imu.Set_G_Filter_Mode(0, LSM6DSV_GY_LIGHT); // Set FIFO mode to "continuous", so old data gets thrown away status |= imu.FIFO_Set_Mode(LSM6DSV_STREAM_MODE); From f57495621baeed5f5bed33b1cee2b5fa03538e81 Mon Sep 17 00:00:00 2001 From: wigwagwent <66268000+wigwagwent@users.noreply.github.com> Date: Thu, 28 Dec 2023 18:47:31 -0700 Subject: [PATCH 60/60] Update README.md --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 14d33c2b6..470129e69 100644 --- a/README.md +++ b/README.md @@ -35,7 +35,7 @@ The following IMUs and their corresponding `IMU` values are supported by the fir * Using fusion in internal DMP for 6Dof or 9DoF, 9DoF mode requires good magnetic environment. * Comment out `USE_6DOF` in `debug.h` for 9DoF mode. * Experimental support!\ -* LSM6DSV (IMU_ICM20948) +* LSM6DSV (IMU_LSM6DSV) * Supports the LSM6DSV, LSM6DSV16X, LSM6DSV16B, LSM6DSV16XB * Using fusion in internal DMP for 6Dof or support for VQF fusion on the esp using accelerometer and gyroscope data. * **See Sensor calibration** below for info on calibrating this sensor in VQF fusion mode.